void cSpace::movePointOutsideCSpace(btVector3& pt)
{
float angle=0;
float zHeight = pt.z();
pt.setZ(0);
btVector3 nudge(0.01,0,0);
btVector3 npt = pt;
while(isPointInsideCSpace(npt)) // check if the point is inside of the object
{
npt = pt + nudge.rotate(btVector3(0,0,1),angle); // calculate a new point a radius of vector nudge away at angle
angle -= 0.2; // in radians, CCW search direction
if(angle < -PI) { // if the search has gone a full rotation
nudge.setX(nudge.x() + 0.01); // add another centemeter to the search vector radius
if(nudge.length() > 1) {
qDebug("Nudge length is greater than 1");
break;
}
angle = 0;
}
}
pt = npt;
pt.setZ(zHeight);
}
btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
{
//project the center of the sphere on the closest face of the box
btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
btScalar minDist = faceDist;
closestPoint.setX( boxHalfExtent.getX() );
normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setX( -boxHalfExtent.getX() );
normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( -boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
}
faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( -boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
}
return minDist;
}
void btConeTwistConstraint::adjustSwingAxisToUseEllipseNormal(btVector3& vSwingAxis) const
{
// the swing axis is computed as the "twist-free" cone rotation,
// but the cone limit is not circular, but elliptical (if swingspan1 != swingspan2).
// so, if we're outside the limits, the closest way back inside the cone isn't
// along the vector back to the center. better (and more stable) to use the ellipse normal.
// convert swing axis to direction from center to surface of ellipse
// (ie. rotate 2D vector by PI/2)
btScalar y = -vSwingAxis.z();
btScalar z = vSwingAxis.y();
// do the math...
if (fabs(z) > SIMD_EPSILON) // avoid division by 0. and we don't need an update if z == 0.
{
// compute gradient/normal of ellipse surface at current "point"
btScalar grad = y/z;
grad *= m_swingSpan2 / m_swingSpan1;
// adjust y/z to represent normal at point (instead of vector to point)
if (y > 0)
y = fabs(grad * z);
else
y = -fabs(grad * z);
// convert ellipse direction back to swing axis
vSwingAxis.setZ(-y);
vSwingAxis.setY( z);
vSwingAxis.normalize();
}
}
// dir = 1 for CCW search or clockwise around obstacle
// dir = -1 for CW search or counter clockwise around obstacle
bool cSpace::movePointAroundCSpace(btVector3& pt, btVector3 startVect, float stpMag, int dir)
{
float angle=0;
float zHeight = pt.z();
pt.setZ(0);
btVector3 nudge = stpMag * startVect.normalize();
btVector3 npt = pt + nudge;
while(isPointInsideCSpace(npt)) // check if the point is inside of the object
{
npt = pt + nudge.rotate(btVector3(0,0,1),angle); // calculate a new point a radius of vector nudge away at angle
angle += (dir * 0.1); // in radians
if(fabs(angle) > TWOPI) // if the search has gone a full rotation
return false; // and there is no path outside a C-Space return false, stuck
}
pt = npt;
pt.setZ(zHeight);
return true;
}
ItemController::ItemController(btVector3 position, std::weak_ptr<PhysicsWorld> world, TroenGame* troenGame, LevelView* levelView)
{
AbstractController();
m_type = (ItemController::Type) (int) floor(randf(0, COUNT));
m_position = position;
m_troenGame = troenGame;
osg::Vec3 viewDimensions = getDimensions();
btVector3 modelDimensions(viewDimensions.x(), viewDimensions.y(), 10);
position.setZ(position.z() + viewDimensions.z() / 2.f);
m_model = m_itemModel = std::make_shared<ItemModel>(modelDimensions, position, world, this);
m_view = m_itemView = std::make_shared<ItemView>(getDimensions(), btToOSGVec3(position), levelView, m_type);
}
// Converts a quaternion to an euler angle
void _Physics::QuaternionToEuler(const btQuaternion &Quat, btVector3 &Euler) {
btScalar W = Quat.getW();
btScalar X = Quat.getX();
btScalar Y = Quat.getY();
btScalar Z = Quat.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
Euler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
Euler.setY(asinf(-2.0f * (X * Z - Y * W)));
Euler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
Euler *= irr::core::RADTODEG;
}
// Converts a quaternion to an euler angle
void QuaternionToEuler(const btQuaternion &tQuat, btVector3 &tEuler) {
btScalar w = tQuat.getW();
btScalar x = tQuat.getX();
btScalar y = tQuat.getY();
btScalar z = tQuat.getZ();
float wSquared = w * w;
float xSquared = x * x;
float ySquared = y * y;
float zSquared = z * z;
tEuler.setX(atan2f(2.0f * (y * z + x * w), -xSquared - ySquared + zSquared + wSquared));
tEuler.setY(asinf(-2.0f * (x * z - y * w)));
tEuler.setZ(atan2f(2.0f * (x * y + z * w), xSquared - ySquared - zSquared + wSquared));
tEuler *= SIMD_DEGS_PER_RAD;
}
void Physics::QuaternionToEuler( const CIwFQuat& TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.s;
const float x = TQuat.x;
const float y = TQuat.y;
const float z = TQuat.z;
QuaternionToEuler( w, x, y, z, a );
TEuler.setX( a[0] );
TEuler.setY( a[1] );
TEuler.setZ( a[2] );
}
// Converts a quaternion to an euler angle
void CTBulletHelper::QuaternionToEuler(const btQuaternion &TQuat, btVector3 &TEuler) {
btScalar W = TQuat.getW();
btScalar X = TQuat.getX();
btScalar Y = TQuat.getY();
btScalar Z = TQuat.getZ();
float WSquared = W * W;
float XSquared = X * X;
float YSquared = Y * Y;
float ZSquared = Z * Z;
TEuler.setX(atan2f(2.0f * (Y * Z + X * W), -XSquared - YSquared + ZSquared + WSquared));
TEuler.setY(asinf(-2.0f * (X * Z - Y * W)));
TEuler.setZ(atan2f(2.0f * (X * Y + Z * W), XSquared - YSquared - ZSquared + WSquared));
TEuler *= core::RADTODEG;
}
void Physics::QuaternionToEuler( const btQuaternion &TQuat, btVector3 &TEuler )
{
float a[3];
const float w = TQuat.getW();
const float x = TQuat.getX();
const float y = TQuat.getY();
const float z = TQuat.getZ();
QuaternionToEuler( w, x, y, z, a );
TEuler.setX( a[0] );
TEuler.setY( a[1] );
TEuler.setZ( a[2] );
}
void ConvertPosToBull(const Vector& pos, btVector3& bull) {
bull.setX(HL2BULL(pos.x));
bull.setY(HL2BULL(pos.z));
bull.setZ(-HL2BULL(pos.y));
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
gContactAddedCallback = &MyContactCallback;
setupEmptyDynamicsWorld();
getDynamicsWorld()->setDebugDrawer(&gDebugDrawer);
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
#ifndef NO_OBJ_TO_BULLET
ConvexDecomposition::WavefrontObj wo;
tcount = 0;
const char* prefix[]={"./","../","../../","../../../","../../../../", "ConvexDecompositionDemo/", "Demos/ConvexDecompositionDemo/",
"../Demos/ConvexDecompositionDemo/","../../Demos/ConvexDecompositionDemo/"};
int numPrefixes = sizeof(prefix)/sizeof(const char*);
char relativeFileName[1024];
for (int i=0;i<numPrefixes;i++)
{
sprintf(relativeFileName,"%s%s",prefix[i],filename);
tcount = wo.loadObj(relativeFileName);
if (tcount)
break;
}
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->addTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->addTriangle(vertex0,vertex1,vertex2);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
bool updateLocalAabb = false;
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i],updateLocalAabb);
}
convexShape->recalcLocalAabb();
if (sEnableSAT)
convexShape->initializePolyhedralFeatures();
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,14));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
if (tcount)
{
//-----------------------------------
// Bullet Convex Decomposition
//-----------------------------------
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile,this);
desc.mCallback = &convexDecomposition;
//-----------------------------------------------
// HACD
//-----------------------------------------------
std::vector< HACD::Vec3<HACD::Real> > points;
std::vector< HACD::Vec3<long> > triangles;
for(int i=0; i<wo.mVertexCount; i++ )
{
int index = i*3;
HACD::Vec3<HACD::Real> vertex(wo.mVertices[index], wo.mVertices[index+1],wo.mVertices[index+2]);
points.push_back(vertex);
}
for(int i=0;i<wo.mTriCount;i++)
{
int index = i*3;
HACD::Vec3<long> triangle(wo.mIndices[index], wo.mIndices[index+1], wo.mIndices[index+2]);
triangles.push_back(triangle);
}
HACD::HACD myHACD;
myHACD.SetPoints(&points[0]);
myHACD.SetNPoints(points.size());
myHACD.SetTriangles(&triangles[0]);
myHACD.SetNTriangles(triangles.size());
myHACD.SetCompacityWeight(0.1);
myHACD.SetVolumeWeight(0.0);
// HACD parameters
// Recommended parameters: 2 100 0 0 0 0
size_t nClusters = 2;
double concavity = 100;
bool invert = false;
bool addExtraDistPoints = false;
bool addNeighboursDistPoints = false;
bool addFacesPoints = false;
myHACD.SetNClusters(nClusters); // minimum number of clusters
myHACD.SetNVerticesPerCH(100); // max of 100 vertices per convex-hull
myHACD.SetConcavity(concavity); // maximum concavity
myHACD.SetAddExtraDistPoints(addExtraDistPoints);
myHACD.SetAddNeighboursDistPoints(addNeighboursDistPoints);
myHACD.SetAddFacesPoints(addFacesPoints);
myHACD.Compute();
nClusters = myHACD.GetNClusters();
myHACD.Save("output.wrl", false);
//convexDecomposition.performConvexDecomposition(desc);
// ConvexBuilder cb(desc.mCallback);
// cb.process(desc);
//now create some bodies
if (1)
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int c=0;c<nClusters;c++)
{
//generate convex result
size_t nPoints = myHACD.GetNPointsCH(c);
size_t nTriangles = myHACD.GetNTrianglesCH(c);
float* vertices = new float[nPoints*3];
unsigned int* triangles = new unsigned int[nTriangles*3];
HACD::Vec3<HACD::Real> * pointsCH = new HACD::Vec3<HACD::Real>[nPoints];
HACD::Vec3<long> * trianglesCH = new HACD::Vec3<long>[nTriangles];
myHACD.GetCH(c, pointsCH, trianglesCH);
// points
for(size_t v = 0; v < nPoints; v++)
{
vertices[3*v] = pointsCH[v].X();
vertices[3*v+1] = pointsCH[v].Y();
vertices[3*v+2] = pointsCH[v].Z();
}
// triangles
for(size_t f = 0; f < nTriangles; f++)
{
triangles[3*f] = trianglesCH[f].X();
triangles[3*f+1] = trianglesCH[f].Y();
triangles[3*f+2] = trianglesCH[f].Z();
}
delete [] pointsCH;
delete [] trianglesCH;
ConvexResult r(nPoints, vertices, nTriangles, triangles);
convexDecomposition.ConvexDecompResult(r);
}
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}
/* for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}*/
#if 1
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
btRigidBody* body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
}
if (outputFile)
fclose(outputFile);
}
#ifdef TEST_SERIALIZATION
//test serializing this
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
exitPhysics();
//now try again from the loaded file
setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION
#endif //NO_OBJ_TO_BULLET
#ifdef TEST_SERIALIZATION
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
//fileLoader->setVerboseMode(true);
fileLoader->loadFile("testFile.bullet");
//fileLoader->loadFile("testFile64Double.bullet");
//fileLoader->loadFile("testFile64Single.bullet");
//fileLoader->loadFile("testFile32Single.bullet");
#endif //TEST_SERIALIZATION
}
void ConvertAngularImpulseToBull(const AngularImpulse& angularimp, btVector3& bull) {
bull.setX(DEG2RAD(angularimp.x));
bull.setY(DEG2RAD(angularimp.z));
bull.setZ(-DEG2RAD(angularimp.y));
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
ConvexDecomposition::WavefrontObj wo;
tcount = wo.loadObj(filename);
if (!tcount)
{
//when running this app from visual studio, the default starting folder is different, so make a second attempt...
tcount = wo.loadObj("../../file.obj");
}
m_collisionConfiguration = new btDefaultCollisionConfiguration();
#ifdef USE_PARALLEL_DISPATCHER
#ifdef USE_WIN32_THREADING
int maxNumOutstandingTasks = 4;//number of maximum outstanding tasks
Win32ThreadSupport* threadSupport = new Win32ThreadSupport(Win32ThreadSupport::Win32ThreadConstructionInfo(
"collision",
processCollisionTask,
createCollisionLocalStoreMemory,
maxNumOutstandingTasks));
#else
///@todo other platform threading
///Playstation 3 SPU (SPURS) version is available through PS3 Devnet
///Libspe2 SPU support will be available soon
///pthreads version
///you can hook it up to your custom task scheduler by deriving from btThreadSupportInterface
#endif
m_dispatcher = new SpuGatheringCollisionDispatcher(threadSupport,maxNumOutstandingTasks,m_collisionConfiguration);
#else
m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
#endif//USE_PARALLEL_DISPATCHER
convexDecompositionObjectOffset.setValue(10,0,0);
btVector3 worldAabbMin(-10000,-10000,-10000);
btVector3 worldAabbMax(10000,10000,10000);
m_broadphase = new btAxisSweep3(worldAabbMin,worldAabbMax);
//m_broadphase = new btSimpleBroadphase();
m_solver = new btSequentialImpulseConstraintSolver();
m_dynamicsWorld = new btDiscreteDynamicsWorld(m_dispatcher,m_broadphase,m_solver,m_collisionConfiguration);
#ifdef USE_PARALLEL_DISPATCHER
m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
#endif //USE_PARALLEL_DISPATCHER
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->addTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
float mass = 1.f;
//float collisionMargin = 0.01f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
std::vector<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
std::vector<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += 5*collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
std::vector<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
convexShape->setMargin(0.01);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->addTriangle(vertex0,vertex1,vertex2);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i]);
}
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,0));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
if (tcount)
{
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile,this);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
cb.process(desc);
//now create some bodies
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
}
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
localCreateRigidBody( mass, trans,compound);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
localCreateRigidBody( mass, trans,compound);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
localCreateRigidBody( mass, trans,compound);
}
if (outputFile)
fclose(outputFile);
}
}
void ConvexDecompositionDemo::initPhysics(const char* filename)
{
gContactAddedCallback = &MyContactCallback;
setupEmptyDynamicsWorld();
setTexturing(true);
setShadows(true);
setCameraDistance(26.f);
#ifndef NO_OBJ_TO_BULLET
ConvexDecomposition::WavefrontObj wo;
tcount = wo.loadObj(filename);
if (!tcount)
{
//when running this app from visual studio, the default starting folder is different, so make a second attempt...
tcount = wo.loadObj("../../file.obj");
}
if (!tcount)
{
//cmake generated msvc files need 4 levels deep back... so make a 3rd attempt...
tcount = wo.loadObj("../../../../file.obj");
}
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,-4.5,0));
btCollisionShape* boxShape = new btBoxShape(btVector3(30,2,30));
m_collisionShapes.push_back(boxShape);
localCreateRigidBody(0.f,startTransform,boxShape);
class MyConvexDecomposition : public ConvexDecomposition::ConvexDecompInterface
{
ConvexDecompositionDemo* m_convexDemo;
public:
btAlignedObjectArray<btConvexHullShape*> m_convexShapes;
btAlignedObjectArray<btVector3> m_convexCentroids;
MyConvexDecomposition (FILE* outputFile,ConvexDecompositionDemo* demo)
:m_convexDemo(demo),
mBaseCount(0),
mHullCount(0),
mOutputFile(outputFile)
{
}
virtual void ConvexDecompResult(ConvexDecomposition::ConvexResult &result)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_convexDemo->m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
//export data to .obj
printf("ConvexResult. ");
if (mOutputFile)
{
fprintf(mOutputFile,"## Hull Piece %d with %d vertices and %d triangles.\r\n", mHullCount, result.mHullVcount, result.mHullTcount );
fprintf(mOutputFile,"usemtl Material%i\r\n",mBaseCount);
fprintf(mOutputFile,"o Object%i\r\n",mBaseCount);
for (unsigned int i=0; i<result.mHullVcount; i++)
{
const float *p = &result.mHullVertices[i*3];
fprintf(mOutputFile,"v %0.9f %0.9f %0.9f\r\n", p[0], p[1], p[2] );
}
//calc centroid, to shift vertices around center of mass
centroid.setValue(0,0,0);
btAlignedObjectArray<btVector3> vertices;
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
centroid += vertex;
}
}
centroid *= 1.f/(float(result.mHullVcount) );
if ( 1 )
{
//const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullVcount; i++)
{
btVector3 vertex(result.mHullVertices[i*3],result.mHullVertices[i*3+1],result.mHullVertices[i*3+2]);
vertex *= localScaling;
vertex -= centroid ;
vertices.push_back(vertex);
}
}
if ( 1 )
{
const unsigned int *src = result.mHullIndices;
for (unsigned int i=0; i<result.mHullTcount; i++)
{
unsigned int index0 = *src++;
unsigned int index1 = *src++;
unsigned int index2 = *src++;
btVector3 vertex0(result.mHullVertices[index0*3], result.mHullVertices[index0*3+1],result.mHullVertices[index0*3+2]);
btVector3 vertex1(result.mHullVertices[index1*3], result.mHullVertices[index1*3+1],result.mHullVertices[index1*3+2]);
btVector3 vertex2(result.mHullVertices[index2*3], result.mHullVertices[index2*3+1],result.mHullVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
vertex0 -= centroid;
vertex1 -= centroid;
vertex2 -= centroid;
trimesh->addTriangle(vertex0,vertex1,vertex2);
index0+=mBaseCount;
index1+=mBaseCount;
index2+=mBaseCount;
fprintf(mOutputFile,"f %d %d %d\r\n", index0+1, index1+1, index2+1 );
}
}
// float mass = 1.f;
//this is a tools issue: due to collision margin, convex objects overlap, compensate for it here:
//#define SHRINK_OBJECT_INWARDS 1
#ifdef SHRINK_OBJECT_INWARDS
float collisionMargin = 0.01f;
btAlignedObjectArray<btVector3> planeEquations;
btGeometryUtil::getPlaneEquationsFromVertices(vertices,planeEquations);
btAlignedObjectArray<btVector3> shiftedPlaneEquations;
for (int p=0;p<planeEquations.size();p++)
{
btVector3 plane = planeEquations[p];
plane[3] += collisionMargin;
shiftedPlaneEquations.push_back(plane);
}
btAlignedObjectArray<btVector3> shiftedVertices;
btGeometryUtil::getVerticesFromPlaneEquations(shiftedPlaneEquations,shiftedVertices);
btConvexHullShape* convexShape = new btConvexHullShape(&(shiftedVertices[0].getX()),shiftedVertices.size());
#else //SHRINK_OBJECT_INWARDS
btConvexHullShape* convexShape = new btConvexHullShape(&(vertices[0].getX()),vertices.size());
#endif
convexShape->setMargin(0.01f);
m_convexShapes.push_back(convexShape);
m_convexCentroids.push_back(centroid);
m_convexDemo->m_collisionShapes.push_back(convexShape);
mBaseCount+=result.mHullVcount; // advance the 'base index' counter.
}
}
int mBaseCount;
int mHullCount;
FILE* mOutputFile;
};
if (tcount)
{
btTriangleMesh* trimesh = new btTriangleMesh();
m_trimeshes.push_back(trimesh);
btVector3 localScaling(6.f,6.f,6.f);
int i;
for ( i=0;i<wo.mTriCount;i++)
{
int index0 = wo.mIndices[i*3];
int index1 = wo.mIndices[i*3+1];
int index2 = wo.mIndices[i*3+2];
btVector3 vertex0(wo.mVertices[index0*3], wo.mVertices[index0*3+1],wo.mVertices[index0*3+2]);
btVector3 vertex1(wo.mVertices[index1*3], wo.mVertices[index1*3+1],wo.mVertices[index1*3+2]);
btVector3 vertex2(wo.mVertices[index2*3], wo.mVertices[index2*3+1],wo.mVertices[index2*3+2]);
vertex0 *= localScaling;
vertex1 *= localScaling;
vertex2 *= localScaling;
trimesh->addTriangle(vertex0,vertex1,vertex2);
}
btConvexShape* tmpConvexShape = new btConvexTriangleMeshShape(trimesh);
printf("old numTriangles= %d\n",wo.mTriCount);
printf("old numIndices = %d\n",wo.mTriCount*3);
printf("old numVertices = %d\n",wo.mVertexCount);
printf("reducing vertices by creating a convex hull\n");
//create a hull approximation
btShapeHull* hull = new btShapeHull(tmpConvexShape);
btScalar margin = tmpConvexShape->getMargin();
hull->buildHull(margin);
tmpConvexShape->setUserPointer(hull);
printf("new numTriangles = %d\n", hull->numTriangles ());
printf("new numIndices = %d\n", hull->numIndices ());
printf("new numVertices = %d\n", hull->numVertices ());
btConvexHullShape* convexShape = new btConvexHullShape();
for (i=0;i<hull->numVertices();i++)
{
convexShape->addPoint(hull->getVertexPointer()[i]);
}
delete tmpConvexShape;
delete hull;
m_collisionShapes.push_back(convexShape);
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(0,2,14));
localCreateRigidBody(mass, startTransform,convexShape);
bool useQuantization = true;
btCollisionShape* concaveShape = new btBvhTriangleMeshShape(trimesh,useQuantization);
startTransform.setOrigin(convexDecompositionObjectOffset);
localCreateRigidBody(0.f,startTransform,concaveShape);
m_collisionShapes.push_back (concaveShape);
}
if (tcount)
{
char outputFileName[512];
strcpy(outputFileName,filename);
char *dot = strstr(outputFileName,".");
if ( dot )
*dot = 0;
strcat(outputFileName,"_convex.obj");
FILE* outputFile = fopen(outputFileName,"wb");
unsigned int depth = 5;
float cpercent = 5;
float ppercent = 15;
unsigned int maxv = 16;
float skinWidth = 0.0;
printf("WavefrontObj num triangles read %i\n",tcount);
ConvexDecomposition::DecompDesc desc;
desc.mVcount = wo.mVertexCount;
desc.mVertices = wo.mVertices;
desc.mTcount = wo.mTriCount;
desc.mIndices = (unsigned int *)wo.mIndices;
desc.mDepth = depth;
desc.mCpercent = cpercent;
desc.mPpercent = ppercent;
desc.mMaxVertices = maxv;
desc.mSkinWidth = skinWidth;
MyConvexDecomposition convexDecomposition(outputFile,this);
desc.mCallback = &convexDecomposition;
//convexDecomposition.performConvexDecomposition(desc);
ConvexBuilder cb(desc.mCallback);
cb.process(desc);
//now create some bodies
if (1)
{
btCompoundShape* compound = new btCompoundShape();
m_collisionShapes.push_back (compound);
btTransform trans;
trans.setIdentity();
for (int i=0;i<convexDecomposition.m_convexShapes.size();i++)
{
btVector3 centroid = convexDecomposition.m_convexCentroids[i];
trans.setOrigin(centroid);
btConvexHullShape* convexShape = convexDecomposition.m_convexShapes[i];
compound->addChildShape(trans,convexShape);
btRigidBody* body;
body = localCreateRigidBody( 1.0, trans,convexShape);
}
#if 1
btScalar mass=10.f;
trans.setOrigin(-convexDecompositionObjectOffset);
btRigidBody* body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
convexDecompositionObjectOffset.setZ(-6);
trans.setOrigin(-convexDecompositionObjectOffset);
body = localCreateRigidBody( mass, trans,compound);
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_CUSTOM_MATERIAL_CALLBACK);
#endif
}
if (outputFile)
fclose(outputFile);
}
#ifdef TEST_SERIALIZATION
//test serializing this
int maxSerializeBufferSize = 1024*1024*5;
btDefaultSerializer* serializer = new btDefaultSerializer(maxSerializeBufferSize);
m_dynamicsWorld->serialize(serializer);
FILE* f2 = fopen("testFile.bullet","wb");
fwrite(serializer->getBufferPointer(),serializer->getCurrentBufferSize(),1,f2);
fclose(f2);
exitPhysics();
//now try again from the loaded file
setupEmptyDynamicsWorld();
#endif //TEST_SERIALIZATION
#endif //NO_OBJ_TO_BULLET
#ifdef TEST_SERIALIZATION
btBulletWorldImporter* fileLoader = new btBulletWorldImporter(m_dynamicsWorld);
//fileLoader->setVerboseMode(true);
fileLoader->loadFile("testFile.bullet");
//fileLoader->loadFile("testFile64Double.bullet");
//fileLoader->loadFile("testFile64Single.bullet");
//fileLoader->loadFile("testFile32Single.bullet");
#endif //TEST_SERIALIZATION
}
void ConvertDirectionToBull(const Vector& dir, btVector3& bull) {
bull.setX(dir.x);
bull.setY(dir.z);
bull.setZ(-dir.y);
}
inline void Assign( btVector3& vec, const Vec3D& other )
{
vec.setX( other.X );
vec.setY( other.Y );
vec.setZ( other.Z );
}
// clips an obstacle shape if it is within range of the sensors
// if not returns false
bool cSpace::clipShape(btVector3 cc, QList<btVector3>& ls)
{
if(ls.size() < 2) return false;
int j=0;
cc.setZ(0);
// check if the whole obstacle is in range
do{
if(cc.distance2(ls[j]) > m_detectRangeSq) break; // check if the point is in-range
j++;
}while(j<ls.size());
if(j == ls.size()) return true; // the whole obstacle is in range
int i;
int nexti;
int arcType;
btVector3 xpoint1,xpoint2;
bool outofrange = true;
i=0;
while(i<ls.size())
{
if(i == ls.size()-1) nexti = 0;
else nexti = i+1;
arcType = arcIntersection(cc, m_detectRange, ls[i], ls[nexti], &xpoint1, &xpoint2);
if(arcType == 0) // NO intersection
i++;
else if(arcType == 1 || arcType == 2){ // ONE intersection
if(arcType == 2) xpoint1 = xpoint2;
if(nexti == 0) ls.append(xpoint1); // append new point ls if at the wraparound point
else ls.insert(nexti,xpoint1); // insert the intersection point to the ls
i+=2;
outofrange = false;
}
else{ // TWO intersections
if(nexti == 0){ // add the two intersect points between i and nexti
if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){ // append them in order
ls << xpoint1;
ls << xpoint2;
}
else{
ls << xpoint2;
ls << xpoint1;
}
}
else{
if(ls[i].distance2(xpoint1) < ls[i].distance2(xpoint2)){ // insert them in order
ls.insert(nexti,xpoint1);
ls.insert(nexti+1,xpoint2);
}
else{
ls.insert(nexti,xpoint2);
ls.insert(nexti+1,xpoint1);
}
}
i+=3;
outofrange = false;
}
}
if(outofrange) return false; // the entire obstacle is out of range
// remove any points outside of the sensor range
i=0;
while(i<ls.size()){
if(cc.distance2(ls[i]) > m_detectRangeSq+0.001)
ls.removeAt(i);
else
i++;
}
if(ls.size() == 2){
btVector3 cx;
cx = (ls[1] - ls[0]).cross((ls[0]-btVector3(0,0,1)) - ls[0]);
cx.normalize();
ls << (ls[1]+(cx*0.1));
ls << (ls[0]+(cx*0.1));
}
return true;
}