C# Quaternion.Normalize方法代码示例

 public void applyPrecedingTransform(SSSkeletalJointLocation parentLoc)
 {
     position = parentLoc.position
         + Vector3.Transform (position, parentLoc.orientation);
     orientation = Quaternion.Multiply (parentLoc.orientation, orientation);
     orientation.Normalize ();
 }

        protected override void OnInitializePhysics()
        {
            BoxShape boxA = new BoxShape(new Vector3(1, 1, 1));
            boxA.Margin = 0;

            BoxShape boxB = new BoxShape(new Vector3(0.5f, 0.5f, 0.5f));
            boxB.Margin = 0;

            objects[0] = new CollisionObject();
            objects[1] = new CollisionObject();

            objects[0].CollisionShape = boxA;
            objects[1].CollisionShape = boxB;

            // collision configuration contains default setup for memory, collision setup
            CollisionConf = new DefaultCollisionConfiguration();
            Dispatcher = new CollisionDispatcher(CollisionConf);

            Broadphase = new AxisSweep3(new Vector3(-1000, -1000, -1000), new Vector3(1000, 1000, 1000));

            World = new DiscreteDynamicsWorld(Dispatcher, Broadphase, null, CollisionConf);
            World.Gravity = new Vector3(0, -10, 0);
            IsDebugDrawEnabled = true;

            //World.AddCollisionObject(objects[0]);
            World.AddCollisionObject(objects[1]);

            Quaternion rotA = new Quaternion(0.739f, -0.204f, 0.587f, 0.257f);
            rotA.Normalize();

            objects[0].WorldTransform = Matrix.RotationQuaternion(rotA) * Matrix.Translation(0, 3, 0);
            objects[1].WorldTransform = Matrix.Translation(0, 4.248f, 0);
        }

 public void undoPrecedingTransform(SSSkeletalJointLocation parentLoc)
 {
     var parOrientInverse = parentLoc.orientation.Inverted ();
     orientation = Quaternion.Multiply (parOrientInverse, orientation);
     orientation.Normalize ();
     position = Vector3.Transform (position - parentLoc.position, parOrientInverse);
 }

        public void Normalize_ReturnsNormalizedQuaternion()
        {
            // Arrange
            var q = new Quaternion(1, 1, 1, 1);

            // Act
            var result = q.Normalize();

            // Assert
            Assert.That(result, Is.EqualTo(new Quaternion(0.5f, 0.5f, 0.5f, 0.5f)));
        }

 private void CompositionTarget_Rendering(object sender, object e)
 {
     MainViewModel.Current.Engine.Clear(0, 0, 0, 255);
     Quaternion p = new Quaternion(new Vector3(1.0f, 1.0f, 1.0f), 50f);
     p.Normalize();
     MainViewModel.Current.Cube.Rotation = MainViewModel.Current.Cube.Rotation * p;
     MainViewModel.Current.Tetra.Rotation = MainViewModel.Current.Tetra.Rotation * p;
     // Doing the various matrix operations
     MainViewModel.Current.Engine.Render(MainViewModel.Current.Camera, MainViewModel.Current.Cube);
     MainViewModel.Current.Engine.Render(MainViewModel.Current.Camera, MainViewModel.Current.Tetra);
     // Flushing the back buffer into the front buffer
     MainViewModel.Current.Engine.Present();
 }

        public static void Test_Quat_Generic_Normalized_Against_System_Numerics_Quat(float a, float b, float c, float d)
        {
            //arrange
            Quaternion<float> genericQuat = new Quaternion<float>(a, b, c, d);
            //System.Numerics.Quaternion quat = new System.Numerics.Quaternion(a, b, c, d);

            //act
            Quaternion<float> genericNormalized = genericQuat.Normalize();

            //assert
            for (int i = 0; i < 4; i++)
                Assert.AreNotEqual(genericNormalized[i], float.NaN, "Index: {0} was NaN", i);

            //normalization is an unmanaged call in Unity3D. Can't test against it.
            Assert.AreEqual(Math.Sqrt(genericNormalized.Length()), genericNormalized.LengthSquared(), 1.1E-05, "Normalization or length failed for " + nameof(Quaternion<float>));
            Assert.AreEqual(genericNormalized.Length() == 0 ? 0.0f : 1.0f, Math.Sqrt(genericNormalized.Length()), 1.1E-05, "Normalization or length failed for " + nameof(Quaternion<float>));
        }

        private Quaternion GetRotation(Vector3 src, Vector3 dest)
        {
            src.Normalize();
            dest.Normalize();

            float d = Vector3.Dot(src, dest);

            if (d >= 1f)
            {
                return Quaternion.Identity;
            }
            else if (d < (1e-6f - 1.0f))
            {
                Vector3 axis = Vector3.Cross(Vector3.UnitX, src);

                if (axis.LengthSquared() == 0)
                {
                    axis = Vector3.Cross(Vector3.UnitY, src);
                }

                axis.Normalize();
                return Quaternion.CreateFromAxisAngle(axis,(float) Math.PI);
            }
            else
            {
                float s = (float)Math.Sqrt((1 + d) * 2);
                float invS = 1 / s;

                Vector3 c = Vector3.Cross(src, dest);
                Vector3 invSc = c * new Vector3(invS, invS, invS);
                Quaternion q = new Quaternion(invSc, 0.5f * s);
                q.Normalize();

                return q;
            }
        }

        //	Special method that loads data into GPU, and can be called only from Draw method, never from LoadContent or from background thread.
        //	Because that would lead to empty vertex/index buffers if they are filled/created while game is minimized (remember the issue - alt-tab during loading screen)
        static void LoadInDraw()
        {
            if (m_loaded) return;

            //  In fact it doesn't matter how large is cube, it will always look same as we are always in its middle
            //  I changed it from 1.0 to 100.0 only because will small length I had problems with near frustum plane and crazy aspect ratios.
            const float CUBE_LENGTH_HALF = 100;

            Vector3 shapeSize = Vector3.One * CUBE_LENGTH_HALF;
            Vector3 shapePosition = Vector3.Zero;

            MyVertexFormatPositionTexture3[] boxVertices = new MyVertexFormatPositionTexture3[36];

            Vector3 topLeftFront = shapePosition + new Vector3(-1.0f, 1.0f, -1.0f) * shapeSize;
            Vector3 bottomLeftFront = shapePosition + new Vector3(-1.0f, -1.0f, -1.0f) * shapeSize;
            Vector3 topRightFront = shapePosition + new Vector3(1.0f, 1.0f, -1.0f) * shapeSize;
            Vector3 bottomRightFront = shapePosition + new Vector3(1.0f, -1.0f, -1.0f) * shapeSize;
            Vector3 topLeftBack = shapePosition + new Vector3(-1.0f, 1.0f, 1.0f) * shapeSize;
            Vector3 topRightBack = shapePosition + new Vector3(1.0f, 1.0f, 1.0f) * shapeSize;
            Vector3 bottomLeftBack = shapePosition + new Vector3(-1.0f, -1.0f, 1.0f) * shapeSize;
            Vector3 bottomRightBack = shapePosition + new Vector3(1.0f, -1.0f, 1.0f) * shapeSize;

            Vector3 textureTopLeftFront = MyMwcUtils.Normalize(topLeftFront);
            Vector3 textureBottomLeftFront = MyMwcUtils.Normalize(bottomLeftFront);
            Vector3 textureTopRightFront = MyMwcUtils.Normalize(topRightFront);
            Vector3 textureBottomRightFront = MyMwcUtils.Normalize(bottomRightFront);
            Vector3 textureTopLeftBack = MyMwcUtils.Normalize(topLeftBack);
            Vector3 textureTopRightBack = MyMwcUtils.Normalize(topRightBack);
            Vector3 textureBottomLeftBack = MyMwcUtils.Normalize(bottomLeftBack);
            Vector3 textureBottomRightBack = MyMwcUtils.Normalize(bottomRightBack);
            textureTopLeftFront.Z *= -1;
            textureBottomLeftFront.Z *= -1;
            textureTopRightFront.Z *= -1;
            textureBottomRightFront.Z *= -1;
            textureTopLeftBack.Z *= -1;
            textureTopRightBack.Z *= -1;
            textureBottomLeftBack.Z *= -1;
            textureBottomRightBack.Z *= -1;

            // Front face.
            boxVertices[0] = new MyVertexFormatPositionTexture3(topLeftFront, textureTopLeftFront);
            boxVertices[1] = new MyVertexFormatPositionTexture3(bottomLeftFront, textureBottomLeftFront);
            boxVertices[2] = new MyVertexFormatPositionTexture3(topRightFront, textureTopRightFront);
            boxVertices[3] = new MyVertexFormatPositionTexture3(bottomLeftFront, textureBottomLeftFront);
            boxVertices[4] = new MyVertexFormatPositionTexture3(bottomRightFront, textureBottomRightFront);
            boxVertices[5] = new MyVertexFormatPositionTexture3(topRightFront, textureTopRightFront);

            // Back face.
            boxVertices[6] = new MyVertexFormatPositionTexture3(topLeftBack, textureTopLeftBack);
            boxVertices[7] = new MyVertexFormatPositionTexture3(topRightBack, textureTopRightBack);
            boxVertices[8] = new MyVertexFormatPositionTexture3(bottomLeftBack, textureBottomLeftBack);
            boxVertices[9] = new MyVertexFormatPositionTexture3(bottomLeftBack, textureBottomLeftBack);
            boxVertices[10] = new MyVertexFormatPositionTexture3(topRightBack, textureTopRightBack);
            boxVertices[11] = new MyVertexFormatPositionTexture3(bottomRightBack, textureBottomRightBack);

            // Top face.
            boxVertices[12] = new MyVertexFormatPositionTexture3(topLeftFront, textureTopLeftFront);
            boxVertices[13] = new MyVertexFormatPositionTexture3(topRightBack, textureTopRightBack);
            boxVertices[14] = new MyVertexFormatPositionTexture3(topLeftBack, textureTopLeftBack);
            boxVertices[15] = new MyVertexFormatPositionTexture3(topLeftFront, textureTopLeftFront);
            boxVertices[16] = new MyVertexFormatPositionTexture3(topRightFront, textureTopRightFront);
            boxVertices[17] = new MyVertexFormatPositionTexture3(topRightBack, textureTopRightBack);

            // Bottom face.
            boxVertices[18] = new MyVertexFormatPositionTexture3(bottomLeftFront, textureBottomLeftFront);
            boxVertices[19] = new MyVertexFormatPositionTexture3(bottomLeftBack, textureBottomLeftBack);
            boxVertices[20] = new MyVertexFormatPositionTexture3(bottomRightBack, textureBottomRightBack);
            boxVertices[21] = new MyVertexFormatPositionTexture3(bottomLeftFront, textureBottomLeftFront);
            boxVertices[22] = new MyVertexFormatPositionTexture3(bottomRightBack, textureBottomRightBack);
            boxVertices[23] = new MyVertexFormatPositionTexture3(bottomRightFront, textureBottomRightFront);

            // Left face.
            boxVertices[24] = new MyVertexFormatPositionTexture3(topLeftFront, textureTopLeftFront);
            boxVertices[25] = new MyVertexFormatPositionTexture3(bottomLeftBack, textureBottomLeftBack);
            boxVertices[26] = new MyVertexFormatPositionTexture3(bottomLeftFront, textureBottomLeftFront);
            boxVertices[27] = new MyVertexFormatPositionTexture3(topLeftBack, textureTopLeftBack);
            boxVertices[28] = new MyVertexFormatPositionTexture3(bottomLeftBack, textureBottomLeftBack);
            boxVertices[29] = new MyVertexFormatPositionTexture3(topLeftFront, textureTopLeftFront);

            // Right face.
            boxVertices[30] = new MyVertexFormatPositionTexture3(topRightFront, textureTopRightFront);
            boxVertices[31] = new MyVertexFormatPositionTexture3(bottomRightFront, textureBottomRightFront);
            boxVertices[32] = new MyVertexFormatPositionTexture3(bottomRightBack, textureBottomRightBack);
            boxVertices[33] = new MyVertexFormatPositionTexture3(topRightBack, textureTopRightBack);
            boxVertices[34] = new MyVertexFormatPositionTexture3(topRightFront, textureTopRightFront);
            boxVertices[35] = new MyVertexFormatPositionTexture3(bottomRightBack, textureBottomRightBack);
            
            // if we've loaded the cube from DDS, orient it towards the sun
            var sun = MyGuiScreenGamePlay.Static.GetDirectionToSunNormalized();
            var toSun = new Quaternion(Vector3.Cross(Vector3.UnitX, sun), Vector3.Dot(Vector3.UnitX, sun));  // default orientation is +x
            toSun.Normalize();
            for (int i = 0; i < boxVertices.Length; i++)
            {
                boxVertices[i].Position = Vector3.Transform(boxVertices[i].Position, toSun);
            }
        
            m_boxVertexBuffer = new VertexBuffer(MyMinerGame.Static.GraphicsDevice, MyVertexFormatPositionTexture3.Stride * boxVertices.Length, Usage.WriteOnly, VertexFormat.None, Pool.Default);
            m_boxVertexBuffer.Lock(0, 0, LockFlags.None).WriteRange(boxVertices);
//.........这里部分代码省略.........

        /// <summary>
        /// Apply the various reaction, propulsion and bouyancy forces created by the cells,
        /// to affect the entire organism's position
        /// </summary>
        private void ApplyForces()
        {
            const float BOUYANCYSCALE = 1.5f;			// scales bouyancy alone
            const float PROPSCALE = 0.05f;				// scales propulsion alone
            const float REACTSCALE = 0.5f;				// scales reaction force alone
            const float TORQUESCALE = 0.01f;			// scales torque element
            const float XLATESCALE = 1;					// scales translation element
            const float MAXFORCE = 0.5f;				// maximum allowed length of force vector
            const float ROTATIONALDRAG = 0.998f;        // rotation slows by at least this fraction, even if no draggy limbs

            // Some organisms are fixtures (e.g. the Lab) and so don't respond to forces at all
            if (Dynamic == false)
            {
                return;
            }

            foreach (Cell c in partList)				// add in each cell's reaction force
            {

                // BUOYANCY FORCE
                Vector3 bouy = new Vector3(0, c.Physiology.Buoyancy
                                            * scale
                                            * BOUYANCYSCALE
                                            * Scene.ElapsedTime,0);

                // COLLISION/PROPULSION FORCE
                // This was calculated during collision detection, and represents the reaction force caused by
                // colliding with another object. It may also include a force produced by 'jet propelled' cells
                Vector3 prop = Vector3.Scale(c.propulsionForce,PROPSCALE);

                // REACTION FORCE
                // This is the force caused by a) animation of cells, pushing on the water
                // and b) the reaction against the resistance of the water produced by all cells as a result of
                // the organism's overall movement. Note: this automatically produces realistic friction!
                // The force is calculated from the old and new positions of the cell & water resistance of cell
                // Square forces, so that fast movements have more effect than slow ones.
                // This allows creatures to swim
                Vector3 reaction = (c.OldLocation - c.Location);
                reaction *= c.Physiology.Resistance							// scale by water resistance
                    * scale                                                 // modified by creature's size
                    * REACTSCALE;											// and a scaling factor

                Vector3 total = bouy + reaction + prop;						// The sum of all the forces
                float reactionForce = total.LengthSq();						// SQUARED for nonlinear propulsion

                total.Normalize();											// The direction of the displacement

                // Limit total force
                if (reactionForce>MAXFORCE)
                {
            ////					Engine.DebugHUD.WriteLine("reactionForce over limit: "+reactionForce);
                    reactionForce = MAXFORCE;
                }

                // APPLY TORQUE
                // Convert force and direction back into a force vector
                Vector3 torqueVector = Vector3.Scale(total,reactionForce);

                // the 'handle of the wrench' (vector from CG to cell position)
                Vector3 wrench = c.Location - CGCell.Location;
                // The torque created by the force is the cross product of this and the force applied to its end.
                // The vector's axis of rotation denotes the direction, and its length is the torque (angular momentum).
                Vector3 axis = Vector3.Cross(wrench,torqueVector);
                float torque = Vector3.Length(axis);							// extract torque
                if (torque!=0)
                {
                    axis.Normalize();											// normalise axis of rotation
                    // scale force to bring it into a reasonable range
                    // and apply proportionally to frame time
                    torque *= TORQUESCALE;
                    // Get the torque as a rotation, stored in a quaternion
                    Quaternion rot = Quaternion.RotationAxis(axis,torque);
                    // Use it to accelerate the organism's rotational velocity
                    turnRate *= rot;
                    // add some drag by subtracting a fraction of the rotation rate. This stops "infinitely thin" creatures from spinning forever
                   turnRate = Quaternion.Slerp(Quaternion.Identity, turnRate, ROTATIONALDRAG);
                }

                // APPLY TRANSLATION
                reactionForce *= XLATESCALE;
                total.Scale(reactionForce);
                translationRate += total;

            }

            // APPLY OVERALL FORCE TO SPEED
            // rotate the creature at the new speed, proportional to elapsed time
            // (to scale degrees per second by the fraction of a second elapsed, slerp between a
            // zero rotation and the rate of turn)
            // NOTE: multiply elapsed time by 4 here, otherwise max speed will be 0.5 rev per second,
            // since the turnRate can't be greater than 180 degrees
            // Max speed is now 2 rps, when turnRate == PI deg
            turnRate.Normalize();
            orientation *= Quaternion.Slerp(Quaternion.Identity,turnRate,4);
            orientation.Normalize();

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

        /// <summary>
        ///
        /// </summary>
        /// <param name="dMouse"></param>
        protected virtual void MouseRotation(Quaternion dMouse)
        {
            var mRot = Matrix.RotationQuaternion(dMouse);

            LookAt = Position + mRot.TransformNormal(LookAt - Position);
            Up = mRot.TransformNormal(Up);

            m_viewRotQuat *= dMouse;
            m_viewRotQuat.Normalize();
        }

        public void RotateX(float angle)
        {
			if(angle != 0f) {
				_rot = _rot * Quaternion.FromAxisAngle(Vector3.UnitX,angle);
				_rot.Normalize();
				recomputeBasis();
			}
        }

//.........这里部分代码省略.........
            {
                Vector3 Change = Vector3.One;
                //Start Experimental Values
                if (Zchange < -1)
                {
                    m_lastAngularVelocity.X += 1.25f;
                }
                else if (Zchange < -.75)
                {
                    m_lastAngularVelocity.X += 1f;
                }
                else if (Zchange < -.5)
                {
                    m_lastAngularVelocity.X += 0.75f;
                }
                else if (Zchange < -.25)
                {
                    m_lastAngularVelocity.X += .5f;
                }
                else if (Zchange < -.05)
                {
                    m_lastAngularVelocity.X += .25f;
                }

                //End Experimental Values

                if (Change != Vector3.One)
                {
                    //Using the ref frame because this requires the use of the idea of UP
                    Quaternion rotq = new Quaternion(rot.X + m_referenceFrame.X,
                        rot.Y + m_referenceFrame.Y,
                        rot.Z + m_referenceFrame.Z,
                        rot.W);    // rotq = rotation of object
                    rotq.Normalize();
                    Change *= rotq;
                    Change.Z = 0;
                    m_lastAngularVelocity += Change;
                }
            }

            #endregion

            #region Vertical stabilizer

            if (rot.Y > .01 + m_referenceFrame.Y) //Add the reference frame because this requires the use of the idea of UP
            {
                m_lastAngularVelocity.Y -= (m_lastAngularVelocity.Y) * (pTimestep);
            }
            if (rot.Y < -.01 + m_referenceFrame.Y)
            {
                m_lastAngularVelocity.Y += (m_lastAngularVelocity.Y) * (pTimestep);
            }

            #endregion

            #region Drift

            //Slide us around a bit
            lastDrift += m_lastAngularVelocity.Z / (7f / pTimestep);

            if (Math.Abs(lastDrift) < 0.1)
                lastDrift = 0;

            lastDrift -= lastDrift / (2f / pTimestep);

            m_lastAngularVelocity.Z += lastDrift;

 private static Quaternion CreateRandomRotation(MyRandom self)
 {
     Quaternion q = new Quaternion(
         self.NextFloat() * 2f - 1f,
         self.NextFloat() * 2f - 1f,
         self.NextFloat() * 2f - 1f,
         self.NextFloat() * 2f - 1f);
     q.Normalize();
     return q;
 }

 // convert a LSL_Rotation to a Quaternion
 public static Quaternion Rot2Quaternion(LSL_Rotation r)
 {
     Quaternion q = new Quaternion((float)r.x, (float)r.y, (float)r.z, (float)r.s);
     q.Normalize();
     return q;
 }

        public static bool IsPenetratingShapeShape(HkShape shape1, ref Vector3D translation1, ref Quaternion rotation1, HkShape shape2, ref Vector3D translation2, ref Quaternion rotation2)
        {
            //rotations have to be normalized
            rotation1.Normalize();
            rotation2.Normalize();

            //jn: TODO this is world independent test, just transform so shape1 is on zero and querry on any world
            m_resultWorlds.Clear();
            Clusters.Intersects(translation1, m_resultWorlds);

            foreach (var world in m_resultWorlds)
            {
                if (world.AABB.Contains(translation2) != ContainmentType.Contains)
                    return false;

                Vector3 translation1F = translation1 - world.AABB.Center;
                Vector3 translation2F = translation2 - world.AABB.Center;

                if (((HkWorld)world.UserData).IsPenetratingShapeShape(shape1, ref translation1F, ref rotation1, shape2, ref translation2F, ref rotation2))
                    return true;
            }

            return false;
        }