PureBasic Forum

   
   Degree2Rad() and Rad2degree()
   Simply substitute according with PB's own Radian() and Degree()
   Thanks to both excellent dudes for pointing it out :)

; ################################################################### ;
;/
;/  Vector Maths
;/
; ################################################################### ;

Structure Vector
  X.f
  Y.f
  Z.f
EndStructure

Procedure 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 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 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 Vector_DotProduct(*V1.Vector, *V2.Vector)
  ProcedureReturn (*V1\X * *V2\X) + (*V1\Y * *V2\Y) + (*V1\Z * *V2\Z)
EndProcedure

Procedure.f Vector_Length(X.f, Y.f, Z.f)
  ProcedureReturn Sqr(X * X + Y * Y + Z * Z)
EndProcedure

Procedure Vector_Normalize(*NormVector.Vector)
  length.f = 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
;/
; ################################################################### ;

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

Procedure 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 Matrix_FromEuler(*ReturnMatrix.Matrix4, angle_x.f, angle_y.f, angle_z.f)
  Protected.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
;/
; ################################################################### ;

Structure Vector
  X.f
  Y.f
  Z.f
EndStructure

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

Procedure Quaternion_ToMatrix(*ReturnMatrix.Matrix4, *q.Quaternion)
  *ReturnMatrix\W1 = 1.0 - 2.0 * ( *q\Y * *q\Y + *q\Z * *q\Z );
  *ReturnMatrix\X1 = 2.0 * (*q\X * *q\Y + *q\Z * *q\w);
  *ReturnMatrix\Y1 = 2.0 * (*q\X * *q\Z - *q\Y * *q\w);
  *ReturnMatrix\Z1 = 0.0;
 
  ; Second row
  *ReturnMatrix\W2 = 2.0 * ( *q\X * *q\Y - *q\Z * *q\w );
  *ReturnMatrix\X2 = 1.0 - 2.0 * ( *q\X * *q\X + *q\Z * *q\Z );
  *ReturnMatrix\Y2 = 2.0 * (*q\Z * *q\Y + *q\X * *q\w );
  *ReturnMatrix\Z2 = 0.0;
 
  ; Third row
  *ReturnMatrix\W3 = 2.0 * ( *q\X * *q\Z + *q\Y * *q\w );
  *ReturnMatrix\X3 = 2.0 * ( *q\Y * *q\Z - *q\X * *q\w );
  *ReturnMatrix\Y3 = 1.0 - 2.0 * ( *q\X * *q\X + *q\Y * *q\Y );
  *ReturnMatrix\Z3 = 0.0;
 
  ; Fourth row
  *ReturnMatrix\W4 = 0;
  *ReturnMatrix\X4 = 0;
  *ReturnMatrix\Y4 = 0;
  *ReturnMatrix\Z4 = 1.0;
EndProcedure

Procedure Quaternion_ToEular(*ReturnEular.Vector, *q.Quaternion)
  Protected test.f = *q\X * *q\Y + *q\Z * *q\w;
  Protected.f heading, attitude, bank
 
  If (test > 0.499) ; singularity at north pole
    heading = 2 * ATan2(*q\X, *q\w);
    attitude = #PI/2;
    bank = 0;
  ElseIf (test < -0.499) ; singularity at south pole
    heading = -2 * ATan2(*q\X, *q\w);
    attitude = - #PI/2;
    bank = 0;
  Else
    Protected sqx.f = *q\X * *q\X;
    Protected sqy.f = *q\Y * *q\Y;
    Protected sqz.f = *q\Z * *q\Z;
   
    heading = ATan2(2 * *q\Y * *q\w - 2 * *q\X * *q\Z, 1 - 2 * sqy - 2 * sqz);
    attitude = ASin(2 * test);
    bank = ATan2(2 * *q\X * *q\w - 2 * *q\Y * *q\Z, 1 - 2 * sqx - 2 * sqz)
  EndIf
 
  *ReturnEular\Z = heading
  *ReturnEular\Y = bank - #PI/2
  *ReturnEular\X = attitude
EndProcedure

Procedure.f Quaternion_Length(*TempQuat.Quaternion)
  ProcedureReturn Sqr(*TempQuat\w * *TempQuat\w  +  *TempQuat\X * *TempQuat\X  +  *TempQuat\Y * *TempQuat\Y  +  *TempQuat\Z * *TempQuat\Z)
EndProcedure

Procedure 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 Quaternion_Multiply(*NewQuat.Quaternion, *Quat1.Quaternion, *Quat2.Quaternion)
  *NewQuat\X =  *Quat1\X * *Quat2\w  +  *Quat1\Y * *Quat2\Z  -  *Quat1\Z * *Quat2\Y  +  *Quat1\w * *Quat2\X
  *NewQuat\Y = -*Quat1\X * *Quat2\Z  +  *Quat1\Y * *Quat2\W  +  *Quat1\Z * *Quat2\X  +  *Quat1\w * *Quat2\Y
  *NewQuat\Z =  *Quat1\X * *Quat2\Y  -  *Quat1\Y * *Quat2\X  +  *Quat1\Z * *Quat2\W  +  *Quat1\w * *Quat2\Z
  *NewQuat\w = -*Quat1\X * *Quat2\X  -  *Quat1\Y * *Quat2\Y  -  *Quat1\Z * *Quat2\Z  +  *Quat1\w * *Quat2\w
EndProcedure

Procedure Quaternion_Normalize(*TempQuat.Quaternion)
  TEMP_length.f = 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 Quaternion_ApplyVector(*quat.Quaternion, *vecin.Vector, *vecout.Vector)
  Protected vecQuat.Quaternion, qResult.Quaternion, qTemp.Quaternion
  vecQuat\X = *vecin\X
  vecQuat\Y = *vecin\Y
  vecQuat\Z = *vecin\Z
  vecQuat\w = 0
 
  Quaternion_Conjugate(@qTemp, @vecQuat)
 
  Quaternion_Multiply(@qResult, @vecQuat, @qTemp)
  CopyStructure(@qResult, @qTemp, QUATERNION)
 
  Quaternion_Multiply(@qResult, *Quat, @qTemp)
 
  *vecout\X = qResult\X
  *vecout\Y = qResult\Y
  *vecout\Z = qResult\Z
EndProcedure

Procedure 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
 
  Quaternion_Normalize(*TempQuat)
EndProcedure

Procedure 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_angle.f = angleDegrees * 0.01745329
  TEMP_angle = TEMP_angle / 2
  TEMP_scale.f = Sin(TEMP_angle)
 
  *TempQuat\X = X * TEMP_scale
  *TempQuat\Y = Y * TEMP_scale
  *TempQuat\Z = Z * TEMP_scale
  *TempQuat\w = Cos(TEMP_angle)
 
  Quaternion_Normalize(*TempQuat)
EndProcedure

; --------------------------------------------------------
; gl_MoveObjectToPointB(Object,X,Y)
; --------------------------------------------------------
; This function moves an object to an X Y point in all
;   directions. Movement speed is based on the array's
; speed. If the point is reached then the procedure
; returns true.
; Object - Name of an array and its element
; ToX, ToY, - Destination of the object
; --------------------------------------------------------
Procedure gl_MoveObjectToPointB(*Object.objstruct,ToX.f,ToY.f)
   Static Object.objstruct
   Static distance.f

   ; -- Checking if we have reached our destination
   If *Object\x >= ToX.f-*Object\h_speed And *Object\x <= ToX.f+*Object\h_speed
         *Object\x = ToX.f
      If *Object\y >= ToY.f-*Object\v_speed And *Object\y <= ToY.f+*Object\v_speed
         *Object\y = ToY.f
         ProcedureReturn #True ; -- Return true when destination has been reached
      EndIf
   EndIf   

   ; -- Increment position until we're at our destination
   distance.f=Sqr((ToX.f-*Object\x)*(ToX.f-*Object\x)+(ToY.f-*Object\y)*(ToY.f-*Object\y));
   *Object\x=*Object\x+*Object\h_speed*(ToX.f-*Object\x)/distance.f
   *Object\y=*Object\y+*Object\v_speed*(ToY.f-*Object\y)/distance.f
   ; x = x+speed * x2 / distance
EndProcedure

; SI-Prefix
#Yotta = 1e24
#Zetta = 1e21
#Exa   = 1e18
#Peta  = 1e15
#Tera  = 1e12
#Giga  = 1e9
#Mega  = 1e6
#Kilo  = 1e3
#Hecto = 1e2
#Deca  = 1e1
#Deci  = 1e-1
#Centi = 1e-2
#Milli = 1e-3
#Micro = 1e-6
#Nano  = 1e-9
#Pico  = 1e-12
#Femto = 1e-15
#Atto  = 1e-18
#Zepto = 1e-21
#Yocto = 1e-24

; Binary-Prefix
#Kibi  = 1<<10
#Mebi  = 1<<20
#Gibi  = 1<<30
#Tebi  = 1<<40
#Pebi  = 1<<50
#Exbi  = 1<<60

; Time-Prefix
#Minute = 60
#Hour   = 3600
#Day    = 86400
#Week   = 604800

; Gaussian Function
Procedure.f Gauss(x.f, Radius.f=1.0)
  ProcedureReturn 0.3989422804014326779*Exp(-0.5*x*x/(Radius*Radius))/Radius
EndProcedure

; Error Function
Procedure.f Erf(x.f)
  ProcedureReturn Sign(x)*Sqr(1-Exp(-x*x*(1.273239544735162686+0.1400122886866666060*x*x)/(1+0.1400122886866666060*x*x)))
EndProcedure

; Inverse Error Function
Procedure.f InvErf(x.f)
  Protected Ln.f = Log(1-x*x)*0.5
  ProcedureReturn Sign(x)*Sqr(Sqr((4.546884979448284327+Ln)*(4.546884979448284327+Ln)-Ln*14.28446044815250805)-4.546884979448284327-Ln)
EndProcedure

; Returns a random float in the interval [0.0, Maximum]
Macro RandomFloat(Maximum=1.0)
  ( Random(2147483647)*Maximum*4.6566128752457969241e-10 ) ; Random * 1/MaxRandom
EndMacro

; Returns a random sign {-1, 1}
Macro RandomSign()
  ( Random(1)*2-1 )
EndMacro

; Returns a random angle in radian in the interval [0.0, 2*Pi[
Macro RandomAngle()
  ( Random(2147483647)*2.9258361585343193621e-9 ) ; Random * 2*Pi/(MaxRandom+1)
EndMacro

; Returns a random float in normal distribution in the interval ]-Infinity, Infinity[
Macro RandomGaussian(Variance=1.0)
  ( InvErf(RandomFloat(2)-1)*Variance*1.414213562373095049 )
EndMacro

;returns the distance between two supplied points p and q
Procedure.d distance(*p.POINT, *q.POINT)
dx.d = *p\x - *q\x                  ;horizontal difference
dy.d = *p\y - *q\y                  ;vertical difference
dist.d = Sqr(dx*dx + dy*dy )     ;distance using Pythagoras theorem
ProcedureReturn dist
EndProcedure

;distance from a point to a vertical line
Procedure.i FindDist2VertLine(px.f, lx.f)
  distance = Abs(Px) - Abs(Lx)
  ProcedureReturn distance
EndProcedure

;distance from a point to a horizontal line
Procedure.i FindDist2HorizLine(py.f, ly.f)
  distance = Abs(Py) - Abs(Ly)
  ProcedureReturn distance
EndProcedure

;Find the slope of a line
Procedure.f FindSlopeOfLine(*p1.POINT, *p2.POINT)
  slope.f = (*p1\y - *p2\y) / (*p1\x + *p2\x)
  ProcedureReturn slope
EndProcedure

;This function will calculate the acceleration in seconds.
Procedure.f calculateAccelerationSeconds( startVelocity.f, finalVelocity.f , time.f)
  ProcedureReturn (finalVelocity-startVelocity)/time
EndProcedure

;purpose: calculate final velocity, given initial velocity, acceleration and time
; input:   vi initial velocity
;            a  acceleration
;            t  time
; output:  our final velocity
Procedure.f  eqOne_vf(vi.f, a.f, t.f)
  ProcedureReturn vi + a * t
EndProcedure

;purpose: calculate change in distance, given final velocity, initial velocity And time
; input:   vf final velocity
;            vi initial velocity
;            t  time
; output:  our change in distance
Procedure.f eqTwo_x(vf.f, vi.f, t.f)
  ProcedureReturn  0.5 * (vf - vi) / t
EndProcedure

;purpose: calculate change in distance, given initial velocity, acceleration and time
; input:   vi initial velocity
;            t  time
;            a  acceleration
; output:  our change in distance
Procedure.f eqThree_x(vi.f, t.f, a.f)
  ProcedureReturn vi * t + 0.5 * a * t * t
EndProcedure

;purpose: To determine whether an object on an incline will slide
; input:   angle       - the current angle of the incline
;            weight     - the weight of the object
;            fric_coeff - the coefficient of Static friction between surfaces
; output:  true If the object should slide, Else false
Procedure checkForMotion(angle.f, weight.f, fric_coeff.f)
    ; Calculate the normal force being exerted by the ramp
    normal.f = weight * Cos(angle * #PI / 180)
    ; Calculate the force perpendicular To the normal
    perpForce.f = weight * Sin(angle * #PI / 180)
    ; Calculate the amount of Static friction keeping the object at rest
    stat_friction.f = fric_coeff * normal
    ; Return true If the object should slide, Else false
    ProcedureReturn perpForce > stat_friction
EndProcedure

;purpose: To determine whether an object on an incline will slide
;             and how fast the object should accelerate
; input:   angle - the current angle of the incline
;            weight - the weight of the object
;            fric_coeff - the coefficient of kinetic friction between surfaces
;            mass - the mass of the object
; output:  the acceleration of the object
Procedure.f calcAccel(angle.f, weight.f, fric_coeff.f, mass.f)
    ;Calculate the normal force being exerted by the object
    normal.f = weight * Cos(angle * PI / 180)
    ; Calculate the force perpendicular To the normal
    perpForce.f = weight * Sin(angle * PI / 180)
    ;Calculate the amount of Static friction keeping the object at rest
    kin_friction.f = fric_coeff * normal
    ;Calculate the sum of forces acting upon the object
    total_force.f = perpForce - kin_friction
    ;Return the acceleration of the object
    ProcedureReturn total_force / mass
EndProcedure

;Here is a procedure that returns the amount of work done given a force,
;a friction force, And a displacement
Procedure.f calculateWork(force.f, friction.f, displacement.f)
     ;calculate the difference of the forces.
    netForce.f = force-friction

    ;multiply by displacement
    temp.f = displacement * netForce
    ; Returns the value of the work in Joules
    ProcedureReturn temp
EndProcedure

Procedure.f calculateKineticEnergy(mass.f, speed.f)
  KE.f = (mass/2)*(Pow(speed,2))
  ProcedureReturn KE
EndProcedure