示例#1
0
    /// <summary>
    /// Multiply two quaternions.
    /// </summary>
    /// <param name="value1">The first quaternion.</param>
    /// <param name="value2">The second quaternion.</param>
    /// <returns>The product of both quaternions.</returns>
    #region public static FP operator *(JQuaternion value1, JQuaternion value2)
    public static TSQuaternion operator *(TSQuaternion value1, TSQuaternion value2)
    {
        TSQuaternion result;

        TSQuaternion.Multiply(ref value1, ref value2, out result);
        return(result);
    }
示例#2
0
    /// <summary>
    /// Scale a quaternion
    /// </summary>
    /// <param name="quaternion1">The quaternion to scale.</param>
    /// <param name="scaleFactor">Scale factor.</param>
    /// <returns>The scaled quaternion.</returns>
    #region public static JQuaternion Multiply(JQuaternion quaternion1, FP scaleFactor)
    public static TSQuaternion Multiply(TSQuaternion quaternion1, FP scaleFactor)
    {
        TSQuaternion result;

        TSQuaternion.Multiply(ref quaternion1, scaleFactor, out result);
        return(result);
    }
示例#3
0
    /// <summary>
    /// Multiply two quaternions.
    /// </summary>
    /// <param name="quaternion1">The first quaternion.</param>
    /// <param name="quaternion2">The second quaternion.</param>
    /// <returns>The product of both quaternions.</returns>
    #region public static JQuaternion Multiply(JQuaternion quaternion1, JQuaternion quaternion2)
    public static TSQuaternion Multiply(TSQuaternion quaternion1, TSQuaternion quaternion2)
    {
        TSQuaternion result;

        TSQuaternion.Multiply(ref quaternion1, ref quaternion2, out result);
        return(result);
    }
示例#4
0
    public static TSQuaternion LerpUnclamped(TSQuaternion a, TSQuaternion b, FP t)
    {
        TSQuaternion result = TSQuaternion.Multiply(a, (1 - t)) + TSQuaternion.Multiply(b, t);

        result.Normalize();

        return(result);
    }
示例#5
0
        private void IntegrateCallback(object obj)
        {
            RigidBody body = obj as RigidBody;

            TSVector temp;

            TSVector.Multiply(ref body.linearVelocity, timestep, out temp);
            TSVector.Add(ref temp, ref body.position, out body.position);

            if (!(body.isParticle))
            {
                //exponential map
                TSVector axis;
                FP       angle             = body.angularVelocity.magnitude;
                FP       halfTimeStep      = FP.Half * timestep;
                FP       halfTimeStepAngle = halfTimeStep * angle;
                if (angle < FP.EN3)
                {
                    // use Taylor's expansions of sync function
                    // axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
                    //TSVector.Multiply(ref body.angularVelocity, (halfTimeStep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
                    TSVector.Multiply(ref body.angularVelocity, halfTimeStep, out axis);
                }
                else
                {
                    // sync(fAngle) = sin(c*fAngle)/t
                    TSVector.Multiply(ref body.angularVelocity, (FP.Sin(halfTimeStepAngle) / angle), out axis);
                }

                TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(halfTimeStepAngle));
                TSQuaternion ornA;
                TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);

                TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);

                dorn.Normalize();
                TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
            }

            body.linearVelocity  /= (1 + timestep * body.linearDrag);
            body.angularVelocity /= (1 + timestep * body.angularDrag);

            /*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
             *  TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
             *
             * if ((body.Damping & RigidBody.DampingType.Angular) != 0)
             *  TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/

            body.Update();


            if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
            {
                body.SweptExpandBoundingBox(timestep);
            }
        }
示例#6
0
文件: World.cs 项目: zhangdb/TrueSync
        private void Integrate()
        {
            for (int index = 0, length = rigidBodies.Count; index < length; index++)
            {
                var body = rigidBodies[index];
                if (body.isStatic || !body.IsActive)
                {
                    continue;
                }

                body.position += body.linearVelocity * timestep;
                if (!(body.isParticle))
                {
                    //exponential map
                    TSVector axis;
                    FP       angle = body.angularVelocity.magnitude;

                    if (angle < FP.EN3)
                    {
                        // use Taylor's expansions of sync function
                        // axis = body.angularVelocity * (FP.Half * timestep - (timestep * timestep * timestep) * (0.020833333333f) * angle * angle);
                        TSVector.Multiply(body.angularVelocity, (FP.Half * timestep - (timestep * timestep * timestep) * (2082 * FP.EN6) * angle * angle), out axis);
                    }
                    else
                    {
                        // sync(fAngle) = sin(c*fAngle)/t
                        TSVector.Multiply(body.angularVelocity, (FP.Sin(FP.Half * angle * timestep) / angle), out axis);
                    }

                    TSQuaternion dorn = new TSQuaternion(axis.x, axis.y, axis.z, FP.Cos(angle * timestep * FP.Half));
                    TSQuaternion ornA; TSQuaternion.CreateFromMatrix(ref body.orientation, out ornA);

                    TSQuaternion.Multiply(ref dorn, ref ornA, out dorn);

                    dorn.Normalize(); TSMatrix.CreateFromQuaternion(ref dorn, out body.orientation);
                }

                body.linearVelocity  *= 1 / (1 + timestep * body.linearDrag);
                body.angularVelocity *= 1 / (1 + timestep * body.angularDrag);

                /*if ((body.Damping & RigidBody.DampingType.Linear) != 0)
                 *  TSVector.Multiply(ref body.linearVelocity, currentLinearDampFactor, out body.linearVelocity);
                 *
                 * if ((body.Damping & RigidBody.DampingType.Angular) != 0)
                 *  TSVector.Multiply(ref body.angularVelocity, currentAngularDampFactor, out body.angularVelocity);*/

                body.Update();


                if (CollisionSystem.EnableSpeculativeContacts || body.EnableSpeculativeContacts)
                {
                    body.SweptExpandBoundingBox(timestep);
                }
            }
        }
示例#7
0
    public static TSQuaternion Inverse(TSQuaternion rotation)
    {
        FP invNorm = FP.One / ((rotation.x * rotation.x) + (rotation.y * rotation.y) + (rotation.z * rotation.z) + (rotation.w * rotation.w));

        return(TSQuaternion.Multiply(TSQuaternion.Conjugate(rotation), invNorm));
    }