using BulletX.LinerMath;

namespace BulletX.BulletCollision.CollisionDispatch
{
    struct PerturbedContactResult
    {
        //ManifoldResult* m_originalManifoldResult;
        btTransform m_transformA;
        btTransform m_transformB;
        btTransform m_unPerturbedTransform;
        bool m_perturbA;
        IDebugDraw m_debugDrawer;


        public PerturbedContactResult(/*ManifoldResult* originalResult,*/ btTransform transformA, btTransform transformB, btTransform unPerturbedTransform, bool perturbA, IDebugDraw debugDrawer)
        {
            //m_originalManifoldResult = originalResult;
            m_transformA = transformA;
            m_transformB = transformB;
            m_unPerturbedTransform = unPerturbedTransform;
            m_perturbA = perturbA;
            m_debugDrawer = debugDrawer;

        }
        public void addContactPoint(btVector3 normalOnBInWorld,btVector3 pointInWorld,float orgDepth,ref ManifoldResult originalManifoldResult)
	    {
		    btVector3 endPt,startPt;
		    float newDepth;
		    //btVector3 newNormal;

		    if (m_perturbA)
		    {
			    btVector3 endPtOrg = pointInWorld + normalOnBInWorld*orgDepth;
                endPt = btVector3.Transform(endPtOrg, m_unPerturbedTransform * m_transformA.inverse());
			    newDepth = (endPt -  pointInWorld).dot(normalOnBInWorld);
			    startPt = endPt+normalOnBInWorld*newDepth;
		    } else
		    {
			    endPt = pointInWorld + normalOnBInWorld*orgDepth;
                startPt = btVector3.Transform(pointInWorld, m_unPerturbedTransform * m_transformB.inverse());
			    newDepth = (endPt -  startPt).dot(normalOnBInWorld);
    			
		    }
    		
		    originalManifoldResult.addContactPoint(ref normalOnBInWorld,ref startPt,newDepth);
	    }

    }
}
