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);
}
