PhysicsWorld can add and remove voxels

libraries/physics/src/DoubleHashKey.h

@@ -18,7 +18,10 @@ public:
     static unsigned int hashFunction(unsigned int value, int primeIndex);
     static unsigned int hashFunction2(unsigned int value);
 
-    DoubleHashKey(unsigned int value, int primIndex = 0) : _hash(hashFunction(value, primIndex)), _hash2(hashFunction2(value)) { }
+    DoubleHashKey(unsigned int value, int primeIndex = 0) : 
+        _hash(hashFunction(value, primeIndex)), 
+        _hash2(hashFunction2(value)) { 
+    }
 
     bool equals(const DoubleHashKey& other) const {
         return _hash == other._hash && _hash2 == other._hash2;
@@ -27,6 +30,7 @@ public:
     unsigned int getHash() const { return (unsigned int)_hash; }
 
 protected:
+    // the default ctor is protected so that only derived classes can use it
     DoubleHashKey() : _hash(0), _hash2(0) { }
 
     int _hash;

libraries/physics/src/PhysicsWorld.cpp

@@ -21,3 +21,63 @@ void PhysicsWorld::init() {
     _constraintSolver = new btSequentialImpulseConstraintSolver;
     _dynamicsWorld = new btDiscreteDynamicsWorld(_collisionDispatcher, _broadphaseFilter, _constraintSolver, _collisionConfig);
 }
+
+/// \return true if Voxel added
+/// \param position the minimum corner of the voxel
+/// \param scale the length of the voxel side
+bool PhysicsWorld::addVoxel(const glm::vec3& position, float scale) {
+    glm::vec3 halfExtents = glm::vec3(0.5f * scale);
+    glm::vec3 center = position + halfExtents;
+    PositionHashKey key(center);
+    VoxelObject* proxy = _voxels.find(key);
+    if (!proxy) {
+        // create a shape
+        ShapeInfo info;
+        info.setBox(halfExtents);
+        btCollisionShape* shape = _shapeManager.getShape(info);
+
+        // NOTE: the shape creation will fail when the size of the voxel is out of range
+        if (shape) {
+            // create a collisionObject
+            btCollisionObject* object = new btCollisionObject();
+            object->setCollisionShape(shape);
+            btTransform transform;
+            transform.setIdentity();
+            transform.setOrigin(btVector3(center.x, center.y, center.z));
+            object->setWorldTransform(transform);
+
+            // add to map and world
+            _voxels.insert(key, VoxelObject(center, object));
+            _dynamicsWorld->addCollisionObject(object);
+            return true;
+        }
+    }
+    return false;
+}
+
+/// \return true if Voxel removed
+/// \param position the minimum corner of the voxel
+/// \param scale the length of voxel side
+bool PhysicsWorld::removeVoxel(const glm::vec3& position, float scale) {
+    glm::vec3 halfExtents = glm::vec3(0.5f * scale);
+    glm::vec3 center = position + halfExtents;
+    PositionHashKey key(center);
+    VoxelObject* proxy = _voxels.find(key);
+    if (proxy) {
+        // remove from world
+        assert(proxy->_object);
+        _dynamicsWorld->removeCollisionObject(proxy->_object);
+
+        // release shape
+        ShapeInfo info;
+        info.setBox(halfExtents);
+        bool released = _shapeManager.releaseShape(info);
+        assert(released);
+
+        // delete object and remove from voxel map
+        delete proxy->_object;
+        _voxels.remove(key);
+        return true;
+    }
+    return false;
+}

libraries/physics/src/PhysicsWorld.h

@@ -17,6 +17,35 @@
 #include <btBulletDynamicsCommon.h>
 
 #include "ShapeManager.h"
+#include "BulletUtil.h"
+#include "PositionHashKey.h"
+#include "VoxelObject.h"
+
+#ifdef COLLIDABLE
+enum MotionType {
+    MOTION_TYPE_STATIC,
+    MOTION_TYPE_DYNAMIC,
+    MOTION_TYPE_KINEMATIC
+};
+
+class EntityObject {
+public:
+    EntityObject();
+
+    bool makeStatic();
+    bool makeDynamic();
+    bool makeKinematic();
+
+    MotionType getMotionType() const { return _motionType; }
+
+private:
+    btCollisionObject* _object;
+    btMotionState* _motionState;
+    MotionType _motionType;
+    btVector3 _inertiaDiagLocal;
+    float _mass;
+};
+#endif // COLLIDABLE
 
 class PhysicsWorld {
 public:
@@ -27,7 +56,17 @@ public:
     ~PhysicsWorld();
 
     void init();
-    
+
+    /// \return true if Voxel added
+    /// \param position the minimum corner of the voxel
+    /// \param scale the length of the voxel side
+    bool addVoxel(const glm::vec3& position, float scale);
+
+    /// \return true if Voxel removed
+    /// \param position the minimum corner of the voxel
+    /// \param scale the length of the voxel side
+    bool removeVoxel(const glm::vec3& position, float scale);
+
 protected:
     btDefaultCollisionConfiguration* _collisionConfig;
     btCollisionDispatcher* _collisionDispatcher;
@@ -38,6 +77,7 @@ protected:
     ShapeManager _shapeManager;
 
 private:
+    btHashMap<PositionHashKey, VoxelObject> _voxels;
 };
 
 

libraries/physics/src/PositionHashKey.cpp

@@ -0,0 +1,37 @@
+//
+//  PositionHashKey.cpp
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2014.11.05
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#include <math.h>
+
+#include "PositionHashKey.h"
+
+// static 
+int PositionHashKey::computeHash(const glm::vec3& center) {
+    // NOTE: 0.49f is used to bump the float up almost half a millimeter
+    // so the cast to int produces a round() effect rather than a floor()
+    int hash = DoubleHashKey::hashFunction((int)(center.x * MILLIMETERS_PER_METER + copysignf(1.0f, center.x) * 0.49f), 0);
+    hash ^= DoubleHashKey::hashFunction((int)(center.y * MILLIMETERS_PER_METER + copysignf(1.0f, center.y) * 0.49f), 1);
+    return hash ^ DoubleHashKey::hashFunction((int)(center.z * MILLIMETERS_PER_METER + copysignf(1.0f, center.z) * 0.49f), 2);
+}
+
+// static 
+int PositionHashKey::computeHash2(const glm::vec3& center) {
+    // NOTE: 0.49f is used to bump the float up almost half a millimeter
+    // so the cast to int produces a round() effect rather than a floor()
+    int hash = DoubleHashKey::hashFunction2((int)(center.x * MILLIMETERS_PER_METER + copysignf(1.0f, center.x) * 0.49f));
+    hash ^= DoubleHashKey::hashFunction2((int)(center.y * MILLIMETERS_PER_METER + copysignf(1.0f, center.y) * 0.49f));
+    return hash ^ DoubleHashKey::hashFunction2((int)(center.z * MILLIMETERS_PER_METER + copysignf(1.0f, center.z) * 0.49f));
+}
+
+PositionHashKey::PositionHashKey(glm::vec3 center) : DoubleHashKey() {
+    _hash = PositionHashKey::computeHash(center);
+    _hash2 = PositionHashKey::computeHash2(center);
+}

libraries/physics/src/PositionHashKey.h

@@ -0,0 +1,28 @@
+//
+//  PositionHashKey.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2014.11.05
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_PositionHashKey_h
+#define hifi_PositionHashKey_h
+
+#include <glm/glm.hpp>
+
+#include <SharedUtil.h>
+
+#include "DoubleHashKey.h"
+
+class PositionHashKey : public DoubleHashKey {
+public:
+    static int computeHash(const glm::vec3& center);
+    static int computeHash2(const glm::vec3& center);
+    PositionHashKey(glm::vec3 center);
+};
+
+#endif // hifi_PositionHashKey_h

libraries/physics/src/ShapeInfo.cpp

@@ -11,8 +11,12 @@
 
 #ifdef USE_BULLET_PHYSICS
 
+#include <math.h>
 #include <btBulletDynamicsCommon.h>
 
+#include <SharedUtil.h> // for MILLIMETERS_PER_METER
+
+#include "BulletUtil.h"
 #include "ShapeInfo.h"
 
 void ShapeInfo::collectInfo(const btCollisionShape* shape) {
@@ -60,6 +64,14 @@ void ShapeInfo::setBox(const btVector3& halfExtents) {
     _data.push_back(halfExtents);
 }
 
+void ShapeInfo::setBox(const glm::vec3& halfExtents) {
+    _type = BOX_SHAPE_PROXYTYPE;
+    _data.clear();
+    btVector3 bulletHalfExtents;
+    glmToBullet(halfExtents, bulletHalfExtents);
+    _data.push_back(bulletHalfExtents);
+}
+
 void ShapeInfo::setSphere(float radius) {
     _type = SPHERE_SHAPE_PROXYTYPE;
     _data.clear();
@@ -80,8 +92,6 @@ void ShapeInfo::setCapsule(float radius, float height) {
     _data.push_back(btVector3(radius, 0.5f * height, 0.0f));
 }
 
-const float MILLIMETERS_PER_METER = 1000.0f;
-
 int ShapeInfo::computeHash() const {
     // scramble the bits of the type
     // TODO?: provide lookup table for hash of _type?
@@ -93,8 +103,10 @@ int ShapeInfo::computeHash() const {
     for (int i = 0; i < numData; ++i) {
         tmpData = _data[i];
         for (int j = 0; j < 3; ++j) {
-            // multiply these mm by a new prime
-            unsigned int floatHash = DoubleHashKey::hashFunction((unsigned int)(tmpData[j] * MILLIMETERS_PER_METER + 0.49f), primeIndex++);
+            // NOTE: 0.49f is used to bump the float up almost half a millimeter
+            // so the cast to int produces a round() effect rather than a floor()
+            unsigned int floatHash = 
+                DoubleHashKey::hashFunction((int)(tmpData[j] * MILLIMETERS_PER_METER + copysignf(1.0f, tmpData[j]) * 0.49f), primeIndex++);
             hash ^= floatHash;
         }
     }
@@ -111,7 +123,10 @@ int ShapeInfo::computeHash2() const {
     for (int i = 0; i < numData; ++i) {
         tmpData = _data[i];
         for (int j = 0; j < 3; ++j) {
-            unsigned int floatHash = DoubleHashKey::hashFunction2((unsigned int)(tmpData[j] * MILLIMETERS_PER_METER + 0.49f));
+            // NOTE: 0.49f is used to bump the float up almost half a millimeter
+            // so the cast to int produces a round() effect rather than a floor()
+            unsigned int floatHash = 
+                DoubleHashKey::hashFunction2((int)(tmpData[j] * MILLIMETERS_PER_METER + copysignf(1.0f, tmpData[j]) * 0.49f));
             hash += ~(floatHash << 17);
             hash ^=  (floatHash >> 11);
             hash +=  (floatHash << 4); 

libraries/physics/src/ShapeInfo.h

@@ -16,6 +16,7 @@
 
 #include <btBulletDynamicsCommon.h>
 #include <LinearMath/btHashMap.h>
+#include <glm/glm.hpp>
 
 #include "DoubleHashKey.h"
 
@@ -30,6 +31,7 @@ public:
     void collectInfo(const btCollisionShape* shape);
 
     void setBox(const btVector3& halfExtents);
+    void setBox(const glm::vec3& halfExtents);
     void setSphere(float radius);
     void setCylinder(float radius, float height);
     void setCapsule(float radius, float height);

libraries/physics/src/VoxelObject.h

@@ -0,0 +1,31 @@
+//
+//  VoxelObject.h
+//  libraries/physcis/src
+//
+//  Created by Andrew Meadows 2014.11.05
+//  Copyright 2014 High Fidelity, Inc.
+//
+//  Distributed under the Apache License, Version 2.0.
+//  See the accompanying file LICENSE or http://www.apache.org/licenses/LICENSE-2.0.html
+//
+
+#ifndef hifi_VoxelObject_h
+#define hifi_VoxelObject_h
+
+#ifdef USE_BULLET_PHYSICS
+
+#include <btBulletDynamicsCommon.h>
+#include <glm/glm.hpp>
+
+// VoxelObject is a simple wrapper for tracking a Voxel in a PhysicsWorld
+class VoxelObject {
+public:
+    VoxelObject(const glm::vec3& center, btCollisionObject* object) : _object(object), _center(center) {
+        assert(object != NULL);
+    }
+    btCollisionObject* _object;
+    glm::vec3 _center;
+};
+
+#endif // USE_BULLET_PHYSICS
+#endif // hifi_VoxelObject_h