Beispiel #1
0
 public Vector3D InverseRotate(Vector3D v)
 {
     return
         (new Vector3D(this[0, 0] * v.GetX() + this[1, 0] * v.GetY() + this[2, 0] * v.GetZ(),
                       this[0, 1] * v.GetX() + this[1, 1] * v.GetY() + this[2, 1] * v.GetZ(),
                       this[0, 2] * v.GetX() + this[1, 2] * v.GetY() + this[2, 2] * v.GetZ()));
 }
Beispiel #2
0
        public float Multiply(LineSeg3D ls1, LineSeg3D ls2)
        {
            Vector3D d1 = new Vector3D(ls1._ep - ls1._sp);
            Vector3D d2 = new Vector3D(ls2._ep - ls2._sp);

            return(d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ());
        }
Beispiel #3
0
        /// <summary>
        /// Note that the axis doesn't have to be a unit vector.
        /// </summary>
        /// <param name="axis">the axis around which to rotate</param>
        /// <param name="angle">the angle of rotation in radians</param>
        public void Set(Vector3D axis, float angle)
        {
            // start with an identity matrix.
            this.SetIdentity();

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

            // normalize to a unit vector u
            float[] u = new float[3];
            float   norm;

            norm = 1.0f / axis.Norm();
            u[0] = axis.GetX() * norm;
            u[1] = axis.GetY() * norm;
            u[2] = axis.GetZ() * norm;

            // add (cos(angle)-1)*(1 - u u').
            float cos_angle = (float)Math.Cos(angle);

            for (uint i = 0; i < 3; ++i)
            {
                for (uint j = 0; j < 3; ++j)
                {
                    this[(int)i, (int)j] += (cos_angle - 1) * ((i == j ? 1:0) - u[i] * u[j]);
                }
            }

            // add sin(angle) * [u]
            float sin_angle = (float)Math.Sin(angle);

            this[0, 1] -= sin_angle * u[2];
            this[0, 2] += sin_angle * u[1];
            this[1, 0] += sin_angle * u[2];
            this[1, 2] -= sin_angle * u[0];
            this[2, 0] -= sin_angle * u[1];
            this[2, 1] += sin_angle * u[0];
        }
Beispiel #4
0
        //returns matrix of dimensions (3,4)
        public static MatrixFixed dRq_times_a_by_dq (Quaternion q, Vector3D a)
        {
            MatrixFixed aMat = new MatrixFixed(3,1);
            aMat[0, 0] = a.GetX(); 
            aMat[1, 0] = a.GetY(); 
            aMat[2, 0] = a.GetZ(); 

            MatrixFixed TempR = new MatrixFixed(3,3);
            MatrixFixed Temp31 = new MatrixFixed(3,1);
            MatrixFixed dRq_times_a_by_dq = new MatrixFixed(3,4);

            // Make Jacobian by stacking four vectors together side by side
            TempR = dR_by_dq0(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 0);

            TempR = dR_by_dqx(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 1);

            TempR = dR_by_dqy(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 2);

            TempR = dR_by_dqz(q);
            Temp31 = TempR * aMat;
            dRq_times_a_by_dq.Update(Temp31, 0, 3);

            return dRq_times_a_by_dq;
        }
Beispiel #5
0
        public static MatrixFixed dvnorm_by_dv(Vector3D v)
        {
            float vv = v.GetX()*v.GetX() + v.GetY()*v.GetY() + v.GetZ()*v.GetZ();

            // We can use dqi_by_dqi and dqi_by_dqj functions because they are the same
            float[] a = new float[9];
            a[0] = dqi_by_dqi(v.GetX(), vv);
            a[1] = dqi_by_dqj(v.GetX(), v.GetY(), vv);
            a[2] = dqi_by_dqj(v.GetX(), v.GetZ(), vv);

            a[3] = dqi_by_dqj(v.GetY(), v.GetX(), vv);
            a[4] = dqi_by_dqi(v.GetY(), vv);
            a[5] = dqi_by_dqj(v.GetY(), v.GetZ(), vv);

            a[6] = dqi_by_dqj(v.GetZ(), v.GetX(), vv);
            a[7] = dqi_by_dqj(v.GetZ(), v.GetY(), vv);
            a[8] = dqi_by_dqi(v.GetZ(), vv);

            MatrixFixed M = new MatrixFixed(3,3);
            return M;
        }
Beispiel #6
0
        public float Multiply(LineSeg3D ls1, LineSeg3D ls2) 
        {
            Vector3D d1 = new Vector3D(ls1._ep - ls1._sp);
            Vector3D d2 = new Vector3D(ls2._ep - ls2._sp);

            return (d1.GetX() * d2.GetX() + d1.GetY() * d2.GetY() + d1.GetZ() * d2.GetZ());
        }
Beispiel #7
0
        /// <summary>
        /// turns a postion and quaternion based orientation into a matrix format
        /// </summary>
        /// <param name="position"></param>
        /// <param name="orientation"></param>
        /// <returns></returns>
        private Matrix QuaternionToMatrixLookAt(Vector3D position, SceneLibrary.Quaternion orientation)
        {
            Microsoft.DirectX.Quaternion q_orientation = 
                new Microsoft.DirectX.Quaternion((float)orientation.GetX(),
                                                 (float)orientation.GetY(),
                                                 (float)orientation.GetZ(),
                                                 (float)orientation.GetR());
            Matrix _matrixRotation = Matrix.RotationQuaternion(q_orientation);
            Vector3 camPos = new Vector3((float)position.GetX(), -(float)position.GetY(), (float)position.GetZ());
            Vector3 camTY = new Vector3(_matrixRotation.M21, _matrixRotation.M22, _matrixRotation.M23);
            Vector3 camTZ = new Vector3(-(_matrixRotation.M31), -(_matrixRotation.M32), -(_matrixRotation.M33));
            return (Matrix.LookAtLH(camPos, camPos + camTZ, camTY));

            //float yaw = 0, pitch = 0, roll = 0;
            //DecomposeRollPitchYawZXYMatrix(_matrixRotation, ref pitch, ref yaw, ref roll);

            //return (Matrix.RotationYawPitchRoll((float)yaw, (float)pitch, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
            //return (Matrix.RotationYawPitchRoll(0, 0, (float)roll) * Matrix.LookAtLH(camPos, camPos + camTZ, camTY));
        }
Beispiel #8
0
        /// <summary>
        /// Calculate commonly used Jacobian part  \partial q(\omega * \Delta t) / \partial \omega .
        /// </summary>
        /// <param name="omega"></param>
        /// <param name="delta_t"></param>
        /// <param name="dqomegadt_by_domega"></param>
        public void dqomegadt_by_domega(Vector3D omega, float delta_t, MatrixFixed dqomegadt_by_domega)
        {
            // Modulus
            float omegamod = (float)Math.Sqrt(omega.GetX() * omega.GetX() + 
			                  omega.GetY() * omega.GetY() + 
			                  omega.GetZ() * omega.GetZ());

            // Use generic ancillary functions to calculate components of Jacobian  
            dqomegadt_by_domega.Put(0, 0, dq0_by_domegaA(omega.GetX(), omegamod, delta_t));
            dqomegadt_by_domega.Put(0, 1, dq0_by_domegaA(omega.GetY(), omegamod, delta_t));
            dqomegadt_by_domega.Put(0, 2, dq0_by_domegaA(omega.GetZ(), omegamod, delta_t));
            dqomegadt_by_domega.Put(1, 0, dqA_by_domegaA(omega.GetX(), omegamod, delta_t));
            dqomegadt_by_domega.Put(1, 1, dqA_by_domegaB(omega.GetX(), omega.GetY(), omegamod, delta_t));
            dqomegadt_by_domega.Put(1, 2, dqA_by_domegaB(omega.GetX(), omega.GetZ(), omegamod, delta_t));
            dqomegadt_by_domega.Put(2, 0, dqA_by_domegaB(omega.GetY(), omega.GetX(), omegamod, delta_t));
            dqomegadt_by_domega.Put(2, 1, dqA_by_domegaA(omega.GetY(), omegamod, delta_t));
            dqomegadt_by_domega.Put(2, 2, dqA_by_domegaB(omega.GetY(), omega.GetZ(), omegamod, delta_t));
            dqomegadt_by_domega.Put(3, 0, dqA_by_domegaB(omega.GetZ(), omega.GetX(), omegamod, delta_t));
            dqomegadt_by_domega.Put(3, 1, dqA_by_domegaB(omega.GetZ(), omega.GetY(), omegamod, delta_t));
            dqomegadt_by_domega.Put(3, 2, dqA_by_domegaA(omega.GetZ(), omegamod, delta_t));
        }
Beispiel #9
0
        public Vector3D InverseRotate(Vector3D v)
        {
            return 
                new Vector3D(this[0,0]*v.GetX()+this[1,0]*v.GetY()+this[2,0]*v.GetZ(),
		                     this[0,1]*v.GetX()+this[1,1]*v.GetY()+this[2,1]*v.GetZ(),
              		         this[0,2]*v.GetX()+this[1,2]*v.GetY()+this[2,2]*v.GetZ());		      
        }
Beispiel #10
0
        /// <summary>
        /// Note that the axis doesn't have to be a unit vector.
        /// </summary>
        /// <param name="axis">the axis around which to rotate</param>
        /// <param name="angle">the angle of rotation in radians</param>
        public void Set(Vector3D axis, float angle)
        {
            // start with an identity matrix.
            this.SetIdentity();

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

            // normalize to a unit vector u
            float[] u = new float[3];
            float norm;
            norm = 1.0f / axis.Norm();
            u[0]=axis.GetX()*norm;
            u[1]=axis.GetY()*norm;
            u[2]=axis.GetZ()*norm;
  
            // add (cos(angle)-1)*(1 - u u').
            float cos_angle = (float)Math.Cos(angle);
            for (uint i=0; i<3; ++i)
                for (uint j=0; j<3; ++j)
                    this[(int)i,(int)j] += (cos_angle-1) * ((i==j ? 1:0) - u[i]*u[j]);

            // add sin(angle) * [u]
            float sin_angle = (float)Math.Sin(angle);
            this[0,1] -= sin_angle*u[2]; 
            this[0,2] += sin_angle*u[1];
            this[1,0] += sin_angle*u[2];
            this[1,2] -= sin_angle*u[0];
            this[2,0] -= sin_angle*u[1]; 
            this[2,1] += sin_angle*u[0];
        }
Beispiel #11
0
        public override void func_xpredef_and_dxpredef_by_dxp_and_dxpredef_by_dxpdef  
                    (Vector xp, Vector xpdef)
        {
            // Split position state into cartesian and quaternion

            // r0, q0: position in original frame
            func_r(xp);
            func_q(xp);
            Vector3D r0 = rRES;
            Quaternion q0 = qRES;

            // rn, qn: definition of new frame
            func_r(xpdef);
            func_q(xpdef);
            Vector3D rn = rRES;
            Quaternion qn = qRES;

            Quaternion qnbar = qn.Conjugate();

            // Calculate new cartesian part of xp
            Vector3D Temp31a = new Vector3D(r0 - rn); // Temp31a is r0 - rn
            RotationMatrix Temp33a = qnbar.RotationMatrix();
            Vector3D Temp31b = new Vector3D(Temp33a * Temp31a);

            // Copy into xpredefRES
            xpredefRES[0] = Temp31b.GetX();
            xpredefRES[1] = Temp31b.GetY();
            xpredefRES[2] = Temp31b.GetZ();

            // Calculate new quaternion part of xp
            Quaternion Tempqa = qnbar.Multiply(q0);

            // Copy into xpredefRES
            Vector vecTempqa = Tempqa.GetRXYZ();
            xpredefRES.Update(vecTempqa, 3);

            // Form Jacobian dxpredef_by_dxpRES
            dxpredef_by_dxpRES.Fill(0.0f);
            dxpredef_by_dxpRES.Update(Temp33a, 0, 0);

            MatrixFixed Temp44a = MatrixFixed.dq3_by_dq1(qnbar);
            dxpredef_by_dxpRES.Update(Temp44a, 3, 3);

            // Form Jacobian dxpredef_by_dxpdefRES
            dxpredef_by_dxpdefRES.Fill(0.0f);
            //Temp33a *= -1.0f;
            Temp33a = (RotationMatrix)(Temp33a  * - 1.0f);
            dxpredef_by_dxpdefRES.Update(Temp33a, 0, 0);

            Temp44a = MatrixFixed.dq3_by_dq2(q0);

            MatrixFixed Temp44b = MatrixFixed.dqbar_by_dq();
            MatrixFixed Temp44c = Temp44a * Temp44b;
            dxpredef_by_dxpdefRES.Update(Temp44c, 3, 3);

            // Top right corner of this Jacobian is tricky because we have to
            // differentiate a rotation matrix
            // Uses function that does this in bits.cc
            // Build this corner in Temp34a
            //MatrixFixed temp = new MatrixFixed(3,4);
            MatrixFixed Temp34a = MatrixFixed.dRq_times_a_by_dq(qnbar, Temp31a.GetVNL3());

            // So far we have drN_by_dqnbar; want _by_dqn
            MatrixFixed Temp34b = Temp34a * Temp44b;

            // Finally copy into result matrix
            dxpredef_by_dxpdefRES.Update(Temp34b, 0, 3);

            // cout << "dxpredef_by_dxpdefRES" << dxpredef_by_dxpdefRES;
        }
Beispiel #12
0
 /// <summary>
 ///
 /// </summary>
 /// <param name="angleaxis">the 3D vector whose size gives the angle and direction</param>
 public void Set(Vector3D angleaxis)
 {
     _x = angleaxis.GetX();
     _y = angleaxis.GetY();
     _z = angleaxis.GetZ();
 }
Beispiel #13
0
 /// <summary>
 ///
 /// </summary>
 /// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
 /// <param name="angle">the rotation angle in radians</param>
 public void Set(Vector3D axis, float angle)
 {
     _x = angle * axis.GetX();
     _y = angle * axis.GetY();
     _z = angle * axis.GetZ();
 }
Beispiel #14
0
 /// <summary>
 /// 
 /// </summary>
 /// <param name="angleaxis">the 3D vector whose size gives the angle and direction</param>
 public void Set(Vector3D angleaxis)
 {
     _x = angleaxis.GetX();
     _y = angleaxis.GetY();
     _z = angleaxis.GetZ();
 } 
Beispiel #15
0
 /// <summary>
 /// 
 /// </summary>
 /// <param name="axis">the axis (doesn't need to be a unit vector) around which to rotate</param>
 /// <param name="angle">the rotation angle in radians</param>
 public void Set(Vector3D axis, float angle)
 {
     _x = angle * axis.GetX();
     _y = angle * axis.GetY();
     _z = angle * axis.GetZ();
 }