Пример #1
0
        private void PositionChanged(object sender, TrackerChangeEventArgs e)
        {
            try
            {
                //TODO: Support user defined sensor index or autodetect
                if (e.Sensor == 0)
                {
                    RawPosition = new Vector3D(
                        -e.Position.X,
                        -e.Position.Z,
                        e.Position.Y);

                    RawRotation = new System.Windows.Media.Media3D.Quaternion(
                        e.Orientation.Y,
                        e.Orientation.W,
                        -e.Orientation.X,
                        e.Orientation.Z);

                    UpdatePositionAndRotation();
                }
            }
            catch (Exception exc)
            {
                Logger.Instance.Error(exc.Message, exc);
            }
        }
Пример #2
0
        // keepCallbackAlive is to prevent crash from garbage collection not being able to track into the unmanged code of ThreeSpace_API.dll
        private void dataCallbackFunc(uint deviceId, IntPtr outputData, uint outputDataLength, IntPtr timestamp)
        {
            unsafe
            {
                float *ptr  = (float *)outputData;
                uint   time = ((uint *)timestamp)[0];
                Monitor.Enter(this);
                this.Dispatcher.Invoke((Action)(() =>
                {
                    /*if (_lastTimeStamp == 0)
                     * {
                     *  RawPosition = PositionScaleFactor * _positionVec;
                     *  _lastTimeStamp = time;
                     * }
                     * else
                     * {
                     *  double timeDiff = (time - _lastTimeStamp)*0.000001;
                     *  _lastTimeStamp = time;
                     *  _velocityVec += new Vector3D(ptr[4] * timeDiff, ptr[5] * timeDiff, ptr[6] * timeDiff);
                     *  _positionVec += new Vector3D(_velocityVec.X * timeDiff, _velocityVec.Y * timeDiff, _velocityVec.Z * timeDiff);
                     *  RawPosition = _positionVec;
                     * }*/
                    RawRotation = new System.Windows.Media.Media3D.Quaternion(
                        ptr[0],
                        ptr[1],
                        ptr[2],
                        ptr[3]);

                    UpdatePositionAndRotation();
                }));

                Monitor.Exit(this);
            }
        }
Пример #3
0
        protected void trackball_TrackBallMoved(Object sender, EventArgs e)
        {
            System.Windows.Media.Media3D.Quaternion delta = ((Trackball)sender).Delta;
            Vector3 deltaAxis = new Vector3();

            deltaAxis.X = (float)delta.Axis.X;
            deltaAxis.Y = (float)delta.Axis.Y;
            deltaAxis.Z = (float)delta.Axis.Z;
            deltaAxis.Normalize();
            Vector3 cameraLookDirection = viewport3DImage.CameraTarget - viewport3DImage.CameraPosition;
            Vector3 cameraUpDirection   = viewport3DImage.CameraUpVector;

            cameraLookDirection.Normalize();
            // Subtract any component of cameraUpDirection along cameraLookDirection
            cameraUpDirection = cameraUpDirection - Vector3.Multiply(cameraLookDirection, Vector3.Dot(cameraUpDirection, cameraLookDirection));
            cameraUpDirection.Normalize();
            Vector3 cameraX = Vector3.Cross(cameraLookDirection, cameraUpDirection);
            // Get axis of rotation in the camera coordinates
            Vector3 deltaAxisWorld = Vector3.Multiply(cameraX, deltaAxis.X) +
                                     Vector3.Multiply(cameraUpDirection, deltaAxis.Y) +
                                     Vector3.Multiply(cameraLookDirection, -deltaAxis.Z);

            SharpDX.Matrix cameraTransform = SharpDX.Matrix.RotationAxis(deltaAxisWorld, (float)(delta.Angle * Math.PI / 180.0));
            viewport3DImage.CameraTarget   = Vector3.Transform(viewport3DImage.CameraTarget, cameraTransform).ToVector3();
            viewport3DImage.CameraPosition = Vector3.Transform(viewport3DImage.CameraPosition, cameraTransform).ToVector3();
            viewport3DImage.CameraUpVector = Vector3.Transform(viewport3DImage.CameraUpVector, cameraTransform).ToVector3();
            double newPhi = FindPhi();

            if (newPhi != lastPhi)
            {
                lastPhi = newPhi;
                axes.UpdateOpenSides(newPhi);
            }
        }
        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;
        }
Пример #5
0
 //Returns the quaternion rotation parsed from a Euler angle rotation
 public static System.Windows.Media.Media3D.Quaternion GetQuaternionRotation(Vector3D eulerAngles)
 {
     System.Windows.Media.Media3D.Quaternion rotation = new System.Windows.Media.Media3D.Quaternion(new Vector3D(1, 0, 0), eulerAngles.X);
     rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 1, 0), eulerAngles.Y);
     rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 0, 1), eulerAngles.Z);
     return(rotation);
 }
Пример #6
0
        /// <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");
        }
Пример #7
0
        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();
            }
        }
        //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)
            };
        }
Пример #9
0
        protected void Track(Point currentPosition)
        {
            Vector3D currentPosition3D = ProjectToTrackball(
                EventSource.ActualWidth, EventSource.ActualHeight, currentPosition);

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

            if (angle == 0.0)
            {
                return;
            }
            delta = new Quaternion(axis, -angle);

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

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

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

            _previousPosition3D = currentPosition3D;

            RaiseTrackballMovedEvent(EventArgs.Empty);
        }
Пример #10
0
        public static Object.Quaternion CreateNormalized(double x, double y, double z, double w)
        {
            var quaternion = new Media.Quaternion(x, y, z, w);

            quaternion.Normalize();
            return(quaternion.Convert());
        }
Пример #11
0
 public MobileState(Mobile mob)
 {
     ObjectInstanceId = mob.ObjectInstanceId;
     State = mob.MobileState;
     Position = mob.Position;
     Rotation = mob.Rotation;
 }
Пример #12
0
 public SpatialEntity(string name, string model, Vector3D position, Quaternion rotation)
 {
     Name = name;
     Model = model;
     Position = position;
     Rotation = rotation;
 }
Пример #13
0
        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;
        }
Пример #14
0
        // 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;
		}
Пример #16
0
        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
                );
        }
Пример #17
0
 public MobileState(EntityModel mob)
 {
     ObjectInstanceId = mob.Id;
     State = mob.MobileState;
     Position = mob.Position;
     Rotation = mob.Rotation;
 }
Пример #18
0
        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;
        }
Пример #19
0
 public MyPosition(int possessingId, Vector3D position, Quaternion rotation, EnumMobileState mobileState)
 {
     PossessingId = possessingId;
     Position = position;
     Rotation = rotation;
     MobileState = mobileState;
 }
		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;
		}
Пример #21
0
 /// <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;
 }
Пример #22
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 });
        }
Пример #24
0
 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);
 }
Пример #25
0
 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();
 }
Пример #26
0
 private void RootRotationMem_ValueChanged(object sender, object value)
 {
     Application.Current.Dispatcher.Invoke(() =>
     {
         System.Windows.Media.Media3D.Quaternion rot = this.RootRotation.ToMedia3DQuaternion();
         RotateTransform3D trans = new RotateTransform3D(new QuaternionRotation3D(rot));
         this.Root.Transform     = trans;
     });
 }
Пример #27
0
 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;
 }
Пример #28
0
        //Returns the quaternion rotation of a given 3D element
        public static System.Windows.Media.Media3D.Quaternion GetQuaternionRotation(Transform3D matrix)
        {
            Vector3D temp = GetAngleRotation(matrix);

            System.Windows.Media.Media3D.Quaternion rotation = new System.Windows.Media.Media3D.Quaternion(new Vector3D(1, 0, 0), temp.X);
            rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 1, 0), temp.Y);
            rotation *= new System.Windows.Media.Media3D.Quaternion(new Vector3D(0, 0, 1), temp.Z);
            return(rotation);
        }
Пример #29
0
        public static Vector3D Rotate(Quaternion q, Vector3D v)
        {
            var conj = new System.Windows.Media.Media3D.Quaternion(q.X, q.Y, q.Z, q.W);

            conj.Conjugate();

            var rotatedVector = conj * new System.Windows.Media.Media3D.Quaternion(v.X, v.Y, v.Z, 0) * q;

            return(new Vector3D(rotatedVector.X, rotatedVector.Y, rotatedVector.Z));
        }
Пример #30
0
 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;
 }
Пример #31
0
        public static Vector3D GetCornerPointToAxis(CameraPosition cameraPosition, Quaternion axis, Direction direction)
        {
            // Orientation in Viewing direction
            var a1 = cameraPosition.Width / 2;
            var a2 = cameraPosition.Height / 2;
            var a3 = cameraPosition.FocalLength;

            var angleY = Math.Atan(a1 / a3);
            var angleX = Math.Atan(a2 / a3);

            // Set sign for corner
            switch (direction)
            {
            default:
            case Direction.TopLeft:
                break;

            case Direction.TopRight:
                angleX = -angleX;
                break;

            case Direction.BottomLeft:
                angleY = -angleY;
                break;

            case Direction.BottomRight:
                angleX = -angleX;
                angleY = -angleY;
                break;
            }

            // Rotation matrices
            var xm = new Matrix3D()
            {
                M11 = 1,
                M22 = Math.Cos(angleX),
                M32 = Math.Sin(angleX),
                M23 = -Math.Sin(angleX),
                M33 = Math.Cos(angleX)
            };

            var ym = new Matrix3D()
            {
                M11 = Math.Cos(angleY),
                M31 = -Math.Sin(angleY),
                M22 = 1,
                M13 = Math.Sin(angleY),
                M33 = Math.Cos(angleY)
            };

            // Rotate the point by multiplying with quaternion and conjugate of quaternion
            var rot = TransformToQuaternion(xm) * TransformToQuaternion(ym) * axis;

            return(Rotate(rot, new Vector3D(0, 0, 1)));
        }
Пример #32
0
 /// <summary>
 /// product of two quaternions
 /// </summary>
 /// <param name="quaternion1"></param>
 /// <param name="quanernion2"></param>
 /// <returns></returns>
 public static Quaternion ProductOfTwoQuaternion(Quaternion quaternion1, Quaternion quanernion2)
 {
     Quaternion product = new Quaternion();
     //Derived from knowing:
     // q1q2 = w1w2 + w1q2 + w2q1 + q1 x q2 - q1.q2
     product.X = quaternion1.W * quanernion2.X + quaternion1.X * quanernion2.W + quaternion1.Y * quanernion2.Z - quaternion1.Z * quanernion2.Y;
     product.Y = quaternion1.W * quanernion2.Y + quaternion1.Y * quanernion2.W + quaternion1.Z * quanernion2.X - quaternion1.X * quanernion2.Z;
     product.Z = quaternion1.W * quanernion2.Z + quaternion1.Z * quanernion2.W + quaternion1.X * quanernion2.Y - quaternion1.Y * quanernion2.X;
     product.W = quaternion1.W * quanernion2.W - quaternion1.X * quanernion2.X - quaternion1.Y * quanernion2.Y - quaternion1.Z * quanernion2.Z;
     return product;
 }
Пример #33
0
        /// <summary>Convertit un <see cref="Object.Quaternion"/> en <see cref="Media.Quaternion"/>.</summary>
        public static Media.Quaternion Convert(this Object.Quaternion quaternion)
        {
            var value = new Media.Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);

            if (!value.IsNormalized)
            {
                value.Normalize();
            }

            return(value);
        }
Пример #34
0
		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));
		}
Пример #35
0
        internal static CollisionHull CreateSensorCollisionHull(WorldBase world, Vector3D scale, Quaternion orientation, Point3D position)
        {
            Transform3DGroup transform = new Transform3DGroup();
            //transform.Children.Add(new ScaleTransform3D(scale));		// it ignores scale
            transform.Children.Add(new RotateTransform3D(new QuaternionRotation3D(orientation)));
            transform.Children.Add(new TranslateTransform3D(position.ToVector()));

            // Scale X and Y should be identical, but average them to be safe
            double radius = ((SIZEPERCENTOFSCALE_XY * scale.X) + (SIZEPERCENTOFSCALE_XY * scale.Y)) / 2d;

            return CollisionHull.CreateCylinder(world, 0, radius, SIZEPERCENTOFSCALE_Z * scale.Z, transform.Value);
        }
Пример #36
0
        public void ReadTransform()
        {
            if (!this.IsEnabled)
            {
                return;
            }

            this.Position = this.ViewModel.Position;
            this.Rotation = this.ViewModel.Rotation;
            this.Scale    = this.ViewModel.Scale;

            // Convert the character-relative transform into a parent-relative transform
            Point3D    position = this.Position.ToMedia3DPoint();
            Quaternion rotation = this.Rotation.ToMedia3DQuaternion();

            if (this.Parent != null)
            {
                TransformViewModel parentTransform = this.Parent.ViewModel;
                Point3D            parentPosition  = parentTransform.Position.ToMedia3DPoint();
                Quaternion         parentRot       = parentTransform.Rotation.ToMedia3DQuaternion();
                parentRot.Invert();

                // relative position
                position = (Point3D)(position - parentPosition);

                // relative rotation
                rotation = parentRot * rotation;

                // unrotate bones, since we will transform them ourselves.
                RotateTransform3D rotTrans = new RotateTransform3D(new QuaternionRotation3D(parentRot));
                position = rotTrans.Transform(position);
            }

            // Store the new parent-relative transform info
            this.Position = position.ToCmVector();
            this.Rotation = rotation.ToCmQuaternion();

            // Set the Media3D hierarchy transforms
            this.rotation.Rotation = new QuaternionRotation3D(rotation);
            this.position.OffsetX  = position.X;
            this.position.OffsetY  = position.Y;
            this.position.OffsetZ  = position.Z;

            // Draw a line for visualization
            if (this.Parent != null && this.lineToParent != null)
            {
                Point3D p = this.lineToParent.Points[1];
                p.X = position.X;
                p.Y = position.Y;
                p.Z = position.Z;
                this.lineToParent.Points[1] = p;
            }
        }
Пример #37
0
        void MoveUpdateCallback(int id, MoveWrapper.Vector3 pos, MoveWrapper.Quaternion rot, int trigger)
        {
            RawPosition = new Vector3D(pos.x, pos.y, pos.z);
            RawRotation = new Quaternion(rot.x, -rot.y, rot.z, -rot.w);

            if (MoveWrapper.getButtonState(0, MoveButton.B_START))
            {
                Dispatcher.Invoke((Action)(Calibrate));
            }

            Dispatcher.Invoke((Action)(UpdatePositionAndRotation));
        }
Пример #38
0
        public void Transform(Matrix3D transform)
        {
            var tg = new MatrixTransform3D(transform);

            CameraCenter = tg.Transform(CameraCenter.ToPoint3D()).ToVector3D();

            var cameraOrientation = new Quaternion(Orientation.X, Orientation.Y, Orientation.Z, Orientation.W);

            var left  = cameraOrientation;
            var right = TransformToQuaternion(transform);

            Orientation = left * right;
        }
        //Copy constructor. Does not copy attachments or ID.
        public N8Block(N8Block source, int ID)
        {
            AttachedTo = null;
            Attachees = new List<N8Block>();

            this._id = ID;

            type = source.type;
            name = source.name;

            position = source.position;
            rotation = source.rotation;
        }
Пример #40
0
        protected void UpdateRotation(object sender, EventArgs e)
        {
            // Event may fire multiple times per render. Don't do unnecessary updates.
            TimeSpan nextRenderTime = ((RenderingEventArgs)e).RenderingTime;
            if (nextRenderTime != lastRenderTime)
            {
                lastRenderTime = nextRenderTime;

                OpenTK.Quaternion q = rift.PredictedOrientation;
                RawRotation = new System.Windows.Media.Media3D.Quaternion(q.X, -q.Y, q.Z, -q.W);
                UpdatePositionAndRotation();
            }
        }
Пример #41
0
        public static Quaternion RotationMatrix(Matrix3D matrix)
        {
            double sqrt;
            double half;
            var    scale = matrix.M11 + matrix.M22 + matrix.M33;

            var result = new Quaternion();

            if (scale > 0.0)
            {
                sqrt     = Math.Sqrt(scale + 1.0);
                result.W = sqrt * 0.5;
                sqrt     = 0.5 / sqrt;

                result.X = (matrix.M23 - matrix.M32) * sqrt;
                result.Y = (matrix.M31 - matrix.M13) * sqrt;
                result.Z = (matrix.M12 - matrix.M21) * sqrt;
            }
            else if ((matrix.M11 >= matrix.M22) && (matrix.M11 >= matrix.M33))
            {
                sqrt = Math.Sqrt(1.0 + matrix.M11 - matrix.M22 - matrix.M33);
                half = 0.5 / sqrt;

                result.X = 0.5 * sqrt;
                result.Y = (matrix.M12 + matrix.M21) * half;
                result.Z = (matrix.M13 + matrix.M31) * half;
                result.W = (matrix.M23 - matrix.M32) * half;
            }
            else if (matrix.M22 > matrix.M33)
            {
                sqrt = Math.Sqrt(1.0 + matrix.M22 - matrix.M11 - matrix.M33);
                half = 0.5 / sqrt;

                result.X = (matrix.M21 + matrix.M12) * half;
                result.Y = 0.5 * sqrt;
                result.Z = (matrix.M32 + matrix.M23) * half;
                result.W = (matrix.M31 - matrix.M13) * half;
            }
            else
            {
                sqrt = Math.Sqrt(1.0 + matrix.M33 - matrix.M11 - matrix.M22);
                half = 0.5 / sqrt;

                result.X = (matrix.M31 + matrix.M13) * half;
                result.Y = (matrix.M32 + matrix.M23) * half;
                result.Z = 0.5 * sqrt;
                result.W = (matrix.M12 - matrix.M21) * half;
            }

            return(result);
        }
Пример #42
0
        public CombatantModel(int id, string name, string modelId, Vector3D position, Quaternion rotation,
            float health, float energy, EnumMobileState mobileState, float height,
            int constitution, int dexterity, int willpower, int cognition, int strength)
            : base(id, name, modelId, position, rotation, health, energy, mobileState, height)
        {
            Constitution = constitution;
            Dexterity = dexterity;
            Willpower = willpower;
            Cognition = cognition;
            Strength = strength;

            ActivatingSkill = EnumSkill.None;
            SkillQueue = ListModule.Empty<Tuple<EnumSkill, EntityModel>>();
        }
Пример #43
0
        protected void UpdateRotation(object sender, EventArgs e)
        {
            // Event may fire multiple times per render. Don't do unnecessary updates.
            TimeSpan nextRenderTime = ((RenderingEventArgs)e).RenderingTime;

            if (nextRenderTime != lastRenderTime)
            {
                lastRenderTime = nextRenderTime;

                OpenTK.Quaternion q = rift.PredictedOrientation;
                RawRotation = new System.Windows.Media.Media3D.Quaternion(q.X, -q.Y, q.Z, -q.W);
                UpdatePositionAndRotation();
            }
        }
Пример #44
0
        public MotionViewModel(FlightControlConnection connection)
        {
            // Register handler for the orientation update
            connection.RegisterHandler("ROT", rot =>
                {
                    UpdateFrequency = updateFrequencyCounter.Tick();
                    Rotation = new Quaternion(
                        double.Parse(rot[0], CultureInfo.InvariantCulture),
                        double.Parse(rot[1], CultureInfo.InvariantCulture),
                        double.Parse(rot[2], CultureInfo.InvariantCulture),
                        double.Parse(rot[3], CultureInfo.InvariantCulture));
                    ;
                });

            // Register handler for the heading update
            connection.RegisterHandler("HEAD", head =>
                {
                    Heading = new Quaternion(
                        double.Parse(head[0], CultureInfo.InvariantCulture),
                        double.Parse(head[1], CultureInfo.InvariantCulture),
                        double.Parse(head[2], CultureInfo.InvariantCulture),
                        double.Parse(head[3], CultureInfo.InvariantCulture));
                });

            connection.RegisterHandler("OFFSET", offset =>
                {
                    AngularOffset.Yaw = double.Parse(offset[0], CultureInfo.InvariantCulture);
                    AngularOffset.Pitch = double.Parse(offset[1], CultureInfo.InvariantCulture);
                    AngularOffset.Roll = double.Parse(offset[2], CultureInfo.InvariantCulture);
                });

            _engines = new List<EngineViewModel>
                {
                    new EngineViewModel(5, 10, 10),
                    new EngineViewModel(6, 10, -10),
                    new EngineViewModel(9, -10, -10),
                    new EngineViewModel(10, -10, 10)
                };

            var enginesByPin = _engines.ToDictionary(e => e.Pin);
            connection.RegisterHandler("ENG", eng =>
                {
                    var pin = int.Parse(eng[0]);
                    enginesByPin[pin].Power = int.Parse(eng[1]);
                });

            ResetCamera = new RelayCommand(() => CameraRotation = Rotation); // * new Quaternion(0, 0, 1, 0));
            AngularOffset = new EulerAngles();
        }
Пример #45
0
        /// <summary>
        /// 创建纸张快照, folding 用
        /// </summary>
        /// <param name="vp"></param>
        /// <param name="topFaces"></param>
        /// <param name="bgFaces"></param>
        /// <param name="topImgFront"></param>
        /// <param name="bgImg"></param>
        public void CreateShadow(Viewport3D vp, List<Face>topFaces, List<Face>bgFaces, Image topImgFront, Image bgImg, Image topImgBack)
        {
            RenderController render = RenderController.GetInstance();

            if (topFaces != null && topFaces.Count != 0)
            {
                //// 上层纸张
                Model3DGroup topModelGroup = new Model3DGroup();
                foreach (Face face in topFaces)
                {
                    topModelGroup.Children.Add(render.FaceMeshMap[face]);
                }
                render.Entity.Content = topModelGroup;
                // 正面拍一张照
                RenderTargetBitmap bmpf = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
                bmpf.Render(vp);
                topImgFront.Source = bmpf;

                // 背面再拍一张照
                RenderTargetBitmap bmpb = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
                Quaternion quat = new Quaternion(0, 1, 0, 0);
                quat = quat * render.SrcQuaternion;
                render.TransformGroup.Children[0] = new RotateTransform3D(new QuaternionRotation3D(quat));
                bmpb.Render(vp);
                render.TransformGroup.Children[0] = new RotateTransform3D(new QuaternionRotation3D(render.SrcQuaternion));
                topImgBack.Source = bmpb;
                // 将topImgBack移到看不见
                topImgBack.RenderTransform = tg;
                (tg.Children[1] as TranslateTransform).X = -10000;
                (tg.Children[1] as TranslateTransform).Y = -10000;
            }

            if (bgFaces != null && bgFaces.Count != 0)
            // 背景纸张
            {
                Model3DGroup bgModelGroup = new Model3DGroup();
                foreach (Face face in bgFaces)
                {
                    bgModelGroup.Children.Add(render.FaceMeshMap[face]);
                }
                render.Entity.Content = bgModelGroup;
                // 正面拍一张照
                RenderTargetBitmap bmpbg = new RenderTargetBitmap((int)vp.ActualWidth, (int)vp.ActualHeight, 96, 96, PixelFormats.Pbgra32);
                bmpbg.Render(vp);
                bgImg.Source = bmpbg;
            }

            render.Entity.Content = null;
        }
Пример #46
0
        public void Rotate(System.Windows.Media.Media3D.Vector3D axis, double angle)
        {
            var m         = viewport3D.Children[viewport3D.Children.Count - 1].Transform as Transform3DGroup;
            var _rotation = (m.Children[2] as RotateTransform3D).Rotation as AxisAngleRotation3D;

            var q     = new System.Windows.Media.Media3D.Quaternion(_rotation.Axis, _rotation.Angle);
            var delta = new System.Windows.Media.Media3D.Quaternion(axis, angle);

            q *= delta;

            _rotation.Axis  = q.Axis;
            _rotation.Angle = q.Angle;

            m.Children[2] = new RotateTransform3D(_rotation);
        }
Пример #47
0
        public static Transform3D LibTransformToWPFTransform(Vector3 position, AegirLib.MathType.Quaternion rotation, bool freeze = false)
        {
            MatrixTransform3D matrixTransform = new MatrixTransform3D();
            Matrix3D          matrix          = new Matrix3D();

            System.Windows.Media.Media3D.Quaternion q = new System.Windows.Media.Media3D.Quaternion(rotation.X, rotation.Y, rotation.Z, rotation.W);
            matrix.Rotate(q);
            matrix.Translate(new Vector3D(position.X, position.Y, position.Z));

            matrixTransform.Matrix = matrix;
            if (freeze)
            {
                matrixTransform.Freeze();
            }
            return(matrixTransform);
        }
Пример #48
0
        public static Quaternion RotationAxis(Vector3D axis, double angle)
        {
            axis.Normalize();

            var half = angle * 0.5;
            var sin  = Math.Sin(half);
            var cos  = Math.Cos(half);

            Quaternion result = new Quaternion
            {
                X = axis.X * sin,
                Y = axis.Y * sin,
                Z = axis.Z * sin,
                W = cos
            };

            return(result);
        }
Пример #49
0
        public void UpdateViewMatrix()
        {
            _viewMatrix.SetIdentity();
            // fix f*****g crazy z-up
            Quaternion zupfix_x = new Quaternion(new Vector3D(1, 0, 0), -90);
            Quaternion zupfix_y = new Quaternion(new Vector3D(0, 1, 0), 180);

            _viewMatrix.Rotate(zupfix_x);
            _viewMatrix.Rotate(zupfix_y);

            Quaternion quat1 = new Quaternion(new Vector3D(1, 0, 0), CameraPitch);
            Quaternion quat2 = new Quaternion(new Vector3D(0, 1, 0), CameraYaw);

            _viewMatrix.Rotate(quat2);
            _viewMatrix.Rotate(quat1);
            _viewMatrix.Translate(new Vector3D(0, 0, -CameraDistance));
            UpdateCamera();
        }
Пример #50
0
        public void WriteTransform(ModelVisual3D root, bool writeChildren = true)
        {
            if (!this.IsEnabled)
            {
                return;
            }

            // Apply the current values to the visual tree
            this.rotation.Rotation = new QuaternionRotation3D(this.Rotation.ToMedia3DQuaternion());
            this.position.OffsetX  = this.Position.X;
            this.position.OffsetY  = this.Position.Y;
            this.position.OffsetZ  = this.Position.Z;

            // convert the values in the tree to character-relative space
            MatrixTransform3D transform = (MatrixTransform3D)this.TransformToAncestor(root);

            Quaternion rotation = transform.Matrix.ToQuaternion();

            rotation.Invert();

            CmVector position = default;

            position.X = (float)transform.Matrix.OffsetX;
            position.Y = (float)transform.Matrix.OffsetY;
            position.Z = (float)transform.Matrix.OffsetZ;

            // and push those values to the game memory
            this.ViewModel.Position = position;
            this.ViewModel.Scale    = this.Scale;
            this.ViewModel.Rotation = rotation.ToCmQuaternion();

            if (writeChildren)
            {
                foreach (Visual3D child in this.Children)
                {
                    if (child is BoneVisual3d childBone)
                    {
                        childBone.WriteTransform(root);
                    }
                }
            }
        }
Пример #51
0
        //Returns the Vector3D transformed by the Quaternion
        public static Vector3D Transform(System.Windows.Media.Media3D.Quaternion qt, Vector3D vector)
        {
            double x2  = qt.X + qt.X;
            double y2  = qt.Y + qt.Y;
            double z2  = qt.Z + qt.Z;
            double wx2 = qt.W * x2;
            double wy2 = qt.W * y2;
            double wz2 = qt.W * z2;
            double xx2 = qt.X * x2;
            double xy2 = qt.X * y2;
            double xz2 = qt.X * z2;
            double yy2 = qt.Y * y2;
            double yz2 = qt.Y * z2;
            double zz2 = qt.Z * z2;
            double x   = vector.X * (1.0 - yy2 - zz2) + vector.Y * (xy2 - wz2) + vector.Z * (xz2 + wy2);
            double y   = vector.X * (xy2 + wz2) + vector.Y * (1.0 - xx2 - zz2) + vector.Z * (yz2 - wx2);
            double z   = vector.X * (xz2 - wy2) + vector.Y * (yz2 + wx2) + vector.Z * (1.0 - xx2 - yy2);

            return(new Vector3D(x, y, z));
        }
Пример #52
0
        private void WatchCamera()
        {
            IInjectionService injection = Services.Get <IInjectionService>();

            IMemory <float> camX    = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraAngleX);
            IMemory <float> camY    = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraAngleY);
            IMemory <float> camZ    = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraRotation);
            IMemory <float> camDist = injection.GetMemory(Offsets.Main.CameraOffset, Offsets.Main.CameraCurrentZoom);

            Vector3D camEuler = default;

            bool vis = true;

            while (vis && Application.Current != null)
            {
                camEuler.Y = (float)MathUtils.RadiansToDegrees((double)camX.Value) - 180;
                camEuler.Z = (float)-MathUtils.RadiansToDegrees((double)camY.Value);
                camEuler.X = (float)MathUtils.RadiansToDegrees((double)camZ.Value);
                Quaternion q = camEuler.ToQuaternion();

                try
                {
                    Application.Current.Dispatcher.Invoke(() =>
                    {
                        vis = this.IsVisible;                         ////&& this.IsEnabled;
                        Transform3DGroup g = new Transform3DGroup();
                        g.Children.Add(new TranslateTransform3D(0, 0.75, -(camDist.Value - 1)));
                        g.Children.Add(new RotateTransform3D(new QuaternionRotation3D(q)));
                        this.Viewport.Camera.Transform = g;
                    });
                }
                catch (Exception)
                {
                }

                Thread.Sleep(16);
            }
        }
Пример #53
0
        private void OnMavlinkMessageReceived(object sender, MavLinkMessage e)
        {
            if (currentFlightLog != null && !pauseRecording)
            {
                currentFlightLog.AddMessage(e);
            }

            switch (e.MsgId)
            {
            case MAVLink.MAVLINK_MSG_ID.ATTITUDE_QUATERNION:
            {
                var payload = (MAVLink.mavlink_attitude_quaternion_t)e.TypedPayload;
                var q       = new System.Windows.Media.Media3D.Quaternion(payload.q1, payload.q2, payload.q3, payload.q4);
                UiDispatcher.RunOnUIThread(() =>
                    {
                        //ModelViewer.ModelAttitude = initialAttitude * q;
                    });
                break;
            }

            case MAVLink.MAVLINK_MSG_ID.ATTITUDE:
            {
                var        payload = (MAVLink.mavlink_attitude_t)e.TypedPayload;
                Quaternion y       = new Quaternion(new Vector3D(0, 0, 1), -payload.yaw * 180 / Math.PI);
                Quaternion x       = new Quaternion(new Vector3D(1, 0, 0), payload.pitch * 180 / Math.PI);
                Quaternion z       = new Quaternion(new Vector3D(0, 1, 0), payload.roll * 180 / Math.PI);
                UiDispatcher.RunOnUIThread(() =>
                    {
                        ModelViewer.ModelAttitude = initialAttitude * (y * x * z);
                    });
                break;
            }

            case MAVLink.MAVLINK_MSG_ID.HIL_STATE_QUATERNION:
            {
                var        payload = (MAVLink.mavlink_hil_state_quaternion_t)e.TypedPayload;
                Quaternion q       = new Quaternion(payload.attitude_quaternion[0],
                                                    payload.attitude_quaternion[1],
                                                    payload.attitude_quaternion[2],
                                                    payload.attitude_quaternion[3]);
                UiDispatcher.RunOnUIThread(() =>
                    {
                        ModelViewer.ModelAttitude = initialAttitude * q;
                    });
                break;
            }

            case MAVLink.MAVLINK_MSG_ID.GLOBAL_POSITION_INT:
            {
                var payload = (MAVLink.mavlink_global_position_int_t)e.TypedPayload;
                UiDispatcher.RunOnUIThread(() =>
                    {
                        MapLocation((double)payload.lat / 1e7, (double)payload.lon / 1e7);
                    });
                break;
            }

            case MAVLink.MAVLINK_MSG_ID.SERIAL_CONTROL:
            {
                var ctrl = (MAVLink.mavlink_serial_control_t)e.TypedPayload;
                if (ctrl.count > 0)
                {
                    string text = System.Text.Encoding.ASCII.GetString(ctrl.data, 0, ctrl.count);
                    UiDispatcher.RunOnUIThread(() =>
                        {
                            SystemConsole.Write(text);
                        });
                }
                break;
            }

            case MAVLink.MAVLINK_MSG_ID.DATA_TRANSMISSION_HANDSHAKE:
                if (showImageStream)
                {
                    var p = (MAVLink.mavlink_data_transmission_handshake_t)e.TypedPayload;
                    incoming_image.size           = p.size;
                    incoming_image.packets        = p.packets;
                    incoming_image.payload        = p.payload;
                    incoming_image.quality        = p.jpg_quality;
                    incoming_image.type           = p.type;
                    incoming_image.width          = p.width;
                    incoming_image.height         = p.height;
                    incoming_image.start          = Environment.TickCount;
                    incoming_image.packetsArrived = 0;
                    incoming_image.data           = new byte[incoming_image.size];
                }
                break;

            case MAVLink.MAVLINK_MSG_ID.ENCAPSULATED_DATA:
                if (showImageStream)
                {
                    var img = (MAVLink.mavlink_encapsulated_data_t)e.TypedPayload;

                    int  seq = img.seqnr;
                    uint pos = (uint)seq * (uint)incoming_image.payload;

                    // Check if we have a valid transaction
                    if (incoming_image.packets == 0 || incoming_image.size == 0)
                    {
                        // not expecting an image?
                        incoming_image.packetsArrived = 0;
                        break;
                    }

                    uint available = (uint)incoming_image.payload;
                    if (pos + available > incoming_image.size)
                    {
                        available = incoming_image.size - pos;
                    }
                    Array.Copy(img.data, 0, incoming_image.data, pos, available);

                    progress.ShowProgress(0, incoming_image.size, pos + available);

                    ++incoming_image.packetsArrived;
                    //Debug.WriteLine("packet {0} of {1}, position {2} of {3}", incoming_image.packetsArrived, incoming_image.packets,
                    //    pos + available, incoming_image.size);

                    // emit signal if all packets arrived
                    if (pos + available >= incoming_image.size)
                    {
                        // Restart state machine
                        incoming_image.packets        = 0;
                        incoming_image.packetsArrived = 0;
                        byte[] saved = incoming_image.data;
                        incoming_image.data = null;

                        UiDispatcher.RunOnUIThread(() =>
                        {
                            progress.ShowProgress(0, 0, 0);
                            ShowImage(saved);
                        });
                    }
                }
                break;
            }
        }
Пример #54
0
        /// <summary>
        /// Implementation of the Magdewick algorithm, using IMU data to update orientation Quaternion.
        /// </summary>
        /// <param name="gx">Gyro X value</param>
        /// <param name="gy">Gyro Y value</param>
        /// <param name="gz">Gyro Z value</param>
        /// <param name="ax">Accel X value</param>
        /// <param name="ay">Accel Y value</param>
        /// <param name="az">Accel Z value</param>
        /// <param name="mx">Mag X value</param>
        /// <param name="my">Mag Y value</param>
        /// <param name="mz">Mag Z value</param>
        public void update(float gx, float gy, float gz, float ax, float ay, float az, float mx, float my, float mz)
        {
            float q1 = (float)processed.W, q2 = (float)processed.X, q3 = (float)processed.Y, q4 = (float)processed.Z;    // short name local variable for readability
            float norm;
            float hx, hy, _2bx, _2bz, _8bx, _8bz;
            float s1, s2, s3, s4;
            float qDot1, qDot2, qDot3, qDot4;

            // Auxiliary variables to avoid repeated arithmetic
            float _2q1mx;
            float _2q1my;
            float _2q1mz;
            float _2q2mx;
            float _4bx;
            float _4bz;
            float _2q1   = 2f * q1;
            float _2q2   = 2f * q2;
            float _2q3   = 2f * q3;
            float _2q4   = 2f * q4;
            float _2q1q3 = 2f * q1 * q3;
            float _2q3q4 = 2f * q3 * q4;
            float q1q1   = q1 * q1;
            float q1q2   = q1 * q2;
            float q1q3   = q1 * q3;
            float q1q4   = q1 * q4;
            float q2q2   = q2 * q2;
            float q2q3   = q2 * q3;
            float q2q4   = q2 * q4;
            float q3q3   = q3 * q3;
            float q3q4   = q3 * q4;
            float q4q4   = q4 * q4;

            // Normalise accelerometer measurement
            norm = (float)Math.Sqrt(ax * ax + ay * ay + az * az);
            if (norm == 0f)
            {
                return;             // handle NaN
            }
            norm = 1 / norm;        // use reciprocal for division
            ax  *= norm;
            ay  *= norm;
            az  *= norm;

            // Normalise magnetometer measurement
            norm = (float)Math.Sqrt(mx * mx + my * my + mz * mz);
            if (norm == 0f)
            {
                return;             // handle NaN
            }
            norm = 1 / norm;        // use reciprocal for division
            mx  *= norm;
            my  *= norm;
            mz  *= norm;

            // Reference direction of Earth's magnetic field
            _2q1mx = 2f * q1 * mx;
            _2q1my = 2f * q1 * my;
            _2q1mz = 2f * q1 * mz;
            _2q2mx = 2f * q2 * mx;
            hx     = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
            hy     = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
            _2bx   = (float)Math.Sqrt(hx * hx + hy * hy);
            _2bz   = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
            _4bx   = 2f * _2bx;
            _4bz   = 2f * _2bz;
            _8bx   = 2f * _4bx;
            _8bz   = 2f * _4bz;
            // Gradient decent algorithm corrective step
            s1   = -_2q3 * (2f * q2q4 - _2q1q3 - ax) + _2q2 * (2f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s2   = _2q4 * (2f * q2q4 - _2q1q3 - ax) + _2q1 * (2f * q1q2 + _2q3q4 - ay) - 4f * q2 * (1 - 2f * q2q2 - 2f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s3   = -_2q1 * (2f * q2q4 - _2q1q3 - ax) + _2q4 * (2f * q1q2 + _2q3q4 - ay) - 4f * q3 * (1 - 2f * q2q2 - 2f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            s4   = _2q2 * (2f * q2q4 - _2q1q3 - ax) + _2q3 * (2f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
            norm = 1f / (float)Math.Sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4);    // normalise step magnitude
            s1  *= norm;
            s2  *= norm;
            s3  *= norm;
            s4  *= norm;

            // Compute rate of change of quaternion
            qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - 0.5f * s1;
            qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - 0.5f * s2;
            qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - 0.5f * s3;
            qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - 0.5f * s4;

            // Integrate to yield quaternion
            q1       += qDot1 * step_size;
            q2       += qDot2 * step_size;
            q3       += qDot3 * step_size;
            q4       += qDot4 * step_size;
            norm      = 1f / (float)Math.Sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
            processed = new Quaternion(q2 * norm, q3 * norm, q4 * norm, q1 * norm);
        }
Пример #55
0
 /// <summary>Convertit un <see cref="Media.Quaternion"/> en <see cref="Object.Quaternion"/>.</summary>
 public static Object.Quaternion Convert(this Media.Quaternion quaternion) => new Object.Quaternion(quaternion.X, quaternion.Y, quaternion.Z, quaternion.W);
Пример #56
0
 //Returns the Vector3D result of the rotation
 public static Vector3D Rotate(Vector3D vector, Vector3D axis, double angle)
 {
     System.Windows.Media.Media3D.Quaternion qt = new System.Windows.Media.Media3D.Quaternion(axis, angle);
     return(Transform(qt, vector));
 }