// Filename: odeGeom.I
// Created by:  joswilso (27Dec06)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) Carnegie Mellon University.  All rights reserved.
//
// All use of this software is subject to the terms of the revised BSD
// license.  You should have received a copy of this license along
// with this source code in a file named "LICENSE."
//
////////////////////////////////////////////////////////////////////

////////////////////////////////////////////////////////////////////
//     Function: OdeGeom::is_empty
//       Access: Published
//  Description: Returns true if the ID is 0, meaning the OdeGeom
//               does not point to a valid geom. It is an error to
//               call a method on an empty geom.
//               Note that an empty OdeGeom also evaluates to False.
////////////////////////////////////////////////////////////////////
INLINE bool OdeGeom::
is_empty() const {
  return (_id == 0);
}

////////////////////////////////////////////////////////////////////
//     Function: OdeGeom::get_id
//       Access: Published
//  Description: Returns the underlying dGeomID.
////////////////////////////////////////////////////////////////////
INLINE dGeomID OdeGeom::
get_id() const {
  return _id;
}

INLINE bool OdeGeom::
has_body() const {
  return (dGeomGetBody(_id) != NULL);
}

INLINE OdeBody OdeGeom::
get_body() const {
  return OdeBody(dGeomGetBody(_id));
}

INLINE void OdeGeom::
set_body(OdeBody &body) {
  dGeomSetBody(_id, body.get_id());
}

/*
INLINE void OdeGeom::
set_data(void* data) {
  dGeomSetData(_id, data);
}
*/

INLINE void OdeGeom::
set_position(dReal x, dReal y, dReal z) {
  dGeomSetPosition(_id, x, y, z);
}

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

INLINE void OdeGeom::
set_rotation(const LMatrix3f &r) {
  dMatrix3 rot = { 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 };
  dGeomSetRotation(_id, rot);
}

INLINE void OdeGeom::
set_quaternion(const LQuaternionf &q) {
  dQuaternion quat = { q[0], q[1], q[2], q[3] };
  dGeomSetQuaternion(_id, quat);
}

INLINE LPoint3f OdeGeom::
get_position() const {
  const dReal *pos = dGeomGetPosition(_id);
  return LPoint3f(pos[0], pos[1], pos[2]);
}

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

INLINE LQuaternionf OdeGeom::
get_quaternion() const {
  dQuaternion res;
  dGeomGetQuaternion(_id, res);
  return LQuaternionf(res[0], res[1], res[2], res[3]);
}

INLINE void OdeGeom::
get_AABB(LVecBase3f &min, LVecBase3f &max) const {
  dReal result[6];
  dGeomGetAABB(_id, result);
  min.set(result[0], result[2], result[4]);
  max.set(result[1], result[3], result[5]);
}

INLINE int OdeGeom::
is_space() {
  return dGeomIsSpace(_id);
}

INLINE int OdeGeom::
get_class() const {
  return dGeomGetClass(_id);
}

INLINE void OdeGeom::
set_category_bits(const BitMask32 &bits) {
  dGeomSetCategoryBits(_id, bits.get_word());
}

INLINE void OdeGeom::
set_collide_bits(const BitMask32 &bits) {
  dGeomSetCollideBits(_id, bits.get_word());
}

INLINE BitMask32 OdeGeom::
get_category_bits() {
  return BitMask32(dGeomGetCategoryBits(_id));
}

INLINE BitMask32 OdeGeom::
get_collide_bits() {
  return BitMask32(dGeomGetCollideBits(_id));
}

INLINE void OdeGeom::
enable() {
  dGeomEnable(_id);
}

INLINE void OdeGeom::
disable() {
  dGeomDisable(_id);
}

INLINE int OdeGeom::
is_enabled() {
  return dGeomIsEnabled(_id);
}

INLINE void OdeGeom::
set_offset_position(dReal x, dReal y, dReal z) {
  dGeomSetOffsetPosition(_id, x, y, z);
}

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

INLINE void OdeGeom::
set_offset_rotation(const LMatrix3f &r) {
  dMatrix3 rot = { 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 };
  dGeomSetOffsetRotation(_id, rot);
}

INLINE void OdeGeom::
set_offset_quaternion(const LQuaternionf &q) {
  dQuaternion quat = { q[0], q[1], q[2], q[3] };
  dGeomSetOffsetQuaternion(_id, quat);
}

INLINE void OdeGeom::
set_offset_world_position(dReal x, dReal y, dReal z) {
  dGeomSetOffsetWorldPosition(_id, x, y, z);
}

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

INLINE void OdeGeom::
set_offset_world_rotation(const LMatrix3f &r) {
  dMatrix3 rot = { 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 };
  dGeomSetOffsetWorldRotation(_id, rot);
}

INLINE void OdeGeom::
set_offset_world_quaternion(const LQuaternionf &q) {
  dQuaternion quat = { q[0], q[1], q[2], q[3] };
  dGeomSetOffsetWorldQuaternion(_id, quat);
}

INLINE void OdeGeom::
clear_offset() {
  dGeomClearOffset(_id);
}

INLINE int OdeGeom::
is_offset() {
  return dGeomIsOffset(_id);
}

INLINE LPoint3f OdeGeom::
get_offset_position() const {
  const dReal *pos = dGeomGetOffsetPosition(_id);
  return LPoint3f(pos[0], pos[1], pos[2]);
}

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

INLINE LQuaternionf OdeGeom::
get_offset_quaternion() const {
  dQuaternion res;
  dGeomGetOffsetQuaternion(_id, res);
  return LQuaternionf(res[0], res[1], res[2], res[3]);
}

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