/*************************************************************************
 *                                                                       *
 * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
 * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
 *                                                                       *
 * This library is free software; you can redistribute it and/or         *
 * modify it under the terms of EITHER:                                  *
 *   (1) The GNU Lesser General Public License as published by the Free  *
 *       Software Foundation; either version 2.1 of the License, or (at  *
 *       your option) any later version. The text of the GNU Lesser      *
 *       General Public License is included with this library in the     *
 *       file LICENSE.TXT.                                               *
 *   (2) The BSD-style license that is included with this library in     *
 *       the file LICENSE-BSD.TXT.                                       *
 *                                                                       *
 * This library is distributed in the hope that it will be useful,       *
 * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
 * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
 *                                                                       *
 *************************************************************************/


#include <ode/odeconfig.h>
#include "config.h"
#include "dhinge.h"
#include "joint_internal.h"

/*
 * Double Hinge joint
 */

dxJointDHinge::dxJointDHinge(dxWorld* w) :
    dxJointDBall(w)
{
    dSetZero(axis1, 3);
    dSetZero(axis2, 3);
}


void
dxJointDHinge::getSureMaxInfo( SureMaxInfo* info )
{
    info->max_m = 4;
}


void
dxJointDHinge::getInfo1( dxJoint::Info1* info )
{
    info->m = 4;
    info->nub = 4;
}


void
dxJointDHinge::getInfo2( dReal worldFPS, dReal worldERP, 
    int rowskip, dReal *J1, dReal *J2,
    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
    int *findex )
{
    dxJointDBall::getInfo2( worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, pairLoHi, findex ); // sets row0
    
    dVector3 globalAxis1;
    dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);

    dxBody *body1 = node[1].body;

    // angular constraints, perpendicular to axis
    dVector3 p, q;
    dPlaneSpace(globalAxis1, p, q);

    dCopyVector3(J1 + rowskip + GI2__JA_MIN, p);
    if ( body1 ) {
        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, p);
    }

    dCopyVector3(J1 + 2 * rowskip + GI2__JA_MIN, q);
    if ( body1 ) {
        dCopyNegatedVector3(J2 + 2 * rowskip + GI2__JA_MIN, q);
    }

    dVector3 globalAxis2;
    if ( body1 ) {
        dBodyVectorToWorld(body1, axis2[0], axis2[1], axis2[2], globalAxis2);
    } else {
        dCopyVector3(globalAxis2, axis2);
    }
    
    // similar to the hinge joint
    dVector3 u;
    dCalcVectorCross3(u, globalAxis1, globalAxis2);

    const dReal k = worldFPS * this->erp;
    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( u, p );
    pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( u, q );




    /*
     * Constraint along the axis: translation along it should couple angular movement.
     * This is just the ball-and-socket derivation, projected onto the hinge axis,
     * producing a single constraint at the end.
     *
     * The choice of "ball" position can be arbitrary; we could place it at the center
     * of one of the bodies, canceling out its rotational jacobian; or we could make
     * everything symmetrical by just placing at the midpoint between the centers.
     *
     * I like symmetry, so I'll use the second approach here. I'll call the midpoint h.
     *
     * Of course, if the second body is NULL, the first body is pretty much locked
     * along this axis, and the linear constraint is enough.
     */

    int rowskip_mul_3 = 3 * rowskip;
    dCopyVector3(J1 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);

    if ( body1 ) {
        dVector3 h;
        dAddScaledVectors3(h, node[0].body->posr.pos, body1->posr.pos, -0.5, 0.5);

        dCalcVectorCross3(J1 + rowskip_mul_3 + GI2__JA_MIN, h, globalAxis1);

        dCopyNegatedVector3(J2 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);
        dCopyVector3(J2 + rowskip_mul_3 + GI2__JA_MIN, J1 + rowskip_mul_3 + GI2__JA_MIN);
    }

    // error correction: both anchors should lie on the same plane perpendicular to the axis
    dVector3 globalA1, globalA2;
    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);

    if ( body1 ) {
        dBodyGetRelPointPos(body1, anchor2[0], anchor2[1], anchor2[2], globalA2);
    } else {
        dCopyVector3(globalA2, anchor2);
    }

    dVector3 d;
    dSubtractVectors3(d, globalA1, globalA2); // displacement error
    pairRhsCfm[3 * pairskip + GI2_RHS] = -k * dCalcVectorDot3(globalAxis1, d);
}

void dJointSetDHingeAxis( dJointID j, dReal x, dReal y, dReal z )
{
    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
    dUASSERT( joint, "bad joint argument" );

    dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axis1);
    if (joint->node[1].body)
        dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
    else {
        joint->axis2[0] = x;
        joint->axis2[1] = y;
        joint->axis2[2] = z;
    }
    dNormalize3(joint->axis1);
    dNormalize3(joint->axis2);
}

void dJointGetDHingeAxis( dJointID j, dVector3 result )
{
    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
    dUASSERT( joint, "bad joint argument" );

    dBodyVectorToWorld(joint->node[0].body, joint->axis1[0], joint->axis1[1], joint->axis1[2], result);
}


void dJointSetDHingeAnchor1( dJointID j, dReal x, dReal y, dReal z )
{
    dJointSetDBallAnchor1(j, x, y, z);
}


void dJointSetDHingeAnchor2( dJointID j, dReal x, dReal y, dReal z )
{
    dJointSetDBallAnchor2(j, x, y, z);
}

dReal dJointGetDHingeDistance(dJointID j)
{
    return dJointGetDBallDistance(j);
}


void dJointGetDHingeAnchor1( dJointID j, dVector3 result )
{
    dJointGetDBallAnchor1(j, result);
}


void dJointGetDHingeAnchor2( dJointID j, dVector3 result )
{
    dJointGetDBallAnchor2(j, result);
}


void dJointSetDHingeParam( dJointID j, int parameter, dReal value )
{
    dJointSetDBallParam(j, parameter, value);
}


dReal dJointGetDHingeParam( dJointID j, int parameter )
{
    return dJointGetDBallParam(j, parameter);
}

dJointType
dxJointDHinge::type() const
{
    return dJointTypeDHinge;
}

sizeint
dxJointDHinge::size() const
{
    return sizeof( *this );
}
