forked from bartvdbraak/blender
86 lines
2.0 KiB
C++
86 lines
2.0 KiB
C++
/*
|
|
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
|
|
*
|
|
* 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 = sqrtf(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;
|
|
}
|
|
|
|
|