public bool SettingAsReferenceFrameWillCreateALoop( Frame frame )
{
Frame f = frame;
while( f != null )
{
if( f == this )
{
return true;
}
f = f.ReferenceFrame;
}
return false;
}
/// <summary>
/// Screen (pixel) to world
/// </summary>
/// <param name="src"></param>
/// <param name="frame"></param>
/// <returns></returns>
public Vector3f UnprojectedCoordinatesOf( Vector3f src, Frame frame )
{
float x = 2 * src.x / ScreenSize.x - 1;
float y = 2 * src.y / ScreenSize.y - 1;
float z = src.z;
var clip = new Vector4f( x, y, z, 1 );
var eye = InverseProjectionMatrix * clip;
var world = ( InverseViewMatrix * eye ).Homogenized();
if( frame != null )
{
return Frame.CoordinatesOf( world );
}
else
{
return world;
}
}
/// <summary>
/// World to screen (pixel)
/// </summary>
/// <param name="src"></param>
/// <param name="frame"></param>
/// <returns></returns>
public Vector3f ProjectedCoordinatesOf( Vector3f src, Frame frame )
{
Vector3f tmp;
if( frame != null )
{
tmp = frame.InverseCoordinatesOf( src );
}
else
{
tmp = src;
}
Vector4f clip = ProjectionViewMatrix * new Vector4f( tmp, 1.0f );
Vector3f ndc = clip.Homogenized();
float sx = ScreenSize.x * 0.5f * ( ndc.x + 1.0f );
float sy = ScreenSize.y * 0.5f * ( ndc.y + 1.0f );
// OpenGL
// float sz = 0.5f * ( ndc.Z + 1.0f );
// D3D
float sz = ndc.z;
// float w = 1;
return new Vector3f( sx, sy, sz );
}
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 );
}
}
public Frame( Frame frame )
{
SetTranslationAndRotation( frame.Translation, frame.Rotation );
// SetConstraint( frame.Constraint )
ReferenceFrame = frame.ReferenceFrame;
}
public void AlignWithFrame( Frame frame, bool move )
{
// TODO: default_threshold
AlignWithFrame( frame, move, 0.85f );
}
public void AlignWithFrame( Frame frame )
{
AlignWithFrame( frame, false );
}
public Frame( Vector3f translation, Quat4f rotation )
{
this.Translation = translation;
this.Rotation = rotation;
referenceFrame = null;
}
public Frame Inverse()
{
Frame inverseFrame = new Frame( -( Rotation.InverseRotate( Translation ) ), Rotation.Inverse() );
inverseFrame.ReferenceFrame = referenceFrame;
return inverseFrame;
}
public Frame()
{
t_ = Vector3f.Zero;
q_ = Quat4f.Identity;
referenceFrame = null;
}