示例#1
0
        // @returns the rotation matrix corresponding to the inverse rotation by this
        // quaternion.
        public RotationMatrix RotationMatrixTranspose()
        {
            RotationMatrix R = new RotationMatrix();

            float x2 = _x * _x;
            float y2 = _y * _y;
            float z2 = _z * _z;
            float r2 = _r * _r;

            R[0, 0] = r2 + x2 - y2 - z2;         // fill diagonal terms
            R[1, 1] = r2 - x2 + y2 - z2;
            R[2, 2] = r2 - x2 - y2 + z2;
            float xy = _x * _y;
            float yz = _y * _z;
            float zx = _z * _x;
            float rx = _r * _x;
            float ry = _r * _y;
            float rz = _r * _z;

            R[0, 1] = 2 * (xy + rz);             // fill off diagonal terms
            R[0, 2] = 2 * (zx - ry);
            R[1, 2] = 2 * (yz + rx);
            R[1, 0] = 2 * (xy - rz);
            R[2, 0] = 2 * (zx + ry);
            R[2, 1] = 2 * (yz - rx);

            return(R);
        }
示例#2
0
        /// <summary>
        ///
        /// </summary>
        /// <param name="R">the rotation matrix from which the quaternion will be extracted</param>
        public void Set(RotationMatrix R)
        {
            // The trace determines the angle
            float diag = 0.5f * (R[0, 0] + R[1, 1] + R[2, 2] - 1.0f);

            if (diag >= 1.0)
            {
                // zero angle
                _x = _y = _z = 0.0f;
                return;
            }
            else
            {
                float angle = (float)Math.Acos(diag);
                _x = R[2, 1] - R[1, 2];
                _y = R[0, 2] - R[2, 0];
                _z = R[1, 0] - R[0, 1];
                float axisnorm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
                if (axisnorm < 1e-8)
                {
                    // Axis is undefined
                    _x = _y = _z = 0.0f;
                    return;
                }
                else
                {
                    // All OK, so scale it
                    float scale = angle / axisnorm;
                    _x *= scale;
                    _y *= scale;
                    _z *= scale;
                }
            }
        }
示例#3
0
        public override MatrixFixed Transpose()
        {
            RotationMatrix R = new RotationMatrix();

            R[0, 0] = this[0, 0]; R[0, 1] = this[1, 0]; R[0, 2] = this[2, 0];
            R[1, 0] = this[0, 1]; R[1, 1] = this[1, 1]; R[1, 2] = this[2, 1];
            R[2, 0] = this[0, 2]; R[2, 1] = this[1, 2]; R[2, 2] = this[2, 2];

            return(R);
        }
示例#4
0
        //param R the rotation matrix from which the quaternion will be extracted
        public void Set(RotationMatrix R)
        {
            float d0 = R[0, 0], d1 = R[1, 1], d2 = R[2, 2];

            // The trace determines the method of decomposition
            float rr = 1.0f + d0 + d1 + d2;

            if (rr > 0)
            {
                float s = 0.5f / (float)Math.Sqrt(rr);
                _x = (R[2, 1] - R[1, 2]) * s;
                _y = (R[0, 2] - R[2, 0]) * s;
                _z = (R[1, 0] - R[0, 1]) * s;
                _r = 0.25f / s;
            }
            else
            {
                // Trace is less than zero, so need to determine which
                // major diagonal is largest
                if ((d0 > d1) && (d0 > d2))
                {
                    float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                    _x = 0.5f * s;
                    _y = (R[0, 1] + R[1, 0]) * s;
                    _z = (R[0, 2] + R[2, 0]) * s;
                    _r = (R[1, 2] + R[2, 1]) * s;
                }
                else
                if (d1 > d2)
                {
                    float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                    _x = (R[0, 1] + R[1, 0]) * s;
                    _y = 0.5f * s;
                    _z = (R[1, 2] + R[2, 1]) * s;
                    _r = (R[0, 2] + R[2, 0]) * s;
                }
                else
                {
                    float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                    _x = (R[0, 2] + R[2, 0]) * s;
                    _y = (R[1, 2] + R[2, 1]) * s;
                    _z = 0.5f * s;
                    _r = (R[0, 1] + R[1, 0]) * s;
                }
            }
        }
示例#5
0
        /// <summary>
        ///
        /// </summary>
        /// <returns>the rotation matrix corresponding to the inverse rotation by this quaternion</returns>
        public RotationMatrix RotationMatrixTranspose()
        {
            // start with an identity matrix.
            RotationMatrix R     = new RotationMatrix();
            float          angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);

            // Identity is all that can be deduced from zero angle
            if (angle == 0)
            {
                return(R);
            }

            float c = (float)Math.Cos(angle) - 1.0f;
            float s = (float)Math.Sin(angle);

            // normalize axis to a unit vector u
            float norm = 1.0f / angle;
            float nx   = _x * norm;
            float ny   = _y * norm;
            float nz   = _z * norm;

            // add (cos(angle)-1)*(1 - u u').
            R[0, 0] += c * (nx * nx - 1.0f);
            R[0, 1] += c * nx * ny;
            R[0, 2] += c * nx * nz;

            R[1, 0] += c * ny * nx;
            R[1, 1] += c * (ny * ny - 1.0f);
            R[1, 2] += c * ny * nz;

            R[2, 0] += c * nz * nx;
            R[2, 1] += c * nz * ny;
            R[2, 2] += c * (nz * nz - 1.0f);

            // add sin(angle) * [u]
            R[0, 1] += s * nz;
            R[0, 2] -= s * ny;
            R[1, 0] -= s * nz;
            R[1, 2] += s * nx;
            R[2, 0] += s * ny;
            R[2, 1] -= s * nx;

            return(R);
        }
示例#6
0
 public RotationMatrix(RotationMatrix R) : base(3, 3)
 {
     this.Equals(R);
 }
示例#7
0
 public Quaternion(RotationMatrix R)
 {
     Set(R);
 }
示例#8
0
        // @returns the rotation matrix corresponding to the inverse rotation by this
        // quaternion.
        public RotationMatrix RotationMatrixTranspose()
        {
            RotationMatrix R = new RotationMatrix();

            float x2 = _x * _x;
            float y2 = _y * _y;
            float z2 = _z * _z;
            float r2 = _r * _r;
            R[0,0] = r2 + x2 - y2 - z2;         // fill diagonal terms
            R[1,1] = r2 - x2 + y2 - z2;
            R[2,2] = r2 - x2 - y2 + z2;
            float xy = _x * _y;
            float yz = _y * _z;
            float zx = _z * _x;
            float rx = _r * _x;
            float ry = _r * _y;
            float rz = _r * _z;
            R[0,1] = 2 * (xy + rz);             // fill off diagonal terms
            R[0,2] = 2 * (zx - ry);
            R[1,2] = 2 * (yz + rx);
            R[1,0] = 2 * (xy - rz);
            R[2,0] = 2 * (zx + ry);
            R[2,1] = 2 * (yz - rx);

            return(R);
        }
示例#9
0
        //param R the rotation matrix from which the quaternion will be extracted
        public void Set(RotationMatrix R)
        {
            float d0 = R[0,0], d1 = R[1,1], d2 = R[2,2];

            // The trace determines the method of decomposition
            float rr = 1.0f + d0 + d1 + d2;

            if (rr > 0) 
            {
                float s = 0.5f / (float)Math.Sqrt(rr);
                _x = (R[2,1] - R[1,2]) * s;
                _y = (R[0,2] - R[2,0]) * s;
                _z = (R[1,0] - R[0,1]) * s;
                _r = 0.25f / s;
            } 
            else 
            {
                // Trace is less than zero, so need to determine which
                // major diagonal is largest
                if ((d0 > d1) && (d0 > d2)) 
                {
                    float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                    _x = 0.5f * s;
                    _y = (R[0,1] + R[1,0]) * s;
                    _z = (R[0,2] + R[2,0]) * s;
                    _r = (R[1,2] + R[2,1]) * s;
                } 
                else 
                    if (d1 > d2) 
                    {
                        float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                        _x = (R[0,1] + R[1,0]) * s;
                        _y = 0.5f * s;
                        _z = (R[1,2] + R[2,1]) * s;
                        _r = (R[0,2] + R[2,0]) * s;
                    } 
                    else 
                    {
                        float s = 0.5f / (float)Math.Sqrt(1 + d0 - d1 - d2);
                        _x = (R[0,2] + R[2,0]) * s;
                        _y = (R[1,2] + R[2,1]) * s;
                        _z = 0.5f * s;
                        _r = (R[0,1] + R[1,0]) * s;
                    }
            }
        }
示例#10
0
 public Quaternion(RotationMatrix R)
 {
     Set(R);
 }
示例#11
0
        public override MatrixFixed Transpose()
        {
            RotationMatrix R = new RotationMatrix();
            R[0, 0] = this[0, 0]; R[0, 1] = this[1, 0]; R[0, 2] = this[2, 0];
            R[1, 0] = this[0, 1]; R[1, 1] = this[1, 1]; R[1, 2] = this[2, 1];
            R[2, 0] = this[0, 2]; R[2, 1] = this[1, 2]; R[2, 2] = this[2, 2];

            return R;
        }
示例#12
0
 public RotationMatrix(RotationMatrix R) : base(3,3)
 {
     this.Equals(R);
 }
示例#13
0
 public AngleAxis(RotationMatrix R)
 {
     Set(R);
 }
示例#14
0
 public AngleAxis(RotationMatrix R)
 {
     Set(R);
 }
示例#15
0
        /// <summary>
        /// 
        /// </summary>
        /// <returns>the rotation matrix corresponding to the inverse rotation by this quaternion</returns>
        public RotationMatrix RotationMatrixTranspose()
        {
            // start with an identity matrix.
            RotationMatrix R = new RotationMatrix();
            float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);

            // Identity is all that can be deduced from zero angle
            if (angle == 0)
                return R;

            float c = (float)Math.Cos(angle) - 1.0f;
            float s = (float)Math.Sin(angle);

            // normalize axis to a unit vector u
            float norm = 1.0f/angle;
            float nx = _x*norm;
            float ny = _y*norm;
            float nz = _z*norm;
 
            // add (cos(angle)-1)*(1 - u u').
            R[0,0] += c* (nx*nx - 1.0f);
            R[0,1] += c*  nx*ny;
            R[0,2] += c*  nx*nz;

            R[1,0] += c*  ny*nx;
            R[1,1] += c* (ny*ny - 1.0f);
            R[1,2] += c*  ny*nz;

            R[2,0] += c*  nz*nx;
            R[2,1] += c*  nz*ny;
            R[2,2] += c* (nz*nz - 1.0f);

            // add sin(angle) * [u]
            R[0,1] += s*nz; 
            R[0,2] -= s*ny;
            R[1,0] -= s*nz; 
            R[1,2] += s*nx;
            R[2,0] += s*ny; 
            R[2,1] -= s*nx;

            return(R);
        }
示例#16
0
 /// <summary>
 /// 
 /// </summary>
 /// <param name="R">the rotation matrix from which the quaternion will be extracted</param>
 public void Set(RotationMatrix R)
 {
     // The trace determines the angle
     float diag = 0.5f*(R[0,0] + R[1,1] + R[2,2] - 1.0f);
     if (diag >= 1.0) 
     {
         // zero angle
         _x = _y = _z = 0.0f;
         return;
     } 
     else 
     {
         float angle = (float)Math.Acos(diag);
         _x = R[2,1] - R[1,2];
         _y = R[0,2] - R[2,0];
         _z = R[1,0] - R[0,1];
         float axisnorm = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
         if (axisnorm < 1e-8) 
         {
             // Axis is undefined
             _x = _y = _z = 0.0f;
             return;
         } 
         else 
         {
             // All OK, so scale it
             float scale = angle/axisnorm;
             _x *= scale;
             _y *= scale;
             _z *= scale;
         }
     }
 }