// Updates the matrices of the slaves using the rotation quaternion.
        private void UpdateSlaves(Quaternion q, double s, Vector3D t)
        {

            if (_slaves != null)
            {
                foreach (Viewport3D i in _slaves)
                {
                    ModelVisual3D mv = i.Children[0] as ModelVisual3D;
                    Transform3DGroup t3dg = mv.Transform as Transform3DGroup;

                    ScaleTransform3D _GroupScaleTransform = t3dg.Children[0] as ScaleTransform3D;
                    RotateTransform3D _GroupRotateTransform = t3dg.Children[1] as RotateTransform3D;
                    TranslateTransform3D _GroupTranslateTransform = t3dg.Children[2] as TranslateTransform3D;

                    _GroupScaleTransform.ScaleX = s;
                    _GroupScaleTransform.ScaleY = s;
                    _GroupScaleTransform.ScaleZ = s;
                    _GroupRotateTransform.Rotation = new AxisAngleRotation3D(q.Axis, q.Angle);
                    _GroupTranslateTransform.OffsetX = t.X;
                    _GroupTranslateTransform.OffsetY = t.Y;
                    _GroupTranslateTransform.OffsetZ = t.Z;

                }
            }
        }

		public NewtonQuaternion(Quaternion pQuaternion)
		{
			NWQuaternion[ 0 ] = (float)pQuaternion.X;
			NWQuaternion[ 1 ] = (float)pQuaternion.Y;
			NWQuaternion[ 2 ] = (float)pQuaternion.Z;
			NWQuaternion[ 3 ] = (float)pQuaternion.W;
		}

		private void Track(Point currentPosition)
		{
			var currentPosition3D = this.ProjectToTrackball(this.ActualWidth, this.ActualHeight, currentPosition);

			var axis = Vector3D.CrossProduct(this._previousPosition3D, currentPosition3D);
			var angle = Vector3D.AngleBetween(this._previousPosition3D, currentPosition3D);

			// quaterion will throw if this happens - sometimes we can get 3D positions that
			// are very similar, so we avoid the throw by doing this check and just ignoring
			// the event 
			if (axis.Length == 0)
				return;

			var delta = new Quaternion(axis, -angle);

			// Get the current orientantion from the RotateTransform3D
			var r = this._rotation;
			var q = new Quaternion(this._rotation.Axis, this._rotation.Angle);

			// Compose the delta with the previous orientation
			q *= delta;

			// Write the new orientation back to the Rotation3D
			this._rotation.Axis = q.Axis;
			this._rotation.Angle = q.Angle;

			this._previousPosition3D = currentPosition3D;
		}

 public MyPosition(int possessingId, Vector3D position, Quaternion rotation, EnumMobileState mobileState)
 {
     PossessingId = possessingId;
     Position = position;
     Rotation = rotation;
     MobileState = mobileState;
 }

        private void CalculateCurrentQuaternion(double timeFactor)
        {
            if (LERPActivated)
            {
                var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
                var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
                var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
                var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
                currentQuaternion = new Quaternion(x, y, z, w);
                currentQuaternion.Normalize();
            }
            else
            {
                currentQuaternion = Quaternion.Slerp(startQuaternion, endQuaternion, timeFactor);

                double dotProduct = startQuaternion.X * endQuaternion.Y + startQuaternion.Y * endQuaternion.Y
                    + startQuaternion.Z * endQuaternion.Z + startQuaternion.W * endQuaternion.W;

                var theta = Math.Acos(dotProduct);
                if (theta < 0.0) theta = -theta;

                var startFactor = Math.Sin((1 - timeFactor) * theta) / Math.Sin(theta);
                var endFactor = Math.Sin(timeFactor * theta) / Math.Sin(theta);
                var x = startFactor * startQuaternion.X + endFactor * endQuaternion.X;
                var y = startFactor * startQuaternion.Y + endFactor * endQuaternion.Y;
                var z = startFactor * startQuaternion.Z + endFactor * endQuaternion.Z;
                var w = startFactor * startQuaternion.W + endFactor * endQuaternion.W;
                currentQuaternion = new Quaternion(x, y, z, w);
                currentQuaternion.Normalize();
            }
        }

        /// <summary>
        /// Updates the quadcopter model based on roll pitch and yaw (in radians.)
        /// </summary>
        /// <param name="roll"></param>
        /// <param name="pitch"></param>
        /// <param name="yaw"></param>
        public void UpdateModel(float roll, float pitch, float yaw)
        {
            //Credit to http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
            // for the math

            double heading = -1 * yaw;
            //double heading = 0.0;
            double attitude = -1 * roll;
            double bank = pitch;

            double c1 = Math.Cos(heading / 2.0);
            double s1 = Math.Sin(heading / 2.0);
            double c2 = Math.Cos(attitude / 2.0);
            double s2 = Math.Sin(attitude / 2.0);
            double c3 = Math.Cos(bank / 2.0);
            double s3 = Math.Sin(bank / 2.0);
            double c1c2 = c1 * c2;
            double s1s2 = s1 * s2;
            double w = c1c2 * c3 - s1s2 * s3;
            double x = c1c2 * s3 + s1s2 * c3;
            double y = s1 * c2 * c3 + c1 * s2 * s3;
            double z = c1 * s2 * c3 - s1 * c2 * s3;

            Quaternion q = new Quaternion(x, y, z, w);
            this.angleRotation.Angle = q.Angle;
            this.angleRotation.Axis = q.Axis;

            this.Label_Roll.Content = "Roll: " + (roll * 180.0 / Math.PI).ToString("#0.00");
            this.Label_Pitch.Content = "Pitch: " + (pitch * 180.0 / Math.PI).ToString("#0.00");
            this.Label_Yaw.Content = "Yaw: " + (yaw * 180.0 / Math.PI).ToString("#0.00");
        }

        //Source: http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
        public static Vector3D QuaternionToEulerAnglesInRad(Quaternion q)
        {
            q = FormatQuaternion(q);

            var sqw = Math.Pow(q.W,2);
            var sqx = Math.Pow(q.X,2);
            var sqy = Math.Pow(q.Y, 2);
            var sqz = Math.Pow(q.Z, 2);
	        var unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
	        var test = q.X*q.Y + q.Z*q.W;
	        if (test > 0.499*unit) 
            { // singularity at north pole
	            return new Vector3D
	                {
	                    Y = 2*Math.Atan2(q.X, q.W),
	                    Z = Math.PI/2,
	                    X = 0
	                };
	        }
	        if (test < -0.499*unit) 
            { // singularity at south pole
	            return new Vector3D
	                {
	                    Y = -2*Math.Atan2(q.X, q.W),
	                    Z = -Math.PI/2,
	                    X = 0
	                };
	        }
            return new Vector3D
                {
                    Y = -Math.Atan2(2*q.Y*q.W - 2*q.X*q.Z, sqx - sqy - sqz + sqw),
                    Z = Math.Asin(2 * test / unit),
                    X = -Math.Atan2(2 * q.X * q.W - 2 * q.Y * q.Z, -sqx + sqy - sqz + sqw)
            };
        }

        public Matrix3D BuildMatrix3DFromQuaternion(Quaternion q)
        {
            double sqw = q.W * q.W;
            double sqx = q.X * q.X;
            double sqy = q.Y * q.Y;
            double sqz = q.Z * q.Z;

            // invs (inverse square length) is only required if quaternion is not already normalised
            double invs = 1 / (sqx + sqy + sqz + sqw);
            double m00 = (sqx - sqy - sqz + sqw) * invs; // since sqw + sqx + sqy + sqz =1/invs*invs
            double m11 = (-sqx + sqy - sqz + sqw) * invs;
            double m22 = (-sqx - sqy + sqz + sqw) * invs;

            double tmp1 = q.X * q.Y;
            double tmp2 = q.Z * q.W;
            double m10 = 2.0 * (tmp1 + tmp2) * invs;
            double m01 = 2.0 * (tmp1 - tmp2) * invs;

            tmp1 = q.X * q.Z;
            tmp2 = q.Y * q.W;
            double m20 = 2.0 * (tmp1 - tmp2) * invs;
            double m02 = 2.0 * (tmp1 + tmp2) * invs;
            tmp1 = q.Y * q.Z;
            tmp2 = q.X * q.W;
            double m21 = 2.0 * (tmp1 + tmp2) * invs;
            double m12 = 2.0 * (tmp1 - tmp2) * invs;

            return new Matrix3D(
                m00, m01, m02, 0,
                m10, m11, m12, 0,
                m20, m21, m22, 0,
                0, 0, 0, 1
                );
        }

        public void SetupInterpolator(double StartAngleR, double StartAngleP, double StartAngleY, double EndAngleR,
            double EndAngleP, double EndAngleY, double StartPositionX,
         double StartPositionY,
         double StartPositionZ,
         double EndPositionX,
         double EndPositionY,
         double EndPositionZ,
            Quaternion startQuaternion,
            Quaternion endQuaternion
            )
        {
            this.StartAngleP = StartAngleP;
            this.StartAngleR = StartAngleR;
            this.StartAngleY = StartAngleY;

            this.EndAngleR = EndAngleR;
            this.EndAngleP = EndAngleP;
            this.EndAngleY = EndAngleY;

            this.StartPositionX = StartPositionX;
            this.StartPositionY = StartPositionY;
            this.StartPositionZ = StartPositionZ;

            this.EndPositionX = EndPositionX;
            this.EndPositionY = EndPositionY;
            this.EndPositionZ = EndPositionZ;

            this.startQuaternion = startQuaternion;
            this.endQuaternion = endQuaternion;
        }

        public void ODESolverFunction(double[] y, double x, double[] dy, object obj)
        {
            var odeSolverData = obj as ODESolverData;
            var N = CalculateCubeTorque(new Quaternion(y[3], y[4], y[5], y[6]));
            var I = odeSolverData.tensor;
            var IInv = odeSolverData.invertTensor;
            var W = new Vector3D(y[0], y[1], y[2]);
            var IW = MatrixVectorMultiply(I, W);
            var IWW = Vector3D.CrossProduct(IW, W);
            var NIWW = N + IWW;
            var WT = MatrixVectorMultiply(IInv, NIWW);

            var Q = new Quaternion(y[3], y[4], y[5], y[6]);
            var WQ = new Quaternion(y[0], y[1], y[2], 0);
            var QT = Quaternion.Multiply(Q, WQ);
            QT = new Quaternion(QT.X / 2, QT.Y / 2, QT.Z / 2, QT.W / 2);

            dy[0] = WT.X;
            dy[1] = WT.Y;
            dy[2] = WT.Z;
            dy[3] = QT.X;
            dy[4] = QT.Y;
            dy[5] = QT.Z;
            dy[6] = QT.W;
        }

        public static Quaternion getQuaternion(Vector3D v0, Vector3D v1)
        {
            Quaternion q = new Quaternion();
            // Copy, since cannot modify local
            v0.Normalize();
            v1.Normalize();

            double d = Vector3D.DotProduct(v0, v1);
            // If dot == 1, vectors are the same
            if (d >= 1.0f)
            {
                return Quaternion.Identity;
            }

            double s = Math.Sqrt((1 + d) * 2);
            double invs = 1 / s;

            Vector3D c = Vector3D.CrossProduct(v0, v1);

            q.X = c.X * invs;
            q.Y = c.Y * invs;
            q.Z = c.Z * invs;
            q.W = s * 0.5f;
            q.Normalize();

            return q;
        }

 public SpatialEntity(string name, string model, Vector3D position, Quaternion rotation)
 {
     Name = name;
     Model = model;
     Position = position;
     Rotation = rotation;
 }

 /// <summary>
 /// Initializes a new instance of the <see cref="PdbViewState"/> class.
 /// </summary>
 internal PdbViewState()
 {
     this.translation = new Vector3D();
     this.scale = 1;
     this.rotation = Quaternion.Identity;
     this.clip = 1;
     this.slab = 0;
 }

 public void Convert(double R, double P, double Y, ref Quaternion startQuaternion)
 {
     //convert from euler angles to radians
     Singleton<MathHelper>.Instance.EulerToRadian(ref R);
     Singleton<MathHelper>.Instance.EulerToRadian(ref P);
     Singleton<MathHelper>.Instance.EulerToRadian(ref Y);
     startQuaternion = ChangeAngles(P, Y, R);
 }

        public void CalibrateManually(Quaternion quaternion)
        {
            //CalibrationSystem.Transform = new RotateTransform3D(new QuaternionRotation3D(quaternion));

            // Fire event that calibration happened
            if (Calibrated != null)
                Calibrated(this, new CalibratedEventArgs() { CalibrationQuaternion = quaternion });
        }

 private Vector3D CalculateCubeTorque(Quaternion cubeQuaternion)
 {
     if (!GravityEnabled)
         return new Vector3D(0, 0, 0);
     var G = QuaternionVectorMultiply(cubeQuaternion, gravityVector);
     var r = rotationVector;
     return Vector3D.CrossProduct(r, G);
 }

 public void CalculateCurrentQuaternion(ref Quaternion currentQuaternion, double timeFactor)
 {
     var x = startQuaternion.X * (1 - timeFactor) + endQuaternion.X * timeFactor;
     var y = startQuaternion.Y * (1 - timeFactor) + endQuaternion.Y * timeFactor;
     var z = startQuaternion.Z * (1 - timeFactor) + endQuaternion.Z * timeFactor;
     var w = startQuaternion.W * (1 - timeFactor) + endQuaternion.W * timeFactor;
     currentQuaternion = new Quaternion(x, y, z, w);
     currentQuaternion.Normalize();
 }

 internal static MilQuaternionF QuaternionToMilQuaternionF(Quaternion q)
 {
     MilQuaternionF quat;
     quat.X = (float) q.X;
     quat.Y = (float) q.Y;
     quat.Z = (float) q.Z;
     quat.W = (float) q.W;
     return quat;
 }

 public EntityModel(int id, string name, string modelId, Vector3D position, Quaternion rotation)
 {
     Contract.Requires<ArgumentNullException>(!string.IsNullOrEmpty(name));
     Contract.Requires<ArgumentNullException>(!string.IsNullOrEmpty(modelId));
     Id = id;
     Name = name;
     ModelId = modelId;
     Position = position;
     Rotation = rotation;
 }

		public static MatrixTransform3D Rotate3D(Transform3D transform, Vector3D look, Vector3D dir, Point3D center)
		{
			Matrix3D m = new Matrix3D();
			Vector3D realook = transform.Transform(look);
			Vector3D axis = Vector3D.CrossProduct(realook, dir);
			double angle = Math.Acos(Vector3D.DotProduct(realook, dir));
			Quaternion q = new Quaternion(axis, angle);
			m.RotateAt(q, center);
			MatrixTransform3D rlt = transform as MatrixTransform3D;
			return new MatrixTransform3D(Matrix3D.Multiply(rlt.Matrix, m));
		}