Exemplo n.º 1
0
 public void CopyFrom(Quaternion4F q)
 {
     W = q.W;
     X = q.X;
     Y = q.Y;
     Z = q.Z;
 }
Exemplo n.º 2
0
        public void FromQuaternion(Quaternion4F q)
        {
            SetIdentity();

            // Compute a few values to optimize common subexpressions
            var ww = 2.0f * q.W;
            var xx = 2.0f * q.X;
            var yy = 2.0f * q.Y;
            var zz = 2.0f * q.Z;

            // Set the matrix elements.  There is still a little more
            // opportunity for optimization due to the many common
            // subexpressions.  We'll let the compiler handle that...
            Values[0] = 1.0f - (yy * q.Y) - (zz * q.Z);
            Values[1] = (xx * q.Y) + (ww * q.Z);
            Values[2] = (xx * q.Z) - (ww * q.Y);

            Values[4] = (xx * q.Y) - (ww * q.Z);
            Values[5] = 1.0f - (xx * q.X) - (zz * q.Z);
            Values[6] = (yy * q.Z) + (ww * q.X);

            Values[8]  = (xx * q.Z) + (ww * q.Y);
            Values[9]  = (yy * q.Z) - (ww * q.X);
            Values[10] = 1.0f - (xx * q.X) - (yy * q.Y);
        }
Exemplo n.º 3
0
        /// <summary>
        ///     Do Spherical linear interpolation between two quaternions
        /// </summary>
        /// <param name="q1">The first quaternion</param>
        /// <param name="q2">The second quaternion</param>
        /// <param name="blend">The blend factor</param>
        /// <returns>A smooth blend between the given quaternions</returns>
        public static Quaternion4F OtkSlerp(Quaternion4F q1, Quaternion4F q2, float blend)
        {
            // if either input is zero, return the other.
            if (q1.LengthSquared == 0.0f)
            {
                if (q2.LengthSquared == 0.0f)
                {
                    return(Identity);
                }
                return(q2);
            }
            if (q2.LengthSquared == 0.0f)
            {
                return(q1);
            }


            var cosHalfAngle = q1.W * q2.W + q1.Xyz.Dot(q2.Xyz);

            if (cosHalfAngle >= 1.0f || cosHalfAngle <= -1.0f)
            {
                // angle = 0.0f, so just return one input.
                return(q1);
            }
            if (cosHalfAngle < 0.0f)
            {
                q2.Xyz.Negate();
                q2.W         = -q2.W;
                cosHalfAngle = -cosHalfAngle;
            }

            float blendA;
            float blendB;

            if (cosHalfAngle < 0.99f)
            {
                // do proper slerp for big angles
                var halfAngle           = (float)Math.Acos(cosHalfAngle);
                var sinHalfAngle        = (float)Math.Sin(halfAngle);
                var oneOverSinHalfAngle = 1.0f / sinHalfAngle;
                blendA = (float)Math.Sin(halfAngle * (1.0f - blend)) * oneOverSinHalfAngle;
                blendB = (float)Math.Sin(halfAngle * blend) * oneOverSinHalfAngle;
            }
            else
            {
                // do lerp if angle is really small.
                blendA = 1.0f - blend;
                blendB = blend;
            }

            var result = new Quaternion4F(blendA * q1.Xyz + blendB * q2.Xyz, blendA * q1.W + blendB * q2.W);

            if (result.LengthSquared > 0.0f)
            {
                result.Normalize();
                return(result);
            }
            return(Identity);
        }
Exemplo n.º 4
0
        /// <summary>
        ///     Devuelve un nuevo Quaternion que representa la concatenación de rotaciones de ambos.
        /// </summary>
        /// <param name="tempLeft"></param>
        /// <param name="tempRight"></param>
        /// <returns></returns>
        public static Quaternion4F operator *(Quaternion4F left, Quaternion4F right)
        {
            var q = new Quaternion4F();

            q.X = left.W * right.X + left.X * right.W + left.Y * right.Z - left.Z * right.Y;
            q.Y = left.W * right.Y + left.Y * right.W + left.Z * right.X - left.X * right.Z;
            q.Z = left.W * right.Z + left.Z * right.W + left.X * right.Y - left.Y * right.X;
            q.W = left.W * right.W - left.X * right.X - left.Y * right.Y - left.Z * right.Z;

            return(q);
        }
Exemplo n.º 5
0
        /// <summary>
        ///     this = this * q
        /// </summary>
        /// <param name="q"></param>
        public void Multiply(Quaternion4F q)
        {
            var auxX = _w * q._x + _x * q._w + _y * q._z - _z * q._y;
            var auxY = _w * q._y + _y * q._w + _z * q._x - _x * q._z;
            var auxZ = _w * q._z + _z * q._w + _x * q._y - _y * q._x;
            var auxW = _w * q._w - _x * q._x - _y * q._y - _z * q._z;

            _x = auxX;
            _y = auxY;
            _z = auxZ;
            _w = auxW;
        }
Exemplo n.º 6
0
        public static Quaternion4F FromRadiansAxis(float radianes, float x, float y, float z)
        {
            var quaternion = new Quaternion4F();
            var num2       = radianes * 0.5f;
            var num        = (float)Math.Sin(num2);
            var num3       = (float)Math.Cos(num2);

            quaternion._x = x * num;
            quaternion._y = y * num;
            quaternion._z = z * num;
            quaternion._w = num3;
            quaternion.Normalize();
            return(quaternion);
        }
Exemplo n.º 7
0
 /// <summary>
 ///     Multiplica esta matriz (this) por la representada por el quaternion pasado como parámetro.
 ///     <para>this = this * Quaternion</para>
 /// </summary>
 /// <param name="qRot"></param>
 public virtual void Rotate(Quaternion4F qRot)
 {
     //qRot.GetMatriz4(ref auxMat4);
     AuxMat4.FromQuaternion(qRot);
     MultipliesBy(AuxMat4);
 }
Exemplo n.º 8
0
        public static Quaternion4F Slerp(Quaternion4F q0, Quaternion4F q1, float t)
        {
            var qresult = new Quaternion4F();

            // Check for out-of range parameter and return edge points if so
            if (t <= 0.0)
            {
                qresult.CopyFrom(q0);
                return(qresult);
            }
            if (t >= 1.0)
            {
                qresult.CopyFrom(q1);
                return(qresult);
            }

            // Compute "cosine of angle between quaternions" using dot product
            var cosOmega = Dot(q0, q1);

            // If negative dot, use -q1.  Two quaternions q and -q
            // represent the same rotation, but may produce
            // different slerp.  We chose q or -q to rotate using
            // the acute angle.
            var q1W = q1._w;
            var q1X = q1._x;
            var q1Y = q1._y;
            var q1Z = q1._z;

            if (cosOmega < 0.0)
            {
                q1W      = -q1W;
                q1X      = -q1X;
                q1Y      = -q1Y;
                q1Z      = -q1Z;
                cosOmega = -cosOmega;
            }

            // We should have two unit quaternions, so dot should be <= 1.0
            if (!(cosOmega < 1.1f))
            {
                throw new Exception("Error en Quaternion.Slerp(), cosOmega > 1.1f");
            }

            // Compute interpolation fraction, checking for quaternions
            // almost exactly the same
            float k0, k1;

            if (cosOmega > 0.9999)
            {
                // Very close - just use linear interpolation,
                // which will protect againt a divide by zero

                k0 = 1.0f - t;
                k1 = t;
            }
            else
            {
                // Compute the sin of the angle using the
                // trig identity sin^2(omega) + cos^2(omega) = 1
                var sinOmega = (float)Math.Sqrt(1.0 - (cosOmega * cosOmega));

                // Compute the angle from its sin and cosine
                var omega = (float)Math.Atan2(sinOmega, cosOmega);

                // Compute inverse of denominator, so we only have
                // to divide once
                var oneOverSinOmega = 1.0f / sinOmega;

                // Compute interpolation parameters
                k0 = (float)(Math.Sin((1.0 - t) * omega) * oneOverSinOmega);
                k1 = (float)(Math.Sin(t * omega) * oneOverSinOmega);
            }

            // Interpolate and return new quaternion
            return(new Quaternion4F(
                       (k0 * q0._w) + (k1 * q1W),
                       (k0 * q0._x) + (k1 * q1X),
                       (k0 * q0._y) + (k1 * q1Y),
                       (k0 * q0._z) + (k1 * q1Z)
                       ));
        }
Exemplo n.º 9
0
 public static float Dot(Quaternion4F a, Quaternion4F b)
 {
     return((a._w * b._w) + (a._x * b._x) + (a._y * b._y) + (a._z * b._z));
 }
Exemplo n.º 10
0
 public float Dot(Quaternion4F quat)
 {
     return(_w * quat._w + _x * quat._x + _y * quat._y + _z * quat._z);
 }