/*************************************************************************

 *                                                                       *

 * 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.                     *

 *                                                                       *

 *************************************************************************/

/*

design note: the general principle for giving a joint the option of connecting

to the static environment (i.e. the absolute frame) is to check the second

body (joint->node[1].body), and if it is zero then behave as if its body

transform is the identity.

*/

#include <ode/ode.h>

#include <ode/odemath.h>

#include <ode/rotation.h>

#include <ode/matrix.h>

#include "joint.h"

#define dCROSSMUL(a,op,b,c) \

do { \

  (a)[0] op dMUL(REAL(0.5),(dMUL((b)[1],(c)[2]) - dMUL((b)[2],(c)[1]))); \

  (a)[1] op dMUL(REAL(0.5),(dMUL((b)[2],(c)[0]) - dMUL((b)[0],(c)[2]))); \

  (a)[2] op dMUL(REAL(0.5),(dMUL((b)[0],(c)[1]) - dMUL((b)[1],(c)[0]))); \

} while(0)

//****************************************************************************

// externs

// extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);

// extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);

//****************************************************************************

// utility

// set three "ball-and-socket" rows in the constraint equation, and the

// corresponding right hand side.

static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,

			    dVector3 anchor1, dVector3 anchor2)

{

  // anchor points in global coordinates with respect to body PORs.

  dVector3 a1,a2;

  int s = info->rowskip;

  // set jacobian

  info->J1l[0] = REAL(1.0);

  info->J1l[s+1] = REAL(1.0);

  info->J1l[2*s+2] = REAL(1.0);

  dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1);

  dCROSSMAT (info->J1a,a1,s,-,+);

  if (joint->node[1].body) {

    info->J2l[0] = -REAL(1.0);

    info->J2l[s+1] = -REAL(1.0);

    info->J2l[2*s+2] = -REAL(1.0);

    dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2);

    dCROSSMAT (info->J2a,a2,s,+,-);

  }

  // set right hand side

  dReal k = dMUL(info->fps,info->erp);

  if (joint->node[1].body) {

    for (int j=0; j<3; j++) {

      info->c[j] = dMUL(k,(a2[j] + joint->node[1].body->posr.pos[j] -

			a1[j] - joint->node[0].body->posr.pos[j]));

    }

  }

  else {

    for (int j=0; j<3; j++) {

      info->c[j] = dMUL(k,(anchor2[j] - a1[j] -

			joint->node[0].body->posr.pos[j]));

    }

  }

}

// this is like setBall(), except that `axis' is a unit length vector

// (in global coordinates) that should be used for the first jacobian

// position row (the other two row vectors will be derived from this).

// `erp1' is the erp value to use along the axis.

static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,

			    dVector3 anchor1, dVector3 anchor2,

			    dVector3 axis, dReal erp1)

{

  // anchor points in global coordinates with respect to body PORs.

  dVector3 a1,a2;

  int i,s = info->rowskip;

  // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],

  // [0 1 0] and [0 0 1], which makes everything much easier.

  dVector3 q1,q2;

  dPlaneSpace (axis,q1,q2);

  // set jacobian

  for (i=0; i<3; i++) info->J1l[i] = axis[i];

  for (i=0; i<3; i++) info->J1l[s+i] = q1[i];

  for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i];

  dMULTIPLY0_331 (a1,joint->node[0].body->posr.R,anchor1);

  dCROSS (info->J1a,=,a1,axis);

  dCROSS (info->J1a+s,=,a1,q1);

  dCROSS (info->J1a+2*s,=,a1,q2);

  if (joint->node[1].body) {

    for (i=0; i<3; i++) info->J2l[i] = -axis[i];

    for (i=0; i<3; i++) info->J2l[s+i] = -q1[i];

    for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i];

    dMULTIPLY0_331 (a2,joint->node[1].body->posr.R,anchor2);

    dCROSS (info->J2a,= -,a2,axis);

    dCROSS (info->J2a+s,= -,a2,q1);

    dCROSS (info->J2a+2*s,= -,a2,q2);

  }

  // set right hand side - measure error along (axis,q1,q2)

  dReal k1 = dMUL(info->fps,erp1);

  dReal k = dMUL(info->fps,info->erp);

  for (i=0; i<3; i++) a1[i] += joint->node[0].body->posr.pos[i];

  if (joint->node[1].body) {

    for (i=0; i<3; i++) a2[i] += joint->node[1].body->posr.pos[i];

    info->c[0] = dMUL(k1,(dDOT(axis,a2) - dDOT(axis,a1)));

    info->c[1] = dMUL(k,(dDOT(q1,a2) - dDOT(q1,a1)));

    info->c[2] = dMUL(k,(dDOT(q2,a2) - dDOT(q2,a1)));

  }

  else {

    info->c[0] = dMUL(k1,(dDOT(axis,anchor2) - dDOT(axis,a1)));

    info->c[1] = dMUL(k,(dDOT(q1,anchor2) - dDOT(q1,a1)));

    info->c[2] = dMUL(k,(dDOT(q2,anchor2) - dDOT(q2,a1)));

  }

}

// set three orientation rows in the constraint equation, and the

// corresponding right hand side.

static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row)

{

  int s = info->rowskip;

  int start_index = start_row * s;

  // 3 rows to make body rotations equal

  info->J1a[start_index] = REAL(1.0);

  info->J1a[start_index + s + 1] = REAL(1.0);

  info->J1a[start_index + s*2+2] = REAL(1.0);

  if (joint->node[1].body) {

    info->J2a[start_index] = REAL(-1.0);

    info->J2a[start_index + s+1] = REAL(-1.0);

    info->J2a[start_index + s*2+2] = REAL(-1.0);

  }

  // compute the right hand side. the first three elements will result in

  // relative angular velocity of the two bodies - this is set to bring them

  // back into alignment. the correcting angular velocity is

  //   |angular_velocity| = angle/time = erp*theta / stepsize

  //                      = (erp*fps) * theta

  //    angular_velocity  = |angular_velocity| * u

  //                      = (erp*fps) * theta * u

  // where rotation along unit length axis u by theta brings body 2's frame

  // to qrel with respect to body 1's frame. using a small angle approximation

  // for sin(), this gives

  //    angular_velocity  = (erp*fps) * 2 * v

  // where the quaternion of the relative rotation between the two bodies is

  //    q = [cos(theta/2) sin(theta/2)*u] = [s v]

  // get qerr = relative rotation (rotation error) between two bodies

  dQuaternion qerr,e;

  if (joint->node[1].body) {

    dQuaternion qq;

    dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);

    dQMultiply2 (qerr,qq,qrel);

  }

  else {

    dQMultiply3 (qerr,joint->node[0].body->q,qrel);

  }

  if (qerr[0] < 0) {

    qerr[1] = -qerr[1];		// adjust sign of qerr to make theta small

    qerr[2] = -qerr[2];

    qerr[3] = -qerr[3];

  }

  dMULTIPLY0_331 (e,joint->node[0].body->posr.R,qerr+1); // @@@ bad SIMD padding!

  dReal k = dMUL(info->fps,info->erp);

  info->c[start_row] = 2*dMUL(k,e[0]);

  info->c[start_row+1] = 2*dMUL(k,e[1]);

  info->c[start_row+2] = 2*dMUL(k,e[2]);

}

// compute anchor points relative to bodies

static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,

			dVector3 anchor1, dVector3 anchor2)

{

  if (j->node[0].body) {

    dReal q[4];

    q[0] = x - j->node[0].body->posr.pos[0];

    q[1] = y - j->node[0].body->posr.pos[1];

    q[2] = z - j->node[0].body->posr.pos[2];

    q[3] = 0;

    dMULTIPLY1_331 (anchor1,j->node[0].body->posr.R,q);

    if (j->node[1].body) {

      q[0] = x - j->node[1].body->posr.pos[0];

      q[1] = y - j->node[1].body->posr.pos[1];

      q[2] = z - j->node[1].body->posr.pos[2];

      q[3] = 0;

      dMULTIPLY1_331 (anchor2,j->node[1].body->posr.R,q);

    }

    else {

      anchor2[0] = x;

      anchor2[1] = y;

      anchor2[2] = z;

    }

  }

  anchor1[3] = 0;

  anchor2[3] = 0;

}

// compute axes relative to bodies. either axis1 or axis2 can be 0.

static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,

		     dVector3 axis1, dVector3 axis2)

{

  if (j->node[0].body) {

    dReal q[4];

    q[0] = x;

    q[1] = y;

    q[2] = z;

    q[3] = 0;

    dNormalize3 (q);

    if (axis1) {

      dMULTIPLY1_331 (axis1,j->node[0].body->posr.R,q);

      axis1[3] = 0;

    }

    if (axis2) {

      if (j->node[1].body) {

	dMULTIPLY1_331 (axis2,j->node[1].body->posr.R,q);

      }

      else {

	axis2[0] = x;

	axis2[1] = y;

	axis2[2] = z;

      }

      axis2[3] = 0;

    }

  }

}

static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)

{

  if (j->node[0].body) {

    dMULTIPLY0_331 (result,j->node[0].body->posr.R,anchor1);

    result[0] += j->node[0].body->posr.pos[0];

    result[1] += j->node[0].body->posr.pos[1];

    result[2] += j->node[0].body->posr.pos[2];

  }

}

static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2)

{

  if (j->node[1].body) {

    dMULTIPLY0_331 (result,j->node[1].body->posr.R,anchor2);

    result[0] += j->node[1].body->posr.pos[0];

    result[1] += j->node[1].body->posr.pos[1];

    result[2] += j->node[1].body->posr.pos[2];

  }

  else {

    result[0] = anchor2[0];

    result[1] = anchor2[1];

    result[2] = anchor2[2];

  }

}

static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)

{

  if (j->node[0].body) {

    dMULTIPLY0_331 (result,j->node[0].body->posr.R,axis1);

  }

}

static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2)

{

  if (j->node[1].body) {

    dMULTIPLY0_331 (result,j->node[1].body->posr.R,axis2);

  }

  else {

    result[0] = axis2[0];

    result[1] = axis2[1];

    result[2] = axis2[2];

  }

}

static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis)

{

  // the angle between the two bodies is extracted from the quaternion that

  // represents the relative rotation between them. recall that a quaternion

  // q is:

  //    [s,v] = [ cos(theta/2) , sin(theta/2) * u ]

  // where s is a scalar and v is a 3-vector. u is a unit length axis and

  // theta is a rotation along that axis. we can get theta/2 by:

  //    theta/2 = atan2 ( sin(theta/2) , cos(theta/2) )

  // but we can't get sin(theta/2) directly, only its absolute value, i.e.:

  //    |v| = |sin(theta/2)| * |u|

  //        = |sin(theta/2)|

  // using this value will have a strange effect. recall that there are two

  // quaternion representations of a given rotation, q and -q. typically as

  // a body rotates along the axis it will go through a complete cycle using

  // one representation and then the next cycle will use the other

  // representation. this corresponds to u pointing in the direction of the

  // hinge axis and then in the opposite direction. the result is that theta

  // will appear to go "backwards" every other cycle. here is a fix: if u

  // points "away" from the direction of the hinge (motor) axis (i.e. more

  // than 90 degrees) then use -q instead of q. this represents the same

  // rotation, but results in the cos(theta/2) value being sign inverted.

  // extract the angle from the quaternion. cost2 = cos(theta/2),

  // sint2 = |sin(theta/2)|

  dReal cost2 = qrel[0];

  dReal sint2 = dSqrt (dMUL(qrel[1],qrel[1])+dMUL(qrel[2],qrel[2])+dMUL(qrel[3],qrel[3]));

  dReal theta = (dDOT(qrel+REAL(1.0),axis) >= 0) ?	// @@@ padding assumptions

    (2 * dArcTan2(sint2,cost2)) :		// if u points in direction of axis

    (2 * dArcTan2(sint2,-cost2));		// if u points in opposite direction

  // the angle we get will be between 0..2*pi, but we want to return angles

  // between -pi..pi

  if (theta > dPI) theta -= 2*dPI;

  // the angle we've just extracted has the wrong sign

  theta = -theta;

  return theta;

}

// given two bodies (body1,body2), the hinge axis that they are connected by

// w.r.t. body1 (axis), and the initial relative orientation between them

// (q_initial), return the relative rotation angle. the initial relative

// orientation corresponds to an angle of zero. if body2 is 0 then measure the

// angle between body1 and the static frame.

//

// this will not return the correct angle if the bodies rotate along any axis

// other than the given hinge axis.

static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,

			    dQuaternion q_initial)

{

  // get qrel = relative rotation between the two bodies

  dQuaternion qrel;

  if (body2) {

    dQuaternion qq;

    dQMultiply1 (qq,body1->q,body2->q);

    dQMultiply2 (qrel,qq,q_initial);

  }

  else {

    // pretend body2->q is the identity

    dQMultiply3 (qrel,body1->q,q_initial);

  }

  return getHingeAngleFromRelativeQuat (qrel,axis);

}

//****************************************************************************

// dxJointLimitMotor

void dxJointLimitMotor::init (dxWorld *world)

{

  vel = 0;

  fmax = 0;

  lostop = -dInfinity;

  histop = dInfinity;

  fudge_factor = REAL(1.0);

  normal_cfm = world->global_cfm;

  stop_erp = world->global_erp;

  stop_cfm = world->global_cfm;

  bounce = 0;

  limit = 0;

  limit_err = 0;

}

void dxJointLimitMotor::set (int num, dReal value)

{

  switch (num) {

  case dParamLoStop:

    lostop = value;

    break;

  case dParamHiStop:

    histop = value;

    break;

  case dParamVel:

    vel = value;

    break;

  case dParamFMax:

    if (value >= 0) fmax = value;

    break;

  case dParamFudgeFactor:

    if (value >= 0 && value <= REAL(1.0)) fudge_factor = value;

    break;

  case dParamBounce:

    bounce = value;

    break;

  case dParamCFM:

    normal_cfm = value;

    break;

  case dParamStopERP:

    stop_erp = value;

    break;

  case dParamStopCFM:

    stop_cfm = value;

    break;

  }

}

dReal dxJointLimitMotor::get (int num)

{

  switch (num) {

  case dParamLoStop: return lostop;

  case dParamHiStop: return histop;

  case dParamVel: return vel;

  case dParamFMax: return fmax;

  case dParamFudgeFactor: return fudge_factor;

  case dParamBounce: return bounce;

  case dParamCFM: return normal_cfm;

  case dParamStopERP: return stop_erp;

  case dParamStopCFM: return stop_cfm;

  default: return 0;

  }

}

int dxJointLimitMotor::testRotationalLimit (dReal angle)

{

  if (angle <= lostop) {

    limit = 1;

    limit_err = angle - lostop;

    return 1;

  }

  else if (angle >= histop) {

    limit = 2;

    limit_err = angle - histop;

    return 1;

  }

  else {

    limit = 0;

    return 0;

  }

}

int dxJointLimitMotor::addLimot (dxJoint *joint,

				 dxJoint::Info2 *info, int row,

				 const dVector3 ax1, int rotational)

{

  int srow = row * info->rowskip;

  // if the joint is powered, or has joint limits, add in the extra row

  int powered = fmax > 0;

  if (powered || limit) {

    dReal *J1 = rotational ? info->J1a : info->J1l;

    dReal *J2 = rotational ? info->J2a : info->J2l;

    J1[srow+0] = ax1[0];

    J1[srow+1] = ax1[1];

    J1[srow+2] = ax1[2];

    if (joint->node[1].body) {

      J2[srow+0] = -ax1[0];

      J2[srow+1] = -ax1[1];

      J2[srow+2] = -ax1[2];

    }

    // linear limot torque decoupling step:

    //

    // if this is a linear limot (e.g. from a slider), we have to be careful

    // that the linear constraint forces (+/- ax1) applied to the two bodies

    // do not create a torque couple. in other words, the points that the

    // constraint force is applied at must lie along the same ax1 axis.

    // a torque couple will result in powered or limited slider-jointed free

    // bodies from gaining angular momentum.

    // the solution used here is to apply the constraint forces at the point

    // halfway between the body centers. there is no penalty (other than an

    // extra tiny bit of computation) in doing this adjustment. note that we

    // only need to do this if the constraint connects two bodies.

    dVector3 ltd = {};	// Linear Torque Decoupling vector (a torque)

    if (!rotational && joint->node[1].body) {

      dVector3 c;

      c[0]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[0]-joint->node[0].body->posr.pos[0]));

      c[1]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[1]-joint->node[0].body->posr.pos[1]));

      c[2]=dMUL(REAL(0.5),(joint->node[1].body->posr.pos[2]-joint->node[0].body->posr.pos[2]));

      dCROSS (ltd,=,c,ax1);

      info->J1a[srow+0] = ltd[0];

      info->J1a[srow+1] = ltd[1];

      info->J1a[srow+2] = ltd[2];

      info->J2a[srow+0] = ltd[0];

      info->J2a[srow+1] = ltd[1];

      info->J2a[srow+2] = ltd[2];

    }

    // if we're limited low and high simultaneously, the joint motor is

    // ineffective

    if (limit && (lostop == histop)) powered = 0;

    if (powered) {

      info->cfm[row] = normal_cfm;

      if (! limit) {