RHCoordSys3 RHCoordSys3::Rotate3a(Double Azimuth,Double Tilt,Double AxialRot)
// spits out a copy, leaving original CS unchanged
{
// rotation angles - radians
// rotation matrixes R1 etc always left-multiply vector!
vector3 xx = cs3[0], zz = cs3[2];
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
// Rotation 1
if (Azimuth) {
xx = Rot3(zz, Azimuth)*xx; // rotation is around ICS_z
}
// Rotation 2
// Tilt = 0, PI are special cases that leave or flip the surface in the x-y plane
if (Tilt) {
zz = Rot3(xx, Tilt)*zz; // rotation is around x1
}
// Rotation 3
if (AxialRot) {
xx = Rot3(zz, AxialRot)*xx; // rotation is around z2 (= surface normal)
}
return RHCoordSys3(xx,cross(zz,xx),zz);
}
/* ************************************************************************* */
Rot3 Rot3::Rz(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
ct,-st, 0,
st, ct, 0,
0, 0, 1);
}
/* ************************************************************************* */
Rot3 Rot3::Ry(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
ct, 0, st,
0, 1, 0,
-st, 0, ct);
}
/* ************************************************************************* */
Rot3 Rot3::Rx(double t) {
double st = sin(t), ct = cos(t);
return Rot3(
1, 0, 0,
0, ct,-st,
0, st, ct);
}
void timer (int dummy)
{
glutTimerFunc (100, timer, 0);
point += Rot3 (Vec3(0,1,0),angle/180*vl_pi) * Vec3(1,0,0)*speed/10;
pose_fsm();
glutPostRedisplay();
}
RHCoordSys3 RHCoordSys3::Rotate3(Double Azimuth,Double Tilt,Double AxialRot)
// spits out a copy, leaving original CS unchanged
{
// rotation angles - radians
// rotation matrixes R1 etc always left-multiply vector!
vector3 x1, z2;
matrix3 R1, R2, R3, Rtot;
// Rotation 1
if (Azimuth) {
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
R1 = Rot3(cs3[2], Azimuth); // rotation is around ICS_z
x1 = R1*cs3[0];
// z1 = cs3[2]; = ICS_z
}
else {
R1 = vl_I;
x1 = cs3[0];
}
// Rotation 2
// Tilt = 0, PI are special cases that leave or flip the surface in the x-y plane
if (Tilt) {
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
R2 = Rot3(x1, Tilt); // rotation is around x1
z2 = R2*cs3[2]; //z2 = R2*z1; z1 = ICS_z;
}
else {
R2 = vl_I;
z2 = cs3[2];
}
// Rotation 3
if (AxialRot) {
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
R3 = Rot3(z2, AxialRot); // rotation is around z2 (= surface normal)
}
else R3 = vl_I;
// composite rotation
Rtot = R3*R2*R1; //order is important
return RHCoordSys3(Rtot*cs3[0],Rtot*cs3[1],Rtot*cs3[2]);
}
/* ************************************************************************* */
Rot3 Rot3::CayleyChart::Retract(const Vector3& omega, OptionalJacobian<3,3> H) {
if (H) throw std::runtime_error("Rot3::CayleyChart::Retract Derivative");
const double x = omega(0), y = omega(1), z = omega(2);
const double x2 = x * x, y2 = y * y, z2 = z * z;
const double xy = x * y, xz = x * z, yz = y * z;
const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f;
return Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f,
(xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f,
(xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f);
}
RHCoordSys3 RHCoordSys3::Rotate1(const vector3& axis, Double angle)
// spits out a copy, leaving original CS unchanged
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
{
RHCoordSys3 csTemp;
// Rot3 NOTE: vector3 "axis" MUST BE UNIT LENGTH
matrix3 R1 = Rot3(axis,angle);
// cout << R1 << "\n";
csTemp[0] = R1*cs3[0];
csTemp[1] = R1*cs3[1];
csTemp[2] = R1*cs3[2];
return csTemp;
}
// Considerably faster than composing matrices above !
Rot3 Rot3::RzRyRx(double x, double y, double z) {
double cx=cos(x),sx=sin(x);
double cy=cos(y),sy=sin(y);
double cz=cos(z),sz=sin(z);
double ss_ = sx * sy;
double cs_ = cx * sy;
double sc_ = sx * cy;
double cc_ = cx * cy;
double c_s = cx * sz;
double s_s = sx * sz;
double _cs = cy * sz;
double _cc = cy * cz;
double s_c = sx * cz;
double c_c = cx * cz;
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
return Rot3(
_cc,- c_s + ssc, s_s + csc,
_cs, c_c + sss, -s_c + css,
-sy, sc_, cc_
);
}
/* ************************************************************************* */
Rot3 Rot3::AlignPair(const Unit3& axis, const Unit3& a_p, const Unit3& b_p) {
// if a_p is already aligned with b_p, return the identity rotation
if (std::abs(a_p.dot(b_p)) > 0.999999999) {
return Rot3();
}
// Check axis was not degenerate cross product
const Vector3 z = axis.unitVector();
if (z.hasNaN())
throw std::runtime_error("AlignSinglePair: axis has Nans");
// Now, calculate rotation that takes b_p to a_p
const Matrix3 P = I_3x3 - z * z.transpose(); // orthogonal projector
const Vector3 a_po = P * a_p.unitVector(); // point in a orthogonal to axis
const Vector3 b_po = P * b_p.unitVector(); // point in b orthogonal to axis
const Vector3 x = a_po.normalized(); // x-axis in axis-orthogonal plane, along a_p vector
const Vector3 y = z.cross(x); // y-axis in axis-orthogonal plane
const double u = x.dot(b_po); // x-coordinate for b_po
const double v = y.dot(b_po); // y-coordinate for b_po
double angle = std::atan2(v, u);
return Rot3::AxisAngle(z, -angle);
}
void scene_timer ()
{
Food::self_rotation_angle += 2.0;
point += Rot3 (Vec3(0,1,0),angle/180*vl_pi) * Vec3(1,0,0)*speed/10;
pose_fsm();
}
/* ************************************************************************* */
Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_));
}
