private Matrix4d alignAxes ( Vector3d [ ] axisVectors , Vector3d [ ] referenceVectors ) { Matrix4d m1 = new Matrix4d ( ) ; AxisAngle4d a = new AxisAngle4d ( ) ; Vector3d axis = new Vector3d ( ) ; // calculate rotation matrix to rotate refPoints[0] into coordPoints[0] Vector3d v1 = new Vector3d ( axisVectors [ 0 ] ) ; Vector3d v2 = new Vector3d ( referenceVectors [ 0 ] ) ; double dot = v1 . dot ( v2 ) ; if ( Math . abs ( dot ) < 0.999 ) { axis . cross ( v1 , v2 ) ; axis . normalize ( ) ; a . set ( axis , v1 . angle ( v2 ) ) ; m1 . set ( a ) ; // make sure matrix element m33 is 1.0. It's 0 on Linux. m1 . setElement ( 3 , 3 , 1.0 ) ; } else if ( dot > 0 ) { // parallel axis, nothing to do -> identity matrix m1 . setIdentity ( ) ; } else if ( dot < 0 ) { // anti-parallel axis, flip around x-axis m1 . set ( flipX ( ) ) ; } // apply transformation matrix to all refPoints m1 . transform ( axisVectors [ 0 ] ) ; m1 . transform ( axisVectors [ 1 ] ) ; // calculate rotation matrix to rotate refPoints[1] into coordPoints[1] v1 = new Vector3d ( axisVectors [ 1 ] ) ; v2 = new Vector3d ( referenceVectors [ 1 ] ) ; Matrix4d m2 = new Matrix4d ( ) ; dot = v1 . dot ( v2 ) ; if ( Math . abs ( dot ) < 0.999 ) { axis . cross ( v1 , v2 ) ; axis . normalize ( ) ; a . set ( axis , v1 . angle ( v2 ) ) ; m2 . set ( a ) ; // make sure matrix element m33 is 1.0. It's 0 on Linux. m2 . setElement ( 3 , 3 , 1.0 ) ; } else if ( dot > 0 ) { // parallel axis, nothing to do -> identity matrix m2 . setIdentity ( ) ; } else if ( dot < 0 ) { // anti-parallel axis, flip around z-axis m2 . set ( flipZ ( ) ) ; } // apply transformation matrix to all refPoints m2 . transform ( axisVectors [ 0 ] ) ; m2 . transform ( axisVectors [ 1 ] ) ; // combine the two rotation matrices m2 . mul ( m1 ) ; // the RMSD should be close to zero Point3d [ ] axes = new Point3d [ 2 ] ; axes [ 0 ] = new Point3d ( axisVectors [ 0 ] ) ; axes [ 1 ] = new Point3d ( axisVectors [ 1 ] ) ; Point3d [ ] ref = new Point3d [ 2 ] ; ref [ 0 ] = new Point3d ( referenceVectors [ 0 ] ) ; ref [ 1 ] = new Point3d ( referenceVectors [ 1 ] ) ; if ( CalcPoint . rmsd ( axes , ref ) > 0.1 ) { logger . warn ( "AxisTransformation: axes alignment is off. RMSD: " + CalcPoint . rmsd ( axes , ref ) ) ; } return m2 ; }
Returns a transformation matrix that rotates refPoints to match coordPoints