Physicstest

Community Forums/Showcase/Physicstest

Sweenie (Posted 2005) [#1]

Ok, new version. Springs, "proper" timing and rendertweening are in. Includefile...(name it Types.bmx) Strict Const epsilon:Float = 0.00001 Const epsilonSquared:Float = epsilon * epsilon Type Vector Field x:Float,y:Float,z:Float 'Create a new vector using parametes Function Create:Vector(in_x:Float,in_y:Float,in_z:Float) Local vec:Vector = New Vector vec.x = in_x vec. y= in_y vec. z= in_z Return vec End Function ' Set vector parameters Method Set(in_x:Float,in_y:Float,in_z:Float) x = in_x y = in_y z = in_z End Method ' Clear vector Method Zero() x = 0 y = 0 z = 0 End Method 'Copy values from another vector Method CopyVector(in_vec:Vector) x = in_vec.x y = in_vec.y z = in_vec.z End Method 'Negate self Method Negate() x = -x y = -y z = -z End Method ' Add vectors Method Add:Vector(in_vec:Vector) Local result:Vector = New Vector result.x = x + in_vec.x result.y = y + in_vec.y result.z = z + in_vec.z Return result End Method ' Subtract vectors Method Sub:Vector(in_vec:Vector) Local result:Vector = New Vector result.x = x - in_vec.x result.y = y - in_vec.y result.z = z - in_vec.z Return result End Method ' Multiply Self with scalar Method MulScalar:Vector(in_scalar:Float) Local result:Vector = New Vector result.x = x * in_scalar result.y = y * in_scalar result.z = z * in_scalar Return result End Method ' Divide Self with scalar Method DivScalar:Vector(in_scalar:Float) Local result:Vector = New Vector If in_scalar<>0 Then Local inv = 1.0/in_scalar result.x = x * inv result.y = y * inv result.z = z * inv EndIf Return result End Method ' Return Dotproduct Method Dot:Float(in_vec:Vector) Return x * in_vec.x + y * in_vec.y + z * in_vec.z End Method ' Return Crossproduct Method Cross:Vector(in_vec:Vector) Return Vector.Create(y*in_vec.z - z*in_vec.y , z*in_vec.x - x*in_vec.z , x*in_vec.y - y*in_vec.x) End Method ' Return Squared Length Method LengthSquared:Float() Return x*x + y*y + z*z End Method 'Return Length(Magnitude) Method Length:Float() Return Sqr(x*x + y*y + z*z) End Method ' Return normal of Self Method Normalize:Vector() Local mag:Float = Self.Length() Local x_out:Float Local y_out:Float Local z_out:Float If mag>0 Then x_out = x / mag y_out = y / mag z_out = z / mag EndIf Return Vector.Create(x_out, y_out, z_out) End Method Method MulVec:Vector(invec:Vector) Local result:Vector = New Vector result.x = y*invec.z - z*invec.y result.y = z*invec.x - x*invec.z result.z = x*invec.y - y*invec.x Return result End Method Method MulVecMat:Vector(inmat:Matrix) Local result:Vector = New Vector result.x = x*inmat.m11 + y*inmat.m21 + z*inmat.m31 + inmat.m41 result.y = x*inmat.m12 + y*inmat.m22 + z*inmat.m32 + inmat.m42 result.z = x*inmat.m13 + y*inmat.m23 + z*inmat.m33 + inmat.m43 Return result End Method Method MulMatVec:Vector(inmat:Matrix) Local result:Vector = New Vector result.x = x*inmat.m11 + y*inmat.m12 + z*inmat.m13 + inmat.m14 result.y = x*inmat.m21 + y*inmat.m22 + z*inmat.m23 + inmat.m24 result.z = x*inmat.m31 + y*inmat.m32 + z*inmat.m33 + inmat.m34 Return result End Method End Type Type Matrix Field m11:Float,m12:Float,m13:Float,m14:Float Field m21:Float,m22:Float,m23:Float,m24:Float Field m31:Float,m32:Float,m33:Float,m34:Float Field m41:Float,m42:Float,m43:Float,m44:Float Function CreateFromVec:Matrix(in_vecA:Vector,in_vecB:Vector,in_vecC:Vector) Local tmat:Matrix = New Matrix tmat.m11 = in_vecA.x tmat.m12 = in_vecA.y tmat.m13 = in_vecA.z tmat.m14 = 0.0 tmat.m21 = in_vecB.x tmat.m22 = in_vecB.y tmat.m23 = in_vecB.z tmat.m24 = 0.0 tmat.m31 = in_vecC.x tmat.m32 = in_vecC.y tmat.m33 = in_vecC.z tmat.m34 = 0.0 tmat.m41 = 0.0 tmat.m42 = 0.0 tmat.m43 = 0.0 tmat.m44 = 1.0 Return tmat End Function Function Create:Matrix(im11:Float,im12:Float,im13:Float,im14:Float,im21:Float,im22:Float,im23:Float,im24:Float,im31:Float,im32:Float,im33:Float,im34:Float,im41:Float,im42:Float,im43:Float,im44:Float) Local tmat:Matrix = New Matrix tmat.m11 = im11 tmat.m12 = im12 tmat.m13 = im13 tmat.m14 = im14 tmat.m21 = im21 tmat.m22 = im22 tmat.m23 = im23 tmat.m24 = im24 tmat.m31 = im31 tmat.m32 = im32 tmat.m33 = im33 tmat.m34 = im34 tmat.m41 = im41 tmat.m42 = im42 tmat.m43 = im43 tmat.m44 = im44 Return tmat End Function Method Get:Float(i:Int,j:Int) If i=1 And j=1 Then Return m11 If i=1 And j=2 Then Return m12 If i=1 And j=3 Then Return m13 If i=1 And j=4 Then Return m14 If i=2 And j=1 Then Return m21 If i=2 And j=2 Then Return m22 If i=2 And j=3 Then Return m23 If i=2 And j=4 Then Return m24 If i=3 And j=1 Then Return m31 If i=3 And j=2 Then Return m32 If i=3 And j=3 Then Return m33 If i=3 And j=4 Then Return m34 If i=4 And j=1 Then Return m41 If i=4 And j=2 Then Return m42 If i=4 And j=3 Then Return m43 If i=4 And j=4 Then Return m44 End Method Method Zero() m11 = 0 m12 = 0 m13 = 0 m14 = 0 m21 = 0 m22 = 0 m23 = 0 m24 = 0 m31 = 0 m32 = 0 m33 = 0 m34 = 0 m41 = 0 m42 = 0 m43 = 0 m44 = 0 End Method Method CopyMatrix(SrcMat:Matrix) m11 = SrcMat.m11 m12 = SrcMat.m12 m13 = SrcMat.m13 m14 = SrcMat.m14 m21 = SrcMat.m21 m22 = SrcMat.m22 m23 = SrcMat.m23 m24 = SrcMat.m24 m31 = SrcMat.m31 m32 = SrcMat.m32 m33 = SrcMat.m33 m34 = SrcMat.m34 m41 = SrcMat.m41 m42 = SrcMat.m42 m43 = SrcMat.m43 m44 = SrcMat.m44 End Method Method Identity() m11 = 1 m12 = 0 m13 = 0 m14 = 0 m21 = 0 m22 = 1 m23 = 0 m24 = 0 m31 = 0 m32 = 0 m33 = 1 m34 = 0 m41 = 0 m42 = 0 m43 = 0 m44 = 1 End Method Method Translate:Matrix(in_x:Float,in_y:Float,in_z:Float) Local tmat:Matrix = New Matrix tmat.m11 = 1 tmat.m12 = 0 tmat.m13 = 0 tmat.m14 = in_x tmat.m21 = 0 tmat.m22 = 1 tmat.m23 = 0 tmat.m24 = in_y tmat.m31 = 0 tmat.m32 = 0 tmat.m33 = 1 tmat.m34 = in_z tmat.m41 = 0 tmat.m42 = 0 tmat.m43 = 0 tmat.m44 = 1 Return tmat End Method Method Scale(in_s:Float) m11 = in_s m12 = 0 m13 = 0 m14 = 0 m21 = 0 m22 = in_s m23 = 0 m24 = 0 m31 = 0 m32 = 0 m33 = in_s m34 = 0 m41 = 0 m42 = 0 m43 = 0 m44 = 1 End Method Method Diagonal(a:Float,b:Float,c:Float,d:Float = 1) m11 = a m12 = 0 m13 = 0 m14 = 0 m21 = 0 m22 = b m23 = 0 m24 = 0 m31 = 0 m32 = 0 m33 = c m34 = 0 m41 = 0 m42 = 0 m43 = 0 m44 = d End Method Method Rotate(angle:Float,axis:Vector) If axis.LengthSquared()<epsilonSquared Then Self.Identity() EndIf axis.Normalize() Local fCos:Float = Cos(angle) Local fSin:Float = Sin(angle) Local fOneMinusCos:Float = 1.0 - fCos Local fX2:Float = axis.x*axis.x Local fY2:Float = axis.y*axis.y Local fZ2:Float = axis.z*axis.z Local fXYM:Float = axis.x*axis.y*fOneMinusCos Local fXZM:Float = axis.x*axis.z*fOneMinusCos Local fYZM:Float = axis.y*axis.z*fOneMinusCos Local fXSin:Float = axis.x*fSin Local fYSin:Float = axis.y*fSin Local fZSin:Float = axis.z*fSin m11 = fX2*fOneMinusCos+fCos m12 = fXYM-fZSin m13 = fXZM+fYSin m14 = 0 m21 = fXYM+fZSin m22 = fY2*fOneMinusCos+fCos m23 = fYZM-fXSin m24 = 0 m31 = fXZM-fYSin m32 = fYZM+fXSin m33 = fZ2*fOneMinusCos+fCos m34 = 0 m41 = 0 m42 = 0 m43 = 0 m44 = 1 End Method Method LookAt(eye:Vector,at:Vector,up:Vector) Local zaxis:Vector = at ; zaxis.sub(eye) ' zaxis = at - eye Local xaxis:Vector = up.Cross(zaxis) Local yaxis:Vector = zaxis.Cross(xaxis) xaxis.Normalize() yaxis.Normalize() zaxis.Normalize() m11 = xaxis.x m12 = xaxis.y m13 = xaxis.z m14 = -xaxis.Dot(eye) m21 = yaxis.x m22 = yaxis.y m23 = yaxis.z m24 = -yaxis.Dot(eye) m31 = zaxis.x m32 = zaxis.y m33 = zaxis.z m34 = -zaxis.Dot(eye) m41 = 0 m42 = 0 m43 = 0 m44 = 1 End Method Method Orthographic(l:Float,r:Float,b:Float,t:Float,n:Float,f:Float) Local sx:Float = 1.0 / (r-l) Local sy:Float = 1.0 / (t-b) Local sz:Float = 1.0 / (f-n) m11 = 2 * sx m21 = 0 m31 = 0 m41 = -(r+l) * sx m12 = 0 m22 = 2 * sy m32 = 0 m42 = -(t+b) * sy m13 = 0 m23 = 0 m33 = -2 * sz m43 = -(n+f) * sz m14 = 0 m24 = 0 m34 = 0 m44 = 1 End Method Method Perspective(l:Float,r:Float,b:Float,t:Float,n:Float,f:Float) m11 = 2*n / (r-l) m12 = 0 m13 = 0 m14 = 0 m21 = 0 m22 = 2*n / (t-b) m23 = 0 m24 = 0 m31 = 0 m32 = 0 m33 = f / (f-n) m34 = n*f / (n-f) m41 = 0 m42 = 0 m43 = 1 m44 = 0 End Method Method Determinant:Float() Return -m13*m22*m31 + m12*m23*m31 + m13*m21*m32 - m11*m23*m32 - m12*m21*m33 + m11*m22*m33 End Method Method Inverse:Matrix() ' Returns inverted matrix Local mat:Matrix = New Matrix mat.Invert() Return mat End Method Method Invert() ' Invert the current matrix Local determinant:Float = Self.Determinant() If determinant <> 0 Then Local k:Float = 1 / determinant Local tmat:Matrix = New Matrix tmat.m11 = (m22*m33 - m32*m23) * k tmat.m12 = (m32*m13 - m12*m33) * k tmat.m13 = (m12*m23 - m22*m13) * k tmat.m21 = (m23*m31 - m33*m21) * k tmat.m22 = (m33*m11 - m13*m31) * k tmat.m23 = (m13*m21 - m23*m11) * k tmat.m31 = (m21*m32 - m31*m22) * k tmat.m32 = (m31*m12 - m11*m32) * k tmat.m33 = (m11*m22 - m21*m12) * k tmat.m14 = -(tmat.m11*m14 - tmat.m12*m24 + tmat.m13*m34) tmat.m24 = -(tmat.m21*m14 - tmat.m22*m24 + tmat.m23*m34) tmat.m34 = -(tmat.m31*m14 - tmat.m32*m24 + tmat.m33*m34) m11 = tmat.m11 m12 = tmat.m12 m13 = tmat.m13 m14 = tmat.m14 m21 = tmat.m21 m22 = tmat.m22 m23 = tmat.m23 m24 = tmat.m24 m31 = tmat.m31 m32 = tmat.m32 m33 = tmat.m33 m34 = tmat.m34 EndIf End Method Method Transposed:Matrix() ' Return transposed matrix Local mat:Matrix = New Matrix mat.Transpose Return mat End Method Method Transpose() ' Transpose current matrix Local tmat:Matrix = New Matrix tmat.CopyMatrix(Self) m11 = tmat.m11 m12 = tmat.m21 m13 = tmat.m31 m14 = tmat.m41 m21 = tmat.m12 m22 = tmat.m22 m23 = tmat.m32 m24 = tmat.m42 m31 = tmat.m13 m32 = tmat.m23 m33 = tmat.m33 m34 = tmat.m43 m41 = tmat.m14 m42 = tmat.m24 m43 = tmat.m34 m44 = tmat.m44 End Method Method AddMat(inmat:Matrix) m11 :+ inmat.m11 m12 :+ inmat.m12 m13 :+ inmat.m13 m14 :+ inmat.m14 m21 :+ inmat.m21 m22 :+ inmat.m22 m23 :+ inmat.m23 m24 :+ inmat.m24 m31 :+ inmat.m31 m32 :+ inmat.m32 m33 :+ inmat.m33 m34 :+ inmat.m34 m41 :+ inmat.m41 m42 :+ inmat.m42 m43 :+ inmat.m43 m44 :+ inmat.m44 End Method Method SubMat(inmat:Matrix) m11 :- inmat.m11 m12 :- inmat.m12 m13 :- inmat.m13 m14 :- inmat.m14 m21 :- inmat.m21 m22 :- inmat.m22 m23 :- inmat.m23 m24 :- inmat.m24 m31 :- inmat.m31 m32 :- inmat.m32 m33 :- inmat.m33 m34 :- inmat.m34 m41 :- inmat.m41 m42 :- inmat.m42 m43 :- inmat.m43 m44 :- inmat.m44 End Method Method MulScalar(s:Float) m11 :* s m12 :* s m13 :* s m14 :* s m21 :* s m22 :* s m23 :* s m24 :* s m31 :* s m32 :* s m33 :* s m34 :* s m41 :* s m42 :* s m43 :* s m44 :* s End Method Method MulMat:Matrix(inmat:Matrix) Local tmat:Matrix = New Matrix tmat.m11 = m11*inmat.m11 + m12*inmat.m21 + m13*inmat.m31 + m14*inmat.m41 tmat.m12 = m11*inmat.m12 + m12*inmat.m22 + m13*inmat.m32 + m14*inmat.m42 tmat.m13 = m11*inmat.m13 + m12*inmat.m23 + m13*inmat.m33 + m14*inmat.m43 tmat.m14 = m11*inmat.m14 + m12*inmat.m24 + m13*inmat.m34 + m14*inmat.m44 tmat.m21 = m21*inmat.m11 + m22*inmat.m21 + m23*inmat.m31 + m24*inmat.m41 tmat.m22 = m21*inmat.m12 + m22*inmat.m22 + m23*inmat.m32 + m24*inmat.m42 tmat.m23 = m21*inmat.m13 + m22*inmat.m23 + m23*inmat.m33 + m24*inmat.m43 tmat.m24 = m21*inmat.m14 + m22*inmat.m24 + m23*inmat.m34 + m24*inmat.m44 tmat.m31 = m31*inmat.m11 + m32*inmat.m21 + m33*inmat.m31 + m34*inmat.m41 tmat.m32 = m31*inmat.m12 + m32*inmat.m22 + m33*inmat.m32 + m34*inmat.m42 tmat.m33 = m31*inmat.m13 + m32*inmat.m23 + m33*inmat.m33 + m34*inmat.m43 tmat.m34 = m31*inmat.m14 + m32*inmat.m24 + m33*inmat.m34 + m34*inmat.m44 tmat.m41 = m41*inmat.m11 + m42*inmat.m21 + m43*inmat.m31 + m44*inmat.m41 tmat.m42 = m41*inmat.m12 + m42*inmat.m22 + m43*inmat.m32 + m44*inmat.m42 tmat.m43 = m41*inmat.m13 + m42*inmat.m23 + m43*inmat.m33 + m44*inmat.m43 tmat.m44 = m41*inmat.m14 + m42*inmat.m24 + m43*inmat.m34 + m44*inmat.m44 Return tmat End Method End Type Type Quaternion Field w:Float,x:Float,y:Float,z:Float Function Create:Quaternion(in_w:Float,in_x:Float,in_y:Float,in_z:Float) Local tQuat:Quaternion = New Quaternion tQuat.w = in_w tQuat.x = in_x tQuat.y = in_y tQuat.z = in_z Return tQuat End Function Function CreateFromAngleAxis:Quaternion(angle:Float,axis:Vector) Local tQuat:Quaternion = New Quaternion Local a:Float = angle * 0.5 Local s:Float = Sin(a) Local c:Float = Cos(a) tQuat.w = c tQuat.x = axis.x * s tQuat.y = axis.y * s tQuat.z = axis.z * s Return tQuat End Function Function CreateFromMatrix:Quaternion(inmat:Matrix) Local tQuat:Quaternion = New Quaternion Local trace:Float = inmat.m11 + inmat.m22 + inmat.m33 If trace>0 Then Local root:Float = Sqr(trace+1.0) tQuat.w = 0.5 * root root = 0.5 / root tQuat.x = (inmat.m32 - inmat.m23) * root tQuat.y = (inmat.m13 - inmat.m31) * root tQuat.z = (inmat.m21 - inmat.m12) * root Else Local n[] = [2,3,1] Local i:Int = 1 If inmat.m22>inmat.m11 Then i = 2 If inmat.m33>inmat.Get(i,i) Then i = 3 Local j:Int = n[i] Local k:Int = n[j] Local root:Float = Sqr(inmat.Get(i,i)-inmat.Get(j,j)-inmat.Get(k,k) + 1.0) If i=1 Then tQuat.x = 0.5 * root If i=2 Then tQuat.y = 0.5 * root If i=3 Then tQuat.z = 0.5 * root root = 0.5 / root tQuat.w = (inmat.Get(k,j)-inmat.Get(j,k))*root If j=1 Then tQuat.x=(inmat.Get(j,i)+inmat.Get(i,j))*root If j=2 Then tQuat.y=(inmat.Get(j,i)+inmat.Get(i,j))*root If j=3 Then tQuat.z=(inmat.Get(j,i)+inmat.Get(i,j))*root If k=1 Then tQuat.x=(inmat.Get(k,i)+inmat.Get(i,k))*root If k=2 Then tQuat.y=(inmat.Get(k,i)+inmat.Get(i,k))*root If k=3 Then tQuat.z=(inmat.Get(k,i)+inmat.Get(i,k))*root EndIf Return tQuat End Function Method ToMatrix:Matrix() Local tmat:Matrix = New Matrix Local fTx:Float = 2.0 * x Local fTy:Float = 2.0 * y Local fTz:Float = 2.0 * z Local fTwx:Float = fTx * w Local fTwy:Float = fTy * w Local fTwz:Float = fTz * w Local fTxx:Float = fTx * x Local fTxy:Float = fTy * x Local fTxz:Float = fTz * x Local fTyy:Float = fTy * y Local fTyz:Float = fTz * y Local fTzz:Float = fTz * z tmat.m11 = 1.0 - (fTyy+fTzz) tmat.m12 = fTxy - fTwz tmat.m13 = fTxz+fTwy tmat.m14 = 0 tmat.m21 = fTxy+fTwz tmat.m22 = 1.0 -(fTxx+fTzz) tmat.m23 = fTyz-fTwx tmat.m24 = 0 tmat.m31 = fTxz-fTwy tmat.m32 = ftYz+fTwx tmat.m33 = 1.0 -(fTxx+fTyy) tmat.m34 = 0 tmat.m41 = 0 tmat.m42 = 0 tmat.m43 = 0 tmat.m44 = 1 Return tmat End Method 'Note the parameters Method ToAngleAxis(refangle:Float Ptr,refaxis:Vector Ptr) Local squarelength:Float = x*x + y*y + z*z If squarelength>epsilonsquared refangle[0] = 2.0 * ACos(w) Local invlength:Float = 1.0 / (squarelength^0.5) refaxis[0].x = x * invlength refaxis[0].y = y * invlength refaxis[0].z = z * invlength Else refangle[0] = 0 refaxis[0].x = 1.0 refaxis[0].y = 0.0 refaxis[0].z = 0.0 EndIf End Method Method Zero() w = 0 x = 0 y = 0 z = 0 End Method Method Identity() w = 1 x = 0 y = 0 z = 0 End Method Method CopyQuat(inQuat:Quaternion) w = inQuat.w x = inQuat.x y = inQuat.y z = inQuat.z End Method Method Add:Quaternion(inQuat:Quaternion) Local result:Quaternion = New Quaternion result.w = w + inQuat.w result.x = x + inQuat.x result.y = y + inQuat.y result.z = z + inQuat.z Return result End Method Method Sub:Quaternion(inQuat:Quaternion) Local result:Quaternion = New Quaternion result.w = w - inQuat.w result.x = x - inQuat.x result.y = y - inQuat.y result.z = z - inQuat.z Return result End Method Method MulScalar:Quaternion(s:Float) Local result:Quaternion = New Quaternion result.w = w * s result.x = x * s result.y = y * s result.z = z * s Return result End Method Method DivScalar:Quaternion(s:Float) Local result:Quaternion = New Quaternion If s<>0 Then Local inv:Float = 1.0 / s result.w = w * inv result.x = x * inv result.y = y * inv result.z = z * inv EndIf Return result End Method Method MulQuat:Quaternion(inQuat:Quaternion) Local result:Quaternion = New Quaternion Local rw:Float = w*inQuat.w - x*inQuat.x - y*inQuat.y - z*inQuat.z Local rx:Float = w*inQuat.x + x*inQuat.w + y*inQuat.z - z*inQuat.y Local ry:Float = w*inQuat.y - x*inQuat.z + y*inQuat.w + z*inQuat.x Local rz:Float = w*inQuat.z + x*inQuat.y - y*inQuat.x + z*inQuat.w result.w = rw result.x = rx result.y = ry result.z = rz Return result End Method Method Dot:Quaternion(inQuat:Quaternion) Local tQuat:Quaternion = New Quaternion tQuat.w = w*inQuat.w + x*inQuat.x + y*inQuat.y + z*inQuat.z tQuat.x = 0 tQuat.y = 0 tQuat.z = 0 Return tQuat End Method Method Conjugate:Quaternion() Local tQuat:Quaternion = New Quaternion tQuat.w = w tQuat.x = -x tQuat.y = -y tQuat.z = -z Return tQuat End Method Method Length:Float() Return Sqr(w*w+x*x+y*y+z*z) End Method Method Norm:Float() Return w*w+x*x+y*y+z*z End Method Method Normalize:Quaternion() Local result:Quaternion = New Quaternion Local length:Float = Self.Length() If length=0 Then result.w = 1 result.x = 0 result.y = 0 result.z = 0 Else Local inv:Float = 1.0 / length result.w = w * inv result.x = x * inv result.y = y * inv result.z = z * inv EndIf Return result End Method Method Inverse:Quaternion() Local n:Float = Self.Norm() If n<>0 Then Local tQuat:Quaternion = New Quaternion tQuat.w = w/n tQuat.x = -x/n tQuat.y = -y/n tQuat.z = -z/n EndIf End Method Function Slerp:Quaternion(a:Quaternion,b:Quaternion,t:Float) If t>=0 And t<=1.0 Then Local flp:Float = 1 Local cosine:Float = a.w*b.w + a.x*b.x + a.y*b.y + a.z*b.z If cosine<0 Then cosine = -cosine flp = -1 EndIf If (1-cosine)<epsilon Then a.MulScalar(1.0-t) b.MulScalar(t*flp) a.Add(b) Return a EndIf Local theta:Float = ACos(cosine) Local sine:Float = Sin(cosine) Local beta:Float = Sin((1-t)*theta) / sine Local alpha:Float = Sin(t*theta) / sine * flp a.MulScalar(beta) b.MulScalar(alpha) a.Add(b) Return a EndIf End Function End Type And the main app. Strict Import "Types.bmx" Const FPS = 300 ' Updates physics 300 times per second (If you lower this you must lower the springstiffness below or they will EXPLODE!) Const BODYCOUNT = 50 Const Gravity = 9.8 Global Bodies:RigidBody[BODYCOUNT] Type Transform Field Position:Vector = New Vector Field Orientation:Quaternion = New Quaternion End Type ' Interpolates two transforms (not scale though) Function Interpolate:Transform(a:Transform,b:Transform,Alpha:Float) Local tTransform:Transform = New Transform tTransform.Position = a.Position.MulScalar(1.0-Alpha) tTransform.Position = tTransform.Position.Add(b.Position.MulScalar(Alpha)) tTransform.Orientation = Quaternion.Slerp(a.Orientation,b.Orientation,Alpha) Return tTransform End Function Type RigidBody Field ID:Int ' Primary state Field Position:Vector = New Vector Field Momentum:Vector = New Vector Field Orientation:Quaternion = New Quaternion Field Angularmomentum:Vector = New Vector ' Secondary state Field Velocity:Vector = New Vector Field Spin:Quaternion = New Quaternion Field Angularvelocity:Vector = New Vector Field BodyToWorld:Matrix = New Matrix Field WorldToBody:Matrix = New Matrix ' Constant state Field Size:Float Field Mass:Float Field InvMass:Float Field Inertia:Float Field InvInertia:Float Field Force:Vector = New Vector Field Torque:Vector = New Vector Field Body2World:Matrix = New Matrix Field World2Body:Matrix = New Matrix Field LinearDamping:Float Field AngularDamping:Float Field CurrentTransform:Transform = New Transform Field OldTransform:Transform = New Transform Field VatP:Vector = New Vector ' Other stuff... Field color:Float[] = [Float(rnd!(0.5,1.0)),Float(rnd!(0.5,1.0)),Float(rnd!(0.5,1.0)),1.0] ' Calculate secondary state Method Recalculate() FlushMem() End Method Method New() Size = 1.0 Mass = 1.0 InvMass = 1.0/Mass Orientation.Identity() Inertia = Mass * Size * Size * 1.0 / 6.0 ' Tensor for cube InvInertia = 1.0 / Inertia LinearDamping = 0.1 AngularDamping = 0.1 End Method Method Update(dt:Float) OldTransform.Position.CopyVector(Position) OldTransform.Orientation.CopyQuat(Orientation) Momentum = Momentum.Add(Force.MulScalar(dt)) Velocity = Momentum.MulScalar(InvMass) Position = Position.Add(Velocity.MulScalar(dt)) AngularMomentum = AngularMomentum.Add(Torque.MulScalar(dt)) AngularVelocity = AngularMomentum.MulScalar(InvInertia) Spin = Quaternion.Create(0,AngularVelocity.x,AngularVelocity.y,AngularVelocity.z) Spin = Spin.MulScalar(0.5) Spin = Spin.MulQuat(Orientation) Orientation = Orientation.Add(Spin.MulScalar(dt)) Orientation = Orientation.Normalize() Local Translation:Matrix = New Matrix Translation = Translation.Translate(Position.x,Position.y,Position.z) Body2World = Translation.MulMat(Orientation.ToMatrix()) World2Body = Body2World.Inverse() CurrentTransform.Position.CopyVector(Position) CurrentTransform.Orientation.CopyQuat(Orientation) FlushMem() End Method Method BallSocket_BodyToBody(BodyA:RigidBody Ptr,PointA:Vector,BodyB:RigidBody Ptr,PointB:Vector,Stiffness:Float,Damping:Float) Local SpringForce:Vector = New Vector Local SpringTorque:Vector = New Vector Local Diff:Vector = New Vector Local VelAtPointA:Vector = New Vector Local VelAtPointB:Vector = New Vector Local WorldPointA:Vector = New Vector Local WorldPointB:Vector = New Vector WorldPointA = PointA.MulMatVec(BodyA[0].Body2World) ' Convert Local Point to World space WorldPointB = PointB.MulMatVec(BodyB[0].Body2World) ' Convert Local Point to World space 'SpringForce Diff = WorldPointA.Sub(WorldPointB) SpringForce = Diff.MulScalar(-Stiffness) 'Damping VelAtPointA = BodyA[0].VelocityAtPoint(PointA) VelAtPointB = BodyB[0].VelocityAtPoint(PointB) Diff = VelAtPointA.Sub(VelAtPointB) SpringForce = SpringForce.Sub(Diff.MulScalar(Damping)) 'Force on Body A Diff = WorldPointA.Sub(BodyA[0].Position) SpringTorque = Diff.Cross(SpringForce) BodyA[0].Force = BodyA[0].Force.Add(SpringForce) BodyA[0].Torque = BodyA[0].Torque.Add(SpringTorque) 'Force on Body B Diff = WorldPointB.Sub(BodyB[0].Position) SpringTorque = Diff.Cross(SpringForce) BodyB[0].Force = BodyB[0].Force.Sub(SpringForce) BodyB[0].Torque = BodyB[0].Torque.Sub(SpringTorque) End Method Method BallSocket_BodyToWorld(BodyA:RigidBody Ptr,PointA:Vector,PointB:Vector,Stiffness:Float,Damping:Float) Local SpringForce:Vector = New Vector Local SpringTorque:Vector = New Vector Local Diff:Vector = New Vector Local VelAtPointA:Vector = New Vector Local WorldPointA:Vector = New Vector WorldPointA = PointA.MulMatVec(BodyA[0].Body2World) ' Convert Local Point to World space 'SpringForce Diff = WorldPointA.Sub(PointB) SpringForce = Diff.MulScalar(-Stiffness) 'Damping VelAtPointA = BodyA[0].VelocityAtPoint(PointA) SpringForce = SpringForce.Sub(VelAtPointA.MulScalar(Damping)) 'Force on Body A Diff = WorldPointA.Sub(BodyA[0].Position) SpringTorque = Diff.Cross(SpringForce) BodyA[0].Force = BodyA[0].Force.Add(SpringForce) BodyA[0].Torque = BodyA[0].Torque.Add(SpringTorque) End Method Method Forces() 'Gravity Force.y :+ -Gravity*Mass 'Connect to the next body(If there is one) 'The spring is pretty stiff so therefore the timestep has to very small(hence the high physic-rate set at the top) 'This is because the Simple Euler Integration sucks when it comes to springs If ID>0 Then BallSocket_BodyToBody(Varptr(Bodies[ID]),Vector.Create(0.5,0.5,0.5),Varptr(Bodies[ID-1]),Vector.Create(-0.5,-0.5,-0.5),1900.0,18.0) Else BallSocket_BodyToWorld(Varptr(Bodies[ID]),Vector.Create(0.5,0.5,0.5),Vector.Create(35.0,0.0,0.0),1900.0,18.0) EndIf If ID=BODYCOUNT-1 Then BallSocket_BodyToWorld(Varptr(Bodies[ID]),Vector.Create(-0.5,-0.5,-0.5),Vector.Create(-35.0,0.0,0.0),1900.0,18.0) EndIf If ID=BODYCOUNT/2 Then BallSocket_BodyToWorld(Varptr(Bodies[ID]),Vector.Create(0.0,0.0,0.0),Vector.Create(0.0,0.0,0.0),1900.0,18.0) EndIf If KeyDown(KEY_SPACE) Then Force.z :+ 35 EndIf ' Damping (Simple Airfriction/Drag) Force.x :- (LinearDamping * Velocity.x) Force.y :- (LinearDamping * Velocity.y) Force.z :- (LinearDamping * Velocity.z) Torque.x :- AngularDamping * AngularVelocity.x Torque.y :- AngularDamping * AngularVelocity.y Torque.z :- AngularDamping * AngularVelocity.z FlushMem() End Method Method Render(Tween:Float) glPushMatrix() Local tmpTransform:Transform tmpTransform = Interpolate(OldTransform,CurrentTransform,Tween) Local angle:Float Local axis:Vector = New Vector glTranslatef(tmpTransform.Position.x,tmpTransform.Position.y,tmpTransform.Position.z) tmpTransform.Orientation.ToAngleAxis(Varptr angle,Varptr axis) glRotatef(angle,axis.x,axis.y,axis.z) 'Local color:Float[] = [0.0,1.0,0.0,1.0] glMaterialfv(GL_FRONT,GL_AMBIENT,color) glMaterialfv(GL_FRONT,GL_DIFFUSE,color) Local s:Float = Size * 0.5 glBegin(GL_QUADS) glNormal3f(0,0,1) glVertex3f(-s,-s,s) glVertex3f(s,-s,s) glVertex3f(s,s,s) glVertex3f(-s,s,s) glNormal3f(0,0,-1) glVertex3f(-s,-s,-s) glVertex3f(-s,s,-s) glVertex3f(s,s,-s) glVertex3f(s,-s,-s) glNormal3f(0,1,0) glVertex3f(-s,s,-s) glVertex3f(-s,s,s) glVertex3f(s,s,s) glVertex3f(s,s,-s) glNormal3f(0,-1,0) glVertex3f(-s,-s,-s) glVertex3f(s,-s,-s) glVertex3f(s,-s,s) glVertex3f(-s,-s,s) glNormal3f(1,0,0) glVertex3f(s,-s,-s) glVertex3f(s,s,-s) glVertex3f(s,s,s) glVertex3f(s,-s,s) glNormal3f(-1,0,0) glVertex3f(-s,-s,-s) glVertex3f(-s,-s,s) glVertex3f(-s,s,s) glVertex3f(-s,s,-s) glEnd() glPopMatrix() FlushMem End Method Method VelocityAtPoint:Vector(Point:Vector) Local tVel:Vector = New Vector Local tPoint:Vector = New Vector tPoint = Point.MulMatVec(Body2World) ' Body2World * Point tPoint = tPoint.Sub(Position) tVel = AngularVelocity.Cross(tPoint) tVel = tVel.Add(Velocity) 'state.angularVelocity.cross(point-state.position) + state.velocity Return tVel End Method End Type Local i:Int For i=0 To BODYCOUNT-1 Bodies[i] = New RigidBody Bodies[i].ID = i Bodies[i].Position.y = -Sin(i*4)*2 Bodies[i].Position.x = 30-(i*1.2) Next bglCreateContext 800,600,32,0,BGL_BACKBUFFER | BGL_DEPTHBUFFER Local Ambient:Float[] = [0.1,0.1,0.1,1.0] Local Diffuse:Float[] = [1.0,1.0,1.0,1.0] Local Position:Float[] = [100.0,100.0,100.0,10.0] glViewPort(0,0,800,600) glMatrixMode(GL_PROJECTION) glLoadIdentity() gluPerspective(45.0,1.33,0.1,1000.0) glMatrixMode(GL_MODELVIEW) glLoadIdentity() gluLookAt(0,-10,75, 0,-10,0, 0,1,0); glShadeModel(GL_SMOOTH) glClearColor(0.0,0.0,0.0,0.5) glClearDepth(1.0) glHint(GL_PERSPECTIVE_CORRECTION_HINT,GL_NICEST) glEnable(GL_DEPTH_TEST); glDepthFunc(GL_LEQUAL); glEnable(GL_CULL_FACE); glCullFace(GL_BACK); glFrontFace(GL_CCW); glLightfv(GL_LIGHT1,GL_AMBIENT,Ambient) glLightfv(GL_LIGHT1,GL_DIFFUSE,Diffuse) glLightfv(GL_LIGHT1,GL_POSITION,Position) glEnable(GL_LIGHT1) Local ElapsedTime:Float Local TimeStep:Float = 1.0/FPS Local Period:Int Local Time:Int Local Elapsed:Int Local Ticks:Int Local k:Int Local Tween:Float Local STime:Int Local ETime:Int Period = 1000/FPS Time = MilliSecs()-Period ' MainLoop While Not KeyDown(KEY_ESCAPE) Repeat Elapsed = MilliSecs()-Time Until Elapsed>0 Ticks = Elapsed/Period If Ticks>25 Then Ticks=25 Tween = Float(Elapsed Mod Period)/Float(Period) For k=1 To Ticks Time :+ Period STime=MilliSecs() 'Clear Forces For i=0 To BODYCOUNT-1 Bodies[i].Force.Zero() Bodies[i].Torque.Zero() Next 'Update Forces For i=0 To BODYCOUNT-1 Bodies[i].Forces() Next 'Update Bodies For i=0 To BODYCOUNT-1 Bodies[i].Update(Timestep) Next ETime=MilliSecs() Next glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT) glEnable(GL_LIGHTING) For i=0 To BODYCOUNT-1 Bodies[i].Render(Tween) Next glDisable(GL_LIGHTING) FlushMem() bglDrawText "Memory Allocated:"+String.FromLong(MemAlloced()),4,20 bglDrawText "Avg. Physicstime:"+String.FromLong(ETime-STime),4,32 bglDrawText "Press [Space] to apply some force",4,44 bglSwapBuffers Wend End

bradford6 (Posted 2005) [#2]

awesome work, as usual Sweenie!

Pavlov (Posted 2005) [#3]

that was nice

Bot Builder (Posted 2005) [#4]

Cool, some nice vector/quat classes in there. The physics isn't great but I know you don't have collisions yet ;) Try hooking them all up in a string and making a rope :)

Jeremy Alessi (Posted 2005) [#5]

Very sweet!

insomnia (Posted 2005) [#6]

Cute ;)

Takuan (Posted 2005) [#7]

cool, and now make the thing run equal fast on every machine;-)

Sweenie (Posted 2005) [#8]

I've updated my physics-simulation(See top post). It should run equally fast on every machine and should work on Mac & Linux, but I since I don't have any of those I haven't tested it.