summaryrefslogtreecommitdiff
path: root/libs/ode-0.16.1/ode/src/joints/slider.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints/slider.cpp')
-rw-r--r--libs/ode-0.16.1/ode/src/joints/slider.cpp423
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];
+ }
+}
generated by cgit v1.2.1 (git 2.18.0) at 2024-11-01 02:19:16 +0000