diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/joints/lmotor.cpp')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/joints/lmotor.cpp | 214 |
1 files changed, 214 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/joints/lmotor.cpp b/libs/ode-0.16.1/ode/src/joints/lmotor.cpp new file mode 100644 index 0000000..8270188 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/lmotor.cpp @@ -0,0 +1,214 @@ +/*************************************************************************
+ * *
+ * 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 "lmotor.h"
+#include "joint_internal.h"
+
+
+//****************************************************************************
+// lmotor joint
+dxJointLMotor::dxJointLMotor( dxWorld *w ) :
+ dxJoint( w )
+{
+ int i;
+ num = 0;
+ for ( i = 0;i < 3;i++ )
+ {
+ dSetZero( axis[i], 4 );
+ limot[i].init( world );
+ }
+}
+
+void
+dxJointLMotor::computeGlobalAxes( dVector3 ax[3] )
+{
+ for ( int i = 0; i < num; i++ )
+ {
+ if ( rel[i] == 1 )
+ {
+ dMultiply0_331( ax[i], node[0].body->posr.R, axis[i] );
+ }
+ else if ( rel[i] == 2 )
+ {
+ if ( node[1].body ) // jds: don't assert, just ignore
+ {
+ dMultiply0_331( ax[i], node[1].body->posr.R, axis[i] );
+ }
+ }
+ else
+ {
+ ax[i][0] = axis[i][0];
+ ax[i][1] = axis[i][1];
+ ax[i][2] = axis[i][2];
+ }
+ }
+}
+
+void
+dxJointLMotor::getSureMaxInfo( SureMaxInfo* info )
+{
+ info->max_m = num;
+}
+
+void
+dxJointLMotor::getInfo1( dxJoint::Info1 *info )
+{
+ info->m = 0;
+ info->nub = 0;
+ for ( int i = 0; i < num; i++ )
+ {
+ if ( limot[i].fmax > 0 )
+ {
+ info->m++;
+ }
+ }
+}
+
+void
+dxJointLMotor::getInfo2( dReal worldFPS, dReal /*worldERP*/,
+ int rowskip, dReal *J1, dReal *J2,
+ int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
+ int *findex )
+{
+ dVector3 ax[3];
+ computeGlobalAxes( ax );
+
+ int currRowSkip = 0, currPairSkip = 0;
+ for ( int i = 0; i < num; ++i ) {
+ if (limot[i].addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax[i], 0 )) {
+ currRowSkip += rowskip; currPairSkip += pairskip;
+ }
+ }
+}
+
+void dJointSetLMotorAxis( dJointID j, int anum, int rel, dReal x, dReal y, dReal z )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ //for now we are ignoring rel!
+ dAASSERT( joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2 );
+ checktype( joint, LMotor );
+
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+
+ if ( !joint->node[1].body && rel == 2 ) rel = 1; //ref 1
+
+ joint->rel[anum] = rel;
+
+ dVector3 r;
+ r[0] = x;
+ r[1] = y;
+ r[2] = z;
+ r[3] = 0;
+ if ( rel > 0 )
+ {
+ if ( rel == 1 )
+ {
+ dMultiply1_331( joint->axis[anum], joint->node[0].body->posr.R, r );
+ }
+ else
+ {
+ //second body has to exists thanks to ref 1 line
+ dMultiply1_331( joint->axis[anum], joint->node[1].body->posr.R, r );
+ }
+ }
+ else
+ {
+ joint->axis[anum][0] = r[0];
+ joint->axis[anum][1] = r[1];
+ joint->axis[anum][2] = r[2];
+ }
+
+ dNormalize3( joint->axis[anum] );
+}
+
+void dJointSetLMotorNumAxes( dJointID j, int num )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint && num >= 0 && num <= 3 );
+ checktype( joint, LMotor );
+ if ( num < 0 ) num = 0;
+ if ( num > 3 ) num = 3;
+ joint->num = num;
+}
+
+void dJointSetLMotorParam( dJointID j, int parameter, dReal value )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ int anum = parameter >> 8;
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ parameter &= 0xff;
+ joint->limot[anum].set( parameter, value );
+}
+
+int dJointGetLMotorNumAxes( dJointID j )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ return joint->num;
+}
+
+
+void dJointGetLMotorAxis( dJointID j, int anum, dVector3 result )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint && anum >= 0 && anum < 3 );
+ checktype( joint, LMotor );
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ result[0] = joint->axis[anum][0];
+ result[1] = joint->axis[anum][1];
+ result[2] = joint->axis[anum][2];
+}
+
+dReal dJointGetLMotorParam( dJointID j, int parameter )
+{
+ dxJointLMotor* joint = ( dxJointLMotor* )j;
+ dAASSERT( joint );
+ checktype( joint, LMotor );
+ int anum = parameter >> 8;
+ if ( anum < 0 ) anum = 0;
+ if ( anum > 2 ) anum = 2;
+ parameter &= 0xff;
+ return joint->limot[anum].get( parameter );
+}
+
+dJointType
+dxJointLMotor::type() const
+{
+ return dJointTypeLMotor;
+}
+
+
+sizeint
+dxJointLMotor::size() const
+{
+ return sizeof( *this );
+}
+
|