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