3D Maths

legider_pb · Post by **legider_pb** » 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...

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)
EndProcedure

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).