From 8e90cca2901667e6e1ed49b452f9be4de1c7ff06 Mon Sep 17 00:00:00 2001 From: Andrew Meadows Date: Wed, 5 Nov 2014 13:05:43 -0800 Subject: [PATCH] PhysicsWorld can add and remove voxels --- libraries/physics/src/DoubleHashKey.h | 6 ++- libraries/physics/src/PhysicsWorld.cpp | 60 +++++++++++++++++++++++ libraries/physics/src/PhysicsWorld.h | 42 +++++++++++++++- libraries/physics/src/PositionHashKey.cpp | 37 ++++++++++++++ libraries/physics/src/PositionHashKey.h | 28 +++++++++++ libraries/physics/src/ShapeInfo.cpp | 25 ++++++++-- libraries/physics/src/ShapeInfo.h | 2 + libraries/physics/src/VoxelObject.h | 31 ++++++++++++ 8 files changed, 224 insertions(+), 7 deletions(-) create mode 100644 libraries/physics/src/PositionHashKey.cpp create mode 100644 libraries/physics/src/PositionHashKey.h create mode 100644 libraries/physics/src/VoxelObject.h diff --git a/libraries/physics/src/DoubleHashKey.h b/libraries/physics/src/DoubleHashKey.h index 090eab38a6..0dbf2430e2 100644 --- a/libraries/physics/src/DoubleHashKey.h +++ b/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; diff --git a/libraries/physics/src/PhysicsWorld.cpp b/libraries/physics/src/PhysicsWorld.cpp index 9364b77e65..faa5757e4a 100644 --- a/libraries/physics/src/PhysicsWorld.cpp +++ b/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; +} diff --git a/libraries/physics/src/PhysicsWorld.h b/libraries/physics/src/PhysicsWorld.h index 523b1282f7..ca42113a65 100644 --- a/libraries/physics/src/PhysicsWorld.h +++ b/libraries/physics/src/PhysicsWorld.h @@ -17,6 +17,35 @@ #include #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 _voxels; }; diff --git a/libraries/physics/src/PositionHashKey.cpp b/libraries/physics/src/PositionHashKey.cpp new file mode 100644 index 0000000000..e03391c72c --- /dev/null +++ b/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 + +#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); +} diff --git a/libraries/physics/src/PositionHashKey.h b/libraries/physics/src/PositionHashKey.h new file mode 100644 index 0000000000..bb2f277051 --- /dev/null +++ b/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 + +#include + +#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 diff --git a/libraries/physics/src/ShapeInfo.cpp b/libraries/physics/src/ShapeInfo.cpp index 85a01f6bda..cf848793a9 100644 --- a/libraries/physics/src/ShapeInfo.cpp +++ b/libraries/physics/src/ShapeInfo.cpp @@ -11,8 +11,12 @@ #ifdef USE_BULLET_PHYSICS +#include #include +#include // 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); diff --git a/libraries/physics/src/ShapeInfo.h b/libraries/physics/src/ShapeInfo.h index f2a39b5320..40dedfef1e 100644 --- a/libraries/physics/src/ShapeInfo.h +++ b/libraries/physics/src/ShapeInfo.h @@ -16,6 +16,7 @@ #include #include +#include #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); diff --git a/libraries/physics/src/VoxelObject.h b/libraries/physics/src/VoxelObject.h new file mode 100644 index 0000000000..49866e0bce --- /dev/null +++ b/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 +#include + +// 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