ode/src/joints/plane2d.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 <ode/odeconfig.h>
  25 #include "config.h"
  26 #include "plane2d.h"
  27 #include "joint_internal.h"
  28
  29
  30
  31 //****************************************************************************
  32 // Plane2D
  33 /*
  34 This code is part of the Plane2D ODE joint
  35 by psero@gmx.de
  36 Wed Apr 23 18:53:43 CEST 2003
  37 */
  38
  39
  40 static const dReal   Midentity[3][3] =
  41 {
  42     {   1,  0,  0   },
  43     {   0,  1,  0   },
  44     {   0,  0,  1,  }
  45 };
  46
  47
  48 dxJointPlane2D::dxJointPlane2D( dxWorld *w ) :
  49     dxJoint( w )
  50 {
  51     motor_x.init( world );
  52     motor_y.init( world );
  53     motor_angle.init( world );
  54 }
  55
  56
  57 void
  58 dxJointPlane2D::getSureMaxInfo( SureMaxInfo* info )
  59 {
  60     info->max_m = 6;
  61 }
  62
  63
  64 void
  65 dxJointPlane2D::getInfo1( dxJoint::Info1 *info )
  66 {
  67     info->nub = 3;
  68     info->m = 3;
  69
  70     if ( motor_x.fmax > 0 )
  71         row_motor_x = info->m++;
  72     else
  73         row_motor_x = 0;
  74
  75     if ( motor_y.fmax > 0 )
  76         row_motor_y = info->m++;
  77     else
  78         row_motor_y = 0;
  79
  80     if ( motor_angle.fmax > 0 )
  81         row_motor_angle = info->m++;
  82     else
  83         row_motor_angle = 0;
  84 }
  85
  86
  87
  88 void
  89 dxJointPlane2D::getInfo2( dReal worldFPS, dReal worldERP,
  90     int rowskip, dReal *J1, dReal *J2,
  91     int pairskip, dReal *pairRhsCfm, dReal *pairLoHi,
  92     int *findex )
  93 {
  94     dReal eps = worldFPS * worldERP;
  95
  96     /*
  97         v = v1, w = omega1
  98         (v2, omega2 not important (== static environment))
  99
 100         constraint equations:
 101             vz = 0
 102             wx = 0
 103             wy = 0
 104
 105         <=> ( 0 0 1 ) (vx)   ( 0 0 0 ) (wx)   ( 0 )
 106             ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
 107             ( 0 0 0 ) (vz)   ( 0 1 0 ) (wz)   ( 0 )
 108             J1/J1l           Omega1/J1a
 109     */
 110
 111     // fill in linear and angular coeff. for left hand side:
 112
 113     J1[GI2_JLZ] = 1;
 114     J1[rowskip + GI2_JAX] = 1;
 115     J1[2 * rowskip + GI2_JAY] = 1;
 116
 117     // error correction (against drift):
 118
 119     // a) linear vz, so that z (== pos[2]) == 0
 120     pairRhsCfm[GI2_RHS] = eps * -node[0].body->posr.pos[2];
 121
 122 # if 0
 123     // b) angular correction? -> left to application !!!
 124     dReal       *body_z_axis = &node[0].body->R[8];
 125     pairRhsCfm[pairskip + GI2_RHS] = eps * + atan2( body_z_axis[1], body_z_axis[2] );  // wx error
 126     pairRhsCfm[2 * pairskip + GI2_RHS] = eps * -atan2( body_z_axis[0], body_z_axis[2] );  // wy error
 127 # endif
 128
 129     // if the slider is powered, or has joint limits, add in the extra row:
 130
 131     if ( row_motor_x > 0 )
 132     {
 133         int currRowSkip = row_motor_x * rowskip, currPairSkip = row_motor_x * pairskip;
 134         motor_x.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[0], 0 );
 135     }
 136
 137     if ( row_motor_y > 0 )
 138     {
 139         int currRowSkip = row_motor_y * rowskip, currPairSkip = row_motor_y * pairskip;
 140         motor_y.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[1], 0 );
 141     }
 142
 143     if ( row_motor_angle > 0 )
 144     {
 145         int currRowSkip = row_motor_angle * rowskip, currPairSkip = row_motor_angle * pairskip;
 146         motor_angle.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[2], 1 );
 147     }
 148 }
 149
 150
 151 dJointType
 152 dxJointPlane2D::type() const
 153 {
 154     return dJointTypePlane2D;
 155 }
 156
 157
 158 sizeint
 159 dxJointPlane2D::size() const
 160 {
 161     return sizeof( *this );
 162 }
 163
 164
 165
 166 void dJointSetPlane2DXParam( dxJoint *joint,
 167                             int parameter, dReal value )
 168 {
 169     dUASSERT( joint, "bad joint argument" );
 170     checktype( joint, Plane2D );
 171     dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
 172     joint2d->motor_x.set( parameter, value );
 173 }
 174
 175
 176 void dJointSetPlane2DYParam( dxJoint *joint,
 177                             int parameter, dReal value )
 178 {
 179     dUASSERT( joint, "bad joint argument" );
 180     checktype( joint, Plane2D );
 181     dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
 182     joint2d->motor_y.set( parameter, value );
 183 }
 184
 185
 186
 187 void dJointSetPlane2DAngleParam( dxJoint *joint,
 188                                 int parameter, dReal value )
 189 {
 190     dUASSERT( joint, "bad joint argument" );
 191     checktype( joint, Plane2D );
 192     dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
 193     joint2d->motor_angle.set( parameter, value );
 194 }
 195