add ode

1 files changed, 195 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/plane2d.cpp b/libs/ode-0.16.1/ode/src/joints/plane2d.cpp
new file mode 100644
index 0000000..0caecb3
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/plane2d.cpp
@@ -0,0 +1,195 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "plane2d.h"
+#include "joint_internal.h"
+
+
+
+//****************************************************************************
+// Plane2D
+/*
+This code is part of the Plane2D ODE joint
+by psero@gmx.de
+Wed Apr 23 18:53:43 CEST 2003
+*/
+
+
+static const dReal   Midentity[3][3] =
+{
+    {   1,  0,  0   },
+    {   0,  1,  0   },
+    {   0,  0,  1,  }
+};
+
+
+dxJointPlane2D::dxJointPlane2D( dxWorld *w ) :
+    dxJoint( w )
+{
+    motor_x.init( world );
+    motor_y.init( world );
+    motor_angle.init( world );
+}
+
+
+void 
+dxJointPlane2D::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 6;
+}
+
+
+void
+dxJointPlane2D::getInfo1( dxJoint::Info1 *info )
+{
+    info->nub = 3;
+    info->m = 3;
+
+    if ( motor_x.fmax > 0 )
+        row_motor_x = info->m++;
+    else
+        row_motor_x = 0;
+
+    if ( motor_y.fmax > 0 )
+        row_motor_y = info->m++;
+    else
+        row_motor_y = 0;
+
+    if ( motor_angle.fmax > 0 )
+        row_motor_angle = info->m++;
+    else
+        row_motor_angle = 0;
+}
+
+
+
+void
+dxJointPlane2D::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dReal eps = worldFPS * worldERP;
+
+    /*
+        v = v1, w = omega1
+        (v2, omega2 not important (== static environment))
+
+        constraint equations:
+            vz = 0
+            wx = 0
+            wy = 0
+
+        <=> ( 0 0 1 ) (vx)   ( 0 0 0 ) (wx)   ( 0 )
+            ( 0 0 0 ) (vy) + ( 1 0 0 ) (wy) = ( 0 )
+            ( 0 0 0 ) (vz)   ( 0 1 0 ) (wz)   ( 0 )
+            J1/J1l           Omega1/J1a
+    */
+
+    // fill in linear and angular coeff. for left hand side:
+
+    J1[GI2_JLZ] = 1;
+    J1[rowskip + GI2_JAX] = 1;
+    J1[2 * rowskip + GI2_JAY] = 1;
+
+    // error correction (against drift):
+
+    // a) linear vz, so that z (== pos[2]) == 0
+    pairRhsCfm[GI2_RHS] = eps * -node[0].body->posr.pos[2];
+
+# if 0
+    // b) angular correction? -> left to application !!!
+    dReal       *body_z_axis = &node[0].body->R[8];
+    pairRhsCfm[pairskip + GI2_RHS] = eps * + atan2( body_z_axis[1], body_z_axis[2] );  // wx error
+    pairRhsCfm[2 * pairskip + GI2_RHS] = eps * -atan2( body_z_axis[0], body_z_axis[2] );  // wy error
+# endif
+
+    // if the slider is powered, or has joint limits, add in the extra row:
+
+    if ( row_motor_x > 0 )
+    {
+        int currRowSkip = row_motor_x * rowskip, currPairSkip = row_motor_x * pairskip;
+        motor_x.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[0], 0 );
+    }
+
+    if ( row_motor_y > 0 )
+    {
+        int currRowSkip = row_motor_y * rowskip, currPairSkip = row_motor_y * pairskip;
+        motor_y.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[1], 0 );
+    }
+
+    if ( row_motor_angle > 0 )
+    {
+        int currRowSkip = row_motor_angle * rowskip, currPairSkip = row_motor_angle * pairskip;
+        motor_angle.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, Midentity[2], 1 );
+    }
+}
+
+
+dJointType
+dxJointPlane2D::type() const
+{
+    return dJointTypePlane2D;
+}
+
+
+sizeint
+dxJointPlane2D::size() const
+{
+    return sizeof( *this );
+}
+
+
+
+void dJointSetPlane2DXParam( dxJoint *joint,
+                            int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_x.set( parameter, value );
+}
+
+
+void dJointSetPlane2DYParam( dxJoint *joint,
+                            int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_y.set( parameter, value );
+}
+
+
+
+void dJointSetPlane2DAngleParam( dxJoint *joint,
+                                int parameter, dReal value )
+{
+    dUASSERT( joint, "bad joint argument" );
+    checktype( joint, Plane2D );
+    dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
+    joint2d->motor_angle.set( parameter, value );
+}
+