Пример #1
0
        /// <summary>
        /// Constructs a new constraint which prevents relative angular motion between the two connected bodies.
        /// </summary>
        /// <param name="connectionA">First connection of the pair.</param>
        /// <param name="connectionB">Second connection of the pair.</param>
        public NoRotationJoint(Entity connectionA, Entity connectionB)
        {
            ConnectionA = connectionA;
            ConnectionB = connectionB;

            initialQuaternionConjugateA = QuaternionEx.Conjugate(ConnectionA.orientation);
            initialQuaternionConjugateB = QuaternionEx.Conjugate(ConnectionB.orientation);
        }
Пример #2
0
 /// <summary>
 /// Computes the quaternion rotation between two normalized vectors.
 /// </summary>
 /// <param name="v1">First unit-length vector.</param>
 /// <param name="v2">Second unit-length vector.</param>
 /// <param name="q">System.Numerics.Quaternion representing the rotation from v1 to v2.</param>
 public static void GetQuaternionBetweenNormalizedVectors(ref System.Numerics.Vector3 v1, ref System.Numerics.Vector3 v2, out System.Numerics.Quaternion q)
 {
     float dot;
     Vector3Ex.Dot(ref v1, ref v2, out dot);
     //For non-normal vectors, the multiplying the axes length squared would be necessary:
     //float w = dot + (float)Math.Sqrt(v1.LengthSquared() * v2.LengthSquared());
     if (dot < -0.9999f) //parallel, opposing direction
     {
         //If this occurs, the rotation required is ~180 degrees.
         //The problem is that we could choose any perpendicular axis for the rotation. It's not uniquely defined.
         //The solution is to pick an arbitrary perpendicular axis.
         //Project onto the plane which has the lowest component magnitude.
         //On that 2d plane, perform a 90 degree rotation.
         float absX = Math.Abs(v1.X);
         float absY = Math.Abs(v1.Y);
         float absZ = Math.Abs(v1.Z);
         if (absX < absY && absX < absZ)
             q = new System.Numerics.Quaternion(0, -v1.Z, v1.Y, 0);
         else if (absY < absZ)
             q = new System.Numerics.Quaternion(-v1.Z, 0, v1.X, 0);
         else
             q = new System.Numerics.Quaternion(-v1.Y, v1.X, 0, 0);
     }
     else
     {
         System.Numerics.Vector3 axis;
         Vector3Ex.Cross(ref v1, ref v2, out axis);
         q = new System.Numerics.Quaternion(axis.X, axis.Y, axis.Z, dot + 1);
     }
     q = QuaternionEx.Normalize(q);
 }
Пример #3
0
 /// <summary>
 /// Constructs a quaternion from a rotation matrix.
 /// </summary>
 /// <param name="r">Rotation matrix to create the quaternion from.</param>
 /// <param name="q">System.Numerics.Quaternion based on the rotation matrix.</param>
 public static void CreateFromRotationMatrix(ref Matrix3x3 r, out System.Numerics.Quaternion q)
 {
     float trace = r.M11 + r.M22 + r.M33;
     #if !WINDOWS
     q = new System.Numerics.Quaternion();
     #endif
     if (trace >= 0)
     {
         var S = (float)Math.Sqrt(trace + 1.0) * 2; // S=4*qw
         var inverseS = 1 / S;
         q.W = 0.25f * S;
         q.X = (r.M23 - r.M32) * inverseS;
         q.Y = (r.M31 - r.M13) * inverseS;
         q.Z = (r.M12 - r.M21) * inverseS;
     }
     else if ((r.M11 > r.M22) & (r.M11 > r.M33))
     {
         var S = (float)Math.Sqrt(1.0 + r.M11 - r.M22 - r.M33) * 2; // S=4*qx
         var inverseS = 1 / S;
         q.W = (r.M23 - r.M32) * inverseS;
         q.X = 0.25f * S;
         q.Y = (r.M21 + r.M12) * inverseS;
         q.Z = (r.M31 + r.M13) * inverseS;
     }
     else if (r.M22 > r.M33)
     {
         var S = (float)Math.Sqrt(1.0 + r.M22 - r.M11 - r.M33) * 2; // S=4*qy
         var inverseS = 1 / S;
         q.W = (r.M31 - r.M13) * inverseS;
         q.X = (r.M21 + r.M12) * inverseS;
         q.Y = 0.25f * S;
         q.Z = (r.M32 + r.M23) * inverseS;
     }
     else
     {
         var S = (float)Math.Sqrt(1.0 + r.M33 - r.M11 - r.M22) * 2; // S=4*qz
         var inverseS = 1 / S;
         q.W = (r.M12 - r.M21) * inverseS;
         q.X = (r.M31 + r.M13) * inverseS;
         q.Y = (r.M32 + r.M23) * inverseS;
         q.Z = 0.25f * S;
     }
 }
Пример #4
0
        void IPositionUpdateable.PreUpdatePosition(float dt)
        {
            System.Numerics.Vector3 increment;

            Vector3Ex.Multiply(ref angularVelocity, dt * .5f, out increment);
            var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
            QuaternionEx.Multiply(ref multiplier, ref orientation, out multiplier);
            QuaternionEx.Add(ref orientation, ref multiplier, out orientation);
            orientation = System.Numerics.Quaternion.Normalize(orientation);

            Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix);

            //Only do the linear motion if this object doesn't obey CCD.
            if (PositionUpdateMode == PositionUpdateMode.Discrete)
            {
                Vector3Ex.Multiply(ref linearVelocity, dt, out increment);
                Vector3Ex.Add(ref position, ref increment, out position);

                collisionInformation.UpdateWorldTransform(ref position, ref orientation);
                //The position update is complete if this is a discretely updated object.
                if (PositionUpdated != null)
                    PositionUpdated(this);
            }

            MathChecker.Validate(linearVelocity);
            MathChecker.Validate(angularVelocity);
            MathChecker.Validate(position);
            MathChecker.Validate(orientation);
            #if CONSERVE
            MathChecker.Validate(angularMomentum);
            #endif
        }
Пример #5
0
 /// <summary>
 /// Finds the change in the rotation state quaternion provided the local inertia tensor and angular velocity.
 /// </summary>
 /// <param name="orientation">Orienatation of the object.</param>
 /// <param name="localInertiaTensorInverse">Local-space inertia tensor of the object being updated.</param>
 /// <param name="angularMomentum">Angular momentum of the object.</param>
 ///  <param name="orientationChange">Change in quaternion.</param>
 public static void DifferentiateQuaternion(ref System.Numerics.Quaternion orientation, ref Matrix3x3 localInertiaTensorInverse, ref System.Numerics.Vector3 angularMomentum, out System.Numerics.Quaternion orientationChange)
 {
     System.Numerics.Quaternion normalizedOrientation;
     QuaternionEx.Normalize(ref orientation, out normalizedOrientation);
     Matrix3x3 tempRotMat;
     Matrix3x3.CreateFromQuaternion(ref normalizedOrientation, out tempRotMat);
     Matrix3x3 tempInertiaTensorInverse;
     Matrix3x3.MultiplyTransposed(ref tempRotMat, ref localInertiaTensorInverse, out tempInertiaTensorInverse);
     Matrix3x3.Multiply(ref tempInertiaTensorInverse, ref tempRotMat, out tempInertiaTensorInverse);
     System.Numerics.Vector3 halfspin;
     Matrix3x3.Transform(ref angularMomentum, ref tempInertiaTensorInverse, out halfspin);
     Vector3Ex.Multiply(ref halfspin, .5f, out halfspin);
     var halfspinQuaternion = new System.Numerics.Quaternion(halfspin.X, halfspin.Y, halfspin.Z, 0);
     QuaternionEx.Multiply(ref halfspinQuaternion, ref normalizedOrientation, out orientationChange);
 }
Пример #6
0
        /// <summary>
        /// Integrates the position and orientation of the bone forward based upon the current linear and angular velocity.
        /// </summary>
        internal void UpdatePosition()
        {
            //Update the position based on the linear velocity.
            Vector3Ex.Add(ref Position, ref linearVelocity, out Position);

            //Update the orientation based on the angular velocity.
            System.Numerics.Vector3 increment;
            Vector3Ex.Multiply(ref angularVelocity, .5f, out increment);
            var multiplier = new System.Numerics.Quaternion(increment.X, increment.Y, increment.Z, 0);
            QuaternionEx.Multiply(ref multiplier, ref Orientation, out multiplier);
            QuaternionEx.Add(ref Orientation, ref multiplier, out Orientation);
            Orientation = System.Numerics.Quaternion.Normalize(Orientation);

            //Eliminate any latent velocity in the bone to prevent unwanted simulation feedback.
            //This is the only thing conceptually separating this "IK" solver from the regular dynamics loop in BEPUphysics.
            //(Well, that and the whole lack of collision detection...)
            linearVelocity = new System.Numerics.Vector3();
            angularVelocity = new System.Numerics.Vector3();

            //Note: Unlike a regular dynamics simulation, we do not include any 'dt' parameter in the above integration.
            //Setting the velocity to 0 every update means that no more than a single iteration's worth of velocity accumulates.
            //Since the softness of constraints already varies with the time step and bones never accelerate for more than one frame,
            //scaling the velocity for position integration actually turns out generally worse.
            //This is not a rigorously justifiable approach, but this isn't a regular dynamic simulation anyway.
        }
Пример #7
0
 /// <summary>
 /// Constructs a new bone. Assumes the mass will be set later.
 /// </summary>
 /// <param name="position">Initial position of the bone.</param>
 /// <param name="orientation">Initial orientation of the bone.</param>
 /// <param name="radius">Radius of the bone.</param>
 /// <param name="height">Height of the bone.</param>
 public Bone(System.Numerics.Vector3 position, System.Numerics.Quaternion orientation, float radius, float height)
 {
     Mass = 1;
     Position = position;
     Orientation = orientation;
     Radius = radius;
     Height = height;
 }
Пример #8
0
 ///<summary>
 /// Constructs a new rigid transform.
 ///</summary>
 ///<param name="orienation">Rotation component of the transform.</param>
 public RigidTransform(System.Numerics.Quaternion orienation)
 {
     Position = new System.Numerics.Vector3();
     Orientation = orienation;
 }
Пример #9
0
 ///<summary>
 /// Constructs a new rigid transform.
 ///</summary>
 ///<param name="position">Translation component of the transform.</param>
 public RigidTransform(System.Numerics.Vector3 position)
 {
     Position = position;
     Orientation = System.Numerics.Quaternion.Identity;
 }
Пример #10
0
 ///<summary>
 /// Constructs a new rigid transform.
 ///</summary>
 ///<param name="position">Translation component of the transform.</param>
 ///<param name="orientation">Rotation component of the transform.</param>
 public RigidTransform(System.Numerics.Vector3 position, System.Numerics.Quaternion orientation)
 {
     Position = position;
     Orientation = orientation;
 }
Пример #11
0
 ///<summary>
 /// Constructs a new entry with identity orientation.
 ///</summary>
 ///<param name="shape">Shape of the entry.</param>
 public OrientedConvexShapeEntry(ConvexShape shape)
 {
     Orientation = System.Numerics.Quaternion.Identity;
     CollisionShape = shape;
 }
Пример #12
0
 ///<summary>
 /// Constructs a new entry.
 ///</summary>
 ///<param name="orientation">Orientation of the entry.</param>
 ///<param name="shape">Shape of the entry.</param>
 public OrientedConvexShapeEntry(System.Numerics.Quaternion orientation, ConvexShape shape)
 {
     Orientation = orientation;
     CollisionShape = shape;
 }