add ode

1 files changed, 314 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/dball.cpp b/libs/ode-0.16.1/ode/src/joints/dball.cpp
new file mode 100644
index 0000000..3754646
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/dball.cpp
@@ -0,0 +1,314 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "dball.h"
+#include "joint_internal.h"
+
+/*
+ * Double Ball joint: tries to maintain a fixed distance between two anchor
+ * points.
+ */
+
+dxJointDBall::dxJointDBall(dxWorld *w) :
+    dxJoint(w)
+{
+    dSetZero(anchor1, 3);
+    dSetZero(anchor2, 3);
+    targetDistance = 0;
+    erp = world->global_erp;
+    cfm = world->global_cfm;
+}
+
+void
+dxJointDBall::getSureMaxInfo( SureMaxInfo* info )
+{
+    info->max_m = 1;
+}
+void
+dxJointDBall::getInfo1( dxJoint::Info1 *info )
+{
+    info->m = 1;
+    info->nub = 1;
+}
+
+void
+dxJointDBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, 
+    int rowskip, dReal *J1, dReal *J2,
+    int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, 
+    int *findex )
+{
+    dVector3 globalA1, globalA2;
+    dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1);
+    
+    if (node[1].body) {
+        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2);
+    } else {
+        dCopyVector3(globalA2, anchor2);
+    }
+
+    dVector3 q;
+    dSubtractVectors3(q, globalA1, globalA2);
+
+#ifdef dSINGLE
+    const dReal MIN_LENGTH = REAL(1e-7);
+#else
+    const dReal MIN_LENGTH = REAL(1e-12);
+#endif
+
+    if (dCalcVectorLength3(q) < MIN_LENGTH) {
+        // too small, let's choose an arbitrary direction
+        // heuristic: difference in velocities at anchors
+        dVector3 v1, v2;
+        dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1);
+    
+        if (node[1].body) {
+            dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2);
+        } else {
+            dZeroVector3(v2);
+        }
+
+        dSubtractVectors3(q, v1, v2);
+
+        if (dCalcVectorLength3(q) < MIN_LENGTH) {
+            // this direction is as good as any
+            dAssignVector3(q, 1, 0, 0);
+        }
+    }
+    dNormalize3(q);
+
+    dCopyVector3(J1 + GI2__JL_MIN, q);
+
+    dVector3 relA1;
+    dBodyVectorToWorld(node[0].body,
+                       anchor1[0], anchor1[1], anchor1[2],
+                       relA1);
+
+    dMatrix3 a1m;
+    dZeroMatrix3(a1m);
+    dSetCrossMatrixMinus(a1m, relA1, 4);
+
+    dMultiply1_331(J1 + GI2__JA_MIN, a1m, q);
+
+    if (node[1].body) {
+        dCopyNegatedVector3(J2 + GI2__JL_MIN, q);
+
+        dVector3 relA2;
+        dBodyVectorToWorld(node[1].body,
+                           anchor2[0], anchor2[1], anchor2[2],
+                           relA2);
+        dMatrix3 a2m;
+        dZeroMatrix3(a2m);
+        dSetCrossMatrixPlus(a2m, relA2, 4);
+        dMultiply1_331(J2 + GI2__JA_MIN, a2m, q);
+    }
+    
+    const dReal k = worldFPS * this->erp;
+    pairRhsCfm[GI2_RHS] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2));
+    pairRhsCfm[GI2_CFM] = this->cfm;
+}
+
+
+void
+dxJointDBall::updateTargetDistance()
+{
+    dVector3 p1, p2;
+
+    if (node[0].body)
+        dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1);
+    else
+        dCopyVector3(p1, anchor1);
+    if (node[1].body)
+        dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2);
+    else
+        dCopyVector3(p2, anchor2);
+
+    targetDistance = dCalcPointsDistance3(p1, p2);
+}
+
+
+void dJointSetDBallAnchor1( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[1].body)
+            dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2);
+        else {
+            joint->anchor2[0] = x;
+            joint->anchor2[1] = y;
+            joint->anchor2[2] = z;
+        }
+    } else {
+        if (joint->node[0].body)
+            dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1);
+        else {
+            joint->anchor1[0] = x;
+            joint->anchor1[1] = y;
+            joint->anchor1[2] = z;
+        }
+    }
+
+    joint->updateTargetDistance();
+}
+
+
+void dJointSetDBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[0].body)
+            dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1);
+        else {
+            joint->anchor1[0] = x;
+            joint->anchor1[1] = y;
+            joint->anchor1[2] = z;
+        }
+    } else {
+        if (joint->node[1].body)
+            dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2);
+        else {
+            joint->anchor2[0] = x;
+            joint->anchor2[1] = y;
+            joint->anchor2[2] = z;
+        }
+    }
+
+    joint->updateTargetDistance();
+}
+
+dReal dJointGetDBallDistance(dJointID j)
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    return joint->targetDistance;
+}
+
+void dJointSetDBallDistance(dJointID j, dReal dist)
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( dist>=0, "target distance must be non-negative" );
+
+    joint->targetDistance = dist;
+}
+
+
+void dJointGetDBallAnchor1( dJointID j, dVector3 result )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[1].body)
+            dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result);
+        else
+            dCopyVector3(result, joint->anchor2);
+    } else {
+        if (joint->node[0].body)
+            dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result);
+        else
+            dCopyVector3(result, joint->anchor1);
+    }
+}
+
+
+void dJointGetDBallAnchor2( dJointID j, dVector3 result )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+    dUASSERT( result, "bad result argument" );
+
+    if ( joint->flags & dJOINT_REVERSE ) {
+        if (joint->node[0].body)
+            dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result);
+        else
+            dCopyVector3(result, joint->anchor1);
+    } else {
+        if (joint->node[1].body)
+            dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result);
+        else
+            dCopyVector3(result, joint->anchor2);
+    }
+}
+
+
+void dJointSetDBallParam( dJointID j, int parameter, dReal value )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    switch ( parameter ) {
+        case dParamCFM:
+            joint->cfm = value;
+            break;
+        case dParamERP:
+            joint->erp = value;
+            break;
+    }
+}
+
+
+dReal dJointGetDBallParam( dJointID j, int parameter )
+{
+    dxJointDBall* joint = static_cast<dxJointDBall*>(j);
+    dUASSERT( joint, "bad joint argument" );
+
+    switch ( parameter ) {
+        case dParamCFM:
+            return joint->cfm;
+        case dParamERP:
+            return joint->erp;
+        default:
+            return 0;
+    }
+}
+
+
+dJointType
+dxJointDBall::type() const
+{
+    return dJointTypeDBall;
+}
+
+sizeint
+dxJointDBall::size() const
+{
+    return sizeof( *this );
+}
+
+void
+dxJointDBall::setRelativeValues()
+{
+    updateTargetDistance();
+}
+
+
+