void  CheckContactPoint( hkpRigidBody* pRigidBody )

{

    hkStepInfo stepInfo;

    hkReal fTimestep = 1.0f / 60.0f;

    stepInfo.m_deltaTime = fTimestep;

    stepInfo.m_invDeltaTime = 1.0f / fTimestep;

 

    hkpLinkedCollidable* coll = pRigidBody->getLinkedCollidable();

    hkArray< struct  hkpLinkedCollidable::CollisionEntry > collisionEntriesTmp;

    coll->getCollisionEntriesSorted(collisionEntriesTmp);

    hkVector4 vel = pRigidBody->getLinearVelocity();

    hkVector4 angVel = pRigidBody->getAngularVelocity();

    const   hkArray< struct hkpLinkedCollidable::CollisionEntry >& collisionEntries = collisionEntriesTmp;

 

    static  int  nLine = 0;

    for ( int i = 0; i < collisionEntries.getSize(); ++i )

    {

        hkpRigidBody* rb = hkpGetRigidBody( collisionEntries[i].m_partner );

        if ( rb != HK_NULL )

        {

            //if ( collisionEntries[i].m_agentEntry->m_contactMgr->m_type == hkpContactMgr::TYPE_SIMPLE_CONSTRAINT_CONTACT_MGR )

            {

                hkpSimpleConstraintContactMgr* mgr = (hkpSimpleConstraintContactMgr*)(collisionEntries[i].m_agentEntry->m_contactMgr);

                if( mgr->m_contactConstraintData.getNumContactPoints() > 0 )

                {

                    hkContactPoint* contactPoints = mgr->m_contactConstraintData.m_atom->getContactPoints();

 

                    for( int  n = 0; n < mgr->m_contactConstraintData.getNumContactPoints(); ++n )

                    {

                        hkVector4 pos = contactPoints[n].getPosition();

                        hkVector4 nor = contactPoints[n].getNormal();

                        if( FloatEqual(vel(0), 0) && FloatEqual(vel(1), 0) && FloatEqual(vel(2), 0 )

                            && FloatEqual(angVel(0), 0) && FloatEqual(angVel(1), 0) && FloatEqual(angVel(2), 0) )

                            OutputDebugPrintf( "멈춘 상태 %f, %f, %f \n", vel(0), vel(1), vel(2) );

                        else

                            OutputDebugPrintf( "%d 속도 %f, %f, %f 충돌 포인터 %.2f, %.2f, %.2f\n",

                                nLine, vel(0), vel(1), vel(2),

                                nor(0), nor(1), nor(2) );

                        ++nLine;

                    }

                }

            }

        }

    }

}

#define FLOAT_EPSILON    1e-2f   

inline    float ABS(float a)        { return (a < 0 ? -a : a); }

 

bool FloatEqual( float a, float b )

{

    float k = FLOAT_EPSILON;

    return fabs( a - b ) < FLOAT_EPSILON;

}