Added rigid body queries to PhysicsWorld.

Docs/ScriptAPI.dox

@@ -4101,6 +4101,8 @@ Methods:<br>
 - PhysicsRaycastResult[]@ Raycast(const Ray&, float arg1 = M_INFINITY, uint arg2 = 0xffff)
 - PhysicsRaycastResult RaycastSingle(const Ray&, float arg1 = M_INFINITY, uint arg2 = 0xffff)
 - PhysicsRaycastResult SphereCast(const Ray&, float, float arg2 = M_INFINITY, uint arg3 = 0xffff)
+- RigidBody@[]@ GetRigidBodies(const Sphere&, uint arg1 = 0xffff)
+- RigidBody@[]@ GetRigidBodies(const BoundingBox&, uint arg1 = 0xffff)
 - void DrawDebugGeometry(bool)
 
 Properties:<br>

Engine/Engine/PhysicsAPI.cpp

@@ -205,6 +205,20 @@ static PhysicsRaycastResult PhysicsWorldSphereCast(const Ray& ray, float radius,
     return result;
 }
 
+static CScriptArray* PhysicsWorldGetRigidBodiesSphere(const Sphere& sphere, unsigned collisionMask, PhysicsWorld* ptr)
+{
+    PODVector<RigidBody*> result;
+    ptr->GetRigidBodies(result, sphere, collisionMask);
+    return VectorToHandleArray<RigidBody>(result, "Array<RigidBody@>");
+}
+
+static CScriptArray* PhysicsWorldGetRigidBodiesBox(const BoundingBox& box, unsigned collisionMask, PhysicsWorld* ptr)
+{
+    PODVector<RigidBody*> result;
+    ptr->GetRigidBodies(result, box, collisionMask);
+    return VectorToHandleArray<RigidBody>(result, "Array<RigidBody@>");
+}
+
 static void RegisterPhysicsWorld(asIScriptEngine* engine)
 {
     engine->RegisterObjectType("PhysicsRaycastResult", sizeof(PhysicsRaycastResult), asOBJ_VALUE | asOBJ_APP_CLASS_C);
@@ -222,6 +236,8 @@ static void RegisterPhysicsWorld(asIScriptEngine* engine)
     engine->RegisterObjectMethod("PhysicsWorld", "Array<PhysicsRaycastResult>@ Raycast(const Ray&in, float maxDistance = M_INFINITY, uint collisionMask = 0xffff)", asFUNCTION(PhysicsWorldRaycast), asCALL_CDECL_OBJLAST);
     engine->RegisterObjectMethod("PhysicsWorld", "PhysicsRaycastResult RaycastSingle(const Ray&in, float maxDistance = M_INFINITY, uint collisionMask = 0xffff)", asFUNCTION(PhysicsWorldRaycastSingle), asCALL_CDECL_OBJLAST);
     engine->RegisterObjectMethod("PhysicsWorld", "PhysicsRaycastResult SphereCast(const Ray&in, float, float maxDistance = M_INFINITY, uint collisionMask = 0xffff)", asFUNCTION(PhysicsWorldSphereCast), asCALL_CDECL_OBJLAST);
+    engine->RegisterObjectMethod("PhysicsWorld", "Array<RigidBody@>@ GetRigidBodies(const Sphere&in, uint collisionMask = 0xffff)", asFUNCTION(PhysicsWorldGetRigidBodiesSphere), asCALL_CDECL_OBJLAST);
+    engine->RegisterObjectMethod("PhysicsWorld", "Array<RigidBody@>@ GetRigidBodies(const BoundingBox&in, uint collisionMask = 0xffff)", asFUNCTION(PhysicsWorldGetRigidBodiesBox), asCALL_CDECL_OBJLAST);
     engine->RegisterObjectMethod("PhysicsWorld", "void DrawDebugGeometry(bool)", asMETHODPR(PhysicsWorld, DrawDebugGeometry, (bool), void), asCALL_THISCALL);
     engine->RegisterObjectMethod("PhysicsWorld", "void set_gravity(Vector3)", asMETHOD(PhysicsWorld, SetGravity), asCALL_THISCALL);
     engine->RegisterObjectMethod("PhysicsWorld", "Vector3 get_gravity() const", asMETHOD(PhysicsWorld, GetGravity), asCALL_THISCALL);

Engine/Physics/PhysicsWorld.cpp

@@ -40,12 +40,11 @@
 
 #include <BulletCollision/BroadphaseCollision/btDbvtBroadphase.h>
 #include <BulletCollision/CollisionDispatch/btDefaultCollisionConfiguration.h>
+#include <BulletCollision/CollisionShapes/btBoxShape.h>
 #include <BulletCollision/CollisionShapes/btSphereShape.h>
 #include <BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h>
 #include <BulletDynamics/Dynamics/btDiscreteDynamicsWorld.h>
 
-#include "DebugNew.h"
-
 static const int DEFAULT_FPS = 60;
 static const Vector3 DEFAULT_GRAVITY = Vector3(0.0f, -9.81f, 0.0f);
 
@@ -64,6 +63,30 @@ void InternalTickCallback(btDynamicsWorld *world, btScalar timeStep)
     static_cast<PhysicsWorld*>(world->getWorldUserInfo())->PostStep(timeStep);
 }
 
+/// Callback for physics world queries.
+struct PhysicsQueryCallback : public btCollisionWorld::ContactResultCallback
+{
+    /// Construct.
+    PhysicsQueryCallback(PODVector<RigidBody*>& result) : result_(result)
+    {
+    }
+    
+    /// Add a contact result.
+    virtual btScalar addSingleResult(btManifoldPoint &, const btCollisionObject *colObj0, int, int, const btCollisionObject *colObj1, int, int)
+    {
+        RigidBody* body = reinterpret_cast<RigidBody*>(colObj0->getUserPointer());
+        if (body && !result_.Contains(body))
+            result_.Push(body);
+        body = reinterpret_cast<RigidBody*>(colObj1->getUserPointer());
+        if (body && !result_.Contains(body))
+            result_.Push(body);
+        return 0.0f;
+    }
+    
+    /// Found rigid bodies.
+    PODVector<RigidBody*>& result_;
+};
+
 OBJECTTYPESTATIC(PhysicsWorld);
 
 PhysicsWorld::PhysicsWorld(Context* context) :
@@ -261,6 +284,8 @@ void PhysicsWorld::RaycastSingle(PhysicsRaycastResult& result, const Ray& ray, f
 
 void PhysicsWorld::SphereCast(PhysicsRaycastResult& result, const Ray& ray, float radius, float maxDistance, unsigned collisionMask)
 {
+    PROFILE(PhysicsSphereCast);
+    
     btSphereShape shape(radius);
     
     btCollisionWorld::ClosestConvexResultCallback convexCallback(ToBtVector3(ray.origin_), ToBtVector3(ray.origin_ +
@@ -287,6 +312,45 @@ void PhysicsWorld::SphereCast(PhysicsRaycastResult& result, const Ray& ray, floa
     }
 }
 
+void PhysicsWorld::GetRigidBodies(PODVector<RigidBody*>& result, const Sphere& sphere, unsigned collisionMask)
+{
+    PROFILE(PhysicsWorld_GetRigidBodies);
+    
+    result.Clear();
+    
+    btSphereShape sphereShape(sphere.radius_);
+    btRigidBody* tempRigidBody = new btRigidBody(1.0f, 0, &sphereShape);
+    tempRigidBody->setWorldTransform(btTransform(btQuaternion::getIdentity(), ToBtVector3(sphere.center_)));
+    // Need to activate the temporary rigid body to get reliable results from static, sleeping objects
+    tempRigidBody->activate();
+    world_->addRigidBody(tempRigidBody, (short)0xffff, (short)collisionMask);
+    
+    PhysicsQueryCallback callback(result);
+    world_->contactTest(tempRigidBody, callback);
+    
+    world_->removeRigidBody(tempRigidBody);
+    delete tempRigidBody;
+}
+
+void PhysicsWorld::GetRigidBodies(PODVector<RigidBody*>& result, const BoundingBox& box, unsigned collisionMask)
+{
+    PROFILE(PhysicsWorld_GetRigidBodies);
+    
+    result.Clear();
+    
+    btBoxShape boxShape(ToBtVector3(box.HalfSize()));
+    btRigidBody* tempRigidBody = new btRigidBody(1.0f, 0, &boxShape);
+    tempRigidBody->setWorldTransform(btTransform(btQuaternion::getIdentity(), ToBtVector3(box.Center())));
+    tempRigidBody->activate();
+    world_->addRigidBody(tempRigidBody, (short)0xffff, (short)collisionMask);
+    
+    PhysicsQueryCallback callback(result);
+    world_->contactTest(tempRigidBody, callback);
+    
+    world_->removeRigidBody(tempRigidBody);
+    delete tempRigidBody;
+}
+
 Vector3 PhysicsWorld::GetGravity() const
 {
     return ToVector3(world_->getGravity());

Engine/Physics/PhysicsWorld.h

@@ -135,6 +135,10 @@ public:
     /// Perform a physics world swept sphere test and return the closest hit.
     void SphereCast(PhysicsRaycastResult& result, const Ray& ray, float radius, float maxDistance, unsigned collisionMask =
         M_MAX_UNSIGNED);
+    /// Perform a sphere test into the physics world.
+    void GetRigidBodies(PODVector<RigidBody*>& result, const Sphere& sphere, unsigned collisionMask = M_MAX_UNSIGNED);
+    /// Perform a bounding box test into the physics world.
+    void GetRigidBodies(PODVector<RigidBody*>& result, const BoundingBox& box, unsigned collisionMask = M_MAX_UNSIGNED);
     /// Return gravity.
     Vector3 GetGravity() const;
     /// Return whether interpolation between simulation steps is enabled.