3D Maths
Posted: Mon Aug 23, 2004 3:21 pm
I meant to release this a long time ago, it is nothing special but I thought maybe someone could use it. I use these maths in my OpenGL engine which I need to get back to work on. I just now started exploring some old code I have after a long break from programming, so it is possible there is a small bug somewhere. I really do not use the matrix math myself but I do have them in there, I prefer to use Quaternions myself and have found them to be much cleaner but took me a while to understand given I have no math background. I use these math's for pretty much everything thus far, the quaternion code i specificaly use for doing my cameras. Anyway... without further rambling...
If there is a problem, pls post here and I shall fix it right up. This file may look a little dirty atm, I didn't take time to clean it up yet =p. Oh, and sorry for the lack of comments, pressed for time this morning.
BTW, I have prefixed all the functions with LEG_ to lessen the chance of conflict, as I will do with all my public code (since my nickname is Legider).
Code: Select all
; ################################################################### ;
;/ (VMQ) - Vector Matrix Quaternion
;/ By: Matt Greene (aka Legider)
;/ Contact: http://www.devend.com or matt@devend.com
;/ Notes: Here are some functions for handling the rather important
;/ and common maths used in 3d engines. This file may change
;/ over time if I decide to add new functions or fix/edit
;/ some old ones. If you have any suggestions or comments
;/ then please feel free to contact me. Anyone can use this
;/ file any way they wish, but if you could just mention my
;/ name if you decide to, I would appreciate it.
; ################################################################### ;
; ################################################################### ;
;/
;/ Structures
;/
; ################################################################### ;
Structure Matrix4
W1.f
X1.f
Y1.f
Z1.f
W2.f
X2.f
Y2.f
Z2.f
W3.f
X3.f
Y3.f
Z3.f
W4.f
X4.f
Y4.f
Z4.f
EndStructure
Structure QUATERNION
X.f
Y.f
Z.f
w.f
EndStructure
Structure Vector
X.f
Y.f
Z.f
EndStructure
; ################################################################### ;
;/
;/ Vector Maths
;/
; ################################################################### ;
Procedure LEG_Vector_Add(*ReturnVector.Vector, *V1.Vector, *V2.Vector)
*ReturnVector\X = *V1\X + *V2\X
*ReturnVector\Y = *V1\Y + *V2\Y
*ReturnVector\Z = *V1\Z + *V2\Z
EndProcedure
;/
Procedure LEG_Vector_Subtract(*ReturnVector.Vector, *V1.Vector, *V2.Vector)
*ReturnVector\X = *V1\X - *V2\X
*ReturnVector\Y = *V1\Y - *V2\Y
*ReturnVector\Z = *V1\Z - *V2\Z
EndProcedure
;/
Procedure LEG_Vector_CrossProduct(*ReturnVector.Vector, *V1.Vector, *V2.Vector)
*ReturnVector\X = *V1\Y * *V2\Z - *V2\Y * *V1\Z
*ReturnVector\Y = *V1\Z * *V2\X - *V2\Z * *V1\X
*ReturnVector\Z = *V1\X * *V2\Y - *V2\X * *V1\Y
EndProcedure
;/
Procedure.f LEG_Vector_DotProduct(*V1.Vector, *V2.Vector)
ProcedureReturn (*V1\X * *V2\X) + (*V1\Y * *V2\Y) + (*V1\Z * *V2\Z)
EndProcedure
;/
Procedure.f LEG_Vector_Length(X.f, Y.f, Z.f)
ProcedureReturn Sqr(X * X + Y * Y + Z * Z)
EndProcedure
;/
Procedure LEG_Vector_Normalize(*NormVector.Vector)
length.f = LEG_Vector_Length(*NormVector\X, *NormVector\Y, *NormVector\Z)
*NormVector\X = *NormVector\X / length
*NormVector\Y = *NormVector\Y / length
*NormVector\Z = *NormVector\Z / length
EndProcedure
; ################################################################### ;
;/
;/ Matrix Maths
;/
; ################################################################### ;
Procedure LEG_Matrix_Multiply(*NewMatrix.Matrix4, *M.Matrix4, *L.Matrix4)
*NewMatrix\W1 = *M\W1 * *L\W1 + *M\X1 * *L\W2 + *M\Y1 * *L\W3 + *M\Z1 * *L\W4
*NewMatrix\X1 = *M\W2 * *L\W1 + *M\X2 * *L\W2 + *M\Y2 * *L\W3 + *M\Z2 * *L\W4
*NewMatrix\Y1 = *M\W3 * *L\W1 + *M\X3 * *L\W2 + *M\Y3 * *L\W3 + *M\Z3 * *L\W4
*NewMatrix\Z1 = *M\W4 * *L\W1 + *M\X4 * *L\W2 + *M\Y4 * *L\W3 + *M\Z4 * *L\W4
*NewMatrix\W2 = *M\W1 * *L\X1 + *M\X1 * *L\X2 + *M\Y1 * *L\X3 + *M\Z1 * *L\X4
*NewMatrix\X2 = *M\W2 * *L\X1 + *M\X2 * *L\X2 + *M\Y2 * *L\X3 + *M\Z2 * *L\X4
*NewMatrix\Y2 = *M\W3 * *L\X1 + *M\X3 * *L\X2 + *M\Y3 * *L\X3 + *M\Z3 * *L\X4
*NewMatrix\Z2 = *M\W4 * *L\X1 + *M\X4 * *L\X2 + *M\Y4 * *L\X3 + *M\Z4 * *L\X4
*NewMatrix\W3 = *M\W1 * *L\Y1 + *M\X1 * *L\Y2 + *M\Y1 * *L\Y3 + *M\Z1 * *L\Y4
*NewMatrix\X3 = *M\W2 * *L\Y1 + *M\X2 * *L\Y2 + *M\Y2 * *L\Y3 + *M\Z2 * *L\Y4
*NewMatrix\Y3 = *M\W3 * *L\Y1 + *M\X3 * *L\Y2 + *M\Y3 * *L\Y3 + *M\Z3 * *L\Y4
*NewMatrix\Z3 = *M\W4 * *L\Y1 + *M\X4 * *L\Y2 + *M\Y4 * *L\Y3 + *M\Z4 * *L\Y4
*NewMatrix\W4 = *M\W1 * *L\Z1 + *M\X1 * *L\Z2 + *M\Y1 * *L\Z3 + *M\Z1 * *L\Z4
*NewMatrix\X4 = *M\W2 * *L\Z1 + *M\X2 * *L\Z2 + *M\Y2 * *L\Z3 + *M\Z2 * *L\Z4
*NewMatrix\Y4 = *M\W3 * *L\Z1 + *M\X3 * *L\Z2 + *M\Y3 * *L\Z3 + *M\Z3 * *L\Z4
*NewMatrix\Z4 = *M\W4 * *L\Z1 + *M\X4 * *L\Z2 + *M\Y4 * *L\Z3 + *M\Z4 * *L\Z4
EndProcedure
;/
Procedure LEG_Matrix_FromEuler(angle_x.f, angle_y.f, angle_z.f)
DefType.f A,b,C,D,E,f,AD,BD
A = Cos(angle_x)
b = Sin(angle_x)
C = Cos(angle_y)
D = Sin(angle_y)
E = Cos(angle_z)
f = Sin(angle_z)
AD = A * D
BD = b * D
ReturnMatrix\W1 = C * E
ReturnMatrix\X1 = -C * f
ReturnMatrix\Y1 = D
ReturnMatrix\W2 = BD * E + A * f
ReturnMatrix\X2 = -BD * f + A * E
ReturnMatrix\Y2 = -b * C
ReturnMatrix\W3 = -AD * E + b * f
ReturnMatrix\X3 = AD * f + b * E
ReturnMatrix\Y3 = A * C
ReturnMatrix\Z1 = 0
ReturnMatrix\Z2 = 0
ReturnMatrix\Z3 = 0
ReturnMatrix\W4 = 0
ReturnMatrix\X4 = 0
ReturnMatrix\Y4 = 0
ReturnMatrix\Z4 = 1
EndProcedure
; ################################################################### ;
;/
;/ Quaternion Maths
;/
; ################################################################### ;
Procedure.f LEG_Quaternion_Length(*TempQuat.QUATERNION)
ProcedureReturn Sqr(Pow(*TempQuat\X,2) + Pow(*TempQuat\Y,2) + Pow(*TempQuat\Z,2) + Pow(*TempQuat\w,2))
EndProcedure
;/
Procedure LEG_Quaternion_Conjugate(*NewQuat.QUATERNION, *Quat1.QUATERNION)
*NewQuat\X = -1 * *Quat1\X
*NewQuat\Y = -1 * *Quat1\Y
*NewQuat\Z = -1 * *Quat1\Z
*NewQuat\w = *Quat1\w
EndProcedure
;/
Procedure LEG_Quaternion_Multiply(*NewQuat.QUATERNION, *Quat1.QUATERNION, *Quat2.QUATERNION)
*NewQuat\w = *Quat1\w * *Quat2\w - *Quat1\X * *Quat2\X - *Quat1\Y * *Quat2\Y - *Quat1\Z * *Quat2\Z
*NewQuat\X = *Quat1\w * *Quat2\X + *Quat1\X * *Quat2\w + *Quat1\Y * *Quat2\Z - *Quat1\Z * *Quat2\Y
*NewQuat\Y = *Quat1\w * *Quat2\Y + *Quat1\Y * *Quat2\w + *Quat1\Z * *Quat2\X - *Quat1\X * *Quat2\Z
*NewQuat\Z = *Quat1\w * *Quat2\Z + *Quat1\Z * *Quat2\w + *Quat1\X * *Quat2\Y - *Quat1\Y * *Quat2\X
EndProcedure
;/
Procedure LEG_Quaternion_Normalize(*TempQuat.QUATERNION)
TEMP_length.f = LEG_Quaternion_Length(*TempQuat)
*TempQuat\X = *TempQuat\X / TEMP_length
*TempQuat\Y = *TempQuat\Y / TEMP_length
*TempQuat\Z = *TempQuat\Z / TEMP_length
*TempQuat\w = *TempQuat\w / TEMP_length
EndProcedure
;/
Procedure LEG_Quaternion_FromEuler(*TempQuat.QUATERNION, Yaw.f, Pitch.f, Roll.f)
If Yaw = 0 And Pitch = 0 And Roll = 0
*TempQuat\X = 0
*TempQuat\Y = 0
*TempQuat\Z = 0
*TempQuat\w = 1
ProcedureReturn 0
EndIf
Roll = (Roll * 0.01745329) / 2
Pitch = (Pitch * 0.01745329) / 2
Yaw = (Yaw * 0.01745329) / 2
cosRoll.f = Cos(Roll)
cosPitch.f = Cos(Pitch)
cosYaw.f = Cos(Yaw)
sinRoll.f = Sin(Roll)
sinPitch.f = Sin(Pitch)
sinYaw.f = Sin(Yaw)
cosPitchYaw.f = cosPitch * cosYaw
sinPitchYaw.f = sinPitch * sinYaw
*TempQuat\X = sinRoll * cosPitchYaw - cosRoll * sinPitchYaw
*TempQuat\Y = cosRoll * sinPitch * cosYaw + sinRoll * cosPitch * sinYaw
*TempQuat\Z = cosRoll * cosPitch * sinYaw - sinRoll * sinPitch * cosYaw
*TempQuat\w = cosRoll * cosPitchYaw + sinRoll * sinPitchYaw
LEG_Quaternion_Normalize(*TempQuat)
EndProcedure
;/
Procedure LEG_Quaternion_FromAngleAxis(*TempQuat.QUATERNION, X.f, Y.f, Z.f, angleDegrees.f)
If X = 0 And Y = 0 And Z = 0
*TempQuat\X = 0
*TempQuat\Y = 0
*TempQuat\Z = 0
*TempQuat\w = 1
ProcedureReturn 0
EndIf
TEMP_sum.f = X * X + Y * Y + Z * Z
TEMP_length.f = Sqr(TEMP_sum)
TEMP_angle.f = angleDegrees * 0.01745329
TEMP_angle = TEMP_angle / 2
TEMP_scale.f = Sin(TEMP_angle)
*TempQuat\X = TEMP_scale * X / TEMP_sum
*TempQuat\Y = TEMP_scale * Y / TEMP_sum
*TempQuat\Z = TEMP_scale * Z / TEMP_sum
*TempQuat\w = Cos(TEMP_angle)
LEG_Quaternion_Normalize(*TempQuat)
EndProcedureBTW, I have prefixed all the functions with LEG_ to lessen the chance of conflict, as I will do with all my public code (since my nickname is Legider).