// Filename: odeBody.I
// Created by:  joswilso (27Dec06)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) 2001 - 2004, Disney Enterprises, Inc.  All rights reserved
//
// All use of this software is subject to the terms of the Panda 3d
// Software license.  You should have received a copy of this license
// along with this source code; you will also find a current copy of
// the license at http://etc.cmu.edu/panda3d/docs/license/ .
//
// To contact the maintainers of this program write to
// panda3d-general@lists.sourceforge.net .
//
////////////////////////////////////////////////////////////////////

INLINE dBodyID OdeBody::
get_id() const {
  return _id;
}

INLINE dReal OdeBody::
get_auto_disable_linear_threshold() const {
  return dBodyGetAutoDisableLinearThreshold(_id);
}

INLINE void OdeBody::
set_auto_disable_linear_threshold(dReal linear_threshold) {
  dBodySetAutoDisableLinearThreshold(_id, linear_threshold);
}

INLINE dReal OdeBody::
get_auto_disable_angular_threshold() const {
  return dBodyGetAutoDisableAngularThreshold(_id);
}

INLINE void OdeBody::
set_auto_disable_angular_threshold(dReal angular_threshold) {
  dBodySetAutoDisableAngularThreshold(_id, angular_threshold);
}

INLINE int OdeBody::
get_auto_disable_steps() const {
  return dBodyGetAutoDisableSteps(_id);
}

INLINE void OdeBody::
set_auto_disable_steps(int steps) {
  dBodySetAutoDisableSteps(_id, steps);
}

INLINE dReal OdeBody::
get_auto_disable_time() const {
  return dBodyGetAutoDisableTime(_id);
}

INLINE void OdeBody::
set_auto_disable_time(dReal time) {
  dBodySetAutoDisableTime(_id, time);
}

INLINE int OdeBody::
get_auto_disable_flag() const {
  return dBodyGetAutoDisableFlag(_id);
}

INLINE void OdeBody::
set_auto_disable_flag(int do_auto_disable) {
  dBodySetAutoDisableFlag(_id, do_auto_disable);
}

INLINE void OdeBody::
set_auto_disable_defaults() {
  dBodySetAutoDisableDefaults(_id);
}

INLINE void OdeBody::
set_data(void *data) {
  dBodySetData(_id, data);
}

INLINE void* OdeBody::
get_data() const {
  return dBodyGetData(_id);
}

INLINE void OdeBody::
set_position(dReal x, dReal y, dReal z) {
  dBodySetPosition(_id, x, y, z);
}

INLINE void OdeBody::
set_position(const LVecBase3f &pos) {
  set_position(pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
set_rotation(const LMatrix3f r) {
  dMatrix3 mat3 = {r._m.data[0], r._m.data[1], r._m.data[2], 0,
                   r._m.data[3], r._m.data[4], r._m.data[5], 0,
                   r._m.data[6], r._m.data[7], r._m.data[8], 0};
  
  dBodySetRotation(_id, mat3);
}

INLINE void OdeBody::
set_quaternion(const LQuaternionf q) {
  dQuaternion quat = {q._v.data[0], 
                      q._v.data[1], 
                      q._v.data[2], 
                      q._v.data[3]};
  dBodySetQuaternion(_id, quat);
}

INLINE void OdeBody::
set_linear_vel(dReal x, dReal y, dReal z) {
  dBodySetLinearVel(_id, x, y, z);
}

INLINE void OdeBody::
set_linear_vel(const LVecBase3f &vel) {
  set_linear_vel(vel[0], vel[1], vel[2]);
}

INLINE void OdeBody::
set_angular_vel(dReal x, dReal y, dReal z) {
  dBodySetAngularVel(_id, x, y, z);
}

INLINE void OdeBody::
set_angular_vel(const LVecBase3f &vel) {
  set_angular_vel(vel[0], vel[1], vel[2]);
}

INLINE LVecBase3f OdeBody::
get_position() const {
  const dReal *res = dBodyGetPosition(_id);
  return LVecBase3f(res[0], res[1], res[2]);
}

INLINE LMatrix3f OdeBody::
get_rotation() const {
  const dReal *rot = dBodyGetRotation(_id);
  return LMatrix3f(rot[0], rot[1], rot[2],
                   rot[4], rot[5], rot[6],
                   rot[8], rot[9], rot[10]);
}

INLINE LVecBase4f OdeBody::
get_quaternion() const {
  const dReal *res = dBodyGetQuaternion(_id);
  return LVecBase4f(res[0], res[1], res[2], res[3]);
}

INLINE LVecBase3f OdeBody::
get_linear_vel() const {
  const dReal *res = dBodyGetLinearVel(_id);
  return LVecBase3f(res[0], res[1], res[2]);
}

INLINE LVecBase3f OdeBody::
get_angular_vel() const {
  const dReal *res = dBodyGetAngularVel(_id);
  return LVecBase3f(res[0], res[1], res[2]);
}

INLINE void OdeBody::
set_mass(OdeMass &mass) {
  dBodySetMass(_id, mass.get_mass_ptr());
}

INLINE OdeMass OdeBody::
get_mass() const {
  OdeMass mass;
  dBodyGetMass(_id, mass.get_mass_ptr());
  return mass;
}

INLINE void OdeBody::
add_force(dReal fx, dReal fy, dReal fz) {
  dBodyAddForce(_id, fx, fy, fz);
}

INLINE void OdeBody::
add_force(const LVecBase3f &f) {
  add_force(f[0], f[1], f[2]);
}

INLINE void OdeBody::
add_torque(dReal fx, dReal fy, dReal fz) {
  dBodyAddTorque(_id, fx, fy, fz);
}

INLINE void OdeBody::
add_torque(const LVecBase3f &f) {
  add_torque(f[0], f[1], f[2]);
}

INLINE void OdeBody::
add_rel_force(dReal fx, dReal fy, dReal fz) {
  dBodyAddRelForce(_id, fx, fy, fz);
}

INLINE void OdeBody::
add_rel_force(const LVecBase3f &f) {
  add_rel_force(f[0], f[1], f[2]);
}

INLINE void OdeBody::
add_rel_torque(dReal fx, dReal fy, dReal fz) {
  dBodyAddRelTorque(_id, fx, fy, fz);
}

INLINE void OdeBody::
add_rel_torque(const LVecBase3f &f) {
  add_rel_torque(f[0], f[1], f[2]);
}

INLINE void OdeBody::
add_force_at_pos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz) {
  dBodyAddForceAtPos(_id, fx, fy, fz, px, py, pz);
}

INLINE void OdeBody::
add_force_at_pos(const LVecBase3f &f, const LVecBase3f &pos) {
  add_force_at_pos(f[0], f[1], f[2], pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
add_force_at_rel_pos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz) {
  dBodyAddForceAtRelPos(_id, fx, fy, fz, px, py, pz);
}

INLINE void OdeBody::
add_force_at_rel_pos(const LVecBase3f &f, const LVecBase3f &pos) {
  add_force_at_rel_pos(f[0], f[1], f[2], pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
add_rel_force_at_pos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz) {
  dBodyAddRelForceAtPos(_id, fx, fy, fz, px, py, pz);
}

INLINE void OdeBody::
add_rel_force_at_pos(const LVecBase3f &f, const LVecBase3f &pos) {
  add_rel_force_at_pos(f[0], f[1], f[2], pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
add_rel_force_at_rel_pos(dReal fx, dReal fy, dReal fz, dReal px, dReal py, dReal pz) {
  dBodyAddRelForceAtRelPos(_id, fx, fy, fz, px, py, pz);
}

INLINE void OdeBody::
add_rel_force_at_rel_pos(const LVecBase3f &f, const LVecBase3f &pos) {
  add_rel_force_at_rel_pos(f[0], f[1], f[2], pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
set_force(dReal x, dReal y, dReal z) {
  dBodySetForce(_id, x, y, z);
}

INLINE void OdeBody::
set_force(const LVecBase3f &f) {
  set_force(f[0], f[1], f[2]);
}

INLINE void OdeBody::
set_torque(dReal x, dReal y, dReal z) {
  dBodySetTorque(_id, x, y, z);
}

INLINE void OdeBody::
set_torque(const LVecBase3f &f) {
  set_torque(f[0], f[1], f[2]);
}

INLINE LPoint3f OdeBody::
get_rel_point_pos(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyGetRelPointPos(_id, px, py, pz, result);
  return LPoint3f(result[0], result[1], result[2]);
}

INLINE LPoint3f OdeBody::
get_rel_point_pos(const LVecBase3f &pos) const {
  return get_rel_point_pos(pos[0], pos[1], pos[2]);
}

INLINE LPoint3f OdeBody::
get_rel_point_vel(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyGetRelPointVel(_id, px, py, pz, result);
  return LPoint3f(result[0], result[1], result[2]);
}

INLINE LPoint3f OdeBody::
get_rel_point_vel(const LVecBase3f &pos) const {
  return get_rel_point_vel(pos[0], pos[1], pos[2]);
}

INLINE LPoint3f OdeBody::
get_point_vel(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyGetPointVel(_id, px, py, pz, result);
  return LPoint3f(result[0], result[1], result[2]);
}

INLINE LPoint3f OdeBody::
get_point_vel(const LVecBase3f &pos) const {
  return get_point_vel(pos[0], pos[1], pos[2]);
}

INLINE LPoint3f OdeBody::
get_pos_rel_point(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyGetPosRelPoint(_id, px, py, pz, result);
  return LPoint3f(result[0], result[1], result[2]);
}

INLINE LPoint3f OdeBody::
get_pos_rel_point(const LVecBase3f &pos) const {
  return get_pos_rel_point(pos[0], pos[1], pos[2]);
}

INLINE LVecBase3f OdeBody::
vector_to_world(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyVectorToWorld(_id, px, py, pz, result);
  return LVecBase3f(result[0], result[1], result[2]);
}

INLINE LVecBase3f OdeBody::
vector_to_world(const LVecBase3f &pos) const {
  return vector_to_world(pos[0], pos[1], pos[2]);
}

INLINE LVecBase3f OdeBody::
vector_from_world(dReal px, dReal py, dReal pz) const {
  dVector3 result;
  dBodyVectorFromWorld(_id, px, py, pz, result);
  return LVecBase3f(result[0], result[1], result[2]);
}

INLINE LVecBase3f OdeBody::
vector_from_world(const LVecBase3f &pos) const {
  return vector_from_world(pos[0], pos[1], pos[2]);
}

INLINE void OdeBody::
set_finite_rotation_mode(int mode) {
  dBodySetFiniteRotationMode(_id, mode);
}

INLINE void OdeBody::
set_finite_rotation_axis(dReal x, dReal y, dReal z) {
  dBodySetFiniteRotationAxis(_id, x, y, z);
}

INLINE void OdeBody::
set_finite_rotation_axis(const LVecBase3f &axis) {
  set_finite_rotation_axis(axis[0], axis[1], axis[2]);
}

INLINE int OdeBody::
get_finite_rotation_mode() const {
  return dBodyGetFiniteRotationMode(_id);
}

INLINE LVecBase3f OdeBody::
get_finite_rotation_axis() const {
  dVector3 result;
  dBodyGetFiniteRotationAxis(_id, result);
  return LVecBase3f(result[0], result[1], result[2]);
}

INLINE int OdeBody::
get_num_joints() const {
  return dBodyGetNumJoints(_id);
}

INLINE void OdeBody::
enable() {
  dBodyEnable(_id);
}

INLINE void OdeBody::
disable() {
  dBodyDisable(_id);
}

INLINE int OdeBody::
is_enabled() const {
  return dBodyIsEnabled(_id);
}

INLINE void OdeBody::
set_gravity_mode(int mode) {
  dBodySetGravityMode(_id, mode);
}

INLINE int OdeBody::
get_gravity_mode() const {
  return dBodyGetGravityMode(_id);
}

INLINE int OdeBody::
compare_to(const OdeBody &other) const {
  if (_id != other._id) {
    return _id < other._id ? -1 : 1;
  }
  return 0;
}
