/**
 @file odeHelper.cpp

 Dojo Game Engine

 Copyright 2005, Morgan McGuire
 All rights reserved.
 */

#include "dojo/odeHelper.h"

void dGeomGetPosition(dGeomID id, Vector3& pos) {
    const dReal* v = dGeomGetPosition(id);
    for (int i = 0; i < 3; ++i) {
        pos[i] = static_cast<float>(v[i]);
    }
}


void dGeomSetPosition(dGeomID id, const Vector3& pos) {
    dGeomSetPosition(id, pos.x, pos.y, pos.z);
}


void dGeomTranslate(dGeomID id, const Vector3& wsDelta) {
    Vector3 v;
    dGeomGetPosition(id, v);
    dGeomSetPosition(id, v + wsDelta);
}


static void toODE(const Matrix3& gM, dMatrix3& oM) {
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            oM[i * 4 + j] = static_cast<dReal>(gM[i][j]);
        }
        oM[i * 4 + 3] = 0.0;
    }
}


static void toG3D(const dMatrix3& oM, Matrix3& gM) {
    for (int i = 0; i < 3; ++i) {
        for (int j = 0; j < 3; ++j) {
            gM[i][j] = static_cast<float>(oM[i * 4 + j]);
        }
    }
}


void dMassToObjectSpace(dMass *m, const CoordinateFrame& cframe) {
    dMassToWorldSpace(m, cframe.inverse());
}


void dMassToWorldSpace(dMass *m, const CoordinateFrame& cframe) {
    dMassRotate(m, cframe.rotation);
    dMassTranslate(m, cframe.translation);
}


void dMassTranslate(dMass* m, const Vector3& t) {
    dMassTranslate(m, t.x, t.y, t.z);
}


void dMassRotate(dMass* m, const Matrix3& R) {
    dMatrix3 odeR;
    toODE(R, odeR);
    dMassRotate(m, odeR);
}


int dAreConnected(dGeomID g1, dGeomID g2) {
    if ((dGeomGetClass(g1) == dPlaneClass) ||
        (dGeomGetClass(g2) == dPlaneClass)) {
        // Non-placeable; we can't get any information out. 
        // These are only connected if they are the same geom.
        return g1 == g2;
    }

    dBodyID b1 = dGeomGetBody(g1);
    dBodyID b2 = dGeomGetBody(g2);

    if (b1 == b2) {
        return true;
    }

    if ((b1 == 0) || (b2 == 0)) {
        // If one is zero, then they can't be connected
        return false;
    }

    return dAreConnected(b1, b2);
}


int dAreConnectedExcluding(dGeomID g1, dGeomID g2, int joint_type) {
    // We want to ask if two geoms are connected, but ODE only allows
    // connection tests between bodies.  So we have to figure out
    // which bodies contain the geoms.

    if ((dGeomGetClass(g1) == dPlaneClass) ||
        (dGeomGetClass(g2) == dPlaneClass)) {
        // Non-placeable; we can't get any information out. 
        // These are only connected if they are the same geom.
        return g1 == g2;
    }

    dBodyID b1 = dGeomGetBody(g1);
    dBodyID b2 = dGeomGetBody(g2);

    if (b1 == b2) {
        return true;
    }

    if ((b1 == 0) || (b2 == 0)) {
        // If one is zero, then they can't be connected
        return false;
    }

    // Call ODE's are connected function.  
    return dAreConnectedExcluding(b1, b2, joint_type);
}


void dJointGetBallAnchor(dJointID ID, Vector3& anchor) {
    dReal v[3];
    dJointGetBallAnchor(ID, v);
    anchor.x = v[0];
    anchor.y = v[1];
    anchor.z = v[2];
}


void dJointGetHingeAnchor(dJointID ID, Vector3& anchor) {
    dReal v[3];
    dJointGetHingeAnchor(ID, v);
    anchor.x = v[0];
    anchor.y = v[1];
    anchor.z = v[2];
}


void dJointGetHingeAxis(dJointID ID, Vector3& axis) {
    dReal v[3];
    dJointGetHingeAxis(ID, v);
    axis.x = v[0];
    axis.y = v[1];
    axis.z = v[2];
}


void dJointSetHingeAnchor(dJointID id, const Vector3& v) {
    dJointSetHingeAnchor(id, v.x, v.y, v.z);
}


void dJointSetHingeAxis(dJointID id, const Vector3& v) {
    dJointSetHingeAxis(id, v.x, v.y, v.z);
}


void dJointSetUniversalAnchor(dJointID id, const Vector3& v) {
    dJointSetUniversalAnchor(id, v.x, v.y, v.z);
}


void dJointSetUniversalAxes(dJointID id, const Vector3& v, const Vector3& v2) {
    dJointSetUniversalAxis1(id, v.x, v.y, v.z);
    dJointSetUniversalAxis2(id, v2.x, v2.y, v2.z);
}


void dJointGetUniversalAxes(dJointID ID, Vector3& axis, Vector3& axis2) {
    dReal v[3];
    dJointGetUniversalAxis1(ID, v);
    axis.x = v[0];
    axis.y = v[1];
    axis.z = v[2];
    dJointGetUniversalAxis2(ID, v);
    axis2.x = v[0];
    axis2.y = v[1];
    axis2.z = v[2];
}


void dJointGetUniversalAnchor(dJointID ID, Vector3& anchor) {
    dReal v[3];
    dJointGetUniversalAnchor(ID, v);
    anchor.x = v[0];
    anchor.y = v[1];
    anchor.z = v[2];
}

void dGeomSetPositionAndRotation(dGeomID id, const CoordinateFrame& cframe) {
    const Matrix3& g3dR = cframe.rotation;
    dMatrix3 odeR;
    toODE(g3dR, odeR);

    const Vector3& c = cframe.translation;

    dGeomSetPosition(id, c.x, c.y, c.z);
    dGeomSetRotation(id, odeR);
}


void dBodySetPositionAndRotation(dBodyID id, const CoordinateFrame& cframe) {
    const Matrix3& g3dR = cframe.rotation;
    dMatrix3 odeR;
    toODE(g3dR, odeR);
    
    const Vector3& c = cframe.translation;

    dBodySetPosition(id, c.x, c.y, c.z);
    dBodySetRotation(id, odeR);
}


void dBodyGetPositionAndRotation(dBodyID id, CoordinateFrame& c) {

    // dReal may be either single or double
    const dReal* t = dBodyGetPosition(id);
    const dReal* r = dBodyGetRotation(id);    

    for (int i = 0; i < 3; ++i) {
        c.translation[i] = t[i];
        for (int j = 0; j < 3; ++j) {
            c.rotation[i][j] = static_cast<float>(r[i * 4 + j]);
        }
    }
}


void dGeomGetPositionAndRotation(dGeomID id, CoordinateFrame& c) {

    // dReal may be either single or double
    const dReal* t = dGeomGetPosition(id);
    const dReal* r = dGeomGetRotation(id);    

    for (int i = 0; i < 3; ++i) {
        c.translation[i] = t[i];
        for (int j = 0; j < 3; ++j) {
            c.rotation[i][j] = static_cast<float>(r[i * 4 + j]);
        }
    }
}


void dBodySetLinearVel(dBodyID id, const Vector3& v) {
    dBodySetLinearVel(id, v.x, v.y, v.z);
}


void dBodySetAngularVel(dBodyID id, const Vector3& v) {
    dBodySetAngularVel(id, v.x, v.y, v.z);
}


void dBodyGetLinearVel(dBodyID id, Vector3& L) {
    // dReal may be either single or double
    const dReal* t = dBodyGetLinearVel(id);

    for (int i = 0; i < 3; ++i) {
        L[i] = static_cast<float>(t[i]);
    }
}


void dBodyGetAngularVel(dBodyID id, Vector3& A) {
    // dReal may be either single or double
    const dReal* r = dBodyGetAngularVel(id);    

    for (int i = 0; i < 3; ++i) {
        A[i] = static_cast<float>(r[i]);
    }
}


void dBodyGetLinearAndAngularVel(dBodyID id, Vector3& L, Vector3& A) {
    // dReal may be either single or double
    const dReal* t = dBodyGetLinearVel(id);
    const dReal* r = dBodyGetAngularVel(id);    

    for (int i = 0; i < 3; ++i) {
        L[i] = static_cast<float>(t[i]);
        A[i] = static_cast<float>(r[i]);
    }
}
