motion_state.cpp

Go to the documentation of this file.

#include <LinearMath/btDefaultMotionState.h>

extern double bulletWorldScalingFactor;

extern "C"
{

  btDefaultMotionState *newMotionState(const double *transform)
  {
    return new btDefaultMotionState(
      btTransform(
        btQuaternion(transform[3],
          transform[4],
          transform[5],
          transform[6]),
        btVector3(transform[0] * bulletWorldScalingFactor,
          transform[1] * bulletWorldScalingFactor,
          transform[2] * bulletWorldScalingFactor)));
  }

  void deleteMotionState(btDefaultMotionState *motionState)
  {
    delete motionState;
  }

  void setCenterOfMass(btDefaultMotionState *motionState, const double *centerOfMass)
  {
    motionState->m_centerOfMassOffset =
      btTransform(
        btQuaternion(0, 0, 0, 1),
        btVector3(centerOfMass[0] * bulletWorldScalingFactor,
          centerOfMass[1] * bulletWorldScalingFactor,
          centerOfMass[2] * bulletWorldScalingFactor));
  }

  void setWorldTransform(btMotionState *motionState, double *transform)
  {
    motionState->setWorldTransform(
      btTransform(
        btQuaternion(transform[3],
          transform[4],
          transform[5],
          transform[6]),
        btVector3(transform[0] * bulletWorldScalingFactor,
          transform[1] * bulletWorldScalingFactor,
          transform[2] * bulletWorldScalingFactor)));
  }
  
  void getWorldTransform(btMotionState *motionState, double *transform)
  {
    btTransform result;

    motionState->getWorldTransform(result);
    transform[0] = result.getOrigin().x() / bulletWorldScalingFactor;
    transform[1] = result.getOrigin().y() / bulletWorldScalingFactor;
    transform[2] = result.getOrigin().z() / bulletWorldScalingFactor;
    transform[3] = result.getRotation().x();
    transform[4] = result.getRotation().y();
    transform[5] = result.getRotation().z();
    transform[6] = result.getRotation().w();
  }
  
}