/* SPDX-License-Identifier: GPL-2.0-or-later
 * Copyright 2013 Blender Foundation. All rights reserved. */

/** \file
 * \ingroup intern_rigidbody
 * \brief Rigid Body API implementation for Bullet
 */

/*
 * Bullet Continuous Collision Detection and Physics Library
 * Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
 *
 * This software is provided 'as-is', without any express or implied warranty. In no event will the
 * authors be held liable for any damages arising from the use of this software. Permission is
 * granted to anyone to use this software for any purpose, including commercial applications, and
 * to alter it and redistribute it freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not claim that you wrote the
 *    original software. If you use this software in a product, an acknowledgment in the product
 *    documentation would be appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be misrepresented as
 *    being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

/* This file defines the "RigidBody interface" for the
 * Bullet Physics Engine. This API is designed to be used
 * from C-code in Blender as part of the Rigid Body simulation
 * system.
 *
 * It is based on the Bullet C-API, but is heavily modified to
 * give access to more data types and to offer a nicer interface.
 *
 * -- Joshua Leung, June 2010
 */

#include <errno.h>
#include <stdio.h>

#include "RBI_api.h"

#include "btBulletDynamicsCommon.h"

#include "LinearMath/btConvexHullComputer.h"
#include "LinearMath/btMatrix3x3.h"
#include "LinearMath/btScalar.h"
#include "LinearMath/btTransform.h"
#include "LinearMath/btVector3.h"

#include "BulletCollision/CollisionShapes/btScaledBvhTriangleMeshShape.h"
#include "BulletCollision/Gimpact/btGImpactCollisionAlgorithm.h"
#include "BulletCollision/Gimpact/btGImpactShape.h"

struct rbDynamicsWorld {
  btDiscreteDynamicsWorld *dynamicsWorld;
  btDefaultCollisionConfiguration *collisionConfiguration;
  btDispatcher *dispatcher;
  btBroadphaseInterface *pairCache;
  btConstraintSolver *constraintSolver;
  btOverlapFilterCallback *filterCallback;
};
struct rbRigidBody {
  btRigidBody *body;
  int col_groups;
};

struct rbVert {
  btScalar x, y, z;
};
struct rbTri {
  int v0, v1, v2;
};

struct rbMeshData {
  btTriangleIndexVertexArray *index_array;
  rbVert *vertices;
  rbTri *triangles;
  int num_vertices;
  int num_triangles;
};

struct rbCollisionShape {
  btCollisionShape *cshape;
  rbMeshData *mesh;
  rbCollisionShape **compoundChildShapes;
  int compoundChilds;
};

struct rbFilterCallback : public btOverlapFilterCallback {
  virtual bool needBroadphaseCollision(btBroadphaseProxy *proxy0, btBroadphaseProxy *proxy1) const
  {
    rbRigidBody *rb0 = (rbRigidBody *)((btRigidBody *)proxy0->m_clientObject)->getUserPointer();
    rbRigidBody *rb1 = (rbRigidBody *)((btRigidBody *)proxy1->m_clientObject)->getUserPointer();

    bool collides;
    collides = (proxy0->m_collisionFilterGroup & proxy1->m_collisionFilterMask) != 0;
    collides = collides && (proxy1->m_collisionFilterGroup & proxy0->m_collisionFilterMask);
    collides = collides && (rb0->col_groups & rb1->col_groups);

    return collides;
  }
};

static inline void copy_v3_btvec3(float vec[3], const btVector3 &btvec)
{
  vec[0] = (float)btvec[0];
  vec[1] = (float)btvec[1];
  vec[2] = (float)btvec[2];
}
static inline void copy_quat_btquat(float quat[4], const btQuaternion &btquat)
{
  quat[0] = btquat.getW();
  quat[1] = btquat.getX();
  quat[2] = btquat.getY();
  quat[3] = btquat.getZ();
}

/* ********************************** */
/* Dynamics World Methods */

/* Setup ---------------------------- */

rbDynamicsWorld *RB_dworld_new(const float gravity[3])
{
  rbDynamicsWorld *world = new rbDynamicsWorld;

  /* collision detection/handling */
  world->collisionConfiguration = new btDefaultCollisionConfiguration();

  world->dispatcher = new btCollisionDispatcher(world->collisionConfiguration);
  btGImpactCollisionAlgorithm::registerAlgorithm((btCollisionDispatcher *)world->dispatcher);

  world->pairCache = new btDbvtBroadphase();

  world->filterCallback = new rbFilterCallback();
  world->pairCache->getOverlappingPairCache()->setOverlapFilterCallback(world->filterCallback);

  /* constraint solving */
  world->constraintSolver = new btSequentialImpulseConstraintSolver();

  /* world */
  world->dynamicsWorld = new btDiscreteDynamicsWorld(
      world->dispatcher, world->pairCache, world->constraintSolver, world->collisionConfiguration);

  RB_dworld_set_gravity(world, gravity);

  return world;
}

void RB_dworld_delete(rbDynamicsWorld *world)
{
  /* bullet doesn't like if we free these in a different order */
  delete world->dynamicsWorld;
  delete world->constraintSolver;
  delete world->pairCache;
  delete world->dispatcher;
  delete world->collisionConfiguration;
  delete world->filterCallback;
  delete world;
}

/* Settings ------------------------- */

/* Gravity */
void RB_dworld_get_gravity(rbDynamicsWorld *world, float g_out[3])
{
  copy_v3_btvec3(g_out, world->dynamicsWorld->getGravity());
}

void RB_dworld_set_gravity(rbDynamicsWorld *world, const float g_in[3])
{
  world->dynamicsWorld->setGravity(btVector3(g_in[0], g_in[1], g_in[2]));
}

/* Constraint Solver */
void RB_dworld_set_solver_iterations(rbDynamicsWorld *world, int num_solver_iterations)
{
  btContactSolverInfo &info = world->dynamicsWorld->getSolverInfo();

  info.m_numIterations = num_solver_iterations;
}

/* Split Impulse */
void RB_dworld_set_split_impulse(rbDynamicsWorld *world, int split_impulse)
{
  btContactSolverInfo &info = world->dynamicsWorld->getSolverInfo();

  info.m_splitImpulse = split_impulse;
}

/* Simulation ----------------------- */

void RB_dworld_step_simulation(rbDynamicsWorld *world,
                               float timeStep,
                               int maxSubSteps,
                               float timeSubStep)
{
  world->dynamicsWorld->stepSimulation(timeStep, maxSubSteps, timeSubStep);
}

/* Export -------------------------- */

/**
 * Exports entire dynamics world to Bullet's "*.bullet" binary format
 * which is similar to Blender's SDNA system.
 *
 * \param world: Dynamics world to write to file
 * \param filename: Assumed to be a valid filename, with .bullet extension
 */
void RB_dworld_export(rbDynamicsWorld *world, const char *filename)
{
  // create a large enough buffer. There is no method to pre-calculate the buffer size yet.
  int maxSerializeBufferSize = 1024 * 1024 * 5;

  btDefaultSerializer *serializer = new btDefaultSerializer(maxSerializeBufferSize);
  world->dynamicsWorld->serialize(serializer);

  FILE *file = fopen(filename, "wb");
  if (file) {
    fwrite(serializer->getBufferPointer(), serializer->getCurrentBufferSize(), 1, file);
    fclose(file);
  }
  else {
    fprintf(stderr, "RB_dworld_export: %s\n", strerror(errno));
  }
}

/* ********************************** */
/* Rigid Body Methods */

/* Setup ---------------------------- */

void RB_dworld_add_body(rbDynamicsWorld *world, rbRigidBody *object, int col_groups)
{
  btRigidBody *body = object->body;
  object->col_groups = col_groups;

  world->dynamicsWorld->addRigidBody(body);
}

void RB_dworld_remove_body(rbDynamicsWorld *world, rbRigidBody *object)
{
  btRigidBody *body = object->body;

  world->dynamicsWorld->removeRigidBody(body);
}

/* Collision detection */

void RB_world_convex_sweep_test(rbDynamicsWorld *world,
                                rbRigidBody *object,
                                const float loc_start[3],
                                const float loc_end[3],
                                float v_location[3],
                                float v_hitpoint[3],
                                float v_normal[3],
                                int *r_hit)
{
  btRigidBody *body = object->body;
  btCollisionShape *collisionShape = body->getCollisionShape();
  /* only convex shapes are supported, but user can specify a non convex shape */
  if (collisionShape->isConvex()) {
    btCollisionWorld::ClosestConvexResultCallback result(
        btVector3(loc_start[0], loc_start[1], loc_start[2]),
        btVector3(loc_end[0], loc_end[1], loc_end[2]));

    btQuaternion obRot = body->getWorldTransform().getRotation();

    btTransform rayFromTrans;
    rayFromTrans.setIdentity();
    rayFromTrans.setRotation(obRot);
    rayFromTrans.setOrigin(btVector3(loc_start[0], loc_start[1], loc_start[2]));

    btTransform rayToTrans;
    rayToTrans.setIdentity();
    rayToTrans.setRotation(obRot);
    rayToTrans.setOrigin(btVector3(loc_end[0], loc_end[1], loc_end[2]));

    world->dynamicsWorld->convexSweepTest(
        (btConvexShape *)collisionShape, rayFromTrans, rayToTrans, result, 0);

    if (result.hasHit()) {
      *r_hit = 1;

      v_location[0] = result.m_convexFromWorld[0] +
                      (result.m_convexToWorld[0] - result.m_convexFromWorld[0]) *
                          result.m_closestHitFraction;
      v_location[1] = result.m_convexFromWorld[1] +
                      (result.m_convexToWorld[1] - result.m_convexFromWorld[1]) *
                          result.m_closestHitFraction;
      v_location[2] = result.m_convexFromWorld[2] +
                      (result.m_convexToWorld[2] - result.m_convexFromWorld[2]) *
                          result.m_closestHitFraction;

      v_hitpoint[0] = result.m_hitPointWorld[0];
      v_hitpoint[1] = result.m_hitPointWorld[1];
      v_hitpoint[2] = result.m_hitPointWorld[2];

      v_normal[0] = result.m_hitNormalWorld[0];
      v_normal[1] = result.m_hitNormalWorld[1];
      v_normal[2] = result.m_hitNormalWorld[2];
    }
    else {
      *r_hit = 0;
    }
  }
  else {
    /* we need to return a value if user passes non convex body, to report */
    *r_hit = -2;
  }
}

/* ............ */

rbRigidBody *RB_body_new(rbCollisionShape *shape, const float loc[3], const float rot[4])
{
  rbRigidBody *object = new rbRigidBody;
  /* current transform */
  btTransform trans;
  trans.setIdentity();
  trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
  trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));

  /* create motionstate, which is necessary for interpolation (includes reverse playback) */
  btDefaultMotionState *motionState = new btDefaultMotionState(trans);

  /* make rigidbody */
  btRigidBody::btRigidBodyConstructionInfo rbInfo(1.0f, motionState, shape->cshape);

  object->body = new btRigidBody(rbInfo);

  object->body->setUserPointer(object);

  return object;
}

void RB_body_delete(rbRigidBody *object)
{
  btRigidBody *body = object->body;

  /* motion state */
  btMotionState *ms = body->getMotionState();

  delete ms;

  /* collision shape is done elsewhere... */

  /* body itself */

  /* manually remove constraint refs of the rigid body, normally this happens when removing
   * constraints from the world
   * but since we delete everything when the world is rebult, we need to do it manually here */
  for (int i = body->getNumConstraintRefs() - 1; i >= 0; i--) {
    btTypedConstraint *con = body->getConstraintRef(i);
    body->removeConstraintRef(con);
  }

  delete body;
  delete object;
}

/* Settings ------------------------- */

void RB_body_set_collision_shape(rbRigidBody *object, rbCollisionShape *shape)
{
  btRigidBody *body = object->body;

  /* set new collision shape */
  body->setCollisionShape(shape->cshape);

  /* recalculate inertia, since that depends on the collision shape... */
  RB_body_set_mass(object, RB_body_get_mass(object));
}

/* ............ */

float RB_body_get_mass(rbRigidBody *object)
{
  btRigidBody *body = object->body;

  /* there isn't really a mass setting, but rather 'inverse mass'
   * which we convert back to mass by taking the reciprocal again
   */
  float value = (float)body->getInvMass();

  if (value) {
    value = 1.0f / value;
  }

  return value;
}

void RB_body_set_mass(rbRigidBody *object, float value)
{
  btRigidBody *body = object->body;
  btVector3 localInertia(0, 0, 0);

  /* calculate new inertia if non-zero mass */
  if (value) {
    btCollisionShape *shape = body->getCollisionShape();
    shape->calculateLocalInertia(value, localInertia);
  }

  btVector3 minAabb, maxAabb;
  btTransform ident;
  ident.setIdentity();
  body->getCollisionShape()->getAabb(ident, minAabb, maxAabb);
  body->setMassProps(value, localInertia);
  body->updateInertiaTensor();
}

float RB_body_get_friction(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getFriction();
}

void RB_body_set_friction(rbRigidBody *object, float value)
{
  btRigidBody *body = object->body;
  body->setFriction(value);
}

float RB_body_get_restitution(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getRestitution();
}

void RB_body_set_restitution(rbRigidBody *object, float value)
{
  btRigidBody *body = object->body;
  body->setRestitution(value);
}

float RB_body_get_linear_damping(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getLinearDamping();
}

void RB_body_set_linear_damping(rbRigidBody *object, float value)
{
  RB_body_set_damping(object, value, RB_body_get_linear_damping(object));
}

float RB_body_get_angular_damping(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getAngularDamping();
}

void RB_body_set_angular_damping(rbRigidBody *object, float value)
{
  RB_body_set_damping(object, RB_body_get_linear_damping(object), value);
}

void RB_body_set_damping(rbRigidBody *object, float linear, float angular)
{
  btRigidBody *body = object->body;
  body->setDamping(linear, angular);
}

float RB_body_get_linear_sleep_thresh(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getLinearSleepingThreshold();
}

void RB_body_set_linear_sleep_thresh(rbRigidBody *object, float value)
{
  RB_body_set_sleep_thresh(object, value, RB_body_get_angular_sleep_thresh(object));
}

float RB_body_get_angular_sleep_thresh(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  return body->getAngularSleepingThreshold();
}

void RB_body_set_angular_sleep_thresh(rbRigidBody *object, float value)
{
  RB_body_set_sleep_thresh(object, RB_body_get_linear_sleep_thresh(object), value);
}

void RB_body_set_sleep_thresh(rbRigidBody *object, float linear, float angular)
{
  btRigidBody *body = object->body;
  body->setSleepingThresholds(linear, angular);
}

/* ............ */

void RB_body_get_linear_velocity(rbRigidBody *object, float v_out[3])
{
  btRigidBody *body = object->body;

  copy_v3_btvec3(v_out, body->getLinearVelocity());
}

void RB_body_set_linear_velocity(rbRigidBody *object, const float v_in[3])
{
  btRigidBody *body = object->body;

  body->setLinearVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
}

void RB_body_get_angular_velocity(rbRigidBody *object, float v_out[3])
{
  btRigidBody *body = object->body;

  copy_v3_btvec3(v_out, body->getAngularVelocity());
}

void RB_body_set_angular_velocity(rbRigidBody *object, const float v_in[3])
{
  btRigidBody *body = object->body;

  body->setAngularVelocity(btVector3(v_in[0], v_in[1], v_in[2]));
}

void RB_body_set_linear_factor(rbRigidBody *object, float x, float y, float z)
{
  btRigidBody *body = object->body;
  body->setLinearFactor(btVector3(x, y, z));
}

void RB_body_set_angular_factor(rbRigidBody *object, float x, float y, float z)
{
  btRigidBody *body = object->body;
  body->setAngularFactor(btVector3(x, y, z));
}

/* ............ */

void RB_body_set_kinematic_state(rbRigidBody *object, int kinematic)
{
  btRigidBody *body = object->body;
  if (kinematic) {
    body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
  }
  else {
    body->setCollisionFlags(body->getCollisionFlags() & ~btCollisionObject::CF_KINEMATIC_OBJECT);
  }
}

/* ............ */

void RB_body_set_activation_state(rbRigidBody *object, int use_deactivation)
{
  btRigidBody *body = object->body;
  if (use_deactivation) {
    body->forceActivationState(ACTIVE_TAG);
  }
  else {
    body->setActivationState(DISABLE_DEACTIVATION);
  }
}
void RB_body_activate(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  body->setActivationState(ACTIVE_TAG);
}
void RB_body_deactivate(rbRigidBody *object)
{
  btRigidBody *body = object->body;
  body->setActivationState(ISLAND_SLEEPING);
}

/* ............ */

/* Simulation ----------------------- */

/* The transform matrices Blender uses are OpenGL-style matrices,
 * while Bullet uses the Right-Handed coordinate system style instead.
 */

void RB_body_get_transform_matrix(rbRigidBody *object, float m_out[4][4])
{
  btRigidBody *body = object->body;
  btMotionState *ms = body->getMotionState();

  btTransform trans;
  ms->getWorldTransform(trans);

  trans.getOpenGLMatrix((btScalar *)m_out);
}

void RB_body_set_loc_rot(rbRigidBody *object, const float loc[3], const float rot[4])
{
  btRigidBody *body = object->body;
  btMotionState *ms = body->getMotionState();

  /* set transform matrix */
  btTransform trans;
  trans.setIdentity();
  trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
  trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));

  ms->setWorldTransform(trans);
}

void RB_body_set_scale(rbRigidBody *object, const float scale[3])
{
  btRigidBody *body = object->body;

  /* apply scaling factor from matrix above to the collision shape */
  btCollisionShape *cshape = body->getCollisionShape();
  if (cshape) {
    cshape->setLocalScaling(btVector3(scale[0], scale[1], scale[2]));

    /* GIimpact shapes have to be updated to take scaling into account */
    if (cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) {
      ((btGImpactMeshShape *)cshape)->updateBound();
    }
  }
}

/* ............ */
/* Read-only state info about status of simulation */

void RB_body_get_position(rbRigidBody *object, float v_out[3])
{
  btRigidBody *body = object->body;

  copy_v3_btvec3(v_out, body->getWorldTransform().getOrigin());
}

void RB_body_get_orientation(rbRigidBody *object, float v_out[4])
{
  btRigidBody *body = object->body;

  copy_quat_btquat(v_out, body->getWorldTransform().getRotation());
}

void RB_body_get_scale(rbRigidBody *object, float v_out[3])
{
  btRigidBody *body = object->body;

  btCollisionShape *cshape = body->getCollisionShape();
  /* The body should have a collision shape when we try to set the scale. */
  btAssert(cshape);
  copy_v3_btvec3(v_out, cshape->getLocalScaling());
}

/* ............ */
/* Overrides for simulation */

void RB_body_apply_central_force(rbRigidBody *object, const float v_in[3])
{
  btRigidBody *body = object->body;

  body->applyCentralForce(btVector3(v_in[0], v_in[1], v_in[2]));
}

/* ********************************** */
/* Collision Shape Methods */

/* Setup (Standard Shapes) ----------- */

rbCollisionShape *RB_shape_new_box(float x, float y, float z)
{
  rbCollisionShape *shape = new rbCollisionShape;
  shape->cshape = new btBoxShape(btVector3(x, y, z));
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

rbCollisionShape *RB_shape_new_sphere(float radius)
{
  rbCollisionShape *shape = new rbCollisionShape;
  shape->cshape = new btSphereShape(radius);
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

rbCollisionShape *RB_shape_new_capsule(float radius, float height)
{
  rbCollisionShape *shape = new rbCollisionShape;
  shape->cshape = new btCapsuleShapeZ(radius, height);
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

rbCollisionShape *RB_shape_new_cone(float radius, float height)
{
  rbCollisionShape *shape = new rbCollisionShape;
  shape->cshape = new btConeShapeZ(radius, height);
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

rbCollisionShape *RB_shape_new_cylinder(float radius, float height)
{
  rbCollisionShape *shape = new rbCollisionShape;
  shape->cshape = new btCylinderShapeZ(btVector3(radius, radius, height));
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

/* Setup (Convex Hull) ------------ */

rbCollisionShape *RB_shape_new_convex_hull(
    float *verts, int stride, int count, float margin, bool *can_embed)
{
  btConvexHullComputer hull_computer = btConvexHullComputer();

  // try to embed the margin, if that fails don't shrink the hull
  if (hull_computer.compute(verts, stride, count, margin, 0.0f) < 0.0f) {
    hull_computer.compute(verts, stride, count, 0.0f, 0.0f);
    *can_embed = false;
  }

  rbCollisionShape *shape = new rbCollisionShape;
  btConvexHullShape *hull_shape = new btConvexHullShape(&(hull_computer.vertices[0].getX()),
                                                        hull_computer.vertices.size());

  shape->cshape = hull_shape;
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

/* Setup (Triangle Mesh) ---------- */

/* Need to call RB_trimesh_finish() after creating triangle mesh and adding vertices and triangles
 */

rbMeshData *RB_trimesh_data_new(int num_tris, int num_verts)
{
  rbMeshData *mesh = new rbMeshData;
  mesh->vertices = new rbVert[num_verts];
  mesh->triangles = new rbTri[num_tris];
  mesh->num_vertices = num_verts;
  mesh->num_triangles = num_tris;

  return mesh;
}

static void RB_trimesh_data_delete(rbMeshData *mesh)
{
  delete mesh->index_array;
  delete[] mesh->vertices;
  delete[] mesh->triangles;
  delete mesh;
}

void RB_trimesh_add_vertices(rbMeshData *mesh, float *vertices, int num_verts, int vert_stride)
{
  for (int i = 0; i < num_verts; i++) {
    float *vert = (float *)(((char *)vertices + i * vert_stride));
    mesh->vertices[i].x = vert[0];
    mesh->vertices[i].y = vert[1];
    mesh->vertices[i].z = vert[2];
  }
}
void RB_trimesh_add_triangle_indices(rbMeshData *mesh, int num, int index0, int index1, int index2)
{
  mesh->triangles[num].v0 = index0;
  mesh->triangles[num].v1 = index1;
  mesh->triangles[num].v2 = index2;
}

void RB_trimesh_finish(rbMeshData *mesh)
{
  mesh->index_array = new btTriangleIndexVertexArray(mesh->num_triangles,
                                                     (int *)mesh->triangles,
                                                     sizeof(rbTri),
                                                     mesh->num_vertices,
                                                     (btScalar *)mesh->vertices,
                                                     sizeof(rbVert));
}

rbCollisionShape *RB_shape_new_trimesh(rbMeshData *mesh)
{
  rbCollisionShape *shape = new rbCollisionShape;

  /* triangle-mesh we create is a BVH wrapper for triangle mesh data (for faster lookups) */
  // RB_TODO perhaps we need to allow saving out this for performance when rebuilding?
  btBvhTriangleMeshShape *unscaledShape = new btBvhTriangleMeshShape(
      mesh->index_array, true, true);

  shape->cshape = new btScaledBvhTriangleMeshShape(unscaledShape, btVector3(1.0f, 1.0f, 1.0f));
  shape->mesh = mesh;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

void RB_shape_trimesh_update(rbCollisionShape *shape,
                             float *vertices,
                             int num_verts,
                             int vert_stride,
                             const float min[3],
                             const float max[3])
{
  if (shape->mesh == NULL || num_verts != shape->mesh->num_vertices) {
    return;
  }

  for (int i = 0; i < num_verts; i++) {
    float *vert = (float *)(((char *)vertices + i * vert_stride));
    shape->mesh->vertices[i].x = vert[0];
    shape->mesh->vertices[i].y = vert[1];
    shape->mesh->vertices[i].z = vert[2];
  }

  if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
    btScaledBvhTriangleMeshShape *scaled_shape = (btScaledBvhTriangleMeshShape *)shape->cshape;
    btBvhTriangleMeshShape *mesh_shape = scaled_shape->getChildShape();
    mesh_shape->refitTree(btVector3(min[0], min[1], min[2]), btVector3(max[0], max[1], max[2]));
  }
  else if (shape->cshape->getShapeType() == GIMPACT_SHAPE_PROXYTYPE) {
    btGImpactMeshShape *mesh_shape = (btGImpactMeshShape *)shape->cshape;
    mesh_shape->updateBound();
  }
}

rbCollisionShape *RB_shape_new_gimpact_mesh(rbMeshData *mesh)
{
  rbCollisionShape *shape = new rbCollisionShape;

  btGImpactMeshShape *gimpactShape = new btGImpactMeshShape(mesh->index_array);
  gimpactShape->updateBound();  // TODO: add this to the update collision margin call?

  shape->cshape = gimpactShape;
  shape->mesh = mesh;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

/* Compound Shape ---------------- */

rbCollisionShape *RB_shape_new_compound()
{
  rbCollisionShape *shape = new rbCollisionShape;
  btCompoundShape *compoundShape = new btCompoundShape();

  shape->cshape = compoundShape;
  shape->mesh = NULL;
  shape->compoundChilds = 0;
  shape->compoundChildShapes = NULL;
  return shape;
}

void RB_compound_add_child_shape(rbCollisionShape *parentShape,
                                 rbCollisionShape *shape,
                                 const float loc[3],
                                 const float rot[4])
{
  /* set transform matrix */
  btTransform trans;
  trans.setIdentity();
  trans.setOrigin(btVector3(loc[0], loc[1], loc[2]));
  trans.setRotation(btQuaternion(rot[1], rot[2], rot[3], rot[0]));

  btCompoundShape *compoundShape = (btCompoundShape *)(parentShape->cshape);
  compoundShape->addChildShape(trans, shape->cshape);

  /* Store shapes for deletion later */
  parentShape->compoundChildShapes = (rbCollisionShape **)(realloc(
      parentShape->compoundChildShapes,
      sizeof(rbCollisionShape *) * (++parentShape->compoundChilds)));
  parentShape->compoundChildShapes[parentShape->compoundChilds - 1] = shape;
}

/* Cleanup --------------------------- */

void RB_shape_delete(rbCollisionShape *shape)
{
  if (shape->cshape->getShapeType() == SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE) {
    btBvhTriangleMeshShape *child_shape =
        ((btScaledBvhTriangleMeshShape *)shape->cshape)->getChildShape();

    delete child_shape;
  }
  if (shape->mesh) {
    RB_trimesh_data_delete(shape->mesh);
  }
  delete shape->cshape;

  /* Delete compound child shapes if there are any */
  for (int i = 0; i < shape->compoundChilds; i++) {
    RB_shape_delete(shape->compoundChildShapes[i]);
  }
  if (shape->compoundChildShapes != NULL) {
    free(shape->compoundChildShapes);
  }

  delete shape;
}

/* Settings --------------------------- */

float RB_shape_get_margin(rbCollisionShape *shape)
{
  return shape->cshape->getMargin();
}

void RB_shape_set_margin(rbCollisionShape *shape, float value)
{
  shape->cshape->setMargin(value);
}

/* ********************************** */
/* Constraints */

/* Setup ----------------------------- */

void RB_dworld_add_constraint(rbDynamicsWorld *world, rbConstraint *con, int disable_collisions)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);

  world->dynamicsWorld->addConstraint(constraint, disable_collisions);
}

void RB_dworld_remove_constraint(rbDynamicsWorld *world, rbConstraint *con)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);

  world->dynamicsWorld->removeConstraint(constraint);
}

/* ............ */

static void make_constraint_transforms(btTransform &transform1,
                                       btTransform &transform2,
                                       btRigidBody *body1,
                                       btRigidBody *body2,
                                       float pivot[3],
                                       float orn[4])
{
  btTransform pivot_transform = btTransform();
  pivot_transform.setIdentity();
  pivot_transform.setOrigin(btVector3(pivot[0], pivot[1], pivot[2]));
  pivot_transform.setRotation(btQuaternion(orn[1], orn[2], orn[3], orn[0]));

  transform1 = body1->getWorldTransform().inverse() * pivot_transform;
  transform2 = body2->getWorldTransform().inverse() * pivot_transform;
}

rbConstraint *RB_constraint_new_point(float pivot[3], rbRigidBody *rb1, rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;

  btVector3 pivot1 = body1->getWorldTransform().inverse() *
                     btVector3(pivot[0], pivot[1], pivot[2]);
  btVector3 pivot2 = body2->getWorldTransform().inverse() *
                     btVector3(pivot[0], pivot[1], pivot[2]);

  btTypedConstraint *con = new btPoint2PointConstraint(*body1, *body2, pivot1, pivot2);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_fixed(float pivot[3],
                                      float orn[4],
                                      rbRigidBody *rb1,
                                      rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btFixedConstraint *con = new btFixedConstraint(*body1, *body2, transform1, transform2);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_hinge(float pivot[3],
                                      float orn[4],
                                      rbRigidBody *rb1,
                                      rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btHingeConstraint *con = new btHingeConstraint(*body1, *body2, transform1, transform2);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_slider(float pivot[3],
                                       float orn[4],
                                       rbRigidBody *rb1,
                                       rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_piston(float pivot[3],
                                       float orn[4],
                                       rbRigidBody *rb1,
                                       rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btSliderConstraint *con = new btSliderConstraint(*body1, *body2, transform1, transform2, true);
  con->setUpperAngLimit(-1.0f);  // unlock rotation axis

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_6dof(float pivot[3],
                                     float orn[4],
                                     rbRigidBody *rb1,
                                     rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btTypedConstraint *con = new btGeneric6DofConstraint(
      *body1, *body2, transform1, transform2, true);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_6dof_spring(float pivot[3],
                                            float orn[4],
                                            rbRigidBody *rb1,
                                            rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btTypedConstraint *con = new btGeneric6DofSpringConstraint(
      *body1, *body2, transform1, transform2, true);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_6dof_spring2(float pivot[3],
                                             float orn[4],
                                             rbRigidBody *rb1,
                                             rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btTypedConstraint *con = new btGeneric6DofSpring2Constraint(
      *body1, *body2, transform1, transform2);

  return (rbConstraint *)con;
}

rbConstraint *RB_constraint_new_motor(float pivot[3],
                                      float orn[4],
                                      rbRigidBody *rb1,
                                      rbRigidBody *rb2)
{
  btRigidBody *body1 = rb1->body;
  btRigidBody *body2 = rb2->body;
  btTransform transform1;
  btTransform transform2;

  make_constraint_transforms(transform1, transform2, body1, body2, pivot, orn);

  btGeneric6DofConstraint *con = new btGeneric6DofConstraint(
      *body1, *body2, transform1, transform2, true);

  /* unlock constraint axes */
  for (int i = 0; i < 6; i++) {
    con->setLimit(i, 0.0f, -1.0f);
  }
  /* unlock motor axes */
  con->getTranslationalLimitMotor()->m_upperLimit.setValue(-1.0f, -1.0f, -1.0f);

  return (rbConstraint *)con;
}

/* Cleanup ----------------------------- */

void RB_constraint_delete(rbConstraint *con)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);
  delete constraint;
}

/* Settings ------------------------- */

void RB_constraint_set_enabled(rbConstraint *con, int enabled)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);

  constraint->setEnabled(enabled);
}

void RB_constraint_set_limits_hinge(rbConstraint *con, float lower, float upper)
{
  btHingeConstraint *constraint = reinterpret_cast<btHingeConstraint *>(con);

  // RB_TODO expose these
  float softness = 0.9f;
  float bias_factor = 0.3f;
  float relaxation_factor = 1.0f;

  constraint->setLimit(lower, upper, softness, bias_factor, relaxation_factor);
}

void RB_constraint_set_limits_slider(rbConstraint *con, float lower, float upper)
{
  btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint *>(con);

  constraint->setLowerLinLimit(lower);
  constraint->setUpperLinLimit(upper);
}

void RB_constraint_set_limits_piston(
    rbConstraint *con, float lin_lower, float lin_upper, float ang_lower, float ang_upper)
{
  btSliderConstraint *constraint = reinterpret_cast<btSliderConstraint *>(con);

  constraint->setLowerLinLimit(lin_lower);
  constraint->setUpperLinLimit(lin_upper);
  constraint->setLowerAngLimit(ang_lower);
  constraint->setUpperAngLimit(ang_upper);
}

void RB_constraint_set_limits_6dof(rbConstraint *con, int axis, float lower, float upper)
{
  btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);

  constraint->setLimit(axis, lower, upper);
}

void RB_constraint_set_limits_6dof_spring2(rbConstraint *con, int axis, float lower, float upper)
{
  btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
      con);

  constraint->setLimit(axis, lower, upper);
}

void RB_constraint_set_stiffness_6dof_spring(rbConstraint *con, int axis, float stiffness)
{
  btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
      con);

  constraint->setStiffness(axis, stiffness);
}

void RB_constraint_set_damping_6dof_spring(rbConstraint *con, int axis, float damping)
{
  btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
      con);

  // invert damping range so that 0 = no damping
  damping = (damping > 1.0f) ? 0.0f : 1.0f - damping;

  constraint->setDamping(axis, damping);
}

void RB_constraint_set_spring_6dof_spring(rbConstraint *con, int axis, int enable)
{
  btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
      con);

  constraint->enableSpring(axis, enable);
}

void RB_constraint_set_equilibrium_6dof_spring(rbConstraint *con)
{
  btGeneric6DofSpringConstraint *constraint = reinterpret_cast<btGeneric6DofSpringConstraint *>(
      con);

  constraint->setEquilibriumPoint();
}

void RB_constraint_set_stiffness_6dof_spring2(rbConstraint *con, int axis, float stiffness)
{
  btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
      con);

  constraint->setStiffness(axis, stiffness);
}

void RB_constraint_set_damping_6dof_spring2(rbConstraint *con, int axis, float damping)
{
  btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
      con);

  constraint->setDamping(axis, damping);
}

void RB_constraint_set_spring_6dof_spring2(rbConstraint *con, int axis, int enable)
{
  btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
      con);

  constraint->enableSpring(axis, enable);
}

void RB_constraint_set_equilibrium_6dof_spring2(rbConstraint *con)
{
  btGeneric6DofSpring2Constraint *constraint = reinterpret_cast<btGeneric6DofSpring2Constraint *>(
      con);

  constraint->setEquilibriumPoint();
}

void RB_constraint_set_solver_iterations(rbConstraint *con, int num_solver_iterations)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);

  constraint->setOverrideNumSolverIterations(num_solver_iterations);
}

void RB_constraint_set_breaking_threshold(rbConstraint *con, float threshold)
{
  btTypedConstraint *constraint = reinterpret_cast<btTypedConstraint *>(con);

  constraint->setBreakingImpulseThreshold(threshold);
}

void RB_constraint_set_enable_motor(rbConstraint *con, int enable_lin, int enable_ang)
{
  btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);

  constraint->getTranslationalLimitMotor()->m_enableMotor[0] = enable_lin;
  constraint->getRotationalLimitMotor(0)->m_enableMotor = enable_ang;
}

void RB_constraint_set_max_impulse_motor(rbConstraint *con,
                                         float max_impulse_lin,
                                         float max_impulse_ang)
{
  btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);

  constraint->getTranslationalLimitMotor()->m_maxMotorForce.setX(max_impulse_lin);
  constraint->getRotationalLimitMotor(0)->m_maxMotorForce = max_impulse_ang;
}

void RB_constraint_set_target_velocity_motor(rbConstraint *con,
                                             float velocity_lin,
                                             float velocity_ang)
{
  btGeneric6DofConstraint *constraint = reinterpret_cast<btGeneric6DofConstraint *>(con);

  constraint->getTranslationalLimitMotor()->m_targetVelocity.setX(velocity_lin);
  constraint->getRotationalLimitMotor(0)->m_targetVelocity = velocity_ang;
}

/* ********************************** */