btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
int i;
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
for (i=0;i<getNumVertices();i++)
{
getVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
if( m_numPoints > 0 )
{
// Here we take advantage of dot(a*b, c) = dot( a, b*c) to do less work. Note this transformation is true mathematically, not numerically.
// btVector3 scaled = vec * m_localScaling;
int index = (int) vec.maxDot( &m_unscaledPoints[0], m_numPoints, maxDot); //FIXME: may violate encapsulation of m_unscaledPoints
return getScaledPoint(index);
}
return supVec;
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
SimdVector3 PolyhedralConvexShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
int i;
SimdVector3 supVec(0,0,0);
SimdScalar maxDot(-1e30f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
SimdVector3 vtx;
SimdScalar newDot;
for (i=0;i<GetNumVertices();i++)
{
GetVertex(i,vtx);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btConvexPointCloudShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-1e30);
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
for (int i=0;i<m_numPoints;i++)
{
btVector3 vtx = getScaledPoint(i);
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btCapsuleShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
btScalar radius = getRadius();
{
btVector3 pos(0,0,0);
pos[getUpAxis()] = getHalfHeight();
vtx = pos +vec*(radius) - vec * getMargin();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
{
btVector3 pos(0,0,0);
pos[getUpAxis()] = -getHalfHeight();
vtx = pos +vec*(radius) - vec * getMargin();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar maxDot = btScalar(-BT_LARGE_FLOAT);
// Here we take advantage of dot(a, b*c) = dot(a*b, c). Note: This is true mathematically, but not numerically.
if( 0 < m_unscaledPoints.size() )
{
btVector3 scaled = vec * m_localScaling;
int index = (int) scaled.maxDot( &m_unscaledPoints[0], m_unscaledPoints.size(), maxDot); // FIXME: may violate encapsulation of m_unscaledPoints
return m_unscaledPoints[index] * m_localScaling;
}
return supVec;
}
btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < (SIMD_EPSILON*SIMD_EPSILON))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
const btVector3* pos = &m_localPositionArray[0];
const btScalar* rad = &m_radiArray[0];
int numSpheres = m_localPositionArray.size();
for( int k = 0; k < numSpheres; k+= 128 )
{
btVector3 temp[128];
int inner_count = MIN( numSpheres - k, 128 );
for( long i = 0; i < inner_count; i++ )
{
temp[i] = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
}
long i = vec.maxDot( temp, inner_count, newDot);
if( newDot > maxDot )
{
maxDot = newDot;
supVec = temp[i];
}
}
return supVec;
}
btVector3 btConvexHullShape::localGetSupportingVertexWithoutMargin(const btVector3& vec)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btScalar newDot,maxDot = btScalar(-BT_LARGE_FLOAT);
for (int i=0;i<m_unscaledPoints.size();i++)
{
btVector3 vtx = m_unscaledPoints[i] * m_localScaling;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btMultiSphereShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
int i;
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-1e30));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
const btVector3* pos = &m_localPositions[0];
const btScalar* rad = &m_radi[0];
for (i=0;i<m_numSpheres;i++)
{
vtx = (*pos) +vec*m_localScaling*(*rad) - vec * getMargin();
pos++;
rad++;
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return supVec;
}
btVector3 btBarrelShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btVector3 supVecD;
// suppport point on the lathed ellipse?
btScalar pY = vec0.y();
btScalar pR = std::sqrt (vec0.z()*vec0.z() + vec0.x()*vec0.x() );
btScalar pH = pR;
btScalar dX = vec0.x()/pR;
btScalar dZ = vec0.z()/pR;
btScalar tpar = std::atan((pY*R_vert)/(pH*R_hor));
btScalar sY = R_vert * sin(tpar);
btScalar sH = R_hor * cos(tpar);
btScalar sR = sH + R_offset;
btScalar sX = dX * sR;
btScalar sZ = dZ * sR;
supVec.setValue(sX,sY,sZ);
btScalar len = supVec.length();
// support point on the top disc?
if ((fabs(Y_high) < R_vert)&(supVec.y()>Y_high))
{
btScalar R_high_ellips = R_hor * sqrt( 1- pow( Y_high/R_vert ,2) );
btScalar R_high = R_high_ellips + R_offset;
btScalar rad_ratio = pR/R_high;
supVecD.setValue(vec0.x()/rad_ratio, Y_high, vec0.z()/rad_ratio);
supVec = supVecD;
}
// support point on the bottom disc?
if ((fabs(Y_low) < R_vert)&(supVec.y()<Y_low))
{
btScalar R_low_ellips = R_hor * sqrt( 1- pow( Y_low/R_vert ,2) );
btScalar R_low = R_low_ellips + R_offset;
btScalar rad_ratio = pR/R_low;
supVecD.setValue(vec0.x()/rad_ratio, Y_low, vec0.z()/rad_ratio);
supVec = supVecD;
}
return supVec;
}
btVector3 btPolyhedralConvexShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
#ifndef __SPU__
int i;
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
for( int k = 0; k < getNumVertices(); k += 128 )
{
btVector3 temp[128];
int inner_count = MIN(getNumVertices() - k, 128);
for( i = 0; i < inner_count; i++ )
getVertex(i,temp[i]);
i = (int) vec.maxDot( temp, inner_count, newDot);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = temp[i];
}
}
#endif //__SPU__
return supVec;
}
btVector3 btConvexTriangleMeshShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(btScalar(0.),btScalar(0.),btScalar(0.));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
btVector3 aabbMax(btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT),btScalar(BT_LARGE_FLOAT));
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
SimdVector3 ConvexTriangleMeshShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec0)const
{
SimdVector3 supVec(0.f,0.f,0.f);
SimdVector3 vec = vec0;
SimdScalar lenSqr = vec.length2();
if (lenSqr < 0.0001f)
{
vec.setValue(1,0,0);
} else
{
float rlen = 1.f / SimdSqrt(lenSqr );
vec *= rlen;
}
LocalSupportVertexCallback supportCallback(vec);
SimdVector3 aabbMax(1e30f,1e30f,1e30f);
m_stridingMesh->InternalProcessAllTriangles(&supportCallback,-aabbMax,aabbMax);
supVec = supportCallback.GetSupportVertexLocal();
return supVec;
}
btVector3 btConvexShape::localGetSupportVertexWithoutMarginNonVirtual (const btVector3& localDir) const
{
switch (m_shapeType)
{
case SPHERE_SHAPE_PROXYTYPE:
{
return btVector3(0,0,0);
}
case BOX_SHAPE_PROXYTYPE:
{
btBoxShape* convexShape = (btBoxShape*)this;
const btVector3& halfExtents = convexShape->getImplicitShapeDimensions();
#if defined( __APPLE__ ) && (defined( BT_USE_SSE )||defined( BT_USE_NEON ))
#if defined( BT_USE_SSE )
return btVector3( _mm_xor_ps( _mm_and_ps( localDir.mVec128, (__m128){-0.0f, -0.0f, -0.0f, -0.0f }), halfExtents.mVec128 ));
#elif defined( BT_USE_NEON )
return btVector3( (float32x4_t) (((uint32x4_t) localDir.mVec128 & (uint32x4_t){ 0x80000000, 0x80000000, 0x80000000, 0x80000000}) ^ (uint32x4_t) halfExtents.mVec128 ));
#else
#error unknown vector arch
#endif
#else
return btVector3(btFsels(localDir.x(), halfExtents.x(), -halfExtents.x()),
btFsels(localDir.y(), halfExtents.y(), -halfExtents.y()),
btFsels(localDir.z(), halfExtents.z(), -halfExtents.z()));
#endif
}
case TRIANGLE_SHAPE_PROXYTYPE:
{
btTriangleShape* triangleShape = (btTriangleShape*)this;
btVector3 dir(localDir.getX(),localDir.getY(),localDir.getZ());
btVector3* vertices = &triangleShape->m_vertices1[0];
btVector3 dots = dir.dot3(vertices[0], vertices[1], vertices[2]);
btVector3 sup = vertices[dots.maxAxis()];
return btVector3(sup.getX(),sup.getY(),sup.getZ());
}
case CYLINDER_SHAPE_PROXYTYPE:
{
btCylinderShape* cylShape = (btCylinderShape*)this;
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
btVector3 halfExtents = cylShape->getImplicitShapeDimensions();
btVector3 v(localDir.getX(),localDir.getY(),localDir.getZ());
int cylinderUpAxis = cylShape->getUpAxis();
int XX(1),YY(0),ZZ(2);
switch (cylinderUpAxis)
{
case 0:
{
XX = 1;
YY = 0;
ZZ = 2;
}
break;
case 1:
{
XX = 0;
YY = 1;
ZZ = 2;
}
break;
case 2:
{
XX = 0;
YY = 2;
ZZ = 1;
}
break;
default:
btAssert(0);
break;
};
btScalar radius = halfExtents[XX];
btScalar halfHeight = halfExtents[cylinderUpAxis];
btVector3 tmp;
btScalar d ;
btScalar s = btSqrt(v[XX] * v[XX] + v[ZZ] * v[ZZ]);
if (s != btScalar(0.0))
{
d = radius / s;
tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d;
return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
} else {
tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0 ? -halfHeight : halfHeight;
tmp[ZZ] = btScalar(0.0);
return btVector3(tmp.getX(),tmp.getY(),tmp.getZ());
}
}
case CAPSULE_SHAPE_PROXYTYPE:
{
btVector3 vec0(localDir.getX(),localDir.getY(),localDir.getZ());
btCapsuleShape* capsuleShape = (btCapsuleShape*)this;
btScalar halfHeight = capsuleShape->getHalfHeight();
int capsuleUpAxis = capsuleShape->getUpAxis();
btScalar radius = capsuleShape->getRadius();
btVector3 supVec(0,0,0);
btScalar maxDot(btScalar(-BT_LARGE_FLOAT));
btVector3 vec = vec0;
btScalar lenSqr = vec.length2();
if (lenSqr < btScalar(0.0001))
{
vec.setValue(1,0,0);
} else
{
btScalar rlen = btScalar(1.) / btSqrt(lenSqr );
vec *= rlen;
}
btVector3 vtx;
btScalar newDot;
{
btVector3 pos(0,0,0);
pos[capsuleUpAxis] = halfHeight;
//vtx = pos +vec*(radius);
vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
{
btVector3 pos(0,0,0);
pos[capsuleUpAxis] = -halfHeight;
//vtx = pos +vec*(radius);
vtx = pos +vec*(radius) - vec * capsuleShape->getMarginNV();
newDot = vec.dot(vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
return btVector3(supVec.getX(),supVec.getY(),supVec.getZ());
}
case CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
{
btConvexPointCloudShape* convexPointCloudShape = (btConvexPointCloudShape*)this;
btVector3* points = convexPointCloudShape->getUnscaledPoints ();
int numPoints = convexPointCloudShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints,convexPointCloudShape->getLocalScalingNV());
}
case CONVEX_HULL_SHAPE_PROXYTYPE:
{
btConvexHullShape* convexHullShape = (btConvexHullShape*)this;
btVector3* points = convexHullShape->getUnscaledPoints();
int numPoints = convexHullShape->getNumPoints ();
return convexHullSupport (localDir, points, numPoints,convexHullShape->getLocalScalingNV());
}
default:
#ifndef __SPU__
return this->localGetSupportingVertexWithoutMargin (localDir);
#else
btAssert (0);
#endif
}
// should never reach here
btAssert (0);
return btVector3 (btScalar(0.0f), btScalar(0.0f), btScalar(0.0f));
}
