C# Quaternion.Inverse方法代码示例

        public void Quaternion_Inverse_Test()
        {
            var q = new Quaternion(1.0f, 2.0f, 3.0f, 4.0f);
            var expected = q.Conjugate() * (1 / q.Length);
            Assert.AreEqual(expected, q.Inverse());

            Assert.AreEqual(Quaternion.Zero, Quaternion.Zero.Inverse());

            var q2 = SlimDX.Quaternion.Invert(new SlimDX.Quaternion(2.0f, 3.0f, 4.0f, 1.0f));
            q2.Normalize();
            Assert.AreEqual(q2.ToQuaternion(), q.Inverse());
        }

        public Vector3 GetCorrectiveTorque()
        {
            Vector3 pos = new Vector3();
            Quaternion orient = new Quaternion();
            body.getPositionOrientation(out pos, out orient);
            Vector3 omega = body.getOmega();
            //Console.Out.WriteLine(omega.ToString());
            Vector3 torque = (orient.Inverse() * omega) / 50.0f / time;
            /*
            Console.Out.WriteLine("StopRotation");
            Console.Out.WriteLine("torque");
            Console.Out.WriteLine(torque.ToString());
            Console.Out.WriteLine("time");
            Console.Out.WriteLine(time);
            Console.Out.WriteLine("omega");
            Console.Out.WriteLine((orient.Inverse() * body.getOmega()).ToString());
            Console.Out.WriteLine("");
            */

            //Console.Out.WriteLine(torque.ToString());

            Vector3 correctiveTorque = new Vector3();

            //x correction
            //Util.Log(torque.ToString());
            if(torque.x > MAX_X_TORQUE)
            {
                correctiveTorque.x = -1;
            }
            else if (torque.x < -MAX_X_TORQUE)
            {
                correctiveTorque.x = 1;
            }
            else
            {
                correctiveTorque.x = -torque.x / MAX_X_TORQUE;
            }
            //y correction

            if (torque.y > MAX_Y_TORQUE)
            {
                correctiveTorque.y = -1;
            }
            else if (torque.y < -MAX_Y_TORQUE)
            {
                correctiveTorque.y = 1;
            }
            else
            {
                correctiveTorque.y = -torque.y / MAX_Y_TORQUE;
            }
            //z correction
            if (torque.z > MAX_Z_TORQUE)
            {
                correctiveTorque.z = -1;
            }
            else if (torque.z < -MAX_Z_TORQUE)
            {
                correctiveTorque.z = 1;
            }
            else
            {
                correctiveTorque.z = -torque.z / MAX_Z_TORQUE;
            }

            return correctiveTorque;
        }

        MultiBody CreateFeatherstoneMultiBody(MultiBodyDynamicsWorld world, MultiBodySettings settings)
        {
            int nLinks = settings.NumLinks;
            float mass = 13.5f * Scaling;
            Vector3 inertia = new Vector3(91, 344, 253) * Scaling * Scaling;

            var body = new MultiBody(nLinks, mass, inertia, settings.IsFixedBase, settings.CanSleep);
            //body.HasSelfCollision = false;

            //Quaternion orn = new Quaternion(0, 0, 1, -0.125f * Math.PI);
            Quaternion orn = new Quaternion(0, 0, 0, 1);
            body.BasePosition = settings.BasePosition;
            body.WorldToBaseRot = orn;
            body.BaseVelocity = Vector3.Zero;

            Vector3 joint_axis_hinge = new Vector3(1, 0, 0);
            Vector3 joint_axis_prismatic = new Vector3(0, 0, 1);
            Quaternion parent_to_child = orn.Inverse();
            Vector3 joint_axis_child_prismatic = parent_to_child.Rotate(joint_axis_prismatic);
            Vector3 joint_axis_child_hinge = parent_to_child.Rotate(joint_axis_hinge);

            int this_link_num = -1;
            int link_num_counter = 0;

            Vector3 pos = new Vector3(0, 0, 9.0500002f) * Scaling;
            Vector3 joint_axis_position = new Vector3(0, 0, 4.5250001f) * Scaling;

            for (int i = 0; i < nLinks; i++)
            {
                float initial_joint_angle = 0.3f;
                if (i > 0)
                    initial_joint_angle = -0.06f;

                int child_link_num = link_num_counter++;

                if (settings.UsePrismatic) // i == (nLinks - 1))
                {
                    body.SetupPrismatic(child_link_num, mass, inertia, this_link_num,
                        parent_to_child, joint_axis_child_prismatic, parent_to_child.Rotate(pos), Vector3.Zero, settings.DisableParentCollision);
                }
                else
                {
                    body.SetupRevolute(child_link_num, mass, inertia, this_link_num,
                        parent_to_child, joint_axis_child_hinge, joint_axis_position, parent_to_child.Rotate(pos - joint_axis_position), settings.DisableParentCollision);
                }
                body.SetJointPos(child_link_num, initial_joint_angle);
                this_link_num = i;

                /*if (false) //!useGroundShape && i == 4)
                {
                    Vector3 pivotInAworld = new Vector3(0, 20, 46);
                    Vector3 pivotInAlocal = body.WorldPosToLocal(i, pivotInAworld);
                    Vector3 pivotInBworld = pivotInAworld;
                    MultiBodyPoint2Point p2p = new MultiBodyPoint2Point(body, i, TypedConstraint.FixedBody, pivotInAlocal, pivotInBworld);
                    (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(p2p);
                }*/

                if (settings.UsePrismatic)
                {
                    //MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, nLinks - 1, 2, 3);

                    if (settings.CreateConstraints)
                    {
                        MultiBodyConstraint con = new MultiBodyJointLimitConstraint(body, i, -1, 1);
                        (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
                    }
                }
                else
                {
                    //if (true)
                    {
                        var con = new MultiBodyJointMotor(body, i, 0, 50000);
                        (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con);
                    }

                    var con2 = new MultiBodyJointLimitConstraint(body, i, -1, 1);
                    (World as MultiBodyDynamicsWorld).AddMultiBodyConstraint(con2);
                }
            }

            // Add a collider for the base
            Quaternion[] worldToLocal = new Quaternion[nLinks + 1];
            Vector3[] localOrigin = new Vector3[nLinks + 1];

            worldToLocal[0] = body.WorldToBaseRot;
            localOrigin[0] = body.BasePosition;

            //Vector3 halfExtents = new Vector3(7.5f, 0.05f, 4.5f);
            Vector3 halfExtents = new Vector3(7.5f, 0.45f, 4.5f);

            float[] posB = new float[] { localOrigin[0].X, localOrigin[0].Y, localOrigin[0].Z, 1 };
            //float[] quatB = new float[] { worldToLocal[0].X, worldToLocal[0].Y, worldToLocal[0].Z, worldToLocal[0].W };

            //if (true)
            {
                CollisionShape box = new BoxShape(halfExtents * Scaling);
                var bodyInfo = new RigidBodyConstructionInfo(mass, null, box, inertia);
                RigidBody bodyB = new RigidBody(bodyInfo);
                var collider = new MultiBodyLinkCollider(body, -1);

//.........这里部分代码省略.........

        //    *
        //	Run image manipulation
        //	\return Pointer to image buffer which has been set in the constructor.
        //	
        public override TextureBuffer process() {
            Quaternion qion = new Quaternion();
            float sum = 0f;
            Vector3 q = new Vector3();

            int w = (int)mBuffer.getWidth();
            int h = (int)mBuffer.getHeight();
            Quaternion rotation = new Quaternion(mW, mAxis);

            if (mParam != null && (mParam.getWidth() < w || mParam.getHeight() < h))
                return mBuffer;

            for (int y = 0; y < h; y++) {
                for (int x = 0; x < w; x++) {
                    ColourValue pixel = mBuffer.getPixel(x, y);
                    Quaternion v = new Quaternion(0.0f, ((pixel.r * 255.0f) - 127.5f) / 127.5f, ((pixel.b * 255.0f) - 127.5f) / 127.5f, ((pixel.g * 255.0f) - 127.5f) / 127.5f);

                    if (mParam != null) {
                        pixel = mParam.getPixel(x, y);
                        switch (mCompensation) {
                            case ABNORMALS_COMPENSATION.COMPENSATION_NORMAL:
                                qion = new Quaternion(0.0f, (pixel.r * 255.0f) - 127.5f, (pixel.b * 255.0f) - 127.5f, (pixel.g * 255.0f) - 127.5f);
                                v = v * (float)(1 - mSensitivity);
                                v = v + qion * ((float)mSensitivity / 127.5f);
                                break;

                            case ABNORMALS_COMPENSATION.COMPENSATION_HEIGHT:
                                sum = ((pixel.r + pixel.g + pixel.b) / 3.0f) * 255.0f;
                                qion = new Quaternion(new Radian(Math.TWO_PI * sum / 765.0f * mSensitivity), new Vector3(0.0f, 1.0f, 0.0f));
                                rotation = rotation * qion;
                                break;

                            case ABNORMALS_COMPENSATION.COMPENSATION_QUATERNION:
                                q = new Vector3((pixel.r * 255.0f) - 127.5f, (pixel.b * 255.0f) - 127.5f, (pixel.g * 255.0f) - 127.5f);
                                qion = new Quaternion(new Radian(2.0f / 255.0f * Math.PI * pixel.a * mSensitivity), q);
                                rotation = rotation * qion;
                                break;
                        }
                    }

                    v = rotation * v * rotation.Inverse();
                    float norm = v.Normalise();

                    if (mMirror == ABNORMALS_MIRROR.MIRROR_X_YZ || mMirror == ABNORMALS_MIRROR.MIRROR_X_Y_Z)
                        mBuffer.setRed(x, y, (1.0f - v.x * 0.5f + 0.5f));
                    else
                        mBuffer.setRed(x, y, (v.x * 0.5f + 0.5f));
                    if (mMirror == ABNORMALS_MIRROR.MIRROR_Y_XZ || mMirror == ABNORMALS_MIRROR.MIRROR_X_Y_Z)
                        mBuffer.setGreen(x, y, (1.0f - v.z * 0.5f + 0.5f));
                    else
                        mBuffer.setGreen(x, y, (v.z * 0.5f + 0.5f));
                    mBuffer.setBlue(x, y, (v.y * 0.5f + 0.5f));
                }
            }

            Utils.log("Modify texture with abnormals filter");
            return mBuffer;
        }

 /// <summary>
 /// Multiplies a Quaternion with the inverse of another
 /// Quaternion (q*q<sup>-1</sup>). Note that for Quaternions
 /// q*q<sup>-1</sup> is not the same then q<sup>-1</sup>*q,
 /// because this will lead to a rotation in the other direction.
 /// </summary>
 public Quaternion Divide(Quaternion q)
 {
     return Multiply(q.Inverse());
 }

        public void Test_Inverse()
        {
            var q1 = new Quaternion (45, 0, 1, 0);
            var q2 = new Quaternion (270, 0, -1, 0);

            var qInv1 = q1.Inverse ();
            var qInv2 = q2.Inverse ();

            var expected1 = new Quaternion (-45, 0, 1, 0);
            var expected2 = new Quaternion (-270, 0, -1, 0);

            Assert.AreEqual (expected1, qInv1);
            Assert.AreEqual (expected2, qInv2);
        }

 /// <summary>
 /// Inverts a specified Quaternion
 /// </summary>
 /// <param name="q">the quaternion to invert</param>
 /// <returns>the inverse of the specified quaternion</returns>
 public static Quaternion Invert(Quaternion q)
 {
     return q.Inverse();
 }