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 );
}
}
