bool Physics3DShape::initCompoundShape( const std::vector<std::pair<Physics3DShape *, Mat4>> &shapes )
{
_shapeType = ShapeType::COMPOUND;
auto compound = new btCompoundShape;
for (auto iter : shapes){
compound->addChildShape(convertMat4TobtTransform(iter.second), iter.first->getbtShape());
CC_SAFE_RETAIN(iter.first);
_compoundChildShapes.push_back(iter.first);
}
_btShape = compound;
return true;
}
void CompoundShape::addChild(CollisionShape& childShape, const Transform::Variables& childTransform)
{
if (!childShape.m_shape)
{
JOP_DEBUG_ERROR("Compound shape \"" << getName() << "\": Tried to add an empty child shape");
return;
}
auto shape = static_cast<btCompoundShape*>(m_shape.get());
auto& r = childTransform.rotation;
auto& p = childTransform.position;
const btTransform transform(btQuaternion(r.x, r.y, r.z, r.w), btVector3(p.x, p.y, p.z));
shape->addChildShape(transform, childShape.m_shape.get());
}
void RigidBodyNode::sync_shapes(bool do_not_lock) {
has_static_shapes_ = false;
if (!shapes_.size()) {
if (body_->getCollisionShape() != bullet_empty_shape_) {
CONDITION_LOCK(ph_ && !do_not_lock, ph_->lock());
body_->setCollisionShape(bullet_empty_shape_);
}
} else {
if (body_->isStaticObject()) { //Static rigid body shape construction
if (shapes_.size() == 1 && shapes_[0].shape->is_static_shape()) {
//Rigid body has only one collision shape
//
//TODO: Implement offset transform for static rigid bodies.
// Now the transform of the collision shape is ignored.
// Do not forget to fix set_mass and set_kinematic funcs.
auto sh = shapes_[0].shape->construct_static();
assert(sh);
CONDITION_LOCK(ph_ && !do_not_lock, ph_->lock());
body_->setCollisionShape(sh);
if (!shapes_[0].shape->has_identical_shape_constructor())
has_static_shapes_ = true;
} else {
//Rigid body has more than one collision shape or
//it cannot be static
auto cs = new btCompoundShape();
for (auto sh : shapes_) {
if (sh.shape->is_static_shape()) {
cs->addChildShape(
math::mat4_to_btTransform(sh.transform),
sh.shape->construct_static());
if (!sh.shape->has_identical_shape_constructor())
has_static_shapes_ = true;
} else if (sh.shape->is_dynamic_shape())
sh.shape->construct_dynamic(
cs, math::mat4_to_btTransform(sh.transform));
}
//If all the shapes support dynamic bodies, recalculate inertia
//(in case if the body becomes dynamic)
if (!has_static_shapes_) {
cs->calculateLocalInertia(mass_, inertia_);
body_->setMassProps(mass_, inertia_);
}
CONDITION_LOCK(ph_ && !do_not_lock, ph_->lock());
body_->setCollisionShape(cs);
if (bullet_compound_shape_)
delete bullet_compound_shape_;
bullet_compound_shape_ = cs;
}
}
else { //Dynamic rigid body shape construction
auto cs = new btCompoundShape();
for (auto sh : shapes_) {
if (sh.shape->is_dynamic_shape())
sh.shape->construct_dynamic(
cs, math::mat4_to_btTransform(sh.transform));
}
cs->calculateLocalInertia(mass_, inertia_);
body_->setMassProps(mass_, inertia_);
CONDITION_LOCK(ph_ && !do_not_lock, ph_->lock());
body_->setCollisionShape(cs);
if (bullet_compound_shape_)
delete bullet_compound_shape_;
bullet_compound_shape_ = cs;
}
}
}
const btCollisionShape* ShapeFactory::createShapeFromInfo(const ShapeInfo& info) {
btCollisionShape* shape = NULL;
int type = info.getType();
switch(type) {
case SHAPE_TYPE_BOX: {
shape = new btBoxShape(glmToBullet(info.getHalfExtents()));
}
break;
case SHAPE_TYPE_SPHERE: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = glm::max(halfExtents.x, glm::max(halfExtents.y, halfExtents.z));
shape = new btSphereShape(radius);
}
break;
case SHAPE_TYPE_ELLIPSOID: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = halfExtents.x;
const float MIN_RADIUS = 0.001f;
const float MIN_RELATIVE_SPHERICAL_ERROR = 0.001f;
if (radius > MIN_RADIUS
&& fabsf(radius - halfExtents.y) / radius < MIN_RELATIVE_SPHERICAL_ERROR
&& fabsf(radius - halfExtents.z) / radius < MIN_RELATIVE_SPHERICAL_ERROR) {
// close enough to true sphere
shape = new btSphereShape(radius);
} else {
ShapeInfo::PointList points;
points.reserve(NUM_UNIT_SPHERE_DIRECTIONS);
for (uint32_t i = 0; i < NUM_UNIT_SPHERE_DIRECTIONS; ++i) {
points.push_back(bulletToGLM(_unitSphereDirections[i]) * halfExtents);
}
shape = createConvexHull(points);
}
}
break;
case SHAPE_TYPE_CAPSULE_Y: {
glm::vec3 halfExtents = info.getHalfExtents();
float radius = halfExtents.x;
float height = 2.0f * halfExtents.y;
shape = new btCapsuleShape(radius, height);
}
break;
case SHAPE_TYPE_COMPOUND:
case SHAPE_TYPE_SIMPLE_HULL: {
const ShapeInfo::PointCollection& pointCollection = info.getPointCollection();
uint32_t numSubShapes = info.getNumSubShapes();
if (numSubShapes == 1) {
shape = createConvexHull(pointCollection[0]);
} else {
auto compound = new btCompoundShape();
btTransform trans;
trans.setIdentity();
foreach (const ShapeInfo::PointList& hullPoints, pointCollection) {
btConvexHullShape* hull = createConvexHull(hullPoints);
compound->addChildShape(trans, hull);
}
shape = compound;
}
}
break;
case SHAPE_TYPE_SIMPLE_COMPOUND: {
const ShapeInfo::PointCollection& pointCollection = info.getPointCollection();
const ShapeInfo::TriangleIndices& triangleIndices = info.getTriangleIndices();
uint32_t numIndices = triangleIndices.size();
uint32_t numMeshes = info.getNumSubShapes();
const uint32_t MIN_NUM_SIMPLE_COMPOUND_INDICES = 2; // END_OF_MESH_PART + END_OF_MESH
if (numMeshes > 0 && numIndices > MIN_NUM_SIMPLE_COMPOUND_INDICES) {
uint32_t i = 0;
std::vector<btConvexHullShape*> hulls;
for (auto& points : pointCollection) {
// build a hull around each part
while (i < numIndices) {
ShapeInfo::PointList hullPoints;
hullPoints.reserve(points.size());
while (i < numIndices) {
int32_t j = triangleIndices[i];
++i;
if (j == END_OF_MESH_PART) {
// end of part
break;
}
hullPoints.push_back(points[j]);
}
if (hullPoints.size() > 0) {
btConvexHullShape* hull = createConvexHull(hullPoints);
hulls.push_back(hull);
}
assert(i < numIndices);
if (triangleIndices[i] == END_OF_MESH) {
// end of mesh
++i;
break;
}
}
}
uint32_t numHulls = (uint32_t)hulls.size();
if (numHulls == 1) {
shape = hulls[0];
} else {
auto compound = new btCompoundShape();
btTransform trans;
trans.setIdentity();
for (auto hull : hulls) {
compound->addChildShape(trans, hull);
}
shape = compound;
}
}
}
break;
case SHAPE_TYPE_STATIC_MESH: {
btTriangleIndexVertexArray* dataArray = createStaticMeshArray(info);
if (dataArray) {
shape = new StaticMeshShape(dataArray);
}
}
break;
}
void setCollisionShape(CollisionShape c, const std::string &meshName = "")
{
if (c == UNDEFINED)
return;
_meshName = meshName;
_reset();
_shapeType = c;
btTransform transform;
glm::vec3 position = posFromMat4(_entity->getLocalTransform());
glm::vec3 scale = scaleFromMat4(_entity->getLocalTransform());
std::cout << scale.x << " " << scale.y << " " << scale.z << std::endl;
glm::vec3 rot = rotFromMat4(_entity->getLocalTransform(), true);
transform.setIdentity();
transform.setOrigin(convertGLMVectorToBullet(position));
transform.setRotation(btQuaternion(rot.x, rot.y, rot.z));
_motionState = new btDefaultMotionState(transform);
if (c == BOX)
{
_collisionShape = new btBoxShape(btVector3(0.5, 0.5, 0.5));//new btBoxShape(halfScale);
}
else if (c == SPHERE)
{
_collisionShape = new btSphereShape(0.5);//new btSphereShape(scale.x);
}
else if (c == MESH)
{
// THERE IS SOME LEAKS BECAUSE THAT'S TEMPORARY
SmartPointer<Resources::SharedMesh> mesh = _scene->getEngine().getInstance<Resources::ResourceManager>().getResource(meshName);
auto group = new btCompoundShape();
auto &geos = mesh->getGeometry();
for (unsigned int i = 0; i < geos.size(); ++i)
{
const Resources::Geometry &geo = geos[i]; // DIRTY HACK TEMPORARY
// NEED TO REPLACE MESH BY MESH GROUP !
btScalar *t = new btScalar[geo.vertices.size() * 3]();
for (unsigned int it = 0; it < geo.vertices.size(); ++it)
{
t[it * 3] = geo.vertices[it].x;
t[it * 3 + 1] = geo.vertices[it].y;
t[it * 3 + 2] = geo.vertices[it].z;
}
btConvexHullShape *tmp = new btConvexHullShape(t, geo.vertices.size(), 3 * sizeof(btScalar));
btShapeHull *hull = new btShapeHull(tmp);
btScalar margin = tmp->getMargin();
hull->buildHull(margin);
tmp->setUserPointer(hull);
btConvexHullShape *s = new btConvexHullShape();
for (int it = 0; it < hull->numVertices(); ++it)
{
s->addPoint(hull->getVertexPointer()[it], false);
}
s->recalcLocalAabb();
btTransform localTrans;
localTrans.setIdentity();
_collisionShape = s;
group->addChildShape(localTrans,s);
delete[] t;
delete hull;
delete tmp;
}
_collisionShape = group;
}
else if (c == CONCAVE_STATIC_MESH) // dont work
{
SmartPointer<Resources::SharedMesh> mesh = _scene->getEngine().getInstance<Resources::ResourceManager>().getResource(meshName);
auto trimesh = new btTriangleMesh();
auto &geos = mesh->getGeometry();
for (unsigned int j = 0; j < geos.size(); ++j)
{
const Resources::Geometry &geo = geos[j];
for (unsigned int i = 2; i < geo.vertices.size(); i += 3)
{
trimesh->addTriangle(btVector3(geo.vertices[i - 2].x, geo.vertices[i - 2].y, geo.vertices[i - 2].z)
, btVector3(geo.vertices[i - 1].x, geo.vertices[i - 1].y, geo.vertices[i - 1].z)
, btVector3(geo.vertices[i].x, geo.vertices[i].y, geo.vertices[i].z));
}
}
auto bvh = new btBvhTriangleMeshShape(trimesh, true);
bvh->buildOptimizedBvh();
bool isit = bvh->isConcave();
_collisionShape = bvh;
}
if (_mass != 0)
_collisionShape->calculateLocalInertia(_mass, _inertia);
_collisionShape->setLocalScaling(convertGLMVectorToBullet(scale));
_rigidBody = new btRigidBody(_mass, _motionState, _collisionShape, _inertia);
_rigidBody->setUserPointer(&_entity);
_rigidBody->setAngularFactor(convertGLMVectorToBullet(_rotationConstraint));
_rigidBody->setLinearFactor(convertGLMVectorToBullet(_transformConstraint));
if (_rigidBody->isStaticObject())
{
_rigidBody->setActivationState(DISABLE_SIMULATION);
}
_manager->getWorld()->addRigidBody(_rigidBody);
}
