public void AlignWithFrame( Frame frame, bool move, float threshold ) { Vector3f[,] directions = new Vector3f[ 2, 3 ]; for( int d = 0; d < 3; ++d ) { Vector3f dir = new Vector3f( ( d == 0 ) ? 1.0f : 0.0f, ( d == 1 ) ? 1.0f : 0.0f, ( d == 2 ) ? 1.0f : 0.0f ); if( frame != null ) { directions[ 0, d ] = frame.InverseTransformOf( dir ); } else { directions[ 0, d ] = dir; } directions[ 1, d ] = InverseTransformOf( dir ); } float maxProj = 0.0f; float proj; int[] index = new int[ 2 ]; index[ 0 ] = 0; index[ 1 ] = 0; for( int i = 0; i < 3; ++i ) { for( int j = 0; j < 3; ++j ) { proj = ( float )( Math.Abs( Vector3f.Dot( directions[ 0, i ], directions[ 1, j ] ) ) ); if( proj >= maxProj ) { index[ 0 ] = i; index[ 1 ] = j; maxProj = proj; } } } Frame old = new Frame( this ); float coef = Vector3f.Dot( directions[ 0, index[ 0 ] ], directions[ 1, index[ 1 ] ] ); if( Math.Abs( coef ) >= threshold ) { Vector3f axis = Vector3f.Cross( directions[ 0, index[ 0 ] ], directions[ 1, index[ 1 ] ] ); float angle = ( float )( Math.Asin( axis.Norm() ) ); if( coef >= 0.0f ) { angle = -angle; } // setOrientation(Quaternion(axis, angle) * orientation()); // commented out in original code Rotate( Rotation.Inverse() * new Quat4f( axis, angle ) * Orientation ); // Try to align an other axis direction int d = ( index[ 1 ] + 1 ) % 3; Vector3f dir = new Vector3f( ( d == 0 ) ? 1.0f : 0.0f, ( d == 1 )? 1.0f : 0.0f, ( d == 2 ) ? 1.0f : 0.0f ); dir = InverseTransformOf( dir ); float max = 0.0f; for (int i=0; i<3; ++i) { float proj2 = Math.Abs( Vector3f.Dot( directions[ 0, i ], dir ) ); if( proj2 > max ) { index[ 0 ] = i; max = proj2; } } if( max >= threshold ) { Vector3f axis2 = Vector3f.Cross( directions[ 0, index[ 0 ] ], dir); float angle2 = ( float )( Math.Asin( axis2.Norm() ) ); if( Vector3f.Dot( directions[ 0, index[ 0 ] ], dir ) >= 0.0f ) { angle2 = -angle2; } // setOrientation(Quaternion(axis, angle) * orientation()); // commented out in original source Rotate( Rotation.Inverse() * new Quat4f( axis2, angle2 ) * Orientation ); } } if( move ) { Vector3f center = Vector3f.Zero; if( frame != null ) { center = frame.Position; } // setPosition(center - orientation().rotate(old.coordinatesOf(center))); // commented out in original source Vector3f v = old.CoordinatesOf( center ); Vector3f foo = Orientation.Rotate( v ); // Translate( center - Orientation.Rotate( old.CoordinatesOf( center ) ) - Translation ); } }