Collision callbacks for 3D Physics

Modules/Contents/3DPhysics/Include/PolyPhysicsScene.h

@@ -33,6 +33,26 @@ namespace Polycode {
 	class PhysicsSceneEntity;
 	class PhysicsCharacter;
 	class PhysicsVehicle;
+	
+	class _PolyExport PhysicsSceneEvent : public Event {
+		public:
+			PhysicsSceneEvent() : Event () {
+				eventType = "PhysicsSceneEvent";
+			}
+			~PhysicsSceneEvent() {
+			}
+			
+			static const int COLLISION_EVENT = 0;
+			
+			PhysicsSceneEntity *entityA;
+			PhysicsSceneEntity *entityB;
+
+			Number appliedImpulse;
+						
+			Vector3 positionOnA;
+			Vector3 positionOnB;
+			Vector3 worldNormalOnB;				
+	};
 
 	/**
 	* A scene subclass that simulates physics for its children.
@@ -49,6 +69,10 @@ namespace Polycode {
 		
 		void removeEntity(SceneEntity *entity);
 		
+		void processWorldCollisions();
+		
+		PhysicsSceneEntity *getPhysicsEntityByCollisionObject(btCollisionObject *object);
+		
 			/** @name Physics scene
 			*  Public methods
 			*/
@@ -63,7 +87,13 @@ namespace Polycode {
 		PhysicsCharacter *addCharacterChild(SceneEntity *newEntity, Number mass, Number friction, Number stepSize, int group  = 1);
 		void removeCharacterChild(PhysicsCharacter *character);
 		
+		
+		void setVelocity(SceneEntity *entity, Vector3 velocity);
+		void warpEntity(SceneEntity *entity, Vector3 position);
+		
 		PhysicsVehicle *addVehicleChild(SceneEntity *newEntity, Number mass, Number friction, int group  = 1);
+		
+		void setGravity(Vector3 gravity);
 			//@}
 			// ----------------------------------------------------------------------------------------------------------------
 

Modules/Contents/3DPhysics/Include/PolyPhysicsSceneEntity.h

@@ -49,6 +49,9 @@ namespace Polycode {
 		SceneEntity *getSceneEntity();
 		void setFriction(Number friction);		
 		int getType() { return type; }	
+		
+			void setVelocity(Vector3 velocity);
+			void warpTo(Vector3 position);
 			//@}
 			// ----------------------------------------------------------------------------------------------------------------
 			

Modules/Contents/3DPhysics/Source/PolyCollisionSceneEntity.cpp

@@ -61,6 +61,14 @@ btCollisionShape *CollisionSceneEntity::createCollisionShape(SceneEntity *entity
 	
 	btCollisionShape *collisionShape = NULL;	
 	
+	Vector3 entityScale = entity->getScale();
+	Number largestScale = entityScale.x;
+	if(entityScale.y > largestScale)
+		largestScale = entityScale.y;
+	if(entityScale.z > largestScale)
+		largestScale = entityScale.z;
+
+	
 	switch(type) {
 		case SHAPE_CAPSULE:
 		case CHARACTER_CONTROLLER:
@@ -83,10 +91,10 @@ btCollisionShape *CollisionSceneEntity::createCollisionShape(SceneEntity *entity
 			collisionShape = new btBoxShape(btVector3(entity->bBox.x/2.0f, 0.05,entity->bBox.z/2.0f));			
 			break;
 		case SHAPE_BOX:
-			collisionShape = new btBoxShape(btVector3(entity->bBox.x/2.0f, entity->bBox.y/2.0f,entity->bBox.z/2.0f));			
+			collisionShape = new btBoxShape(btVector3(entity->bBox.x/2.0f*entityScale.x, entity->bBox.y/2.0f*entityScale.y,entity->bBox.z/2.0f*entityScale.z));			
 			break;
 		case SHAPE_SPHERE:
-			collisionShape = new btSphereShape(entity->bBox.x/2.0f);
+			collisionShape = new btSphereShape(entity->bBox.x/2.0f*largestScale);
 			break;
 		case SHAPE_MESH:
 		{

Modules/Contents/3DPhysics/Source/PolyPhysicsScene.cpp

@@ -42,6 +42,11 @@ PhysicsScene::~PhysicsScene() {
 	
 }
 
+void worldTickCallback(btDynamicsWorld *world, btScalar timeStep) {
+	PhysicsScene *physicsScene = (PhysicsScene*)world->getWorldUserInfo();
+	physicsScene->processWorldCollisions();
+}
+
 void PhysicsScene::initPhysicsScene() {
 		
 	
@@ -67,6 +72,59 @@ void PhysicsScene::initPhysicsScene() {
 	sweepBP->getOverlappingPairCache()->setInternalGhostPairCallback(new btGhostPairCallback());
 	
 	world = physicsWorld;
+	
+	physicsWorld->setInternalTickCallback(worldTickCallback, this);
+}
+
+void PhysicsScene::setGravity(Vector3 gravity) {
+	physicsWorld->setGravity(btVector3(gravity.x, gravity.y, gravity.z));
+}
+
+PhysicsSceneEntity *PhysicsScene::getPhysicsEntityByCollisionObject(btCollisionObject *object) {
+	for(int i=0; i < physicsChildren.size(); i++) {
+		PhysicsSceneEntity *entity = physicsChildren[i];
+		if(entity->rigidBody == object) {
+			return entity;
+		}
+	}
+	return  NULL;
+}
+
+void PhysicsScene::processWorldCollisions() {
+
+	int numManifolds = physicsWorld->getDispatcher()->getNumManifolds();
+	for (int i=0;i<numManifolds;i++)
+	{
+		btPersistentManifold* contactManifold =  world->getDispatcher()->getManifoldByIndexInternal(i);
+		btCollisionObject* obA = static_cast<btCollisionObject*>(contactManifold->getBody0());
+		btCollisionObject* obB = static_cast<btCollisionObject*>(contactManifold->getBody1());
+	
+		
+		int numContacts = contactManifold->getNumContacts();
+		for (int j=0;j<numContacts;j++)
+		{
+			btManifoldPoint& pt = contactManifold->getContactPoint(j);
+			if (pt.getDistance()<0.f)
+			{
+				const btVector3& ptA = pt.getPositionWorldOnA();
+				const btVector3& ptB = pt.getPositionWorldOnB();
+				const btVector3& normalOnB = pt.m_normalWorldOnB;
+				const btScalar appliedImpulse = pt.m_appliedImpulse;
+				
+				PhysicsSceneEvent *event = new PhysicsSceneEvent();
+				event->positionOnA = Vector3(ptA.x(), ptA.y(), ptA.z());
+				event->positionOnB = Vector3(ptB.x(), ptB.y(), ptB.z());
+				event->worldNormalOnB = Vector3(normalOnB.x(), normalOnB.y(), normalOnB.z());
+				event->appliedImpulse = appliedImpulse;				
+								
+				event->entityA = getPhysicsEntityByCollisionObject(obA);	
+				event->entityB = getPhysicsEntityByCollisionObject(obB);
+												
+				dispatchEvent(event, PhysicsSceneEvent::COLLISION_EVENT);
+			}
+		}
+	}
+
 }
 
 void PhysicsScene::Update() {
@@ -87,6 +145,20 @@ void PhysicsScene::Update() {
 	
 }
 
+void PhysicsScene::setVelocity(SceneEntity *entity, Vector3 velocity) {
+	PhysicsSceneEntity *physicsEntity = getPhysicsEntityBySceneEntity(entity);
+	if(physicsEntity) {
+		physicsEntity->setVelocity(velocity);
+	}
+}
+
+void PhysicsScene::warpEntity(SceneEntity *entity, Vector3 position) {
+	PhysicsSceneEntity *physicsEntity = getPhysicsEntityBySceneEntity(entity);
+	if(physicsEntity) {
+		physicsEntity->warpTo(position);
+	}
+}
+
 PhysicsCharacter *PhysicsScene::addCharacterChild(SceneEntity *newEntity,Number mass, Number friction, Number stepSize, int group) {
 	addEntity(newEntity);	
 	PhysicsCharacter *newPhysicsEntity = new PhysicsCharacter(newEntity, mass, friction, stepSize);
@@ -205,7 +277,7 @@ PhysicsSceneEntity *PhysicsScene::trackPhysicsChild(SceneEntity *newEntity, int
 	PhysicsSceneEntity *newPhysicsEntity = new PhysicsSceneEntity(newEntity, type, mass, friction,restitution);
 	physicsWorld->addRigidBody(newPhysicsEntity->rigidBody, group,  btBroadphaseProxy::AllFilter); //btBroadphaseProxy::StaticFilter|btBroadphaseProxy::DefaultFilter);	
 //	world->addCollisionObject(newPhysicsEntity->collisionObject, group);	
-	//	newPhysicsEntity->rigidBody->setActivationState(ISLAND_SLEEPING);	
+	//newPhysicsEntity->rigidBody->setActivationState(ISLAND_SLEEPING);	
 	physicsChildren.push_back(newPhysicsEntity);
 	collisionChildren.push_back(newPhysicsEntity);	
 	return newPhysicsEntity;	

Modules/Contents/3DPhysics/Source/PolyPhysicsSceneEntity.cpp

@@ -194,7 +194,7 @@ PhysicsSceneEntity::PhysicsSceneEntity(SceneEntity *entity, int type, Number mas
 	for(int i=0; i < 16; i++) {
 		mat[i] = ent_mat.ml[i];
 	}	
-	transform.setFromOpenGLMatrix(mat);
+	transform.setFromOpenGLMatrix(mat);	
 	
 	if(mass != 0.0f) {
 		shape->calculateLocalInertia(mass,localInertia);
@@ -227,11 +227,31 @@ void PhysicsSceneEntity::Update() {
 	}
 	
 	free(mat);
-		
-	sceneEntity->setTransformByMatrixPure(m);	
+	
+	sceneEntity->setTransformByMatrixPure(m);
 //	collisionObject->getWorldTransform().setFromOpenGLMatrix(mat);
 }
 
+void PhysicsSceneEntity::setVelocity(Vector3 velocity) {
+	rigidBody->setLinearVelocity(btVector3(velocity.x, velocity.y, velocity.z));
+//	rigidBody->applyForce(btVector3(velocity.x, velocity.y, velocity.z), btVector3(0,0,0));
+}
+
+void PhysicsSceneEntity::warpTo(Vector3 position) {
+	btTransform transform;
+	
+	Matrix4 ent_mat = sceneEntity->getConcatenatedMatrix();
+	
+	btScalar mat[16];
+	for(int i=0; i < 16; i++) {
+		mat[i] = ent_mat.ml[i];
+	}	
+	transform.setFromOpenGLMatrix(mat);	
+	transform.setOrigin(btVector3(position.x,position.y,position.z));	
+	
+	rigidBody->setCenterOfMassTransform(transform);
+}
+
 SceneEntity *PhysicsSceneEntity::getSceneEntity() {
 	return sceneEntity;
 }