ode/src/joints/hinge2.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001,2002 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23
  24 #include "config.h"
  25 #include "hinge2.h"
  26 #include "joint_internal.h"
  27
  28
  29
  30
  31 //****************************************************************************
  32 // hinge 2. note that this joint must be attached to two bodies for it to work
  33
  34 dReal
  35 dxJointHinge2::measureAngle() const
  36 {
  37     dVector3 a1, a2;
  38     dMultiply0_331( a1, node[1].body->posr.R, axis2 );
  39     dMultiply1_331( a2, node[0].body->posr.R, a1 );
  40     dReal x = dCalcVectorDot3( v1, a2 );
  41     dReal y = dCalcVectorDot3( v2, a2 );
  42     return -dAtan2( y, x );
  43 }
  44
  45
  46 dxJointHinge2::dxJointHinge2( dxWorld *w ) :
  47         dxJoint( w )
  48 {
  49     dSetZero( anchor1, 4 );
  50     dSetZero( anchor2, 4 );
  51     dSetZero( axis1, 4 );
  52     axis1[0] = 1;
  53     dSetZero( axis2, 4 );
  54     axis2[1] = 1;
  55     c0 = 0;
  56     s0 = 0;
  57
  58     dSetZero( v1, 4 );
  59     v1[0] = 1;
  60     dSetZero( v2, 4 );
  61     v2[1] = 1;
  62
  63     limot1.init( world );
  64     limot2.init( world );
  65
  66     susp_erp = world->global_erp;
  67     susp_cfm = world->global_cfm;
  68
  69     flags |= dJOINT_TWOBODIES;
  70 }
  71
  72
  73 void
  74 dxJointHinge2::getSureMaxInfo( SureMaxInfo* info )
  75 {
  76     info->max_m = 6;
  77 }
  78
  79
  80 void
  81 dxJointHinge2::getInfo1( dxJoint::Info1 *info )
  82 {
  83     info->m = 4;
  84     info->nub = 4;
  85
  86     // see if we're powered or at a joint limit for axis 1
  87     limot1.limit = 0;
  88     if (( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) &&
  89             limot1.lostop <= limot1.histop )
  90     {
  91         dReal angle = measureAngle();
  92         limot1.testRotationalLimit( angle );
  93     }
  94     if ( limot1.limit || limot1.fmax > 0 ) info->m++;
  95
  96     // see if we're powering axis 2 (we currently never limit this axis)
  97     limot2.limit = 0;
  98     if ( limot2.fmax > 0 ) info->m++;
  99 }
 100
 101
 102 ////////////////////////////////////////////////////////////////////////////////
 103 /// Function that computes ax1,ax2 = axis 1 and 2 in global coordinates (they are
 104 /// relative to body 1 and 2 initially) and then computes the constrained
 105 /// rotational axis as the cross product of ax1 and ax2.
 106 /// the sin and cos of the angle between axis 1 and 2 is computed, this comes
 107 /// from dot and cross product rules.
 108 ///
 109 /// @param ax1 Will contain the joint axis1 in world frame
 110 /// @param ax2 Will contain the joint axis2 in world frame
 111 /// @param axis Will contain the cross product of ax1 x ax2
 112 /// @param sin_angle
 113 /// @param cos_angle
 114 ////////////////////////////////////////////////////////////////////////////////
 115 void
 116 dxJointHinge2::getAxisInfo(dVector3 ax1, dVector3 ax2, dVector3 axCross,
 117                            dReal &sin_angle, dReal &cos_angle) const
 118 {
 119     dMultiply0_331 (ax1, node[0].body->posr.R, axis1);
 120     dMultiply0_331 (ax2, node[1].body->posr.R, axis2);
 121     dCalcVectorCross3(axCross,ax1,ax2);
 122     sin_angle = dSqrt (axCross[0]*axCross[0] + axCross[1]*axCross[1] + axCross[2]*axCross[2]);
 123     cos_angle = dCalcVectorDot3 (ax1,ax2);
 124 }
 125
 126
 127 void
 128 dxJointHinge2::getInfo2( dxJoint::Info2 *info )
 129 {
 130     // get information we need to set the hinge row
 131     dReal s, c;
 132     dVector3 q;
 133     const dxJointHinge2 *joint = this;
 134
 135     dVector3 ax1, ax2;
 136     joint->getAxisInfo( ax1, ax2, q, s, c );
 137     dNormalize3( q );   // @@@ quicker: divide q by s ?
 138
 139     // set the three ball-and-socket rows (aligned to the suspension axis ax1)
 140     setBall2( this, info, anchor1, anchor2, ax1, susp_erp );
 141
 142     // set the hinge row
 143     int s3 = 3 * info->rowskip;
 144     info->J1a[s3+0] = q[0];
 145     info->J1a[s3+1] = q[1];
 146     info->J1a[s3+2] = q[2];
 147     if ( joint->node[1].body )
 148     {
 149         info->J2a[s3+0] = -q[0];
 150         info->J2a[s3+1] = -q[1];
 151         info->J2a[s3+2] = -q[2];
 152     }
 153
 154     // compute the right hand side for the constrained rotational DOF.
 155     // axis 1 and axis 2 are separated by an angle `theta'. the desired
 156     // separation angle is theta0. sin(theta0) and cos(theta0) are recorded
 157     // in the joint structure. the correcting angular velocity is:
 158     //   |angular_velocity| = angle/time = erp*(theta0-theta) / stepsize
 159     //                      = (erp*fps) * (theta0-theta)
 160     // (theta0-theta) can be computed using the following small-angle-difference
 161     // approximation:
 162     //   theta0-theta ~= tan(theta0-theta)
 163     //                 = sin(theta0-theta)/cos(theta0-theta)
 164     //                 = (c*s0 - s*c0) / (c*c0 + s*s0)
 165     //                 = c*s0 - s*c0         assuming c*c0 + s*s0 ~= 1
 166     // where c = cos(theta), s = sin(theta)
 167     //       c0 = cos(theta0), s0 = sin(theta0)
 168
 169     dReal k = info->fps * info->erp;
 170     info->c[3] = k * ( c0 * s - joint->s0 * c );
 171
 172     // if the axis1 hinge is powered, or has joint limits, add in more stuff
 173     int row = 4 + limot1.addLimot( this, info, 4, ax1, 1 );
 174
 175     // if the axis2 hinge is powered, add in more stuff
 176     limot2.addLimot( this, info, row, ax2, 1 );
 177
 178     // set parameter for the suspension
 179     info->cfm[0] = susp_cfm;
 180 }
 181
 182
 183 // compute vectors v1 and v2 (embedded in body1), used to measure angle
 184 // between body 1 and body 2
 185
 186 void
 187 dxJointHinge2::makeV1andV2()
 188 {
 189     if ( node[0].body )
 190     {
 191         // get axis 1 and 2 in global coords
 192         dVector3 ax1, ax2, v;
 193         dMultiply0_331( ax1, node[0].body->posr.R, axis1 );
 194         dMultiply0_331( ax2, node[1].body->posr.R, axis2 );
 195
 196         // don't do anything if the axis1 or axis2 vectors are zero or the same
 197         if (( ax1[0] == 0 && ax1[1] == 0 && ax1[2] == 0 ) ||
 198                 ( ax2[0] == 0 && ax2[1] == 0 && ax2[2] == 0 ) ||
 199                 ( ax1[0] == ax2[0] && ax1[1] == ax2[1] && ax1[2] == ax2[2] ) ) return;
 200
 201         // modify axis 2 so it's perpendicular to axis 1
 202         dReal k = dCalcVectorDot3( ax1, ax2 );
 203         for ( int i = 0; i < 3; i++ ) ax2[i] -= k * ax1[i];
 204         dNormalize3( ax2 );
 205
 206         // make v1 = modified axis2, v2 = axis1 x (modified axis2)
 207         dCalcVectorCross3( v, ax1, ax2 );
 208         dMultiply1_331( v1, node[0].body->posr.R, ax2 );
 209         dMultiply1_331( v2, node[0].body->posr.R, v );
 210     }
 211 }
 212
 213
 214 void dJointSetHinge2Anchor( dJointID j, dReal x, dReal y, dReal z )
 215 {
 216     dxJointHinge2* joint = ( dxJointHinge2* )j;
 217     dUASSERT( joint, "bad joint argument" );
 218     checktype( joint, Hinge2 );
 219     setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
 220     joint->makeV1andV2();
 221 }
 222
 223
 224 void dJointSetHinge2Axis1( dJointID j, dReal x, dReal y, dReal z )
 225 {
 226     dxJointHinge2* joint = ( dxJointHinge2* )j;
 227     dUASSERT( joint, "bad joint argument" );
 228     checktype( joint, Hinge2 );
 229     if ( joint->node[0].body )
 230     {
 231         setAxes(joint, x, y, z, joint->axis1, NULL);
 232
 233         // compute the sin and cos of the angle between axis 1 and axis 2
 234         dVector3 ax1, ax2, ax;
 235         joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 );
 236     }
 237     joint->makeV1andV2();
 238 }
 239
 240
 241 void dJointSetHinge2Axis2( dJointID j, dReal x, dReal y, dReal z )
 242 {
 243     dxJointHinge2* joint = ( dxJointHinge2* )j;
 244     dUASSERT( joint, "bad joint argument" );
 245     checktype( joint, Hinge2 );
 246     if ( joint->node[1].body )
 247     {
 248         setAxes(joint, x, y, z, NULL, joint->axis2);
 249
 250
 251         // compute the sin and cos of the angle between axis 1 and axis 2
 252         dVector3 ax1, ax2, ax;;
 253         joint->getAxisInfo( ax1, ax2, ax, joint->s0, joint->c0 );
 254     }
 255     joint->makeV1andV2();
 256 }
 257
 258
 259 void dJointSetHinge2Param( dJointID j, int parameter, dReal value )
 260 {
 261     dxJointHinge2* joint = ( dxJointHinge2* )j;
 262     dUASSERT( joint, "bad joint argument" );
 263     checktype( joint, Hinge2 );
 264     if (( parameter & 0xff00 ) == 0x100 )
 265     {
 266         joint->limot2.set( parameter & 0xff, value );
 267     }
 268     else
 269     {
 270         if ( parameter == dParamSuspensionERP ) joint->susp_erp = value;
 271         else if ( parameter == dParamSuspensionCFM ) joint->susp_cfm = value;
 272         else joint->limot1.set( parameter, value );
 273     }
 274 }
 275
 276
 277 void dJointGetHinge2Anchor( dJointID j, dVector3 result )
 278 {
 279     dxJointHinge2* joint = ( dxJointHinge2* )j;
 280     dUASSERT( joint, "bad joint argument" );
 281     dUASSERT( result, "bad result argument" );
 282     checktype( joint, Hinge2 );
 283     if ( joint->flags & dJOINT_REVERSE )
 284         getAnchor2( joint, result, joint->anchor2 );
 285     else
 286         getAnchor( joint, result, joint->anchor1 );
 287 }
 288
 289
 290 void dJointGetHinge2Anchor2( dJointID j, dVector3 result )
 291 {
 292     dxJointHinge2* joint = ( dxJointHinge2* )j;
 293     dUASSERT( joint, "bad joint argument" );
 294     dUASSERT( result, "bad result argument" );
 295     checktype( joint, Hinge2 );
 296     if ( joint->flags & dJOINT_REVERSE )
 297         getAnchor( joint, result, joint->anchor1 );
 298     else
 299         getAnchor2( joint, result, joint->anchor2 );
 300 }
 301
 302
 303 void dJointGetHinge2Axis1( dJointID j, dVector3 result )
 304 {
 305     dxJointHinge2* joint = ( dxJointHinge2* )j;
 306     dUASSERT( joint, "bad joint argument" );
 307     dUASSERT( result, "bad result argument" );
 308     checktype( joint, Hinge2 );
 309     if ( joint->node[0].body )
 310     {
 311         dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis1 );
 312     }
 313 }
 314
 315
 316 void dJointGetHinge2Axis2( dJointID j, dVector3 result )
 317 {
 318     dxJointHinge2* joint = ( dxJointHinge2* )j;
 319     dUASSERT( joint, "bad joint argument" );
 320     dUASSERT( result, "bad result argument" );
 321     checktype( joint, Hinge2 );
 322     if ( joint->node[1].body )
 323     {
 324         dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
 325     }
 326 }
 327
 328
 329 dReal dJointGetHinge2Param( dJointID j, int parameter )
 330 {
 331     dxJointHinge2* joint = ( dxJointHinge2* )j;
 332     dUASSERT( joint, "bad joint argument" );
 333     checktype( joint, Hinge2 );
 334     if (( parameter & 0xff00 ) == 0x100 )
 335     {
 336         return joint->limot2.get( parameter & 0xff );
 337     }
 338     else
 339     {
 340         if ( parameter == dParamSuspensionERP ) return joint->susp_erp;
 341         else if ( parameter == dParamSuspensionCFM ) return joint->susp_cfm;
 342         else return joint->limot1.get( parameter );
 343     }
 344 }
 345
 346
 347 dReal dJointGetHinge2Angle1( dJointID j )
 348 {
 349     dxJointHinge2* joint = ( dxJointHinge2* )j;
 350     dUASSERT( joint, "bad joint argument" );
 351     checktype( joint, Hinge2 );
 352     if ( joint->node[0].body ) return joint->measureAngle();
 353     else return 0;
 354 }
 355
 356
 357 dReal dJointGetHinge2Angle1Rate( dJointID j )
 358 {
 359     dxJointHinge2* joint = ( dxJointHinge2* )j;
 360     dUASSERT( joint, "bad joint argument" );
 361     checktype( joint, Hinge2 );
 362     if ( joint->node[0].body )
 363     {
 364         dVector3 axis;
 365         dMultiply0_331( axis, joint->node[0].body->posr.R, joint->axis1 );
 366         dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
 367         if ( joint->node[1].body )
 368             rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
 369         return rate;
 370     }
 371     else return 0;
 372 }
 373
 374
 375 dReal dJointGetHinge2Angle2Rate( dJointID j )
 376 {
 377     dxJointHinge2* joint = ( dxJointHinge2* )j;
 378     dUASSERT( joint, "bad joint argument" );
 379     checktype( joint, Hinge2 );
 380     if ( joint->node[0].body && joint->node[1].body )
 381     {
 382         dVector3 axis;
 383         dMultiply0_331( axis, joint->node[1].body->posr.R, joint->axis2 );
 384         dReal rate = dCalcVectorDot3( axis, joint->node[0].body->avel );
 385         if ( joint->node[1].body )
 386             rate -= dCalcVectorDot3( axis, joint->node[1].body->avel );
 387         return rate;
 388     }
 389     else return 0;
 390 }
 391
 392
 393 void dJointAddHinge2Torques( dJointID j, dReal torque1, dReal torque2 )
 394 {
 395     dxJointHinge2* joint = ( dxJointHinge2* )j;
 396     dVector3 axis1, axis2;
 397     dUASSERT( joint, "bad joint argument" );
 398     checktype( joint, Hinge2 );
 399
 400     if ( joint->node[0].body && joint->node[1].body )
 401     {
 402         dMultiply0_331( axis1, joint->node[0].body->posr.R, joint->axis1 );
 403         dMultiply0_331( axis2, joint->node[1].body->posr.R, joint->axis2 );
 404         axis1[0] = axis1[0] * torque1 + axis2[0] * torque2;
 405         axis1[1] = axis1[1] * torque1 + axis2[1] * torque2;
 406         axis1[2] = axis1[2] * torque1 + axis2[2] * torque2;
 407         dBodyAddTorque( joint->node[0].body, axis1[0], axis1[1], axis1[2] );
 408         dBodyAddTorque( joint->node[1].body, -axis1[0], -axis1[1], -axis1[2] );
 409     }
 410 }
 411
 412
 413 dJointType
 414 dxJointHinge2::type() const
 415 {
 416     return dJointTypeHinge2;
 417 }
 418
 419
 420 size_t
 421 dxJointHinge2::size() const
 422 {
 423     return sizeof( *this );
 424 }
 425
 426
 427 void
 428 dxJointHinge2::setRelativeValues()
 429 {
 430     dVector3 anchor;
 431     dJointGetHinge2Anchor(this, anchor);
 432     setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 );
 433
 434     dVector3 axis;
 435
 436     if ( node[0].body )
 437     {
 438         dJointGetHinge2Axis1(this, axis);
 439         setAxes( this, axis[0],axis[1],axis[2], axis1, NULL );
 440     }
 441
 442     if ( node[0].body )
 443     {
 444         dJointGetHinge2Axis2(this, axis);
 445         setAxes( this, axis[0],axis[1],axis[2], NULL, axis2 );
 446     }
 447
 448     dVector3 ax1, ax2;
 449     getAxisInfo( ax1, ax2, axis, s0, c0 );
 450
 451     makeV1andV2();
 452 }