C# Quat类代码示例

		public static void ToODE( Quat value, out Ode.dQuaternion result )
		{
			result.X = value.X;
			result.Y = value.Y;
			result.Z = value.Z;
			result.W = value.W;
		}

		public float Eval( float x, float y, float z )
		{
			var q = new Quat();
			var tmp = new Quat();

			var i = 0;

			q.R = x;
			q.I = y;
			q.J = z;
			q.K = 0;

			for ( i = 30; i > 0; i-- )
			{
				QSqr( ref tmp, q );
				QMult( ref q, this.emio, tmp );
				QAdd( ref q, this.c );

				if ( q.R*q.R + q.I*q.I + q.J*q.J + q.K*q.K > 8.0 )
				{
					break;
				}
			}

			return (float)i;
		}

	public Quat(Quat q)
	{
		this.x = q.x;
	    this.y = q.y;
		this.z = q.z;
		this.w = q.w;
	}

		public void QSqr( ref Quat b, Quat a )
		{
			b.R = a.R*a.R - a.I*a.I - a.J*a.J - a.K*a.K;
			b.I = 2.0f*a.R*a.I;
			b.J = 2.0f*a.R*a.J;
			b.K = 2.0f*a.R*a.K;
		}

		public void QMult( ref Quat c, Quat a, Quat b )
		{
			c.R = a.R*b.R - a.I*b.I - a.J*b.J - a.K*b.K;
			c.I = a.R*b.I + a.I*b.R + a.J*b.K - a.K*b.J;
			c.J = a.R*b.J + a.J*b.R + a.K*b.I - a.I*b.K;
			c.K = a.R*b.K + a.K*b.R + a.I*b.J - a.J*b.I;
		}

		public void QAdd( ref Quat a, Quat b )
		{
			a.R += b.R;
			a.I += b.I;
			a.J += b.J;
			a.K += b.K;
		}

 public SkyBox(int id, MessageBus bus)
 {
     Id = id;
     Bus = bus;
     Position = new Vect3(0, 0, 0);
     Rotation = new Quat(Math.Sqrt(0.5), 0, 0, Math.Sqrt(0.5));
 }

        public void SetForceFireRotationLookTo( Vec3 lookTo )
        {
            setForceFireRotation = true;

            Vec3 diff = lookTo - Position;
            //Vec3 diff = lookTo - GetFirePosition( false );

            forceFireRotation = Quat.FromDirectionZAxisUp( diff );
        }

 public override void PhysX_GetDrivePosition( out Vec3 position, out Quat rotation )
 {
     if( nativeJoint == IntPtr.Zero )
     {
         position = Vec3.Zero;
         rotation = Quat.Identity;
         return;
     }
     PhysXNativeWrapper.PhysXNativeD6Joint.GetDrivePosition( nativeJoint, out position, out rotation );
 }

        public void SetLookDirection(Vec3 pos)
        {
            Vec2 diff = pos.ToVec2() - Position.ToVec2();

            if (diff == Vec2.Zero)
                return;

            Radian dir = MathFunctions.ATan16(diff.Y, diff.X);

            float halfAngle = dir * 0.5f;
            Quat rot = new Quat(new Vec3(0, 0, MathFunctions.Sin16(halfAngle)), MathFunctions.Cos16(halfAngle));
            Rotation = rot;
        }

        public SpringWheel(Bone bone, Unit unit, float size, float stiffness, float damping)
        {
            this.bone = bone;
            this.size = size;
            this.stiffness = stiffness;
            this.damping = damping;
            this.unit = unit;
            originalBonePosition = bone.Position;
            originalBoneRotation = bone.Rotation;

            mesh = new MapObjectAttachedMesh();
            mesh.MeshName = "Types/Units/AwesomeAircraft/AwesomeAircraftWheel.mesh";
            mesh.ScaleOffset = new Vec3(1, 1, 1);
            mesh.RotationOffset = Quat.Identity;
            mesh.PositionOffset = bone.GetDerivedPosition();
            unit.Attach(mesh);
        }

		/// <summary>
		/// 
		/// </summary>
		/// <param name="globalReal"></param>
		/// <param name="globalImag"></param>
		/// <param name="globalTheta"></param>
		public Julia( float globalReal, float globalImag, float globalTheta )
		{
			oc = new Quat();
			oc.R = globalReal;
			oc.I = globalImag;
			oc.J = oc.K = 0.0f;

			eio = new Quat();
			eio.R = Utility.Cos( globalTheta );
			eio.I = Utility.Sin( globalTheta );
			eio.J = eio.K = 0;

            emio = new Quat();
            emio.R = Utility.Cos( -globalTheta );
            emio.I = Utility.Sin( -globalTheta );
            emio.J = eio.K = 0;

			QMult( ref c, eio, oc );
		}

		public Julia( float globalReal, float globalImag, float globalTheta )
		{
			this.globalReal = globalReal;
			this.globalImag = globalImag;
			this.globalTheta = globalTheta;

			this.oc = new Quat();
			this.oc.R = globalReal;
			this.oc.I = globalImag;
			this.oc.J = this.oc.K = 0.0f;

			this.eio = new Quat();
			this.eio.R = Utility.Cos( globalTheta );
			this.eio.I = Utility.Sin( globalTheta );
			this.eio.J = this.eio.K = 0;

			this.emio = new Quat();
			this.emio.R = Utility.Cos( -globalTheta );
			this.emio.I = Utility.Sin( -globalTheta );
			this.emio.J = this.emio.K = 0;

			QMult( ref this.c, this.eio, this.oc );
		}

        protected override void OnSetTransform( ref Vec3 pos, ref Quat rot, ref Vec3 scl )
        {
            base.OnSetTransform( ref pos, ref rot, ref scl );

            //server side
            if( EntitySystemWorld.Instance.IsServer() )
            {
                //send new position to clients
                Server_SendPositionToClients( EntitySystemWorld.Instance.RemoteEntityWorlds );
            }
        }

	static public Matrix3x3 Rotate(Vector3 rotation)
	{
		Quat x = new Quat(new Vector3d2(1,0,0), rotation.x * MathUtility.Deg2Rad );
		Quat y = new Quat(new Vector3d2(0,1,0), rotation.y * MathUtility.Deg2Rad );
		Quat z = new Quat(new Vector3d2(0,0,1), rotation.z * MathUtility.Deg2Rad );
		
		return (z*y*x).ToMatrix3x3();
	}