add ode

1 files changed, 220 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/dhinge.cpp b/libs/ode-0.16.1/ode/src/joints/dhinge.cpp
new file mode 100644
index 0000000..e300bf5
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/dhinge.cpp
@@ -0,0 +1,220 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "dhinge.h"
+#include "joint_internal.h"
+
+/*
+ * Double Hinge joint
+ */
+
+dxJointDHinge::dxJointDHinge(dxWorld* w) :
+    dxJointDBall(w)
+{
+    dSetZero(axis1, 3);
+    dSetZero(axis2, 3);
+}
+
+
+void
+dxJointDHinge::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 4;
+}
+
+
+void
+dxJointDHinge::getInfo1( dxJoint::Info1* info )
+{
+    info->m = 4;
+    info->nub = 4;
+}
+
+
+void
+dxJointDHinge::getInfo2( dReal worldFPS, dReal worldERP, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dxJointDBall::getInfo2( worldFPS, worldERP, rowskip, J1, J2, pairskip, pairRhsCfm, pairLoHi, findex ); // sets row0
+    
+    dVector3 globalAxis1;
+    dBodyVectorToWorld(node[0].body, axis1[0], axis1[1], axis1[2], globalAxis1);
+
+    dxBody *body1 = node[1].body;
+
+    // angular constraints, perpendicular to axis
+    dVector3 p, q;
+    dPlaneSpace(globalAxis1, p, q);
+
+    dCopyVector3(J1 + rowskip + GI2__JA_MIN, p);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + rowskip + GI2__JA_MIN, p);
+    }
+
+    dCopyVector3(J1 + 2 * rowskip + GI2__JA_MIN, q);
+    if ( body1 ) {
+        dCopyNegatedVector3(J2 + 2 * rowskip + GI2__JA_MIN, q);
+    }
+
+    dVector3 globalAxis2;
+    if ( body1 ) {
+        dBodyVectorToWorld(body1, axis2[0], axis2[1], axis2[2], globalAxis2);
+    } else {
+        dCopyVector3(globalAxis2, axis2);
+    }
+    
+    // similar to the hinge joint
+    dVector3 u;
+    dCalcVectorCross3(u, globalAxis1, globalAxis2);
+
+    const dReal k = worldFPS * this->erp;
+    pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( u, p );
+    pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( u, q );
+
+
+
+
+    /*
+     * Constraint along the axis: translation along it should couple angular movement.
+     * This is just the ball-and-socket derivation, projected onto the hinge axis,
+     * producing a single constraint at the end.
+     *
+     * The choice of "ball" position can be arbitrary; we could place it at the center
+     * of one of the bodies, canceling out its rotational jacobian; or we could make
+     * everything symmetrical by just placing at the midpoint between the centers.
+     *
+     * I like symmetry, so I'll use the second approach here. I'll call the midpoint h.
+     *
+     * Of course, if the second body is NULL, the first body is pretty much locked
+     * along this axis, and the linear constraint is enough.
+     */
+
+    int rowskip_mul_3 = 3 * rowskip;
+    dCopyVector3(J1 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);
+
+    if ( body1 ) {
+        dVector3 h;
+        dAddScaledVectors3(h, node[0].body->posr.pos, body1->posr.pos, -0.5, 0.5);
+
+        dCalcVectorCross3(J1 + rowskip_mul_3 + GI2__JA_MIN, h, globalAxis1);
+
+        dCopyNegatedVector3(J2 + rowskip_mul_3 + GI2__JL_MIN, globalAxis1);
+        dCopyVector3(J2 + rowskip_mul_3 + GI2__JA_MIN, J1 + rowskip_mul_3 + GI2__JA_MIN);
+    }
+
+    // error correction: both anchors should lie on the same plane perpendicular to the axis
+    dVector3 globalA1, globalA2;
+    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
+
+    if ( body1 ) {
+        dBodyGetRelPointPos(body1, anchor2[0], anchor2[1], anchor2[2], globalA2);
+    } else {
+        dCopyVector3(globalA2, anchor2);
+    }
+
+    dVector3 d;
+    dSubtractVectors3(d, globalA1, globalA2); // displacement error
+    pairRhsCfm[3 * pairskip + GI2_RHS] = -k * dCalcVectorDot3(globalAxis1, d);
+}
+
+void dJointSetDHingeAxis( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    dBodyVectorFromWorld(joint->node[0].body, x, y, z, joint->axis1);
+    if (joint->node[1].body)
+        dBodyVectorFromWorld(joint->node[1].body, x, y, z, joint->axis2);
+    else {
+        joint->axis2[0] = x;
+        joint->axis2[1] = y;
+        joint->axis2[2] = z;
+    }
+    dNormalize3(joint->axis1);
+    dNormalize3(joint->axis2);
+}
+
+void dJointGetDHingeAxis( dJointID j, dVector3 result )
+{
+    dxJointDHinge* joint = static_cast<dxJointDHinge*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    dBodyVectorToWorld(joint->node[0].body, joint->axis1[0], joint->axis1[1], joint->axis1[2], result);
+}
+
+
+void dJointSetDHingeAnchor1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dJointSetDBallAnchor1(j, x, y, z);
+}
+
+
+void dJointSetDHingeAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dJointSetDBallAnchor2(j, x, y, z);
+}
+
+dReal dJointGetDHingeDistance(dJointID j)
+{
+    return dJointGetDBallDistance(j);
+}
+
+
+void dJointGetDHingeAnchor1( dJointID j, dVector3 result )
+{
+    dJointGetDBallAnchor1(j, result);
+}
+
+
+void dJointGetDHingeAnchor2( dJointID j, dVector3 result )
+{
+    dJointGetDBallAnchor2(j, result);
+}
+
+
+void dJointSetDHingeParam( dJointID j, int parameter, dReal value )
+{
+    dJointSetDBallParam(j, parameter, value);
+}
+
+
+dReal dJointGetDHingeParam( dJointID j, int parameter )
+{
+    return dJointGetDBallParam(j, parameter);
+}
+
+dJointType
+dxJointDHinge::type() const
+{
+    return dJointTypeDHinge;
+}
+
+sizeint
+dxJointDHinge::size() const
+{
+    return sizeof( *this );
+}