basic/geometry/MPK_Segment.cpp

Go to the documentation of this file.

#include <math.h>
#include <math/math2.h>

#include "geometry\MPK_Sphere.h"
#include "geometry\MPK_Segment.h"

namespace MPK
{


Segment::Segment (FrameManager* frameManager)
  //## begin Segment::Segment%926539291.hasinit preserve=no
  //## end Segment::Segment%926539291.hasinit
  //## begin Segment::Segment%926539291.initialization preserve=yes
  :ObjectBase( frameManager )
  //## end Segment::Segment%926539291.initialization
{
  //## begin Segment::Segment%926539291.body preserve=yes
  //## end Segment::Segment%926539291.body
}

Segment::Segment (const Segment& right)
  //## begin Segment::Segment%926539292.hasinit preserve=no
  //## end Segment::Segment%926539292.hasinit
  //## begin Segment::Segment%926539292.initialization preserve=yes
  :ObjectBase( right ),
        startPos( right.startPos ),
        endPos( right.endPos )

  //## end Segment::Segment%926539292.initialization
{
  //## begin Segment::Segment%926539292.body preserve=yes
  //## end Segment::Segment%926539292.body
}


Segment::~Segment()
{
  //## begin Segment::~Segment%.body preserve=yes
  //## end Segment::~Segment%.body
}



//## Other Operations (implementation)
Entity* Segment::Clone () const
{
  //## begin Segment::Clone%926539287.body preserve=yes
        ObjectBase* returnme = new Segment( *this ) ;
        return returnme ;
  //## end Segment::Clone%926539287.body
}

bool Segment::IsInterfering (const Entity* entity) const
{
  //## begin Segment::IsInterfering%926539288.body preserve=yes
        assert( CanCheckInterference( entity ) ) ;      //IMPROVE: this should be error handled

        //determine the type of the entity
        if( dynamic_cast< const Segment* >( entity ) != NULL )
        {
                return( IsInterfering( dynamic_cast< const Segment* >( entity ) ) ) ;
        }
        else if( dynamic_cast< const MPK_Sphere* >( entity ) != NULL )
        {
                return( IsInterfering( dynamic_cast< const MPK_Sphere* >( entity ) ) ) ; ;
        }
        else
        {
                assert( false ) ;       //should never get to here due to the assert( CanCheckInterference )
                //IMPROVE: this assertion needs to be here
        }
        return false ;

  //## end Segment::IsInterfering%926539288.body
}

bool Segment::CanCheckInterference (const Entity* entity) const
{
  //## begin Segment::CanCheckInterference%926539289.body preserve=yes
        
        if( dynamic_cast< const Segment* >( entity ) != NULL )
        {
                return true ;
        }
        if( dynamic_cast< const MPK_Sphere* >( entity ) != NULL )
        {
                return true ;
        }
        return false ;

  //## end Segment::CanCheckInterference%926539289.body
}

bool Segment::IsInterfering (const Segment* entity) const
{
  //## begin Segment::IsInterfering%926539290.body preserve=yes
        Matrix4x4 relative = frameManager->GetTransformRelative( baseFrame, entity->baseFrame );
        Vector4 p1 = startPos ;
        Vector4 v1 = endPos - startPos ;
        double& v1x = v1[ 0 ] ;
        double& v1y = v1[ 1 ] ;
        double& v1z = v1[ 2 ] ;

        Vector4 q1 = entity->startPos ;
        Vector4 q2 = entity->endPos ;
        q1 = relative * q1 ;
        q2 = relative * q2 ;

        Vector4 p2 = q1 ;
        Vector4 v2 = q2 - q1 ;
        double& v2x = v2[ 0 ] ;
        double& v2y = v2[ 1 ] ;
        double& v2z = v2[ 2 ] ;

        Vector4 p = p2 - p1 ;
        Vector4 d = v1.Cross( v2 ) ;

        double& px = p[ 0 ] ;
        double& py = p[ 1 ] ;
        double& pz = p[ 2 ] ;
        double& dx = d[ 0 ] ;
        double& dy = d[ 1 ] ;
        double& dz = d[ 2 ] ;

        double magSquared = d.MagSquared() ;
        if( magSquared == 0 ) 
        {
                //lines are parallel
                
                //check for colinearity
                Vector4 endPoints = startPos - entity->startPos ;
                if( endPoints.Projection( v1 ).MagSquared() != v1.MagSquared() )
                {
                        //not colinear
                        return false ;
                }
                //need to project all the points onto a line
                double a1 = startPos.ProjectionMag( v1 ) ;
                double a2 = endPos.ProjectionMag( v1 ) ;
                double b1 = entity->startPos.ProjectionMag( v1 ) ;
                double b2 = entity->endPos.ProjectionMag( v1 ) ;

                //if either b1 or b2 is between a1 and a2 they overlap
                if( Between( a1, b1, b2 ) )     { return true ; }
                if( Between( a2, b1, b2 ) )     { return true ; }

                //if either a1 or a2 is between b1 and b2 they overlap
                if( Between( b1, a1, a2 ) )     { return true ; }
                if( Between( b2, a1, a2 ) )     { return true ; }
                return false ;
        }

        //lines are not parallel - they would intersect at a point
        double t =              ( px * v2y * dz ) + ( v2x * dy * pz ) + ( dx * py * v2z )
                                -       ( dx * v2y * pz ) - ( dy * v2z * px ) - ( dz * v2x * py );
        t /= magSquared ;
        if( t < 0 ) 
        {
                return false ;
        }
        if( t > 1 )
        {
                return false ;
        }

        double s =              ( px * v1y * dz ) + ( v1x * dy * pz ) + ( dx * py * v1z )
                                -       ( dx * v1y * pz ) - ( dy * v1z * px ) - ( dz * v1x * py );
        s /= magSquared ;
        if( s < 0 )
        {
                return false ;
        }
        if( s > 1 )
        {
                return false ;
        }

        assert( ( p1 + v1 * t ) == ( p2 + v2 * s ) ) ;
        return true ;
  //## end Segment::IsInterfering%926539290.body
}

void Segment::SetStart (const Vector4& start)
{
  //## begin Segment::SetStart%926539296.body preserve=yes
        startPos = start ;
  //## end Segment::SetStart%926539296.body
}

void Segment::SetEnd (const Vector4& end)
{
  //## begin Segment::SetEnd%926539297.body preserve=yes
        endPos = end ;
  //## end Segment::SetEnd%926539297.body
}

bool Segment::IsInterfering (const MPK_Sphere* entity) const
{
  //## begin Segment::IsInterfering%926709625.body preserve=yes
        //transform the sphere
        Matrix4x4 r1 = frameManager->GetTransformRelative( baseFrame, 0 );
        Matrix4x4 r2 = frameManager->GetTransformRelative( entity->BaseFrame(), 0 );
        Matrix4x4 relative = r1.Inverse() * r2 ;

        Vector4 center = entity->Position() ;
        center = relative * center ;
        Vector4 g = startPos - center ;         //this is the stert point relative to the center of the sphere 
        Vector4 lv = endPos - startPos ;
        double a = lv.Dot( lv ) ;
        double b = 2 * ( lv.Dot( g ) ) ;
        double c = ( g.Dot( g ) - pow( entity->Radius(), 2 ) ) ;
        double d = pow( b, 2 ) - 4 * a * c ;
        if( d < 0 )
        {
                return false ;
        }

        assert( ( 2 * a ) != 0 ) ;
        double p1 = ( -b + sqrt( d ) ) / ( 2 * a ) ;
        double p2 = ( -b - sqrt( d ) ) / ( 2 * a ) ;
        if( Between( p1, 0.0, 1.0 ) )
        {
                return true ;
        }
        if( Between( p2, 0.0, 1.0 ) )
        {
                return true ;
        }
        if( Between( 0.0, p1, p2 ) )
        {
                return true ;
        }
        assert( !Between( 1.0, p1, p2 ) ) ;     //Shouldn't have to check this
        return false ;
  //## end Segment::IsInterfering%926709625.body
}

void Segment::Deserialize (istream& is)
{
  //## begin Segment::Deserialize%934220968.body preserve=yes
        assert( false );
  //## end Segment::Deserialize%934220968.body
}

void Segment::Serialize (ostream& os) const
{
  //## begin Segment::Serialize%934220969.body preserve=yes
        assert( false );
  //## end Segment::Serialize%934220969.body
}


bool Segment::Verify() const
{
        return true;
}
// Additional Declarations
  //## begin Segment%3739DECC01C7.declarations preserve=yes
  //## end Segment%3739DECC01C7.declarations

//## begin module%3739DECC01C7.epilog preserve=yes
//## end module%3739DECC01C7.epilog
}

---