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

Picking test works, holding shift in App_BasicGpuDemo_*

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

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)
+#if (BT_BULLET_VERSION >= 282)
-//#define BT_USE_BODY_UPDATE_REVISION
+#define BT_USE_BODY_UPDATE_REVISION
-//#endif
+#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;

demos3/BasicGpuDemo/b3GpuDynamicsWorld.h

@@ -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);
 

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?

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();

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);

src/Bullet3OpenCL/Raycast/b3GpuRaycast.cpp

@@ -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)
-
-			if (sphere_intersect(pos,  radius, rayFrom, rayTo,hitFraction))
 			{
-				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_hitNormal = (hitResults[r].m_hitPoint-bodies[hitBodyIndex].m_pos).normalize();
-			hitResults[r].m_hitResult0 = sphereHit;
+			hitResults[r].m_hitResult0 = hitBodyIndex;
 		}
 
 	}

src/Bullet3OpenCL/RigidBody/b3Solver.cpp

@@ -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);
+	
 }
 
 /*