Logo Search packages:      
Sourcecode: blender version File versions  Download package

PersistentManifold.cpp

/*
* Copyright (c) 2005 Erwin Coumans http://continuousphysics.com/Bullet/
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability 
* of this software for any purpose.  
* It is provided "as is" without express or implied warranty.
*/


#include "PersistentManifold.h"
#include "SimdTransform.h"
#include <assert.h>

float gContactBreakingTreshold = 0.02f;

PersistentManifold::PersistentManifold()
:m_body0(0),
m_body1(0),
m_cachedPoints (0),
m_index1(0)
{
}


void  PersistentManifold::ClearManifold()
{
      m_cachedPoints = 0;
}





00037 int PersistentManifold::SortCachedPoints(const ManifoldPoint& pt) 
{

            //calculate 4 possible cases areas, and take biggest area

            SimdScalar res0,res1,res2,res3;

            {
                  SimdVector3 a0 = pt.m_localPointA-m_pointCache[1].m_localPointA;
                  SimdVector3 b0 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
                  SimdVector3 cross = a0.cross(b0);
                  res0 = cross.length2();
            }
            {
                  SimdVector3 a1 = pt.m_localPointA-m_pointCache[0].m_localPointA;
                  SimdVector3 b1 = m_pointCache[3].m_localPointA-m_pointCache[2].m_localPointA;
                  SimdVector3 cross = a1.cross(b1);
                  res1 = cross.length2();
            }
            {
                  SimdVector3 a2 = pt.m_localPointA-m_pointCache[0].m_localPointA;
                  SimdVector3 b2 = m_pointCache[3].m_localPointA-m_pointCache[1].m_localPointA;
                  SimdVector3 cross = a2.cross(b2);
                  res2 = cross.length2();
            }
            {
                  SimdVector3 a3 = pt.m_localPointA-m_pointCache[0].m_localPointA;
                  SimdVector3 b3 = m_pointCache[2].m_localPointA-m_pointCache[1].m_localPointA;
                  SimdVector3 cross = a3.cross(b3);
                  res3 = cross.length2();
            }

            SimdVector4 maxvec(res0,res1,res2,res3);
            int biggestarea = maxvec.closestAxis4();

            return biggestarea;


}



int PersistentManifold::GetCacheEntry(const ManifoldPoint& newPoint) const
{
      SimdScalar shortestDist =  GetManifoldMargin() * GetManifoldMargin();
      int size = GetNumContacts();
      int nearestPoint = -1;
      for( int i = 0; i < size; i++ )
      {
            const ManifoldPoint &mp = m_pointCache[i];

            SimdVector3 diffA =  mp.m_localPointA- newPoint.m_localPointA;
            const SimdScalar distToManiPoint = diffA.dot(diffA);
            if( distToManiPoint < shortestDist )
            {
                  shortestDist = distToManiPoint;
                  nearestPoint = i;
            }
      }
      return nearestPoint;
}

void PersistentManifold::AddManifoldPoint(const ManifoldPoint& newPoint)
{
      assert(ValidContactDistance(newPoint));

      int insertIndex = GetNumContacts();
      if (insertIndex == MANIFOLD_CACHE_SIZE)
      {
#if MANIFOLD_CACHE_SIZE >= 4
            //sort cache so best points come first, based on area
            insertIndex = SortCachedPoints(newPoint);
#else
            insertIndex = 0;
#endif

      } else
      {
            m_cachedPoints++;
      }
      ReplaceContactPoint(newPoint,insertIndex);
}

00120 float PersistentManifold::GetManifoldMargin() const
{
      return gContactBreakingTreshold;
}

00125 void PersistentManifold::RefreshContactPoints(const SimdTransform& trA,const SimdTransform& trB)
{
      int i;

      /// first refresh worldspace positions and distance
      for (i=GetNumContacts()-1;i>=0;i--)
      {
            ManifoldPoint &manifoldPoint = m_pointCache[i];
            manifoldPoint.m_positionWorldOnA = trA( manifoldPoint.m_localPointA );
            manifoldPoint.m_positionWorldOnB = trB( manifoldPoint.m_localPointB );
            manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA -  manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
            manifoldPoint.m_lifeTime++;
      }

      /// then 
      SimdScalar distance2d;
      SimdVector3 projectedDifference,projectedPoint;
      for (i=GetNumContacts()-1;i>=0;i--)
      {
            
            ManifoldPoint &manifoldPoint = m_pointCache[i];
            //contact becomes invalid when signed distance exceeds margin (projected on contactnormal direction)
            if (!ValidContactDistance(manifoldPoint))
            {
                  RemoveContactPoint(i);
            } else
            {
                  //contact also becomes invalid when relative movement orthogonal to normal exceeds margin
                  projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
                  projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
                  distance2d = projectedDifference.dot(projectedDifference);
                  if (distance2d  > GetManifoldMargin()*GetManifoldMargin() )
                  {
                        RemoveContactPoint(i);
                  }
            }
      }
}


//todo: remove this treshold
float gPenetrationDistanceCheck = -0.05f;

float PersistentManifold::GetCollisionImpulse() const
{
      float averageImpulse = 0.f;
      if (GetNumContacts() > 0)
      {
            float totalImpulse = 0.f;

            //return the sum of the applied impulses on the box
            for (int i=0;i<GetNumContacts();i++)
            {
                  const ManifoldPoint& cp = GetContactPoint(i);
                  //avoid conflic noice
                  if ( cp.GetDistance() <gPenetrationDistanceCheck)
                        return 0.f;

                  totalImpulse += cp.m_appliedImpulse;

            }
            averageImpulse = totalImpulse / ((float)GetNumContacts());

      }
      return averageImpulse;

}



Generated by  Doxygen 1.6.0   Back to index