add ode

1 files changed, 423 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/slider.cpp b/libs/ode-0.16.1/ode/src/joints/slider.cpp
new file mode 100644
index 0000000..2c9b008
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/slider.cpp
@@ -0,0 +1,423 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "slider.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// slider
+
+dxJointSlider::dxJointSlider ( dxWorld *w ) :
+    dxJoint ( w )
+{
+    dSetZero ( axis1, 4 );
+    axis1[0] = 1;
+    dSetZero ( qrel, 4 );
+    dSetZero ( offset, 4 );
+    limot.init ( world );
+}
+
+
+dReal dJointGetSliderPosition ( dJointID j )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+
+    // get axis1 in global coordinates
+    dVector3 ax1, q;
+    dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
+
+    if ( joint->node[1].body )
+    {
+        // get body2 + offset point in global coordinates
+        dMultiply0_331 ( q, joint->node[1].body->posr.R, joint->offset );
+        for ( int i = 0; i < 3; i++ )
+            q[i] = joint->node[0].body->posr.pos[i]
+                - q[i]
+                - joint->node[1].body->posr.pos[i];
+    }
+    else
+    {
+        q[0] = joint->node[0].body->posr.pos[0] - joint->offset[0];
+        q[1] = joint->node[0].body->posr.pos[1] - joint->offset[1];
+        q[2] = joint->node[0].body->posr.pos[2] - joint->offset[2];
+
+        if ( joint->flags & dJOINT_REVERSE )
+        {
+            // N.B. it could have been simplier to only inverse the sign of
+            //      the dCalcVectorDot3 result but this case is exceptional and doing
+            //      the check for all case can decrease the performance.
+            ax1[0] = -ax1[0];
+            ax1[1] = -ax1[1];
+            ax1[2] = -ax1[2];
+        }
+    }
+
+    return dCalcVectorDot3 ( ax1, q );
+}
+
+
+dReal dJointGetSliderPositionRate ( dJointID j )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+
+    // get axis1 in global coordinates
+    dVector3 ax1;
+    dMultiply0_331 ( ax1, joint->node[0].body->posr.R, joint->axis1 );
+
+    if ( joint->node[1].body )
+    {
+        return dCalcVectorDot3 ( ax1, joint->node[0].body->lvel ) -
+            dCalcVectorDot3 ( ax1, joint->node[1].body->lvel );
+    }
+    else
+    {
+        dReal rate = dCalcVectorDot3 ( ax1, joint->node[0].body->lvel );
+        if ( joint->flags & dJOINT_REVERSE ) rate = - rate;
+        return rate;
+    }
+}
+
+
+void 
+dxJointSlider::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointSlider::getInfo1 ( dxJoint::Info1 *info )
+{
+    info->nub = 5;
+
+    // see if joint is powered
+    if ( limot.fmax > 0 )
+        info->m = 6; // powered slider needs an extra constraint row
+    else info->m = 5;
+
+    // see if we're at a joint limit.
+    limot.limit = 0;
+    if ( ( limot.lostop > -dInfinity || limot.histop < dInfinity ) &&
+        limot.lostop <= limot.histop )
+    {
+        // measure joint position
+        dReal pos = dJointGetSliderPosition ( this );
+        if ( pos <= limot.lostop )
+        {
+            limot.limit = 1;
+            limot.limit_err = pos - limot.lostop;
+            info->m = 6;
+        }
+        else if ( pos >= limot.histop )
+        {
+            limot.limit = 2;
+            limot.limit_err = pos - limot.histop;
+            info->m = 6;
+        }
+    }
+}
+
+
+void
+dxJointSlider::getInfo2 ( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    // 3 rows to make body rotations equal
+    setFixedOrientation ( this, worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, qrel );
+
+    // pull out pos and R for both bodies. also get the `connection'
+    // vector pos2-pos1.
+    dVector3 c;
+    dReal *pos2 = NULL, *R2 = NULL;
+
+    dReal *pos1 = node[0].body->posr.pos;
+    dReal *R1 = node[0].body->posr.R;
+
+    dVector3 ax1; // joint axis in global coordinates (unit length)
+    dVector3 p, q; // plane space of ax1
+    dMultiply0_331 ( ax1, R1, axis1 );
+    dPlaneSpace ( ax1, p, q );
+
+    dxBody *body1 = node[1].body;
+    
+    if ( body1 )
+    {
+        R2 = body1->posr.R;
+        pos2 = body1->posr.pos;
+        dSubtractVectors3( c, pos2, pos1 );
+    }
+
+    // remaining two rows. we want: vel2 = vel1 + w1 x c ... but this would
+    // result in three equations, so we project along the planespace vectors
+    // so that sliding along the slider axis is disregarded. for symmetry we
+    // also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
+    int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip;
+    {
+        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, p );
+
+        if ( body1 )
+        {
+            dVector3 tmp;
+
+            dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, p);
+
+            dCalcVectorCross3( tmp, c, p );
+            dCopyScaledVector3( J1 + currRowSkip + GI2__JA_MIN, tmp, REAL(0.5) );
+            dCopyVector3( J2 + currRowSkip + GI2__JA_MIN, J1 + currRowSkip + GI2__JA_MIN );
+        }
+    }
+
+    currRowSkip += rowskip;
+    {
+        dCopyVector3( J1 + currRowSkip + GI2__JL_MIN, q );
+
+        if ( body1 )
+        {
+            dVector3 tmp;
+
+            dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, q);
+
+            dCalcVectorCross3( tmp, c, q );
+            dCopyScaledVector3( J1 + currRowSkip + GI2__JA_MIN, tmp, REAL(0.5) );
+            dCopyVector3( J2 + currRowSkip + GI2__JA_MIN, J1 + currRowSkip + GI2__JA_MIN );
+        }
+    }
+
+    // compute last two elements of right hand side. we want to align the offset
+    // point (in body 2's frame) with the center of body 1.
+    dReal k = worldFPS * worldERP;
+
+    if ( body1 )
+    {
+        dVector3 ofs;  // offset point in global coordinates
+        dMultiply0_331 ( ofs, R2, offset );
+        dAddVectors3(c, c, ofs);
+        
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, c );
+
+        currPairSkip += pairskip;
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, c );
+    }
+    else
+    {
+        dVector3 ofs;  // offset point in global coordinates
+        dSubtractVectors3(ofs, offset, pos1);
+        
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( p, ofs );
+        
+        currPairSkip += pairskip;
+        pairRhsCfm[currPairSkip + GI2_RHS] = k * dCalcVectorDot3 ( q, ofs );
+
+        if ( (flags & dJOINT_REVERSE) != 0 )
+        {
+            dNegateVector3(ax1);
+        }
+    }
+
+    // if the slider is powered, or has joint limits, add in the extra row
+    currRowSkip += rowskip; currPairSkip += pairskip;
+    limot.addLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 0 );
+}
+
+
+void dJointSetSliderAxis ( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+    setAxes ( joint, x, y, z, joint->axis1, 0 );
+
+    joint->computeOffset();
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+void dJointSetSliderAxisDelta ( dJointID j, dReal x, dReal y, dReal z, dReal dx, dReal dy, dReal dz )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+    setAxes ( joint, x, y, z, joint->axis1, 0 );
+
+    joint->computeOffset();
+
+    // compute initial relative rotation body1 -> body2, or env -> body1
+    // also compute center of body1 w.r.t body 2
+    if ( !(joint->node[1].body) )
+    {
+        joint->offset[0] += dx;
+        joint->offset[1] += dy;
+        joint->offset[2] += dz;
+    }
+
+    joint->computeInitialRelativeRotation();
+}
+
+
+
+void dJointGetSliderAxis ( dJointID j, dVector3 result )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    dUASSERT ( result, "bad result argument" );
+    checktype ( joint, Slider );
+    getAxis ( joint, result, joint->axis1 );
+}
+
+
+void dJointSetSliderParam ( dJointID j, int parameter, dReal value )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+    joint->limot.set ( parameter, value );
+}
+
+
+dReal dJointGetSliderParam ( dJointID j, int parameter )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+    return joint->limot.get ( parameter );
+}
+
+
+void dJointAddSliderForce ( dJointID j, dReal force )
+{
+    dxJointSlider* joint = ( dxJointSlider* ) j;
+    dVector3 axis;
+    dUASSERT ( joint, "bad joint argument" );
+    checktype ( joint, Slider );
+
+    if ( joint->flags & dJOINT_REVERSE )
+        force = -force;
+
+    getAxis ( joint, axis, joint->axis1 );
+    axis[0] *= force;
+    axis[1] *= force;
+    axis[2] *= force;
+
+    if ( joint->node[0].body != 0 )
+        dBodyAddForce ( joint->node[0].body, axis[0], axis[1], axis[2] );
+    if ( joint->node[1].body != 0 )
+        dBodyAddForce ( joint->node[1].body, -axis[0], -axis[1], -axis[2] );
+
+    if ( joint->node[0].body != 0 && joint->node[1].body != 0 )
+    {
+        // linear torque decoupling:
+        // we have to compensate the torque, that this slider force may generate
+        // if body centers are not aligned along the slider axis
+
+        dVector3 ltd; // Linear Torque Decoupling vector (a torque)
+
+        dVector3 c;
+        c[0] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[0] - joint->node[0].body->posr.pos[0] );
+        c[1] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[1] - joint->node[0].body->posr.pos[1] );
+        c[2] = REAL ( 0.5 ) * ( joint->node[1].body->posr.pos[2] - joint->node[0].body->posr.pos[2] );
+        dCalcVectorCross3( ltd, c, axis );
+
+        dBodyAddTorque ( joint->node[0].body, ltd[0], ltd[1], ltd[2] );
+        dBodyAddTorque ( joint->node[1].body, ltd[0], ltd[1], ltd[2] );
+    }
+}
+
+
+dJointType
+dxJointSlider::type() const
+{
+    return dJointTypeSlider;
+}
+
+
+sizeint
+dxJointSlider::size() const
+{
+    return sizeof ( *this );
+}
+
+
+void
+dxJointSlider::setRelativeValues()
+{
+    computeOffset();
+    computeInitialRelativeRotation();
+}
+
+
+
+/// Compute initial relative rotation body1 -> body2, or env -> body1
+void
+dxJointSlider::computeInitialRelativeRotation()
+{
+    if ( node[0].body )
+    {
+        // compute initial relative rotation body1 -> body2, or env -> body1
+        // also compute center of body1 w.r.t body 2
+        if ( node[1].body )
+        {
+            dQMultiply1 ( qrel, node[0].body->q, node[1].body->q );
+        }
+        else
+        {
+            // set qrel to the transpose of the first body's q
+            qrel[0] =  node[0].body->q[0];
+            qrel[1] = -node[0].body->q[1];
+            qrel[2] = -node[0].body->q[2];
+            qrel[3] = -node[0].body->q[3];
+        }
+    }
+}
+
+
+/// Compute center of body1 w.r.t body 2
+void
+dxJointSlider::computeOffset()
+{
+    if ( node[1].body )
+    {
+        dVector3 c;
+        c[0] = node[0].body->posr.pos[0] - node[1].body->posr.pos[0];
+        c[1] = node[0].body->posr.pos[1] - node[1].body->posr.pos[1];
+        c[2] = node[0].body->posr.pos[2] - node[1].body->posr.pos[2];
+
+        dMultiply1_331 ( offset, node[1].body->posr.R, c );
+    }
+    else if ( node[0].body )
+    {
+        offset[0] = node[0].body->posr.pos[0];
+        offset[1] = node[0].body->posr.pos[1];
+        offset[2] = node[0].body->posr.pos[2];
+    }
+}