void CRRCMath::Quaternion_002::init(CRRCMath::Vector3 eulerAngle) { // Aus [1], ab (2.9) Matrix33 A = initFromEuler(eulerAngle); e3.init(0.5*sqrt(1 + A.v[0][0] + A.v[1][1] + A.v[2][2] ), 0); if (e3.val != 0) { double d = 1.0/(4*e3.val); e0.init(d*(A.v[1][2] - A.v[2][1]), 0); e1.init(d*(A.v[2][0] - A.v[0][2]), 0); e2.init(d*(A.v[0][1] - A.v[1][0]), 0); } else { e0.init(0.5*sqrt(1 + A.v[0][0] - A.v[1][1] - A.v[2][2]), 0); e1.init(0.5*sqrt(1 - A.v[2][2] - 2 * e0.val*e0.val), 0); e2.init(0.5*sqrt(1 - A.v[1][1] - 2 * e0.val*e0.val), 0); } update_mat(); updateEuler(); }
void Crotation::print(const bool rd) { //update things if ( rStatus & OLD_EULER ) updateEuler(); if ( rStatus & OLD_MATRIX ) updateMatrix(); if ( rStatus & OLD_QUATERNION ) updateQuaternion(); //print them ! std::cout << "euler(hpr): [" << ea[0].getAngle() << "," << ea[1].getAngle() << "," << ea[2].getAngle() << "]" << std::endl; std::cout << "matrix: [" << rM(0,0) << "," << rM(0,1) << "," << rM(0,2) << " | " << rM(1,0) << "," << rM(1,1) << "," << rM(1,2) << " | " << rM(2,0) << "," << rM(2,1) << "," << rM(2,2) << "]" << std::endl; std::cout << "quaternion: [" << qt.qq(0) << "," << qt.qq(1) << "," << qt.qq(2) << "," << qt.qq(3) << "]" << std::endl; }
//**************************************************** // Force Calculation Functions // - Use Euler's Methods to update Position & // Velocity //**************************************************** void Vertex::update(float timeChange, bool euler) { if(firstUpdate) { oldTimeChange = timeChange; // Gives a 1.0 ratio firstUpdate = false; } // updateInternal(); if(!fixed) { if(euler) { updateEuler(timeChange); } else { updateVerlet(timeChange); } } // Make sure this is correct this->acceleration = glm::vec3(0,0,0); }
void CRRCMath::Quaternion_003::init(CRRCMath::Vector3 eulerAngle) { double sphi = sin(0.5*eulerAngle.r[0]); double cphi = cos(0.5*eulerAngle.r[0]); double stheta = sin(0.5*eulerAngle.r[1]); double ctheta = cos(0.5*eulerAngle.r[1]); double spsi = sin(0.5*eulerAngle.r[2]); double cpsi = cos(0.5*eulerAngle.r[2]); e0.init(+cpsi*ctheta*cphi +spsi*stheta*sphi, 0); e1.init(+cpsi*ctheta*sphi -spsi*stheta*cphi, 0); e2.init(+cpsi*stheta*cphi +spsi*ctheta*sphi, 0); e3.init(-cpsi*stheta*sphi +spsi*ctheta*cphi, 0); update_mat(); // std::cout << "Quaternion::init mat=\n"; // mat.print(); updateEuler(); // eulerAngle.print("Quaternion::init Euler=", "\n"); }
double Crotation::roll(bool rd) { if ( rStatus & OLD_EULER ) updateEuler(); return ea[2].getAngle(rd); }