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

ConeShape.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 "ConeShape.h"
#include "SimdPoint3.h"

#ifdef WIN32
static int coneindices[3] = {1,2,0};
#else
static int coneindices[3] = {2,1,0};
#endif

ConeShape::ConeShape (SimdScalar radius,SimdScalar height):
m_radius (radius),
m_height(height)
{
      SimdVector3 halfExtents;
      m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height));
}


SimdVector3 ConeShape::ConeLocalSupport(const SimdVector3& v) const
{
      
      float halfHeight = m_height * 0.5f;

 if (v[coneindices[1]] > v.length() * m_sinAngle)
 {
      SimdVector3 tmp;

      tmp[coneindices[0]] = 0.f;
      tmp[coneindices[1]] = halfHeight;
      tmp[coneindices[2]] = 0.f;
      return tmp;
 }
  else {
    SimdScalar s = SimdSqrt(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]);
    if (s > SIMD_EPSILON) {
      SimdScalar d = m_radius / s;
        SimdVector3 tmp;
        tmp[coneindices[0]] = v[coneindices[0]] * d;
        tmp[coneindices[1]] = -halfHeight;
        tmp[coneindices[2]] = v[coneindices[2]] * d;
        return tmp;
    }
    else  {
            SimdVector3 tmp;
            tmp[coneindices[0]] = 0.f;
            tmp[coneindices[1]] = -halfHeight;
            tmp[coneindices[2]] = 0.f;
            return tmp;
      }
  }

}

SimdVector3 ConeShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const
{
            return ConeLocalSupport(vec);
}

SimdVector3 ConeShape::LocalGetSupportingVertex(const SimdVector3& vec)  const
{
      SimdVector3 supVertex = ConeLocalSupport(vec);
      if ( GetMargin()!=0.f )
      {
            SimdVector3 vecnorm = vec;
            if (vecnorm .length2() == 0.f)
            {
                  vecnorm.setValue(-1.f,-1.f,-1.f);
            } 
            vecnorm.normalize();
            supVertex+= GetMargin() * vecnorm;
      }
      return supVertex;
}



Generated by  Doxygen 1.6.0   Back to index