public IndexedVector3 LocalGetSupportVertexWithoutMarginNonVirtual(ref IndexedVector3 localDir)
{
singleResult = IndexedVector3.Zero;
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual " + GetName());
}
switch (m_shapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
singleResult = new IndexedVector3();
break;
}
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape convexShape = this as BoxShape;
IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();
singleResult = new IndexedVector3(MathUtil.FSel(localDir.X, halfExtents.X, -halfExtents.X),
MathUtil.FSel(localDir.Y, halfExtents.Y, -halfExtents.Y),
MathUtil.FSel(localDir.Z, halfExtents.Z, -halfExtents.Z));
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Box");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "halfEx", halfExtents);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult);
}
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
{
TriangleShape triangleShape = (TriangleShape)this;
IndexedVector3 dir = localDir;
IndexedVector3[] vertices = triangleShape.m_vertices1;
IndexedVector3 dots = new IndexedVector3(IndexedVector3.Dot(ref dir, ref vertices[0]), IndexedVector3.Dot(ref dir, ref vertices[1]), IndexedVector3.Dot(ref dir, ref vertices[2]));
int maxAxis = MathUtil.MaxAxis(ref dots);
IndexedVector3 sup = vertices[maxAxis];
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual::Triangle");
BulletGlobals.g_streamWriter.WriteLine(String.Format("MaxAxis [{0}]", maxAxis));
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx0", vertices[0]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx1", vertices[1]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "vtx2", vertices[2]);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dir", dir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "dots", dots);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "sup", sup);
}
singleResult = sup;
break;
}
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
{
CylinderShape cylShape = (CylinderShape)this;
//mapping of halfextents/dimension onto radius/height depends on how cylinder local orientation is (upAxis)
IndexedVector3 halfExtents = cylShape.GetImplicitShapeDimensions();
IndexedVector3 v = localDir;
int cylinderUpAxis = cylShape.GetUpAxis();
int XX = 1;
int YY = 0;
int 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:
Debug.Assert(false);
break;
}
;
float radius = halfExtents[XX];
float halfHeight = halfExtents[cylinderUpAxis];
IndexedVector3 tmp = new IndexedVector3();
float d;
float vx = v[XX];
float vz = v[ZZ];
float s = (float)Math.Sqrt(vx * vx + vz * vz);
if (s != 0f)
{
d = radius / s;
tmp[XX] = v[XX] * d;
tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
tmp[ZZ] = v[ZZ] * d;
singleResult = tmp;
}
else
{
tmp[XX] = radius;
tmp[YY] = v[YY] < 0.0f ? -halfHeight : halfHeight;
tmp[ZZ] = 0.0f;
singleResult = tmp;
}
break;
}
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
IndexedVector3 vec0 = localDir;
CapsuleShape capsuleShape = this as CapsuleShape;
float halfHeight = capsuleShape.GetHalfHeight();
int capsuleUpAxis = capsuleShape.GetUpAxis();
float radius = capsuleShape.GetRadius();
IndexedVector3 supVec = new IndexedVector3();
float maxDot = float.MinValue;
IndexedVector3 vec = vec0;
float lenSqr = vec.LengthSquared();
if (lenSqr < 0.0001f)
{
vec = new IndexedVector3(1, 0, 0);
}
else
{
float rlen = (1.0f) / (float)Math.Sqrt(lenSqr);
vec *= rlen;
//vec = IndexedVector3.Normalize(vec);
}
IndexedVector3 vtx;
float newDot;
{
IndexedVector3 pos = new IndexedVector3();
pos[capsuleUpAxis] = halfHeight;
//vtx = pos +vec*(radius);
vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
newDot = IndexedVector3.Dot(ref vec, ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
{
IndexedVector3 pos = new IndexedVector3();
pos[capsuleUpAxis] = -halfHeight;
//vtx = pos +vec*(radius);
vtx = pos + vec * (radius) - vec * capsuleShape.GetMarginNV();
newDot = IndexedVector3.Dot(ref vec, ref vtx);
if (newDot > maxDot)
{
maxDot = newDot;
supVec = vtx;
}
}
singleResult = supVec;
break;
}
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
{
ConvexPointCloudShape convexPointCloudShape = (ConvexPointCloudShape)this;
IList <IndexedVector3> points = convexPointCloudShape.GetUnscaledPoints();
int numPoints = convexPointCloudShape.GetNumPoints();
IndexedVector3 localScaling = convexPointCloudShape.GetLocalScalingNV();
singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
break;
}
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
{
ConvexHullShape convexHullShape = (ConvexHullShape)this;
IList <IndexedVector3> points = convexHullShape.GetUnscaledPoints();
int numPoints = convexHullShape.GetNumPoints();
IndexedVector3 localScaling = convexHullShape.GetLocalScalingNV();
singleResult = ConvexHullSupport(ref localDir, points, numPoints, ref localScaling);
break;
}
default:
singleResult = LocalGetSupportingVertexWithoutMargin(ref localDir);
break;
}
// should never reach here
//Debug.Assert(false);
if (BulletGlobals.g_streamWriter != null && BulletGlobals.debugConvexShape)
{
BulletGlobals.g_streamWriter.WriteLine("localGetSupportVertexWithoutMarginNonVirtual");
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "localDir", localDir);
MathUtil.PrintVector3(BulletGlobals.g_streamWriter, "result", singleResult);
}
return(singleResult);
}
public virtual void GetAabbNonVirtual(ref IndexedMatrix t, ref IndexedVector3 aabbMin, ref IndexedVector3 aabbMax)
{
switch (m_shapeType)
{
case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
{
SphereShape sphereShape = this as SphereShape;
float radius = sphereShape.GetImplicitShapeDimensions().X;// * convexShape->getLocalScaling().getX();
float margin = radius + sphereShape.GetMarginNonVirtual();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(margin);
aabbMin = center - extent;
aabbMax = center + extent;
}
break;
case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
/* fall through */
case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
{
BoxShape convexShape = this as BoxShape;
float margin = convexShape.GetMarginNonVirtual();
IndexedVector3 halfExtents = convexShape.GetImplicitShapeDimensions();
halfExtents += new IndexedVector3(margin);
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
break;
}
case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
{
TriangleShape triangleShape = (TriangleShape)this;
float margin = triangleShape.GetMarginNonVirtual();
for (int i = 0; i < 3; i++)
{
IndexedVector3 vec = new IndexedVector3();
vec[i] = 1f;
IndexedVector3 sv = LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis);
IndexedVector3 tmp = t * sv;
aabbMax[i] = tmp[i] + margin;
vec[i] = -1.0f;
tmp = t * (LocalGetSupportVertexWithoutMarginNonVirtual(vec * t._basis));
aabbMin[i] = tmp[i] - margin;
}
}
break;
case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
{
CapsuleShape capsuleShape = this as CapsuleShape;
float r = capsuleShape.GetRadius();
IndexedVector3 halfExtents = new IndexedVector3(r);
int m_upAxis = capsuleShape.GetUpAxis();
halfExtents[m_upAxis] = r + capsuleShape.GetHalfHeight();
float nvMargin = capsuleShape.GetMarginNonVirtual();
halfExtents += new IndexedVector3(nvMargin);
IndexedBasisMatrix abs_b = t._basis.Absolute();
IndexedVector3 center = t._origin;
IndexedVector3 extent = new IndexedVector3(abs_b._el0.Dot(ref halfExtents), abs_b._el1.Dot(ref halfExtents), abs_b._el2.Dot(ref halfExtents));
aabbMin = center - extent;
aabbMax = center + extent;
}
break;
case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
{
PolyhedralConvexAabbCachingShape convexHullShape = (PolyhedralConvexAabbCachingShape)this;
float margin = convexHullShape.GetMarginNonVirtual();
convexHullShape.GetNonvirtualAabb(ref t, out aabbMin, out aabbMax, margin);
}
break;
default:
GetAabb(ref t, out aabbMin, out aabbMax);
break;
}
// should never reach here
Debug.Assert(false);
}