Beispiel #1
0
        /// <summary>
        /// This function returns a vector that is rotated by the opposite of me
        /// </summary>
        public MyVector GetRotatedVectorReverse(MyVector vector, bool isQuatNormalized)
        {
            if (!isQuatNormalized)
            {
                // I'm not normalized, clone myself and normalize it
                MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
                myUnitClone.BecomeUnitQuaternion();

                return(myUnitClone.GetRotatedVectorReverse(vector, true));
            }

            MyVector qvec = new MyVector(this.X, this.Y, this.Z);

            //Vector uv = qvec.Cross(vector);
            MyVector uv = MyVector.Cross(qvec, vector);

            //Vector uuv = qvec.Cross(uv);
            MyVector uuv = MyVector.Cross(qvec, uv);

            //uv *= (2.0f * quat.w);
            uv.Multiply(this.W * -2d);

            //uuv *= 2.0f;
            uuv.Multiply(2d);

            //return vector + uv + uuv;
            MyVector retVal = vector.Clone();

            retVal.Add(uv);
            retVal.Add(uuv);
            return(retVal);
        }
Beispiel #2
0
 /// <summary>
 /// There are classes that expose quaternions as read only properties (so the pointers can be shared).  I just made
 /// this function so it's easier to shuttle values around in one shot
 /// </summary>
 public void StoreNewValues(MyQuaternion valuesToGrab)
 {
     this.X = valuesToGrab.X;
     this.Y = valuesToGrab.Y;
     this.Z = valuesToGrab.Z;
     this.W = valuesToGrab.W;
 }
Beispiel #3
0
 /// <summary>
 /// This overload should only be used during a clone.  I simply trust the values passed to me
 /// </summary>
 protected Sphere(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius)
 {
     _position      = position;
     _radius        = radius;
     _origDirFacing = origDirectionFacing;
     _rotation      = rotation;
 }
Beispiel #4
0
        public void GetAngleAroundAxis(out MyQuaternion rotation, MyVector destination)
        {
            MyVector axis;
            double   radians;

            GetAngleAroundAxis(out axis, out radians, destination);

            rotation = new MyQuaternion(axis, radians);
        }
Beispiel #5
0
        public static MyQuaternion Multiply(MyQuaternion q1, MyQuaternion q2)
        {
            MyQuaternion retVal = new MyQuaternion();

            retVal.W = (q1.W * q2.W) - (q1.X * q2.X) - (q1.Y * q2.Y) - (q1.Z * q2.Z);
            retVal.X = (q1.W * q2.X) + (q1.X * q2.W) + (q1.Y * q2.Z) - (q1.Z * q2.Y);
            retVal.Y = (q1.W * q2.Y) - (q1.X * q2.Z) + (q1.Y * q2.W) + (q1.Z * q2.X);
            retVal.Z = (q1.W * q2.Z) + (q1.X * q2.Y) - (q1.Y * q2.X) + (q1.Z * q2.W);

            return(retVal);
        }
Beispiel #6
0
        public DoubleVector GetRotatedVectorReverse(DoubleVector doubleVector, bool isQuatNormalized)
        {
            if (!isQuatNormalized)
            {
                // I'm not normalized, clone myself and normalize it
                MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
                myUnitClone.BecomeUnitQuaternion();

                return(myUnitClone.GetRotatedVectorReverse(doubleVector, true));
            }

            return(new DoubleVector(GetRotatedVectorReverse(doubleVector.Standard, true), GetRotatedVectorReverse(doubleVector.Orth, true)));
        }
Beispiel #7
0
        /// <summary>
        /// This function will rotate me around any arbitrary axis (no gimbal lock for me!!!!!!!)
        /// </summary>
        /// <remarks>
        /// From some web site talking about rotations (seems like good stuff, but I don't know how it's used):
        ///
        /// // I'm not sure why this multiplication is important
        /// Quaternion totalQuat = Quaternion.MultiplyQuaternions(rotationQuat, new Quaternion(1, 0, 0, 0));      // order of multiplication is important
        ///
        /// // Get a matrix telling me how to rotate
        /// Matrix rotationMatrix = totalQuat.ToMatrixFromUnitQuaternion();
        ///
        /// // AND THEN??????????
        /// </remarks>
        /// <param name="rotateAround">Any vector to rotate around</param>
        /// <param name="radians">How far to rotate</param>
        public void RotateAroundAxis(MyVector rotateAround, double radians)
        {
            // Create a quaternion that represents the axis and angle passed in
            MyQuaternion rotationQuat = new MyQuaternion(rotateAround, radians);

            // Get a vector that represents me rotated by the quaternion
            MyVector newValue = rotationQuat.GetRotatedVector(this, true);

            // Store my new values
            this.X = newValue.X;
            this.Y = newValue.Y;
            this.Z = newValue.Z;
        }
Beispiel #8
0
        /// <summary>
        /// This one is used to assist with the clone method (especially for my derived classes)
        /// </summary>
        /// <param name="usesBoundingBox">Just pass in what you have</param>
        /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
        /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
        protected Ball(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
            : base(position, origDirectionFacing, rotation, radius)
        {
            // I use the property sets to enforce the values
            this.Mass            = mass;
            this.Elasticity      = elasticity;
            this.KineticFriction = kineticFriction;
            this.StaticFriction  = staticFriction;

            _usesBoundingBox = usesBoundingBox;
            _boundingLower   = boundingBoxLower;
            _boundingUpper   = boundingBoxUpper;
        }
Beispiel #9
0
        public override void TimerTestPosition(double elapsedTime)
        {
            base.TimerTestPosition(elapsedTime);

            // Either remember the orig rotation, or restore to that orig rotation (as of PrepareForNew)
            if (_savedRotation == null)
            {
                _savedRotation = this.Rotation.Clone();
            }
            else
            {
                this.Rotation.StoreNewValues(_savedRotation);
            }

            // Remember the elapsed time that was passed in
            _elapsedTime = elapsedTime;

            // Figure out the new rotation (the body has been rotating at some velocity during the previous tick)
            if (!_angularVelocity.IsZero)
            {
                // Figure out what the angle will be
                double timedAngle = _angularVelocity.GetMagnitude();
                timedAngle *= elapsedTime;

                if (!_centerOfMass.IsZero)
                {
                    #region Rotate around center of mass

                    // Remember where the center of mass is in world coords
                    MyVector cmRotated = this.Rotation.GetRotatedVector(_centerOfMass, true);
                    MyVector cmWorld   = this.Position + cmRotated;

                    // Get the opposite of the cm
                    MyVector posRelativeToCM = cmRotated.Clone();
                    posRelativeToCM.Multiply(-1d);

                    // Rotate the center of position around the center of mass
                    posRelativeToCM.RotateAroundAxis(_angularVelocity, timedAngle);

                    // Now figure out the new center of position
                    this.Position.X = cmWorld.X + posRelativeToCM.X;
                    this.Position.Y = cmWorld.Y + posRelativeToCM.Y;
                    this.Position.Z = cmWorld.Z + posRelativeToCM.Z;

                    #endregion
                }

                // Rotate myself
                this.RotateAroundAxis(_angularVelocity, timedAngle);
            }
        }
Beispiel #10
0
        /// <summary>
        /// This sets up everything in one shot
        /// </summary>
        public Sphere(MyVector position, DoubleVector origDirectionFacing, double radius)
        {
            // Store what was passed in
            _position      = position;
            _radius        = radius;
            _origDirFacing = origDirectionFacing;

            // Force the original direction facing to be unit vectors
            _origDirFacing.Standard.BecomeUnitVector();
            _origDirFacing.Orth.BecomeUnitVector();

            // Create a default quaternion (I don't want to instantiate dirfacing until it's requested)
            _rotation = new MyQuaternion(new MyVector(0, 0, 0), 0);
        }
Beispiel #11
0
        /// <summary>
        /// This adds external torque to internal torque.  Before I do that, I need to rotate a copy of external torque
        /// so that it is the same orientation as the internal torque.
        /// </summary>
        private void CombineExternalAndInternalTorques()
        {
            // My base's rotation quaternion already knows how to rotate from orig to dirfacing.  I need one that
            // goes the other way
            MyQuaternion rotation = base.Rotation.Clone();

            rotation.W *= -1;

            // Rotate the external to line up with the internal
            MyVector externalRotated = rotation.GetRotatedVector(_externalTorque, true);

            // Now that it's aligned, I can just add it onto the internal
            _internalTorque.Add(externalRotated);
        }
Beispiel #12
0
        /// <summary>
        /// Since the quaternion doesn't have any events, this function needs to be called manually whenever
        /// a rotation occurs
        /// </summary>
        /// <remarks>
        /// I need to take in the rotation, because the Sphere class keeps blowing away it's rotation, so this would
        /// immediately fall out of sync
        /// </remarks>
        public void SyncRotation(MyQuaternion rotation)
        {
            // I don't know if there's much of a gain in caching (or even a loss)
            MyVector[] cachedBasePoints = base.UniquePoints;
            MyVector[] cachedThisPoints = _rotatedPoly.UniquePoints;

            // Rotate all the points
            for (int pointCntr = 0; pointCntr < cachedBasePoints.Length; pointCntr++)
            {
                cachedThisPoints[pointCntr].StoreNewValues(rotation.GetRotatedVector(cachedBasePoints[pointCntr], true));
            }

            // Remember this for later
            _rotationForClone = rotation;
        }
Beispiel #13
0
        public void ResetRotation()
        {
            // I don't want to destroy the instances, just reset the values underneath
            if (_dirFacingRequested)
            {
                _dirFacing.Standard.StoreNewValues(_origDirFacing.Standard);
                _dirFacing.Orth.StoreNewValues(_origDirFacing.Orth);
            }

            MyQuaternion newRotation = new MyQuaternion(new MyVector(0, 0, 0), 0);

            _rotation.X = newRotation.X;
            _rotation.Y = newRotation.Y;
            _rotation.Z = newRotation.Z;
            _rotation.W = newRotation.W;
        }
Beispiel #14
0
        public override void PrepareForNewTimerCycle()
        {
            _savedRotation = null;
            _elapsedTime   = 0;

            _externalTorque.X = 0;
            _externalTorque.Y = 0;
            _externalTorque.Z = 0;

            _internalTorque.X = 0;
            _internalTorque.Y = 0;
            _internalTorque.Z = 0;

            // Call the base
            base.PrepareForNewTimerCycle();
        }
Beispiel #15
0
        public static MyVector Multiply(MyQuaternion quat, MyVector vector)
        {
            // nVidia SDK implementation
            MyVector uv, uuv;
            MyVector qvec = new MyVector(quat.X, quat.Y, quat.Z);

            uv  = MyVector.Cross(qvec, vector);
            uuv = MyVector.Cross(qvec, uv);
            uv.Multiply(2.0d * quat.W);
            uuv.Multiply(2.0d);

            // Add them together
            return(vector + uv + uuv);

            // get the rotation matrix of the Quaternion and multiply it times the vector
            //return quat.ToRotationMatrix() * vector;
        }
Beispiel #16
0
        /// <summary>
        /// WARNING: Don't rotate my direction facing outside of me, we will fall out of sync
        /// </summary>
        public virtual void RotateAroundAxis(MyVector rotateAround, double radians)
        {
            // Avoid mathmatical drift
            _numRotations++;
            if (_numRotations > 10000)
            {
                _numRotations = 0;
                _rotation.BecomeUnitQuaternion();
            }

            // Make a quaternion that holds the axis and radians passed in
            MyQuaternion intermediate = new MyQuaternion(rotateAround, radians);

            // Apply that to my existing one
            _rotation = MyQuaternion.Multiply(intermediate, _rotation);         // it seems counterintuitive to multiply them in this order, but testing shows it doesn't work the other way (and reading some articles confirms the order)

            if (_dirFacingRequested)
            {
                SyncDirFacing();
            }
        }
 public PolygonTest(MyVector position, DoubleVector origDirFacing, MyPolygon polygon, double radius)
     : base(position, origDirFacing, polygon, radius)
 {
     _rotateHandleX = new MyVector(this.Radius + DOTRADIUS, 0, 0);
     _rotateHandleY = new MyVector(this.Radius + (DOTRADIUS * 3), 0, 0);
     _rotateHandleZ = new MyVector(this.Radius + (DOTRADIUS * 5), 0, 0);
     _rotationX = new MyQuaternion(new MyVector(0, 0, 1), 0);
     _rotationY = new MyQuaternion(new MyVector(0, 0, 1), 0);
     _rotationZ = new MyQuaternion(new MyVector(0, 0, 1), 0);
 }
Beispiel #18
0
        /// <summary>
        /// Since the quaternion doesn't have any events, this function needs to be called manually whenever
        /// a rotation occurs
        /// </summary>
        /// <remarks>
        /// I need to take in the rotation, because the Sphere class keeps blowing away it's rotation, so this would
        /// immediately fall out of sync
        /// </remarks>
        public void SyncRotation(MyQuaternion rotation)
        {

            // I don't know if there's much of a gain in caching (or even a loss)
            MyVector[] cachedBasePoints = base.UniquePoints;
            MyVector[] cachedThisPoints = _rotatedPoly.UniquePoints;

            // Rotate all the points
            for (int pointCntr = 0; pointCntr < cachedBasePoints.Length; pointCntr++)
            {
                cachedThisPoints[pointCntr].StoreNewValues(rotation.GetRotatedVector(cachedBasePoints[pointCntr], true));
            }

            // Remember this for later
            _rotationForClone = rotation;

        }
Beispiel #19
0
        /// <summary>
        /// This one is used to assist with the clone method (especially for my derived classes)
        /// </summary>
        /// <param name="usesBoundingBox">Just pass in what you have</param>
        /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
        /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
        protected Ball(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
            : base(position, origDirectionFacing, rotation, radius)
        {
            // I use the property sets to enforce the values
            this.Mass = mass;
            this.Elasticity = elasticity;
            this.KineticFriction = kineticFriction;
            this.StaticFriction = staticFriction;

            _usesBoundingBox = usesBoundingBox;
            _boundingLower = boundingBoxLower;
            _boundingUpper = boundingBoxUpper;
        }
 /// <summary>
 /// There are classes that expose quaternions as read only properties (so the pointers can be shared).  I just made
 /// this function so it's easier to shuttle values around in one shot
 /// </summary>
 public void StoreNewValues(MyQuaternion valuesToGrab)
 {
     this.X = valuesToGrab.X;
     this.Y = valuesToGrab.Y;
     this.Z = valuesToGrab.Z;
     this.W = valuesToGrab.W;
 }
        public static MyQuaternion Multiply(MyQuaternion q1, MyQuaternion q2)
        {
            MyQuaternion retVal = new MyQuaternion();

            retVal.W = (q1.W * q2.W) - (q1.X * q2.X) - (q1.Y * q2.Y) - (q1.Z * q2.Z);
            retVal.X = (q1.W * q2.X) + (q1.X * q2.W) + (q1.Y * q2.Z) - (q1.Z * q2.Y);
            retVal.Y = (q1.W * q2.Y) - (q1.X * q2.Z) + (q1.Y * q2.W) + (q1.Z * q2.X);
            retVal.Z = (q1.W * q2.Z) + (q1.X * q2.Y) - (q1.Y * q2.X) + (q1.Z * q2.W);

            return retVal;
        }
        private void button3_Click(object sender, EventArgs e)
        {
            ClearPictureBox();

            // Setup Orig Vector
            MyVector origVector = new MyVector(9, 0, 0);
            DrawVector(origVector, Color.Silver);

            #region Single Rotation

            // Setup quat to do the whole rotation in one shot
            MyQuaternion largeRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(90));
            MyVector largeRotation = largeRotationQuat.GetRotatedVector(origVector, true);
            DrawVector(largeRotation, Color.White);

            #endregion

            #region Multi Rotations

            // Setup quat that holds a 30 degree angle
            MyQuaternion multiRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));
            MyVector subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
            DrawVector(subRotation, Color.Orange);


            // Setup a quat that holds a 1 degree angle
            MyQuaternion anotherRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(1));

            // Apply this to the multi quat 60 times
            for (int cntr = 1; cntr <= 60; cntr++)
            {
                multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
            }

            // Let's see what happened
            subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
            DrawVector(subRotation, Color.HotPink);

            #endregion
            #region Multi Rotations (3D)
            /*
			// Rotate around Y
			multiRotationQuat = new MyQuaternion(new MyVector(0, 1, 0), Utility3D.GetDegreesToRadians(90));

			// Rotate around X
			anotherRotationQuat = new MyQuaternion(new MyVector(1, 0, 0), Utility3D.GetDegreesToRadians(90));
			multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);

			// Draw the final output
			subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
			DrawVector(subRotation, Color.Yellow);
			*/
            #endregion
            #region Multi Rotations (Vector3D)
            /*
			subRotation = origVector.Clone();

			subRotation.RotateAroundAxis(new MyVector(0, 1, 0), Utility3D.GetDegreesToRadians(90));
			subRotation.RotateAroundAxis(new MyVector(1, 0, 0), Utility3D.GetDegreesToRadians(90));

			DrawVector(subRotation, Color.Yellow);
			*/
            #endregion
        }
        private void btnRotationMatrix_Click(object sender, EventArgs e)
        {
            ClearPictureBox();

            // Setup Orig Vector
            MyVector origVector = new MyVector(9, 0, 0);
            DrawVector(origVector, Color.Silver);

            // Rotate around Z
            MyQuaternion rotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));

            MyVector rotated = rotationQuat.GetRotatedVector(origVector, true);
            DrawVector(rotated, Color.Black);

            MyMatrix3 rotationMatrix;
            for (int cntr = 1; cntr <= 10000000; cntr++)
            {
                rotationMatrix = rotationQuat.ToMatrix3FromUnitQuaternion();

                rotationQuat = null;
                rotationQuat = new MyQuaternion();
                rotationQuat.FromRotationMatrix(rotationMatrix);

                rotationMatrix = null;
            }


            rotated = rotationQuat.GetRotatedVector(origVector, true);
            DrawVector(rotated, Color.DodgerBlue);

            rotationQuat.W *= -1;
            MyVector rotatedNegated = rotationQuat.GetRotatedVector(origVector, true);
            DrawVector(rotatedNegated, Color.Yellow);
        }
Beispiel #24
0
 /// <summary>
 /// This one is used to assist with the clone method (especially for my derived classes)
 /// </summary>
 /// <param name="usesBoundingBox">Just pass in what you have</param>
 /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
 /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
 protected RigidBodyPolygon(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, MyPolygonSyncedRotation polygon, double radius, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
     _polygon = polygon;
 }
Beispiel #25
0
 /// <summary>
 /// This overload should only be used during a clone.  I simply trust the values passed to me
 /// </summary>
 protected SpherePolygon(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, MyPolygonSyncedRotation polygon, double radius)
     : base(position, origDirectionFacing, rotation, radius)
 {
     _polygon = polygon;
 }
Beispiel #26
0
 /// <summary>
 /// This one is used to assist with the clone method (especially for my derived classes)
 /// </summary>
 /// <param name="usesBoundingBox">Just pass in what you have</param>
 /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
 /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
 protected SolidBall(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, mass, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
 }
Beispiel #27
0
        public MyPolygonSyncedRotation(MyVector[] uniquePoints, Triangle[] triangles, MyQuaternion rotation)
            : base(uniquePoints, triangles)
        {
            MyVector[] clonedPoints = Utility3D.GetClonedArray(uniquePoints);
            _rotatedPoly = new MyPolygon(clonedPoints, Utility3D.GetClonedArray(clonedPoints, triangles));

            SyncRotation(rotation);
        }
Beispiel #28
0
 /// <summary>
 /// This one is used to assist with the clone method (especially for my derived classes)
 /// </summary>
 /// <param name="usesBoundingBox">Just pass in what you have</param>
 /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
 /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
 protected RigidBodyPolygon(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, MyPolygonSyncedRotation polygon, double radius, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
     _polygon = polygon;
 }
Beispiel #29
0
 /// <summary>
 /// This overload should only be used during a clone.  I simply trust the values passed to me
 /// </summary>
 protected SpherePolygon(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, MyPolygonSyncedRotation polygon, double radius)
     : base(position, origDirectionFacing, rotation, radius)
 {
     _polygon = polygon;
 }
Beispiel #30
0
        /// <summary>
        /// This function takes in a destination double vector, and I will tell you how much you need to rotate me in order for me to end up
        /// along that destination double vector.
        /// </summary>
        /// <remarks>
        /// This function is a mutated copy of MyVector.GetAngleBetweenVectors.  It is almost identical, but slightly more complex  :)
        /// 
        /// If I am already aligned with the vector passed in, then I will return an arbitrary orthoganal, and an angle of zero.
        /// </remarks>
        /// <param name="destination">This is the double vector you want me to align myself with</param>
        public MyQuaternion GetAngleAroundAxis(DoubleVector destination)
        {
            #region Standard

            // Get the angle
            double rotationRadians = MyVector.GetAngleBetweenVectors(this.Standard, destination.Standard);
            if (Double.IsNaN(rotationRadians))
            {
                rotationRadians = 0d;
            }

            // I need to pull the cross product from me to the vector passed in
            MyVector rotationAxis = MyVector.Cross(this.Standard, destination.Standard);

            // If the cross product is zero, then there are two possibilities.  The vectors point in the same direction, or opposite directions.
            if (rotationAxis.IsNearZero)
            {
                // If I am here, then the angle will either be 0 or PI.
                if (Utility3D.IsNearZero(rotationRadians))
                {
                    // The vectors sit on top of each other.  I will set the orthoganal to an arbitrary value, and return zero for the radians
                    rotationAxis.X = 1d;
                    rotationAxis.Y = 0d;
                    rotationAxis.Z = 0d;
                    rotationRadians = 0d;
                }
                else
                {
                    // The vectors are pointing directly away from each other, because this is a double vector, I must rotate along an axis that
                    // is orthogonal to my standard and orth
                    rotationAxis = this.Orth; //MyVector.Cross(this.Standard, this.Orth);
                }
            }

            MyQuaternion quatStandard = new MyQuaternion(rotationAxis, rotationRadians);



            //return quatStandard;




            #endregion

            // I only need to rotate the orth, because I already know where the standard will be
            MyVector rotatedOrth = quatStandard.GetRotatedVector(this.Orth, true);

            #region Orthogonal

            // Grab the angle
            rotationRadians = MyVector.GetAngleBetweenVectors(rotatedOrth, destination.Orth);
            if (Double.IsNaN(rotationRadians))
            {
                rotationRadians = 0d;
            }

            // Since I've rotated the standards onto each other, the rotation axis of the orth is the standard (asumming it was truely orthogonal
            // to begin with)
            rotationAxis = destination.Standard.Clone();

            MyQuaternion quatOrth = new MyQuaternion(rotationAxis, rotationRadians);

            #endregion

            // Exit Function
            //return MyQuaternion.Multiply(quatOrth, quatStandard);
            return MyQuaternion.Multiply(quatStandard, quatOrth);
        }
        private void button4_Click(object sender, EventArgs e)
        {
            ClearPictureBox();

            // Setup Orig Vector
            MyVector origVector = new MyVector(9, 0, 0);
            DrawVector(origVector, Color.Silver);

            MyQuaternion multiRotationQuat = new MyQuaternion(new MyVector(0, 0, 0), Utility3D.GetDegreesToRadians(0));

            // Rotate around Z
            MyQuaternion anotherRotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(1));

            //List<double> lengths = new List<double>();

            for (int outerCntr = 1; outerCntr <= 100000; outerCntr++)
            {
                //lengths.Add(multiRotationQuat.GetMagnitude());
                for (int innerCntr = 1; innerCntr <= 360; innerCntr++)
                {
                    multiRotationQuat = MyQuaternion.Multiply(anotherRotationQuat, multiRotationQuat);
                }
                //multiRotationQuat.BecomeUnitQuaternion();
            }

            // Draw the final output
            MyVector subRotation = multiRotationQuat.GetRotatedVector(origVector, true);
            DrawVector(subRotation, Color.Yellow);
        }
Beispiel #32
0
 /// <summary>
 /// Use this overload for clone
 /// </summary>
 protected TorqueBall(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, mass, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
     ResetInertiaTensorAndCenterOfMass();
 }
        private void button5_Click(object sender, EventArgs e)
        {



            // This button tests TorqueBall.OrthonormalizeOrientation




            ClearPictureBox();

            // Setup Orig Vector
            MyVector origVector = new MyVector(9, 0, 0);
            DrawVector(origVector, Color.Silver);

            // Rotate around Z
            MyQuaternion rotationQuat = new MyQuaternion(new MyVector(0, 0, 1), Utility3D.GetDegreesToRadians(30));

            MyVector rotated = rotationQuat.GetRotatedVector(origVector, true);
            DrawVector(rotated, Color.Black);

            MyMatrix3 rotationMatrix = rotationQuat.ToMatrix3FromUnitQuaternion();




            // See if this affects the rotation matrix
            TorqueBall.OrthonormalizeOrientation(rotationMatrix);





            rotationQuat = null;
            rotationQuat = new MyQuaternion();
            rotationQuat.FromRotationMatrix(rotationMatrix);

            rotationMatrix = null;


            // Draw the results
            rotated = rotationQuat.GetRotatedVector(origVector, true);
            DrawVector(rotated, Color.DodgerBlue);
        }
Beispiel #34
0
        public override void TimerTestPosition(double elapsedTime)
        {
            base.TimerTestPosition(elapsedTime);

            // Either remember the orig rotation, or restore to that orig rotation (as of PrepareForNew)
            if (_savedRotation == null)
            {
                _savedRotation = this.Rotation.Clone();
            }
            else
            {
                this.Rotation.StoreNewValues(_savedRotation);
            }

            // Remember the elapsed time that was passed in
            _elapsedTime = elapsedTime;

            // Figure out the new rotation (the body has been rotating at some velocity during the previous tick)
            if (!_angularVelocity.IsZero)
            {
                // Figure out what the angle will be
                double timedAngle = _angularVelocity.GetMagnitude();
                timedAngle *= elapsedTime;

                if (!_centerOfMass.IsZero)
                {
                    #region Rotate around center of mass

                    // Remember where the center of mass is in world coords
                    MyVector cmRotated = this.Rotation.GetRotatedVector(_centerOfMass, true);
                    MyVector cmWorld = this.Position + cmRotated;

                    // Get the opposite of the cm
                    MyVector posRelativeToCM = cmRotated.Clone();
                    posRelativeToCM.Multiply(-1d);

                    // Rotate the center of position around the center of mass
                    posRelativeToCM.RotateAroundAxis(_angularVelocity, timedAngle);

                    // Now figure out the new center of position
                    this.Position.X = cmWorld.X + posRelativeToCM.X;
                    this.Position.Y = cmWorld.Y + posRelativeToCM.Y;
                    this.Position.Z = cmWorld.Z + posRelativeToCM.Z;

                    #endregion
                }

                // Rotate myself
                this.RotateAroundAxis(_angularVelocity, timedAngle);
            }
        }
        public DoubleVector GetRotatedVectorReverse(DoubleVector doubleVector, bool isQuatNormalized)
        {
            if (!isQuatNormalized)
            {
                // I'm not normalized, clone myself and normalize it
                MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
                myUnitClone.BecomeUnitQuaternion();

                return myUnitClone.GetRotatedVectorReverse(doubleVector, true);
            }

            return new DoubleVector(GetRotatedVectorReverse(doubleVector.Standard, true), GetRotatedVectorReverse(doubleVector.Orth, true));
        }
Beispiel #36
0
        /// <summary>
        /// WARNING: Don't rotate my direction facing outside of me, we will fall out of sync
        /// </summary>
        public virtual void RotateAroundAxis(MyVector rotateAround, double radians)
        {
            // Avoid mathmatical drift
            _numRotations++;
            if (_numRotations > 10000)
            {
                _numRotations = 0;
                _rotation.BecomeUnitQuaternion();
            }

            // Make a quaternion that holds the axis and radians passed in
            MyQuaternion intermediate = new MyQuaternion(rotateAround, radians);

            // Apply that to my existing one
            _rotation = MyQuaternion.Multiply(intermediate, _rotation);		// it seems counterintuitive to multiply them in this order, but testing shows it doesn't work the other way (and reading some articles confirms the order)

            if (_dirFacingRequested)
            {
                SyncDirFacing();
            }
        }
        public static MyVector Multiply(MyQuaternion quat, MyVector vector)
        {
            // nVidia SDK implementation
            MyVector uv, uuv;
            MyVector qvec = new MyVector(quat.X, quat.Y, quat.Z);

            uv = MyVector.Cross(qvec, vector);
            uuv = MyVector.Cross(qvec, uv);
            uv.Multiply(2.0d * quat.W);
            uuv.Multiply(2.0d);

            // Add them together
            return vector + uv + uuv;

            // get the rotation matrix of the Quaternion and multiply it times the vector
            //return quat.ToRotationMatrix() * vector;
        }
Beispiel #38
0
        /// <summary>
        /// This sets up everything in one shot
        /// </summary>
        public Sphere(MyVector position, DoubleVector origDirectionFacing, double radius)
        {
            // Store what was passed in
            _position = position;
            _radius = radius;
            _origDirFacing = origDirectionFacing;

            // Force the original direction facing to be unit vectors
            _origDirFacing.Standard.BecomeUnitVector();
            _origDirFacing.Orth.BecomeUnitVector();

            // Create a default quaternion (I don't want to instantiate dirfacing until it's requested)
            _rotation = new MyQuaternion(new MyVector(0, 0, 0), 0);
        }
        /// <summary>
        /// This function creates a vector that is the vector passed in rotated by my definition
        /// </summary>
        /// <param name="vector">The vector to rotate (I don't touch this vector, I create a new one that is rotated)</param>
        /// <param name="isQuatNormalized">Whether this class is already normalized or not (if you don't know, pass false)</param>
        public MyVector GetRotatedVector(MyVector vector, bool isQuatNormalized)
        {
            if (!isQuatNormalized)
            {
                // I'm not normalized, clone myself and normalize it
                MyQuaternion myUnitClone = new MyQuaternion(this.X, this.Y, this.Z, this.W);
                myUnitClone.BecomeUnitQuaternion();

                return myUnitClone.GetRotatedVector(vector, true);
            }

            MyVector qvec = new MyVector(this.X, this.Y, this.Z);

            //Vector uv = qvec.Cross(vector);
            MyVector uv = MyVector.Cross(qvec, vector);

            //Vector uuv = qvec.Cross(uv);
            MyVector uuv = MyVector.Cross(qvec, uv);

            //uv *= (2.0f * quat.w);
            uv.Multiply(this.W * 2d);

            //uuv *= 2.0f;
            uuv.Multiply(2d);

            //return vector + uv + uuv;
            MyVector retVal = vector.Clone();
            retVal.Add(uv);
            retVal.Add(uuv);
            return retVal;
        }
Beispiel #40
0
        /// <summary>
        /// This function will rotate me around any arbitrary axis (no gimbal lock for me!!!!!!!)
        /// </summary>
        /// <remarks>
        /// From some web site talking about rotations (seems like good stuff, but I don't know how it's used):
        /// 
        /// // I'm not sure why this multiplication is important
        /// Quaternion totalQuat = Quaternion.MultiplyQuaternions(rotationQuat, new Quaternion(1, 0, 0, 0));      // order of multiplication is important
        /// 
        /// // Get a matrix telling me how to rotate
        /// Matrix rotationMatrix = totalQuat.ToMatrixFromUnitQuaternion();
        /// 
        /// // AND THEN??????????
        /// </remarks>
        /// <param name="rotateAround">Any vector to rotate around</param>
        /// <param name="radians">How far to rotate</param>
        public void RotateAroundAxis(MyVector rotateAround, double radians)
        {
            // Create a quaternion that represents the axis and angle passed in
            MyQuaternion rotationQuat = new MyQuaternion(rotateAround, radians);

            // Get a vector that represents me rotated by the quaternion
            MyVector newValue = rotationQuat.GetRotatedVector(this, true);

            // Store my new values
            this.X = newValue.X;
            this.Y = newValue.Y;
            this.Z = newValue.Z;
        }
Beispiel #41
0
 /// <summary>
 /// This overload should only be used during a clone.  I simply trust the values passed to me
 /// </summary>
 protected Sphere(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius)
 {
     _position = position;
     _radius = radius;
     _origDirFacing = origDirectionFacing;
     _rotation = rotation;
 }
Beispiel #42
0
        public MyPolygonSyncedRotation(MyVector[] uniquePoints, Triangle[] triangles, MyQuaternion rotation)
            : base(uniquePoints, triangles)
        {
            MyVector[] clonedPoints = Utility3D.GetClonedArray(uniquePoints);
            _rotatedPoly = new MyPolygon(clonedPoints, Utility3D.GetClonedArray(clonedPoints, triangles));

            SyncRotation(rotation);
        }
Beispiel #43
0
        public void ResetRotation()
        {
            // I don't want to destroy the instances, just reset the values underneath
            if (_dirFacingRequested)
            {
                _dirFacing.Standard.StoreNewValues(_origDirFacing.Standard);
                _dirFacing.Orth.StoreNewValues(_origDirFacing.Orth);
            }

            MyQuaternion newRotation = new MyQuaternion(new MyVector(0, 0, 0), 0);
            _rotation.X = newRotation.X;
            _rotation.Y = newRotation.Y;
            _rotation.Z = newRotation.Z;
            _rotation.W = newRotation.W;
        }
 public void AddBarrel(MyVector offset, MyQuaternion rotation)
 {
     _barrels.Add(new Barrel(offset, rotation));
 }
Beispiel #45
0
        public void GetAngleAroundAxis(out MyQuaternion rotation, MyVector destination)
        {
            MyVector axis;
            double radians;
            GetAngleAroundAxis(out axis, out radians, destination);

            rotation = new MyQuaternion(axis, radians);
        }
Beispiel #46
0
        public override void PrepareForNewTimerCycle()
        {
            _savedRotation = null;
            _elapsedTime = 0;

            _externalTorque.X = 0;
            _externalTorque.Y = 0;
            _externalTorque.Z = 0;

            _internalTorque.X = 0;
            _internalTorque.Y = 0;
            _internalTorque.Z = 0;

            // Call the base
            base.PrepareForNewTimerCycle();
        }
Beispiel #47
0
 /// <summary>
 /// This one is used to assist with the clone method (especially for my derived classes)
 /// </summary>
 /// <param name="usesBoundingBox">Just pass in what you have</param>
 /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
 /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
 protected RigidBody(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, 0, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
     _constructorCalled = true;
 }
Beispiel #48
0
        /// <summary>
        /// This function takes in a destination double vector, and I will tell you how much you need to rotate me in order for me to end up
        /// along that destination double vector.
        /// </summary>
        /// <remarks>
        /// This function is a mutated copy of MyVector.GetAngleBetweenVectors.  It is almost identical, but slightly more complex  :)
        ///
        /// If I am already aligned with the vector passed in, then I will return an arbitrary orthoganal, and an angle of zero.
        /// </remarks>
        /// <param name="destination">This is the double vector you want me to align myself with</param>
        public MyQuaternion GetAngleAroundAxis(DoubleVector destination)
        {
            #region Standard

            // Get the angle
            double rotationRadians = MyVector.GetAngleBetweenVectors(this.Standard, destination.Standard);
            if (Double.IsNaN(rotationRadians))
            {
                rotationRadians = 0d;
            }

            // I need to pull the cross product from me to the vector passed in
            MyVector rotationAxis = MyVector.Cross(this.Standard, destination.Standard);

            // If the cross product is zero, then there are two possibilities.  The vectors point in the same direction, or opposite directions.
            if (rotationAxis.IsNearZero)
            {
                // If I am here, then the angle will either be 0 or PI.
                if (Utility3D.IsNearZero(rotationRadians))
                {
                    // The vectors sit on top of each other.  I will set the orthoganal to an arbitrary value, and return zero for the radians
                    rotationAxis.X  = 1d;
                    rotationAxis.Y  = 0d;
                    rotationAxis.Z  = 0d;
                    rotationRadians = 0d;
                }
                else
                {
                    // The vectors are pointing directly away from each other, because this is a double vector, I must rotate along an axis that
                    // is orthogonal to my standard and orth
                    rotationAxis = this.Orth; //MyVector.Cross(this.Standard, this.Orth);
                }
            }

            MyQuaternion quatStandard = new MyQuaternion(rotationAxis, rotationRadians);



            //return quatStandard;



            #endregion

            // I only need to rotate the orth, because I already know where the standard will be
            MyVector rotatedOrth = quatStandard.GetRotatedVector(this.Orth, true);

            #region Orthogonal

            // Grab the angle
            rotationRadians = MyVector.GetAngleBetweenVectors(rotatedOrth, destination.Orth);
            if (Double.IsNaN(rotationRadians))
            {
                rotationRadians = 0d;
            }

            // Since I've rotated the standards onto each other, the rotation axis of the orth is the standard (asumming it was truely orthogonal
            // to begin with)
            rotationAxis = destination.Standard.Clone();

            MyQuaternion quatOrth = new MyQuaternion(rotationAxis, rotationRadians);

            #endregion

            // Exit Function
            //return MyQuaternion.Multiply(quatOrth, quatStandard);
            return(MyQuaternion.Multiply(quatStandard, quatOrth));
        }
Beispiel #49
0
 /// <summary>
 /// This one is used to assist with the clone method (especially for my derived classes)
 /// </summary>
 /// <param name="usesBoundingBox">Just pass in what you have</param>
 /// <param name="boundingBoxLower">Set this to null if bounding box is false</param>
 /// <param name="boundingBoxUpper">Set this to null if bounding box is false</param>
 protected SolidBall(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, mass, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper) { }
            private void RotateMe(MyVector localPos, MyVector rotateHandle, MyQuaternion rotation2D, MyVector rotateAround3D)
            {
                // Get Current Angle
                double curRadians = MyVector.GetAngleBetweenVectors(rotateHandle, rotation2D.GetRotatedVector(rotateHandle, true));

                // Rotate the 2D Handle
                MyQuaternion newRotationX;
                rotateHandle.GetAngleAroundAxis(out newRotationX, localPos);
                rotation2D.StoreNewValues(newRotationX);

                // Get New Angle
                double newRadians = MyVector.GetAngleBetweenVectors(rotateHandle, rotation2D.GetRotatedVector(rotateHandle, true));

                double rotateRadians = newRadians - curRadians;
                if (rotation2D.GetRotatedVector(rotateHandle, true).Y < 0) rotateRadians *= -1;		// This statement is cheating.  I'm using the fact that the rotate handles all lie on the XY plane

                // Apply the difference to the 3D object (me)
                this.RotateAroundAxis(this.Rotation.GetRotatedVector(rotateAround3D, true), rotateRadians);
            }
Beispiel #51
0
 /// <summary>
 /// Use this overload for clone
 /// </summary>
 protected TorqueBall(MyVector position, DoubleVector origDirectionFacing, MyQuaternion rotation, double radius, double mass, double elasticity, double kineticFriction, double staticFriction, bool usesBoundingBox, MyVector boundingBoxLower, MyVector boundingBoxUpper)
     : base(position, origDirectionFacing, rotation, radius, mass, elasticity, kineticFriction, staticFriction, usesBoundingBox, boundingBoxLower, boundingBoxUpper)
 {
     ResetInertiaTensorAndCenterOfMass();
 }
 public void AddBarrel(MyQuaternion rotation)
 {
     _barrels.Add(new Barrel(rotation));
 }