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