Ejemplo n.º 1
0
        public void SetMotorTarget(ref IndexedQuaternion qAinB, float dt)         // qAinB is rotation of body A wrt body B.
        {
            // convert target from body to constraint space
            IndexedQuaternion qConstraint = MathUtil.QuaternionInverse(m_rbBFrame.GetRotation()) * qAinB * m_rbAFrame.GetRotation();

            qConstraint.Normalize();

            // extract "pure" hinge component
            IndexedVector3 vNoHinge = MathUtil.QuatRotate(ref qConstraint, ref vHinge);

            vNoHinge.Normalize();
            IndexedQuaternion qNoHinge = MathUtil.ShortestArcQuat(ref vHinge, ref vNoHinge);
            IndexedQuaternion qHinge   = MathUtil.QuaternionInverse(ref qNoHinge) * qConstraint;

            qHinge.Normalize();

            // compute angular target, clamped to limits
            float targetAngle = MathUtil.QuatAngle(ref qHinge);

            if (targetAngle > MathUtil.SIMD_PI)             // long way around. flip quat and recalculate.
            {
                qHinge      = -qHinge;
                targetAngle = MathUtil.QuatAngle(ref qHinge);
            }
            if (qHinge.Z < 0)
            {
                targetAngle = -targetAngle;
            }

            SetMotorTarget(targetAngle, dt);
        }
Ejemplo n.º 2
0
        public void UpdateSeparatingDistance(ref IndexedMatrix transA, ref IndexedMatrix transB)
        {
            IndexedVector3 toPosA = transA._origin;
            IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();

            if (m_separatingDistance > 0.0f)
            {
                IndexedVector3 linVelA;
                IndexedVector3 angVelA;
                IndexedVector3 linVelB;
                IndexedVector3 angVelB;

                TransformUtil.CalculateVelocityQuaternion(ref m_posA, ref toPosA, ref m_ornA, ref toOrnA, 1f, out linVelA, out angVelA);
                TransformUtil.CalculateVelocityQuaternion(ref m_posB, ref toPosB, ref m_ornB, ref toOrnB, 1f, out linVelB, out angVelB);
                float maxAngularProjectedVelocity = angVelA.Length() * m_boundingRadiusA + angVelB.Length() * m_boundingRadiusB;
                IndexedVector3 relLinVel = (linVelB - linVelA);
                float relLinVelocLength = IndexedVector3.Dot((linVelB - linVelA), m_separatingNormal);
                if (relLinVelocLength < 0f)
                {
                    relLinVelocLength = 0f;
                }

                float projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
                m_separatingDistance -= projectedMotion;
            }

            m_posA = toPosA;
            m_posB = toPosB;
            m_ornA = toOrnA;
            m_ornB = toOrnB;
        }
Ejemplo n.º 3
0
 public SimMotionState(CollisionWorld pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
 {
     m_properties = new EntityProperties()
     {
         ID       = id,
         Position = starTransform._origin,
         Rotation = starTransform.GetRotation()
     };
     m_lastProperties = new EntityProperties()
     {
         ID       = id,
         Position = starTransform._origin,
         Rotation = starTransform.GetRotation()
     };
     m_world = pWorld;
     m_xform = starTransform;
 }
Ejemplo n.º 4
0
        public EntityProperties FromTransform(uint id, IndexedMatrix startTransform)
        {
            EntityProperties ret = new EntityProperties();

            ID       = id;
            Position = startTransform._origin;
            Rotation = startTransform.GetRotation();
            return(ret);
        }
Ejemplo n.º 5
0
        public void ConvexSweepTest(ConvexShape castShape, ref IndexedMatrix convexFromWorld, ref IndexedMatrix convexToWorld, ConvexResultCallback resultCallback, float allowedCcdPenetration)
        {
            IndexedMatrix convexFromTrans = convexFromWorld;
            IndexedMatrix convexToTrans   = convexToWorld;

            IndexedVector3 castShapeAabbMin;
            IndexedVector3 castShapeAabbMax;
            /* Compute AABB that encompasses angular movement */
            IndexedVector3 linVel, angVel;

            TransformUtil.CalculateVelocity(ref convexFromTrans, ref convexToTrans, 1.0f, out linVel, out angVel);

            // FIXME MAN check this - should be a get/set rotation call, basis copy like this may break with scale?
            IndexedMatrix R = IndexedMatrix.Identity;

            R.SetRotation(convexFromTrans.GetRotation());


            castShape.CalculateTemporalAabb(ref R, ref linVel, ref angVel, 1.0f, out castShapeAabbMin, out castShapeAabbMax);

            /// go over all objects, and if the ray intersects their aabb + cast shape aabb,
            // do a ray-shape query using convexCaster (CCD)
            for (int i = 0; i < m_overlappingObjects.Count; i++)
            {
                CollisionObject collisionObject = m_overlappingObjects[i];
                //only perform raycast if filterMask matches
                if (resultCallback.NeedsCollision(collisionObject.GetBroadphaseHandle()))
                {
                    //RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
                    IndexedVector3 collisionObjectAabbMin;
                    IndexedVector3 collisionObjectAabbMax;
                    IndexedMatrix  t = collisionObject.GetWorldTransform();
                    collisionObject.GetCollisionShape().GetAabb(ref t, out collisionObjectAabbMin, out collisionObjectAabbMax);
                    AabbUtil2.AabbExpand(ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref castShapeAabbMin, ref castShapeAabbMax);
                    float          hitLambda = 1f; //could use resultCallback.m_closestHitFraction, but needs testing
                    IndexedVector3 hitNormal;

                    if (AabbUtil2.RayAabb(convexFromWorld._origin, convexToWorld._origin, ref collisionObjectAabbMin, ref collisionObjectAabbMax, ref hitLambda, out hitNormal))
                    {
                        IndexedMatrix wt = collisionObject.GetWorldTransform();
                        CollisionWorld.ObjectQuerySingle(castShape, ref convexFromTrans, ref convexToTrans,
                                                         collisionObject,
                                                         collisionObject.GetCollisionShape(),
                                                         ref wt,
                                                         resultCallback,
                                                         allowedCcdPenetration);
                    }
                }
            }
        }
Ejemplo n.º 6
0
        void InitSeparatingDistance(ref IndexedVector3 separatingVector, float separatingDistance, ref IndexedMatrix transA, ref IndexedMatrix transB)
	    {
		    m_separatingNormal = separatingVector;
		    m_separatingDistance = separatingDistance;
    		
		    IndexedVector3 toPosA = transA._origin;
		    IndexedVector3 toPosB = transB._origin;
            IndexedQuaternion toOrnA = transA.GetRotation();
            IndexedQuaternion toOrnB = transB.GetRotation();
		    m_posA = toPosA;
		    m_posB = toPosB;
		    m_ornA = toOrnA;
		    m_ornB = toOrnB;
	    }
Ejemplo n.º 7
0
	    public static void IntegrateTransform(ref IndexedMatrix curTrans,ref IndexedVector3 linvel,ref IndexedVector3 angvel,float timeStep,out IndexedMatrix predictedTransform)
	    {
            predictedTransform = IndexedMatrix.CreateTranslation(curTrans._origin + linvel * timeStep);
    //	#define QUATERNION_DERIVATIVE
	    #if QUATERNION_DERIVATIVE
            IndexedVector3 pos;
            IndexedQuaternion predictedOrn;
            IndexedVector3 scale;

            curTrans.Decompose(ref scale, ref predictedOrn, ref pos);


		    predictedOrn += (angvel * predictedOrn) * (timeStep * .5f));
		    predictedOrn.Normalize();
        #else
            //Exponential map
		    //google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia

		    IndexedVector3 axis;
		    float	fAngle = angvel.Length(); 
		    //limit the angular motion
		    if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
		    {
			    fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
		    }

		    if ( fAngle < 0.001f )
		    {
			    // use Taylor's expansions of sync function
			    axis   = angvel*( 0.5f*timeStep-(timeStep*timeStep*timeStep)*(0.020833333333f)*fAngle*fAngle );
		    }
		    else
		    {
			    // sync(fAngle) = sin(c*fAngle)/t
			    axis   = angvel*( (float)Math.Sin(0.5f*fAngle*timeStep)/fAngle );
		    }
		    IndexedQuaternion dorn = new IndexedQuaternion(axis.X,axis.Y,axis.Z,(float)Math.Cos( fAngle*timeStep*.5f) );

            IndexedQuaternion orn0 = curTrans.GetRotation();

		    IndexedQuaternion predictedOrn = dorn * orn0;
		    predictedOrn.Normalize();
	    #endif

            IndexedMatrix newMatrix = IndexedMatrix.CreateFromQuaternion(predictedOrn);
            predictedTransform._basis = newMatrix._basis;
	    }
Ejemplo n.º 8
0
        public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
        {
            m_xform = worldTrans;
            // Put the new transform into m_properties
            m_properties.Position = m_xform._origin;
            m_properties.Rotation = m_xform.GetRotation();
            // A problem with stock Bullet is that we don't get an event when an object is deactivated.
            // This means that the last non-zero values for linear and angular velocity
            // are left in the viewer who does dead reconning and the objects look like
            // they float off.
            // BulletSim ships with a patch to Bullet which creates such an event.
            m_properties.Velocity        = Rigidbody.GetLinearVelocity();
            m_properties.AngularVelocity = Rigidbody.GetAngularVelocity();

            if (force

                || !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE) ||
                !AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
                // If the Velocity and AngularVelocity are zero, most likely the object has
                //    been deactivated. If they both are zero and they have become zero recently,
                //    make sure a property update is sent so the zeros make it to the viewer.
                || ((m_properties.Velocity == ZeroVect && m_properties.AngularVelocity == ZeroVect)
                    &&
                    (m_properties.Velocity != m_lastProperties.Velocity ||
                     m_properties.AngularVelocity != m_lastProperties.AngularVelocity))
                //	If Velocity and AngularVelocity are non-zero but have changed, send an update.
                || !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
                ||
                !AlmostEqual(ref m_properties.AngularVelocity, ref m_lastProperties.AngularVelocity,
                             ANGULARVELOCITY_TOLERANCE)
                )


            {
                // Add this update to the list of updates for this frame.
                m_lastProperties = m_properties;
                if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
                {
                    m_world.UpdatedObjects[m_world.LastEntityProperty++] = (m_properties);
                }

                //(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
            }
        }
Ejemplo n.º 9
0
        public static void runTests()
        {
            String       filename     = @"e:\users\man\bullet\xna-math-debug-output.txt";
            FileStream   filestream   = File.Open(filename, FileMode.Create, FileAccess.Write, FileShare.None);
            StreamWriter streamWriter = new StreamWriter(filestream);

            streamWriter.WriteLine("Identity");
            IndexedMatrix m = IndexedMatrix.Identity;

            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 2,0,0");
            m._basis.SetEulerZYX(2, 0, 0);
            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 0,2,0");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(0, 2, 0);
            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 0,0,2");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(0, 0, 2);
            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 2,2,0");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(2, 2, 0);
            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 2,0,2");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(2, 0, 2);
            MathUtil.PrintMatrix(streamWriter, m);
            streamWriter.WriteLine("setEuler 2,2,2");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(2, 2, 2);
            MathUtil.PrintMatrix(streamWriter, m);

            streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 100,100,100 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
            m._origin = new IndexedVector3(100, 100, 100);

            IndexedMatrix m2 = IndexedMatrix.Identity;

            m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
            m2._origin = new IndexedVector3(-10, 10, 0);

            IndexedMatrix m3 = m * m2;

            MathUtil.PrintMatrix(streamWriter, m3);

            //streamWriter.WriteLine("Broken axis comparison");
            //m = IndexedMatrix.Identity;
            //m.Right = new IndexedVector3(1, 0, 0);
            //m.Up = new IndexedVector3(0, 1, 0);
            //m.Backward = new IndexedVector3(0, 0, 1);
            //m._origin = new IndexedVector3(0, 20, 0);

            //m2 = IndexedMatrix.Identity;
            //m2.Right = new IndexedVector3(0, -1, 0);
            //m2.Up = new IndexedVector3(1, 0, 0);
            //m2.Backward = new IndexedVector3(0, 0, 1);
            //m2._origin = new IndexedVector3(1, -1, -1);

            //m3 = m *  m2;
            //MathUtil.PrintMatrix(streamWriter, m3);


            //streamWriter.WriteLine("setEuler 0.5*PI,0,0 Trans 0,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans -10,10,0");
            //m = IndexedMatrix.Identity;
            //m._basis.SetEulerZYX(MathUtil.SIMD_HALF_PI, 0, 0);
            //m._origin = new IndexedVector3(0, 0, 0);

            //m2 = IndexedMatrix.Identity;
            //m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
            //m2._origin = new IndexedVector3(-10, 10, 0);

            //m3 = m * m2;
            //MathUtil.PrintMatrix(streamWriter, m3);

            //streamWriter.WriteLine("setEuler 0.25*PI,0,0 Trans 33,0,0 MULTIPLIED BY setEuler 0,0,0.5*PI Trans 0,0,0");
            //m = IndexedMatrix.Identity;
            //m._basis.SetEulerZYX(MathUtil.SIMD_QUARTER_PI, 0, 0);
            //m._origin = new IndexedVector3(33, 0, 0);

            //m2 = IndexedMatrix.Identity;
            //m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
            //m2._origin = new IndexedVector3(0, 0, 0);

            //m3 = m *m2;
            //MathUtil.PrintMatrix(streamWriter, m3);



            streamWriter.WriteLine("transposeTimes");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(2, 1, 2);
            m._origin = new IndexedVector3(3, 3, 3);
            streamWriter.WriteLine("");
            //MathUtil.PrintMatrix(streamWriter, m);

            m2 = IndexedMatrix.Identity;
            m2._basis.SetEulerZYX(1, 2, -2);
            m2._origin = new IndexedVector3(5, 2, 13);
            //MathUtil.PrintMatrix(streamWriter, m2);

            IndexedBasisMatrix im3 = m._basis.TransposeTimes(m2._basis);

            MathUtil.PrintMatrix(streamWriter, im3);

            streamWriter.WriteLine("inverseTransform.");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(1, -2, -1);
            m._origin = new IndexedVector3(-1, 2, -3);
            IndexedVector3 v      = new IndexedVector3(20, 25, 30);
            IndexedVector3 result = MathUtil.InverseTransform(ref m, ref v);

            MathUtil.PrintVector3(streamWriter, result);
            streamWriter.WriteLine("");

            streamWriter.WriteLine("inverseTimes.");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(1, -2, -1);
            m._origin = new IndexedVector3(-1, 2, -3);

            m2 = IndexedMatrix.Identity;
            m2._basis.SetEulerZYX(0.3f, 0.8f, 2.3f);
            m2._origin = new IndexedVector3(20, 25, 30);
            m3         = m.InverseTimes(ref m2);
            MathUtil.PrintMatrix(streamWriter, m3);


            streamWriter.WriteLine("Transform.");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(1, -2, -1);
            m._origin = new IndexedVector3(-1, 2, -3);
            v         = new IndexedVector3(20, 25, 30);
            result    = m * v;
            MathUtil.PrintVector3(streamWriter, result);
            streamWriter.WriteLine("");

            streamWriter.WriteLine("TransformNormal.");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(1, -2, -1);
            m._origin = new IndexedVector3(-1, 2, -3);
            v         = new IndexedVector3(20, 25, 30);

            //IndexedMatrix tm = IndexedMatrix.Transpose(m);
            //result = IndexedVector3.TransformNormal(v,tm);
            result = m._basis * v;

            MathUtil.PrintVector3(streamWriter, result);
            streamWriter.WriteLine("");

            streamWriter.WriteLine("quatAngle");
            Quaternion q       = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
            float      fresult = MathUtil.QuatAngle(ref q);

            streamWriter.WriteLine(String.Format("{0:0.00000000}", fresult));

            streamWriter.WriteLine("quatRotate");
            v      = new IndexedVector3(20, -25, 30);
            result = MathUtil.QuatRotate(q, v);
            MathUtil.PrintVector3(streamWriter, result);
            streamWriter.WriteLine("");

            streamWriter.WriteLine("shortestArcQuat");
            v = new IndexedVector3(2f, -1f, 3f);
            IndexedVector3 v2 = new IndexedVector3(0.5f, 0.1f, 0.7f);

            q = MathUtil.ShortestArcQuat(v, v2);
            MathUtil.PrintQuaternion(streamWriter, q);

            streamWriter.WriteLine("");

            streamWriter.WriteLine("quaternionMultiply");
            q = Quaternion.CreateFromYawPitchRoll(0.3f, 0.2f, 0.7f);
            Quaternion q2 = Quaternion.CreateFromYawPitchRoll(-0.71f, 0.8f, 0.3f);

            q = MathUtil.QuaternionMultiply(q, q2);
            MathUtil.PrintQuaternion(streamWriter, q);
            streamWriter.WriteLine("");

            //streamWriter.WriteLine("matrixToEulerXYZ");
            //m = IndexedMatrix.Identity;
            //m._basis..SetEulerZYX(1, -2, -1);
            //m._origin = new IndexedVector3(-1, 2, -3);
            //MathUtil.MatrixToEulerXYZ(ref m, out result);
            //MathUtil.PrintVector3(streamWriter, result);
            //streamWriter.WriteLine("");


            streamWriter.WriteLine("getSkewSymmetrixMatrix");
            v = new IndexedVector3(0.2f, 0.7f, -0.3f);
            IndexedVector3 v3;
            IndexedVector3 v4;;

            MathUtil.GetSkewSymmetricMatrix(ref v, out v2, out v3, out v4);
            MathUtil.PrintVector3(streamWriter, v2);
            streamWriter.WriteLine("");
            MathUtil.PrintVector3(streamWriter, v3);
            streamWriter.WriteLine("");
            MathUtil.PrintVector3(streamWriter, v4);
            streamWriter.WriteLine("");


            streamWriter.WriteLine("quaternion create");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
            m._origin = new IndexedVector3(0.0f, 0.30f, 0.0f);
            m2        = IndexedMatrix.Identity;
            m2._basis.SetEulerZYX(0, 0, MathUtil.SIMD_HALF_PI);
            m2._origin = new IndexedVector3(0.0f, -0.14f, 0.0f);

            q  = m.GetRotation();
            q2 = m2.GetRotation();
            MathUtil.PrintQuaternion(streamWriter, q);
            streamWriter.WriteLine("");
            MathUtil.PrintQuaternion(streamWriter, q2);
            streamWriter.WriteLine("");



            streamWriter.WriteLine("row and column");
            m = IndexedMatrix.Identity;
            m._basis.SetEulerZYX(0.3f, 0.7f, 0.5f);
            m._origin = new IndexedVector3(11, 22, 33);
            MathUtil.PrintVector3(streamWriter, "col0", m._basis.GetColumn(0));
            MathUtil.PrintVector3(streamWriter, "col1", m._basis.GetColumn(1));
            MathUtil.PrintVector3(streamWriter, "col2", m._basis.GetColumn(2));
            //MathUtil.PrintVector3(streamWriter, "col3", MathUtil.MatrixColumn(m, 0));

            MathUtil.PrintVector3(streamWriter, "row0", m._basis.GetRow(0));
            MathUtil.PrintVector3(streamWriter, "row1", m._basis.GetRow(1));
            MathUtil.PrintVector3(streamWriter, "row2", m._basis.GetRow(2));
            //MathUtil.PrintVector3(streamWriter, "row3", MathUtil.MatrixRow(ref m, 3));


            Matrix        lookat1 = Matrix.CreateLookAt(new Vector3(5, 5, 5), new Vector3(10, 10, 10), new Vector3(0, 1, 0));
            IndexedMatrix lookat2 = IndexedMatrix.CreateLookAt(new IndexedVector3(5, 5, 5), new IndexedVector3(10, 10, 10), new IndexedVector3(0, 1, 0));
            Matrix        compare = lookat2.ToMatrix();

            MathUtil.PrintMatrix(streamWriter, "lookat1", lookat1);
            MathUtil.PrintMatrix(streamWriter, "lookat2", lookat2);
            MathUtil.PrintMatrix(streamWriter, "lookat compare", compare);


            float aspect = (float)(800.0f / 600.0f);
            float fov    = MathHelper.ToRadians(40.0f);
            float near   = 1f;
            float far    = 500f;

            Matrix        pov  = Matrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
            IndexedMatrix pov2 = IndexedMatrix.CreatePerspectiveFieldOfView(fov, aspect, near, far);
            Matrix        pov3 = pov2.ToMatrix();

            MathUtil.PrintMatrix(streamWriter, "pov1", pov);
            MathUtil.PrintMatrix(streamWriter, "pov2", pov2);
            MathUtil.PrintMatrix(streamWriter, "pov compare", pov3);


            streamWriter.WriteLine("Complete.");
            streamWriter.Flush();
            filestream.Close();
        }