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