add ode

1 files changed, 546 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/hinge2.cpp b/libs/ode-0.16.1/ode/src/joints/hinge2.cpp
new file mode 100644
index 0000000..89d5e30
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/hinge2.cpp
@@ -0,0 +1,546 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "hinge2.h"
+#include "joint_internal.h"
+
+
+
+
+//****************************************************************************
+// hinge 2. note that this joint must be attached to two bodies for it to work
+
+dReal
+dxJointHinge2::measureAngle1() const
+{
+    // bring axis 2 into first body's reference frame
+    dVector3 p, q;
+    if (node[1].body)
+        dMultiply0_331( p, node[1].body->posr.R, axis2 );
+    else
+        dCopyVector3(p, axis2);
+
+    if (node[0].body)
+        dMultiply1_331( q, node[0].body->posr.R, p );
+    else
+        dCopyVector3(q, p);
+
+    dReal x = dCalcVectorDot3( v1, q );
+    dReal y = dCalcVectorDot3( v2, q );
+    return -dAtan2( y, x );
+}
+
+dReal
+dxJointHinge2::measureAngle2() const
+{
+    // bring axis 1 into second body's reference frame
+    dVector3 p, q;
+    if (node[0].body)
+        dMultiply0_331( p, node[0].body->posr.R, axis1 );
+    else
+        dCopyVector3(p, axis1);
+
+    if (node[1].body)
+        dMultiply1_331( q, node[1].body->posr.R, p );
+    else
+        dCopyVector3(q, p);
+
+    dReal x = dCalcVectorDot3( w1, q );
+    dReal y = dCalcVectorDot3( w2, q );
+    return -dAtan2( y, x );
+}
+
+
+dxJointHinge2::dxJointHinge2( dxWorld *w ) :
+    dxJoint( w )
+{
+    dSetZero( anchor1, 4 );
+    dSetZero( anchor2, 4 );
+    dSetZero( axis1, 4 );
+    axis1[0] = 1;
+    dSetZero( axis2, 4 );
+    axis2[1] = 1;
+    c0 = 0;
+    s0 = 0;
+
+    dSetZero( v1, 4 );
+    v1[0] = 1;
+    dSetZero( v2, 4 );
+    v2[1] = 1;
+
+    limot1.init( world );
+    limot2.init( world );
+
+    susp_erp = world->global_erp;
+    susp_cfm = world->global_cfm;
+
+    flags |= dJOINT_TWOBODIES;
+}
+
+
+void 
+dxJointHinge2::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointHinge2::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 4;
+    info->nub = 4;
+
+    // see if we're powered or at a joint limit for axis 1
+    limot1.limit = 0;
+    if (( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
+        limot1.lostop <= limot1.histop )
+    {
+        dReal angle = measureAngle1();
+        limot1.testRotationalLimit( angle );
+    }
+    if ( limot1.limit || limot1.fmax > 0 ) info->m++;
+
+    // see if we're powering axis 2 (we currently never limit this axis)
+    limot2.limit = 0;
+    if ( limot2.fmax > 0 ) info->m++;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
+/// relative to body 1 and 2 initially) and then computes the constrained
+/// rotational axis as the cross product of ax1 and ax2.
+/// the sin and cos of the angle between axis 1 and 2 is computed, this comes
+/// from dot and cross product rules.
+///
+/// @param ax1 Will contain the joint axis1 in world frame
+/// @param ax2 Will contain the joint axis2 in world frame
+/// @param axis Will contain the cross product of ax1 x ax2
+/// @param sin_angle
+/// @param cos_angle
+////////////////////////////////////////////////////////////////////////////////
+void
+dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
+                           dReal &sin_angle, dReal &cos_angle) const
+{
+    dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
+    dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
+    dCalcVectorCross3(axCross,ax1,ax2);
+    sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
+    cos_angle = dCalcVectorDot3 (ax1,ax2);
+}
+
+
+void
+dxJointHinge2::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // get information we need to set the hinge row
+    dReal s, c;
+    dVector3 q;
+
+    dVector3 ax1, ax2;
+    getAxisInfo( ax1, ax2, q, s, c );
+    dNormalize3( q );   // @@@ quicker: divide q by s ?
+
+    // set the three ball-and-socket rows (aligned to the suspension axis ax1)
+    setBall2( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, anchor1, anchor2, ax1, susp_erp );
+    // set parameter for the suspension
+    pairRhsCfm[GI2_CFM] = susp_cfm;
+
+    // set the hinge row
+    int currRowSkip = 3 * rowskip;
+    dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, q);
+    if ( node[1].body ) {
+        dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, q);
+    }
+
+    // compute the right hand side for the constrained rotational DOF.
+    // axis 1 and axis 2 are separated by an angle `theta'. the desired
+    // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
+    // in the joint structure. the correcting angular velocity is:
+    //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
+    //                      = (erp*fps) * (theta0-theta)
+    // (theta0-theta) can be computed using the following small-angle-difference
+    // approximation:
+    //   theta0-theta ~= tan(theta0-theta)
+    //                 = sin(theta0-theta)/cos(theta0-theta)
+    //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
+    //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
+    // where c = cos(theta), s = sin(theta)
+    //       c0 = cos(theta0), s0 = sin(theta0)
+
+    dReal k = worldFPS * worldERP;
+
+    int currPairSkip = 3 * pairskip;
+    pairRhsCfm[currPairSkip + GI2_RHS] = k * ( c0 * s - this->s0 * c );
+
+    currRowSkip += rowskip; currPairSkip += pairskip;
+    // if the axis1 hinge is powered, or has joint limits, add in more stuff
+    if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) {
+        currRowSkip += rowskip; currPairSkip += pairskip;
+    }
+
+    // if the axis2 hinge is powered, add in more stuff
+    limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 );
+}
+
+
+// compute vectors v1 and v2 (embedded in body1), used to measure angle
+// between body 1 and body 2
+
+void
+dxJointHinge2::makeV1andV2()
+{
+    if ( node[0].body )
+    {
+        // get axis 1 and 2 in global coords
+        dVector3 ax1, ax2, v;
+        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
+
+        // modify axis 2 so it's perpendicular to axis 1
+        dReal k = dCalcVectorDot3( ax1, ax2 );
+        dAddVectorScaledVector3(ax2, ax2, ax1, -k);
+        
+        if (dxSafeNormalize3( ax2 )) {
+            // make v1 = modified axis2, v2 = axis1 x (modified axis2)
+            dCalcVectorCross3( v, ax1, ax2 );
+            dMultiply1_331( v1, node[0].body->posr.R, ax2 );
+            dMultiply1_331( v2, node[0].body->posr.R, v );
+        }
+        else {
+            dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent");
+        }
+    }
+}
+
+// same as above, but for the second axis
+
+void
+dxJointHinge2::makeW1andW2()
+{
+    if ( node[1].body )
+    {
+        // get axis 1 and 2 in global coords
+        dVector3 ax1, ax2, w;
+        dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
+        dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
+
+        // modify axis 1 so it's perpendicular to axis 2
+        dReal k = dCalcVectorDot3( ax2, ax1 );
+        dAddVectorScaledVector3(ax1, ax1, ax2, -k);
+        
+        if (dxSafeNormalize3( ax1 )) {
+            // make w1 = modified axis1, w2 = axis2 x (modified axis1)
+            dCalcVectorCross3( w, ax2, ax1 );
+            dMultiply1_331( w1, node[1].body->posr.R, ax1 );
+            dMultiply1_331( w2, node[1].body->posr.R, w );
+        }
+        else {
+            dUASSERT(false, "Hinge2 axes must be chosen to be linearly independent");
+        }
+    }
+}
+
+
+/*ODE_API */
+void dJointSetHinge2Anchor( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
+    
+    joint->makeV1andV2();
+    joint->makeW1andW2();
+}
+
+
+/*ODE_API */
+void dJointSetHinge2Axes (dJointID j, const dReal *axis1/*=[dSA__MAX],=NULL*/, const dReal *axis2/*=[dSA__MAX],=NULL*/)
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    dAASSERT(axis1 != NULL || axis2 != NULL);
+    dAASSERT(joint->node[0].body != NULL || axis1 == NULL);
+    dAASSERT(joint->node[1].body != NULL || axis2 == NULL);
+
+    if ( axis1 != NULL )
+    {
+        setAxes(joint, axis1[dSA_X], axis1[dSA_Y], axis1[dSA_Z], joint->axis1, NULL);
+    }
+    
+    if ( axis2 != NULL )
+    {
+        setAxes(joint, axis2[dSA_X], axis2[dSA_Y], axis2[dSA_Z], NULL, joint->axis2);
+    }
+
+    // compute the sin and cos of the angle between axis 1 and axis 2
+    dVector3 ax1, ax2, ax;
+    joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 );
+
+    joint->makeV1andV2();
+    joint->makeW1andW2();
+}
+
+
+/*ODE_API_DEPRECATED ODE_API */
+void dJointSetHinge2Axis1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dVector3 axis1;
+    axis1[dSA_X] = x; axis1[dSA_Y] = y; axis1[dSA_Z] = z;
+    dJointSetHinge2Axes(j, axis1, NULL);
+}
+
+/*ODE_API_DEPRECATED ODE_API */
+void dJointSetHinge2Axis2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dVector3 axis2;
+    axis2[dSA_X] = x; axis2[dSA_Y] = y; axis2[dSA_Z] = z;
+    dJointSetHinge2Axes(j, NULL, axis2);
+}
+
+
+void dJointSetHinge2Param( dJointID j, int parameter, dReal value )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        joint->limot2.set( parameter & 0xff, value );
+    }
+    else
+    {
+        if ( parameter == dParamSuspensionERP ) joint->susp_erp = value;
+        else if ( parameter == dParamSuspensionCFM ) joint->susp_cfm = value;
+        else joint->limot1.set( parameter, value );
+    }
+}
+
+
+void dJointGetHinge2Anchor( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor2( joint, result, joint->anchor2 );
+    else
+        getAnchor( joint, result, joint->anchor1 );
+}
+
+
+void dJointGetHinge2Anchor2( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->flags & dJOINT_REVERSE )
+        getAnchor( joint, result, joint->anchor1 );
+    else
+        getAnchor2( joint, result, joint->anchor2 );
+}
+
+
+void dJointGetHinge2Axis1( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body )
+    {
+        dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis1 );
+    }
+    else
+    {
+        dZeroVector3(result);
+        dUASSERT( false, "the joint does not have first body attached" );
+    }
+}
+
+
+void dJointGetHinge2Axis2( dJointID j, dVector3 result )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[1].body )
+    {
+        dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
+    }
+    else
+    {
+        dZeroVector3(result);
+        dUASSERT( false, "the joint does not have second body attached" );
+    }
+}
+
+
+dReal dJointGetHinge2Param( dJointID j, int parameter )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if (( parameter & 0xff00 ) == 0x100 )
+    {
+        return joint->limot2.get( parameter & 0xff );
+    }
+    else
+    {
+        if ( parameter == dParamSuspensionERP ) return joint->susp_erp;
+        else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm;
+        else return joint->limot1.get( parameter );
+    }
+}
+
+
+dReal dJointGetHinge2Angle1( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    return joint->measureAngle1();
+}
+
+
+dReal dJointGetHinge2Angle2( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    return joint->measureAngle2();
+}
+
+
+
+dReal dJointGetHinge2Angle1Rate( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body )
+            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    else return 0;
+}
+
+
+dReal dJointGetHinge2Angle2Rate( dJointID j )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+    if ( joint->node[0].body && joint->node[1].body )
+    {
+        dVector3 axis;
+        dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
+        dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
+        if ( joint->node[1].body )
+            rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
+        return rate;
+    }
+    else return 0;
+}
+
+
+void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
+{
+    dxJointHinge2* joint = ( dxJointHinge2* )j;
+    dVector3 axis1, axis2;
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Hinge2 );
+
+    if ( joint->node[0].body && joint->node[1].body )
+    {
+        dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
+        dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
+        axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
+        axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
+        axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
+        dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
+        dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
+    }
+}
+
+
+dJointType
+dxJointHinge2::type() const
+{
+    return dJointTypeHinge2;
+}
+
+
+sizeint
+dxJointHinge2::size() const
+{
+    return sizeof( *this );
+}
+
+
+void
+dxJointHinge2::setRelativeValues()
+{
+    dVector3 anchor;
+    dJointGetHinge2Anchor(this, anchor);
+    setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
+
+    dVector3 axis;
+
+    if ( node[0].body )
+    {
+        dJointGetHinge2Axis1(this, axis);
+        setAxes( this, axis[0],axis[1],axis[2], axis1, NULL );
+    }
+
+    if ( node[0].body )
+    {
+        dJointGetHinge2Axis2(this, axis);
+        setAxes( this, axis[0],axis[1],axis[2], NULL, axis2 );
+    }
+
+    dVector3 ax1, ax2;
+    getAxisInfo( ax1, ax2, axis, s0, c0 );
+
+    makeV1andV2();
+    makeW1andW2();
+}