void Quaternion::AxisToQuat(const Vector3D &v, const float angle) { //float x,y,z; // temp vars of vector double rad, scale; // temp vars if (v.IsZero()) // if axis is zero, then return quaternion (1,0,0,0) { w = 1.0f; x = 0.0f; y = 0.0f; z = 0.0f; return; } assert(v.IsUnit()); // make sure the axis is a unit vector rad = angle / 2; w = (float)cos(rad); scale = sin(rad); //v.GetValues(x, y, z); this->x = float(v.x * scale); this->y = float(v.y * scale); this->z = float(v.z * scale); Normalize(); // make sure a unit quaternion turns up return; } // end void AxisToQuat(..)
void Quaternion::GetAxisAngle(Vector3D &v, float &angle) const { double temp_angle; // temp angle double scale; // temp vars temp_angle = acos(w); //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% // Another version where scale is sqrt (x2 + y2 + z2) //%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% scale = (float)sqrt(SQR(x) + SQR(y) + SQR(z)); // scale = (float)sin(temp_angle); assert(0 <= temp_angle); // make sure angle is 0 - PI assert(PI >= temp_angle); if (FLOAT_EQ(0.0f, scale)) // angle is 0 or 360 so just simply set axis to 0,0,1 with angle 0 { angle = 0.0f; //v.SetValues(0.0f, 0.0f, 1.0f); // any axis will do v.x = 0.0f; v.y = 0.0f; v.z = 0.0f; } else { angle = (float)(temp_angle * 2.0); // angle in radians //v.SetValues(float(x / scale), float(y / scale), float(z / scale)); v.x = float(x / scale); v.y = float(y / scale); v.z = float(z / scale); v.normalize(); assert(0.0f <= angle); // make sure rotation around axis is 0 - 360 assert(2*PI >= angle); assert(v.IsUnit()); // make sure a unit axis comes up } return; } // end void GetAxisAngle(..)