com.actelion.research.chem.alignment3d.transformation.RotationDerivatives Maven / Gradle / Ivy
Go to download
Show more of this group Show more artifacts with this name
Show all versions of openchemlib Show documentation
Show all versions of openchemlib Show documentation
Open Source Chemistry Library
package com.actelion.research.chem.alignment3d.transformation;
/**
* Rotation derivatives for rotations expressed by means of the exponential map
* http://www.cs.cmu.edu/~spiff/exp-map/
* Grassi,98
* --> we take the transpose because of different convention!
*/
public class RotationDerivatives {
private double[] v;
private double theta;
private double cosp;
private double sinp = Math.sin(.5*theta);
Quaternion q;
public RotationDerivatives(double[] v) {
this.v = v;
theta = Math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]);
cosp = Math.cos(.5*theta);
sinp = Math.sin(.5*theta);
ExponentialMap em = new ExponentialMap(v[0],v[1],v[2]);
q = em.toQuaternion();
}
/* -----------------------------------------------------------------
* 'Partial_R_Partial_Vi' Given a quaternion 'q' computed from the
* current 2 or 3 degree of freedom EM vector 'v', and the partial
* derivative of the quaternion with respect to the i'th element of
* 'v' in 'dqdvi' (computed using 'Partial_Q_Partial_3V' or
* 'Partial_Q_Partial_2V'), compute and store in 'dRdvi' the i'th
* partial derivative of the rotation matrix 'R' with respect to the
* i'th element of 'v'.
* -----------------------------------------------------------------*/
void dRdvi(Quaternion dqdvi, double[][] dRdvi){
double[] prod = new double[9];
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
prod[0] = -4*q.getQ1()*dqdvi.getQ1();
prod[1] = -4*q.getQ2()*dqdvi.getQ2();
prod[2] = -4*q.getQ3()*dqdvi.getQ3();
prod[3] = 2*(q.getQ2()*dqdvi.getQ1() + q.getQ1()*dqdvi.getQ2());
prod[4] = 2*(q.getQ0()*dqdvi.getQ3() + q.getQ3()*dqdvi.getQ0());
prod[5] = 2*(q.getQ3()*dqdvi.getQ1() + q.getQ1()*dqdvi.getQ3());
prod[6] = 2*(q.getQ0()*dqdvi.getQ2() + q.getQ2()*dqdvi.getQ0());
prod[7] = 2*(q.getQ3()*dqdvi.getQ2() + q.getQ2()*dqdvi.getQ3());
prod[8] = 2*(q.getQ0()*dqdvi.getQ1() + q.getQ1()*dqdvi.getQ0());
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
dRdvi[1][0] = prod[3] - prod[4];
dRdvi[2][0] = prod[5] + prod[6];
dRdvi[0][1] = prod[3] + prod[4];
dRdvi[1][1] = prod[0] + prod[2];
dRdvi[2][1] = prod[7] - prod[8];
dRdvi[0][2] = prod[5] - prod[6];
dRdvi[1][2] = prod[7] + prod[8];
dRdvi[2][2] = prod[0] + prod[1];
/* the 4th row and column are all zero */
}
public void dRdv(int i, double[][] dRdvi){
Quaternion dqdvi;
ExponentialMap em = new ExponentialMap(v[0],v[1],v[2]);
q = em.toQuaternion();
dqdvi = dqdvi(i);
dRdvi(dqdvi,dRdvi);
}
/**
* derivatives of the quaternions with respect to the exponential map representation (vector p):
* described in: F. Sebastian Grassia (1998) Practical Parameterization of Rotations Using the
* Exponential Map, Journal of Graphics Tools, 3:3, 29-48, DOI: 10.1080/10867651.1998.10487493
* @param p0
* @param p1
* @param p2
* @return
*/
private Quaternion dqdvi(int i)
{
double w;
double[] xyz = new double[3];
assert(i>=0 && i<3);
/* This is an efficient implementation of the derivatives given
* in Appendix A of the paper with common subexpressions factored out */
if (theta < ExponentialMap.EPSILON){
final int i2 = (i+1)%3;
final int i3 = (i+2)%3;
double Tsinc = 0.5 - theta*theta/48.0;
double vTerm = v[i] * (theta*theta/40.0 - 1.0) / 24.0;
w = -.5*v[i]*Tsinc;
xyz[i] = v[i]* vTerm + Tsinc;
xyz[i2] = v[i2]*vTerm;
xyz[i3] = v[i3]*vTerm;
}
else{
final int i2 = (i+1)%3;
final int i3 = (i+2)%3;
final double ang = 1.0/theta;
final double ang2 = ang*ang*v[i];
final double sang = sinp*ang;
final double cterm = ang2*(.5*cosp - sang);
xyz[i] = cterm*v[i] + sang;
xyz[i2] = cterm*v[i2];
xyz[i3] = cterm*v[i3];
w = -.5*v[i]*sang;
}
return new Quaternion(w,xyz[0],xyz[1],xyz[2]);
}
}