diff --git a/Demos3/BasicGpuDemo/BasicGpuDemo.cpp b/Demos3/BasicGpuDemo/BasicGpuDemo.cpp index 881dae722..1022513e1 100644 --- a/Demos3/BasicGpuDemo/BasicGpuDemo.cpp +++ b/Demos3/BasicGpuDemo/BasicGpuDemo.cpp @@ -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 diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp index 6cca67ebf..f815d49d0 100644 --- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp +++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.cpp @@ -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;igetUserConstraintPtr(); + 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;igetUserIndex(); @@ -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;isetUserConstraintPtr(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; diff --git a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h index 28d172726..0b5b2847e 100644 --- a/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h +++ b/demos3/BasicGpuDemo/b3GpuDynamicsWorld.h @@ -20,7 +20,7 @@ class b3GpuDynamicsWorld : public btDynamicsWorld btAlignedObjectArray m_uniqueShapes; btAlignedObjectArray m_uniqueShapeMapping; btAlignedObjectArray m_constraints; - + btAlignedObjectArray m_bodyUpdateRevisions; class b3GpuRigidBodyPipeline* m_rigidBodyPipeline; class b3GpuNarrowPhase* m_np; class b3GpuSapBroadphase* m_bp; @@ -29,7 +29,8 @@ 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); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp index de54235de..d8432555f 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3PgsJacobiSolver.cpp @@ -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? diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp index c2dd32a75..849c40407 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp +++ b/src/Bullet3Dynamics/ConstraintSolver/b3Point2PointConstraint.cpp @@ -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(); diff --git a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h index 5ca5ed7e4..b25c6c085 100644 --- a/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h +++ b/src/Bullet3Dynamics/ConstraintSolver/b3SolverBody.h @@ -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); diff --git a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp index 7bae4b154..df6606273 100644 --- a/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp +++ b/src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp @@ -87,7 +87,7 @@ void b3GpuRaycast::castRaysHost(const b3AlignedObjectArray& 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& rays, b3A const b3Vector3& pos = bodies[b].m_pos; const b3Quaternion& orn = bodies[b].m_quat; - b3Scalar radius = 1; - - if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction)) + switch (collidables[bodies[b].m_collidableIdx].m_shapeType) { - sphereHit = b; + case SHAPE_SPHERE: + { + b3Scalar radius = collidables[bodies[b].m_collidableIdx].m_radius; + if (sphere_intersect(pos, radius, rayFrom, rayTo,hitFraction)) + { + hitBodyIndex = b; + } + } + + default: + { + static bool once=true; + if (once) + { + once=false; + b3Warning("Raytest: unsupported shape type\n"); + } + } } } - if (sphereHit>=0) + 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; } } diff --git a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp index 2f4390219..89672641b 100644 --- a/src/Bullet3OpenCL/RigidBody/b3Solver.cpp +++ b/src/Bullet3OpenCL/RigidBody/b3Solver.cpp @@ -791,7 +791,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf int nContacts, const ConstraintCfg& cfg ) { b3OpenCLArray* constraintNative =0; - + contactCOut->resize(nContacts); struct CB { int m_nContacts; @@ -825,7 +825,7 @@ void b3Solver::convertToConstraints( const b3OpenCLArray* bodyBuf } - contactCOut->resize(nContacts); + } /*