1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43
|
auto prng = RandomGenerator(std::chrono::high_resolution_clock::now().time_since_epoch().count());
const float delta = 1.22173f; //( 180 - 110 ) * PI / 180.0 = 1.22173
const float pi = 3.14159f;
Pt ex = { 1, 0, 0 }, ey = { 0, 1, 0 }, ez = { 0, 0, 1 };
float px, py, pz, ptheta;
previous_rot.get_rotation( px, py, pz, ptheta );
Pt previous = { px, py, pz }; // previous ROTATION axis
// Convert it to vector which is the rotated z axis
Mat R = rotationMatrix( previous, ptheta );
previous = R * ez; // Should be the alignment of the monomer
Pt perp = cross(previous, ex);
if (len(perp) < 0.01) perp = cross(previous, ey);
Mat R = rotationMatrix( previous, prng.get_float( 0.0f, 2 * pi ) );
perp = R * perp;
previous = previous / len(previous); // normalization to length 1
perp = perp / len(perp); // ditto
Pt axis = cos(delta) * previous + sin(delta) * perp; // new rotated z axis
Pt edge = sin(delta) * previous - cos(delta) * perp;
R = rotationMatrix( axis, prng.get_float( 0.0f, 2 * pi ) );
edge = R * edge;
Pt third = cross(axis, edge);
/* Rot(ex) Rot(ey) Rot(ez)
( | | | )
M = ( edge third axis )
( | | | ) */
Mat M = { { edge.x, third.x, axis.x },
{ edge.y, third.y, axis.y },
{ edge.z, third.z, axis.z }};
Pt monomer_axis;
float new_theta;
axisAngle( M, monomer_axis, new_theta ); // careful with float or double for new_theta
return ExponentialMap( monomer_axis.x, monomer_axis.y, monomer_axis.z, new_theta );
|