Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Fixes in raycast (against sphere), point 2 point constraint

Browse Source 
Picking test works, holding shift in App_BasicGpuDemo_*
This commit is contained in:
erwin coumans
2013-06-18 19:33:45 -07:00
parent a81d847c24
commit 481d54967f
8 changed files with 106 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
Demos3/BasicGpuDemo/BasicGpuDemo.cpp
View File

@@ -284,9 +284,9 @@ void BasicGpuDemo::initPhysics()
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btBoxShape* colShape = new btBoxShape(btVector3(SCALING*1,SCALING*1,SCALING*1));
//btCollisionShape* colShape = new btSphereShape(btScalar(SCALING*1.f));
btCollisionShape* colShape = new btSphereShape(btScalar(SCALING*1.f));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects

79
demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp
View File

@@ -8,6 +8,7 @@
#include "BulletCollision/CollisionShapes/btCompoundShape.h"
#include "BulletCollision/CollisionShapes/btSphereShape.h"
#include "BulletCollision/CollisionShapes/btStaticPlaneShape.h"
#include "BulletCollision/CollisionShapes/btConvexHullShape.h"
#include "BulletDynamics/ConstraintSolver/btPoint2PointConstraint.h"
#include "LinearMath/btQuickprof.h"
@@ -25,9 +26,9 @@
#endif
//#if (BT_BULLET_VERSION >= 282)
//#define BT_USE_BODY_UPDATE_REVISION
//#endif
#if (BT_BULLET_VERSION >= 282)
#define BT_USE_BODY_UPDATE_REVISION
#endif
b3GpuDynamicsWorld::b3GpuDynamicsWorld(class b3GpuSapBroadphase* bp,class b3GpuNarrowPhase* np, class b3GpuRigidBodyPipeline* rigidBodyPipeline)
@@ -37,9 +38,12 @@ m_cpuGpuSync(true),
m_bp(bp),
m_np(np),
m_rigidBodyPipeline(rigidBodyPipeline),
m_localTime(0.f)
m_localTime(0.f),
m_staticBody(0)
{
btConvexHullShape* nullShape = new btConvexHullShape();
m_staticBody = new btRigidBody(0,0,nullShape);
addRigidBody(m_staticBody,0,0);
}
b3GpuDynamicsWorld::~b3GpuDynamicsWorld()
@@ -63,6 +67,34 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
BT_PROFILE("stepSimulation");
{
for (int i=0;i<m_constraints.size();i++)
{
btTypedConstraint* constraint = m_constraints[i];
b3TypedConstraint* c = (b3TypedConstraint*) constraint->getUserConstraintPtr();
if (c)
{
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
btPoint2PointConstraint* p2 = (btPoint2PointConstraint*) constraint;
b3Point2PointConstraint* p3 = (b3Point2PointConstraint*) c;
p3->setPivotA((const b3Vector3&)p2->getPivotInA());
p3->setPivotB((const b3Vector3&)p2->getPivotInB());
p3->m_setting.m_damping = p2->m_setting.m_damping;
p3->m_setting.m_impulseClamp = p2->m_setting.m_impulseClamp;
p3->m_setting.m_tau = p2->m_setting.m_tau;
break;
};
default:
{
}
};
}
}
}
// detect any change (very simple)
{
@@ -115,6 +147,7 @@ int b3GpuDynamicsWorld::stepSimulation( btScalar timeStepUnused, int maxSubStep
if (m_cpuGpuSync)
{
BT_PROFILE("cpuGpuSync");
m_cpuGpuSync = false;
m_np->writeAllBodiesToGpu();
m_bp->writeAabbsToGpu();
@@ -409,7 +442,7 @@ void b3GpuDynamicsWorld::addRigidBody(btRigidBody* body)
body->setUserIndex(bodyIndex);
m_collisionObjects.push_back(body);
//btDynamicsWorld::addCollisionObject(
m_bodyUpdateRevisions.push_back(-1);
}
}
@@ -417,6 +450,11 @@ void b3GpuDynamicsWorld::removeRigidBody(btRigidBody* colObj)
{
m_cpuGpuSync = true;
btDynamicsWorld::removeCollisionObject(colObj);
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
{
m_bodyUpdateRevisions[i] = -1;
}
int bodyIndex = colObj->getUserIndex();
@@ -440,6 +478,11 @@ void b3GpuDynamicsWorld::removeCollisionObject(btCollisionObject* colObj)
{
m_cpuGpuSync = true;
btDynamicsWorld::removeCollisionObject(colObj);
m_bodyUpdateRevisions.resize(this->m_collisionObjects.size());
for (int i=0;i<m_bodyUpdateRevisions.size();i++)
{
m_bodyUpdateRevisions[i] = -1;
}
if (getNumCollisionObjects()==0)
{
m_uniqueShapes.resize(0);
@@ -511,28 +554,46 @@ void b3GpuDynamicsWorld::synchronizeSingleMotionState(btRigidBody* body)
}
}
void b3GpuDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("debugDrawWorld");
btCollisionWorld::debugDrawWorld();
}
void b3GpuDynamicsWorld::addConstraint(btTypedConstraint* constraint, bool disableCollisionsBetweenLinkedBodies)
{
constraint->setUserConstraintPtr(0);
m_constraints.push_back(constraint);
switch (constraint->getConstraintType())
{
case POINT2POINT_CONSTRAINT_TYPE:
{
b3Assert(constraint->getUserConstraintId()==-1);
btPoint2PointConstraint* p = (btPoint2PointConstraint*) constraint;
int rbA = p->getRigidBodyA().getUserIndex();
int rbB = p->getRigidBodyB().getUserIndex();
btVector3 pivotInB = p->getPivotInB();
if (rbB<=0)
{
pivotInB = p->getRigidBodyA().getWorldTransform()*p->getPivotInA();
rbB = m_staticBody->getUserIndex();
}
if (rbA>=0 && rbB>=0)
{
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)p->getPivotInB());
b3Point2PointConstraint* p2p = new b3Point2PointConstraint(rbA,rbB, (const b3Vector3&)p->getPivotInA(),(const b3Vector3&)pivotInB);
p2p->setBreakingImpulseThreshold(p->getBreakingImpulseThreshold());
constraint->setUserConstraintPtr(p2p);
m_rigidBodyPipeline->addConstraint(p2p);
} else
{
constraint->setUserConstraintPtr(0);
b3Error("invalid body index in addConstraint.\n");
}
break;

5
demos3/BasicGpuDemo/b3GpuDynamicsWorld.h
View File

@@ -20,7 +20,7 @@ class b3GpuDynamicsWorld : public btDynamicsWorld
btAlignedObjectArray<const class btCollisionShape*> m_uniqueShapes;
btAlignedObjectArray<int> m_uniqueShapeMapping;
btAlignedObjectArray<btTypedConstraint*> m_constraints;
btAlignedObjectArray<int> m_bodyUpdateRevisions;
class b3GpuRigidBodyPipeline* m_rigidBodyPipeline;
class b3GpuNarrowPhase* m_np;
class b3GpuSapBroadphase* m_bp;
@@ -29,6 +29,7 @@ class b3GpuDynamicsWorld : public btDynamicsWorld
btVector3 m_gravity;
bool m_cpuGpuSync;
float m_localTime;
class btRigidBody* m_staticBody;//used for picking and Bullet 2.x compatibility. In Bullet 3.x all constraints have explicitly 2 bodies.
int findOrRegisterCollisionShape(const btCollisionShape* colShape);
@@ -43,7 +44,7 @@ public:
virtual void synchronizeMotionStates();
void debugDrawWorld() {}
void debugDrawWorld();
void setGravity(const btVector3& gravity);

2
src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp
View File

@@ -1289,6 +1289,8 @@ b3Scalar b3PgsJacobiSolver::solveGroupCacheFriendlySetup(b3RigidBodyCL* bodies,
}
{
//it is ok to use solverConstraint.m_contactNormal instead of -solverConstraint.m_contactNormal
//because it gets multiplied iMJlB
b3Vector3 iMJlA = solverConstraint.m_contactNormal*rbA.getInvMass();
b3Vector3 iMJaA = invInertiaWorldA*solverConstraint.m_relpos1CrossNormal;
b3Vector3 iMJlB = solverConstraint.m_contactNormal*rbB.getInvMass();//sign of normal?

2
src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp
View File

@@ -77,7 +77,7 @@ void b3Point2PointConstraint::getInfo2 (b3ConstraintInfo2* info, const b3RigidBo
b3Transform trA;
trA.setIdentity();
trA.setOrigin(bodies[m_rbA].m_pos);
trA.setRotation(bodies[m_rbB].m_quat);
trA.setRotation(bodies[m_rbA].m_quat);
b3Transform trB;
trB.setIdentity();

2
src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h
View File

@@ -247,7 +247,7 @@ B3_ATTRIBUTE_ALIGNED64 (struct) b3SolverBody
//Optimization for the iterative solver: avoid calculating constant terms involving inertia, normal, relative position
B3_FORCE_INLINE void internalApplyImpulse(const b3Vector3& linearComponent, const b3Vector3& angularComponent,const b3Scalar impulseMagnitude)
{
if (m_originalBody)
//if (m_originalBody)
{
m_deltaLinearVelocity += linearComponent*impulseMagnitude*m_linearFactor;
m_deltaAngularVelocity += angularComponent*(impulseMagnitude*m_angularFactor);

29
src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp
View File

@@ -87,7 +87,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3A
b3Vector3 rayTo = rays[r].m_to;
float hitFraction = hitResults[r].m_hitFraction;
int sphereHit = -1;
int hitBodyIndex= -1;
for (int b=0;b<numBodies;b++)
{
@@ -95,20 +95,35 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray<b3RayInfo>& rays, b3A
const b3Vector3& pos = bodies[b].m_pos;
const b3Quaternion& orn = bodies[b].m_quat;
b3Scalar radius = 1;
switch (collidables[bodies[b].m_collidableIdx].m_shapeType)
{
case SHAPE_SPHERE:
{
b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius;
if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction))
{
sphereHit = b;
hitBodyIndex = b;
}
}
if (sphereHit>=0)
default:
{
static bool once=true;
if (once)
{
once=false;
b3Warning("Raytest: unsupported shape type\n");
}
}
}
}
if (hitBodyIndex>=0)
{
hitResults[r].m_hitFraction = hitFraction;
hitResults[r].m_hitPoint.setInterpolate3(rays[r].m_from, rays[r].m_to,hitFraction);
hitResults[r].m_hitNormal = (hitResults[r].m_hitPoint-bodies[sphereHit].m_pos).normalize();
hitResults[r].m_hitResult0 = sphereHit;
hitResults[r].m_hitNormal = (hitResults[r].m_hitPoint-bodies[hitBodyIndex].m_pos).normalize();
hitResults[r].m_hitResult0 = hitBodyIndex;
}
}

4
src/Bullet3OpenCL/RigidBody/b3Solver.cpp
View File

@@ -791,7 +791,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
int nContacts, const ConstraintCfg& cfg )
{
b3OpenCLArray<b3GpuConstraint4>* constraintNative =0;
contactCOut->resize(nContacts);
struct CB
{
int m_nContacts;
@@ -825,7 +825,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray<b3RigidBodyCL>* bodyBuf
}
contactCOut->resize(nContacts);
}
/*