add ode

1 files changed, 326 insertions, 0 deletions

diff --git a/libs/ode-0.16.1/ode/src/joints/joint.h b/libs/ode-0.16.1/ode/src/joints/joint.h
new file mode 100644
index 0000000..b6aa81e
--- /dev/null
+++ b/libs/ode-0.16.1/ode/src/joints/joint.h
@@ -0,0 +1,326 @@
+/*************************************************************************
+ *                                                                       *
+ * 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.                     *
+ *                                                                       *
+ *************************************************************************/
+
+#ifndef _ODE_JOINT_H_
+#define _ODE_JOINT_H_
+
+
+#include <ode/contact.h>
+#include "../common.h"
+#include "../objects.h"
+#include "../obstack.h"
+
+
+// joint flags
+enum
+{
+    // if this flag is set, the joint was allocated in a joint group
+    dJOINT_INGROUP = 1,
+
+    // if this flag is set, the joint was attached with arguments (0,body).
+    // our convention is to treat all attaches as (body,0), i.e. so node[0].body
+    // is always nonzero, so this flag records the fact that the arguments were
+    // swapped.
+    dJOINT_REVERSE = 2,
+
+    // if this flag is set, the joint can not have just one body attached to it,
+    // it must have either zero or two bodies attached.
+    dJOINT_TWOBODIES = 4,
+
+    dJOINT_DISABLED = 8
+};
+
+
+enum dJointConnectedBody
+{
+    dJCB__MIN,
+
+    dJCB_FIRST_BODY = dJCB__MIN,
+    dJCB_SECOND_BODY,
+
+    dJCB__MAX,
+
+};
+
+static inline 
+dJointConnectedBody EncodeJointOtherConnectedBody(dJointConnectedBody cbBodyKind)
+{
+    dIASSERT(dIN_RANGE(cbBodyKind, dJCB__MIN, dJCB__MAX));
+    dSASSERT(dJCB__MAX == 2);
+
+    return (dJointConnectedBody)(dJCB_FIRST_BODY + dJCB_SECOND_BODY - cbBodyKind);
+}
+
+/* joint body relativity enumeration */
+enum dJointBodyRelativity 
+{
+    dJBR__MIN,
+
+    dJBR_GLOBAL = dJBR__MIN,
+
+    dJBR__BODIES_MIN,
+
+    dJBR_BODY1 = dJBR__BODIES_MIN + dJCB_FIRST_BODY,
+    dJBR_BODY2 = dJBR__BODIES_MIN + dJCB_SECOND_BODY,
+
+    dJBR__BODIES_MAX = dJBR__BODIES_MIN + dJCB__MAX,
+
+    dJBR__MAX,
+
+    dJBR__DEFAULT = dJBR_GLOBAL,
+    dJBR__BODIES_COUNT = dJBR__BODIES_MAX - dJBR__BODIES_MIN,
+
+};
+
+ODE_PURE_INLINE int dJBREncodeBodyRelativityStatus(int relativity)
+{
+    return dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX);
+}
+
+ODE_PURE_INLINE dJointBodyRelativity dJBRSwapBodyRelativity(int relativity)
+{
+    dIASSERT(dIN_RANGE(relativity, dJBR__BODIES_MIN, dJBR__BODIES_MAX));
+    return (dJointBodyRelativity)(dJBR_BODY1 + dJBR_BODY2 - relativity);
+}
+
+
+
+
+// there are two of these nodes in the joint, one for each connection to a
+// body. these are node of a linked list kept by each body of it's connecting
+// joints. but note that the body pointer in each node points to the body that
+// makes use of the *other* node, not this node. this trick makes it a bit
+// easier to traverse the body/joint graph.
+
+struct dxJointNode
+{
+    dxJoint *joint;     // pointer to enclosing dxJoint object
+    dxBody *body;       // *other* body this joint is connected to
+    dxJointNode *next;  // next node in body's list of connected joints
+};
+
+
+struct dxJoint : public dObject
+{
+    // naming convention: the "first" body this is connected to is node[0].body,
+    // and the "second" body is node[1].body. if this joint is only connected
+    // to one body then the second body is 0.
+
+    // info returned by getInfo1 function. the constraint dimension is m (<=6).
+    // i.e. that is the total number of rows in the jacobian. `nub' is the
+    // number of unbounded variables (which have lo,hi = -/+ infinity).
+
+    struct Info1
+    {
+        // Structure size should not exceed sizeof(pointer) bytes to have 
+        // to have good memory pattern in dxQuickStepper()
+        uint8 m, nub;
+    };
+
+    // info returned by getInfo2 function
+
+    enum
+    {
+        GI2__J_MIN,
+        GI2__JL_MIN = GI2__J_MIN + dDA__L_MIN,
+
+        GI2_JLX = GI2__J_MIN + dDA_LX,
+        GI2_JLY = GI2__J_MIN + dDA_LY,
+        GI2_JLZ = GI2__J_MIN + dDA_LZ,
+
+        GI2__JL_MAX = GI2__J_MIN + dDA__L_MAX,
+
+        GI2__JA_MIN = GI2__J_MIN + dDA__A_MIN,
+
+        GI2_JAX = GI2__J_MIN + dDA_AX,
+        GI2_JAY = GI2__J_MIN + dDA_AY,
+        GI2_JAZ = GI2__J_MIN + dDA_AZ,
+
+        GI2__JA_MAX = GI2__J_MIN + dDA__A_MAX,
+        GI2__J_MAX = GI2__J_MIN + dDA__MAX,
+    };
+
+    enum
+    {
+        GI2_RHS,
+        GI2_CFM,
+        GI2__RHS_CFM_MAX,
+    };
+
+    enum
+    {
+        GI2_LO,
+        GI2_HI,
+        GI2__LO_HI_MAX,
+    };
+
+    // info returned by getSureMaxInfo function. 
+    // The information is used for memory reservation in calculations.
+
+    struct SureMaxInfo
+    {
+        // The value of `max_m' must ALWAYS be not less than the value of `m'
+        // the getInfo1 call can generate in current joint state. Another 
+        // requirement is that the value should be provided very quickly, 
+        // without the excessive calculations.
+        // If it is hard/impossible to quickly predict the maximal value of `m'
+        // (which is the case for most joint types) the maximum for current 
+        // joint type in general should be returned. If it can be known the `m'
+        // will be smaller, it can save a bit of memory from being reserved 
+        // for calculations if that smaller value is returned.
+
+        uint8 max_m; // Estimate of maximal `m' in Info1
+    };
+
+
+    unsigned flags;             // dJOINT_xxx flags
+    dxJointNode node[2];        // connections to bodies. node[1].body can be 0
+    dJointFeedback *feedback;   // optional feedback structure
+    dReal lambda[6];            // lambda generated by last step
+
+
+    dxJoint( dxWorld *w );
+    virtual ~dxJoint();
+
+    bool GetIsJointReverse() const { return (this->flags & dJOINT_REVERSE) != 0; }
+
+    virtual void getInfo1( Info1* info ) = 0;
+
+    // integrator parameters
+    virtual void getInfo2( 
+        // fps=frames per second (1/stepsize), erp=default error reduction parameter (0..1)
+        dReal worldFPS, dReal worldERP, 
+        // elements to jump from one row to the next in J's
+        int rowskip,
+        // for the first and second body, pointers to two (linear and angular)
+        // n*3 jacobian sub matrices, stored by rows. these matrices will have
+        // been initialized to 0 on entry. if the second body is zero then the
+        // J2xx pointers may be 0.
+        dReal *J1, dReal *J2,
+        // elements to jump from one pair of scalars to the next
+        int pairskip,
+        // right hand sides of the equation J*v = c + cfm * lambda. cfm is the
+        // "constraint force mixing" vector. c is set to zero on entry, cfm is
+        // set to a constant value (typically very small or zero) value on entry.
+        dReal *pairRhsCfm,
+        // lo and hi limits for variables (set to -/+ infinity on entry).
+        dReal *pairLoHi,
+        // findex vector for variables. see the LCP solver interface for a
+        // description of what this does. this is set to -1 on entry.
+        // note that the returned indexes are relative to the first index of
+        // the constraint.
+        int *findex) = 0;
+    // This call quickly!!! estimates maximum value of "m" that could be returned by getInfo1()
+    // See comments at definition of SureMaxInfo for details.
+    virtual void getSureMaxInfo( SureMaxInfo* info ) = 0;
+    virtual dJointType type() const = 0;
+    virtual sizeint size() const = 0;
+
+    /// Set values which are relative with respect to bodies.
+    /// Each dxJoint should redefine it if needed.
+    virtual void setRelativeValues();
+
+    // Test if this joint should be used in the simulation step
+    // (has the enabled flag set, and is attached to at least one dynamic body)
+    bool isEnabled() const;
+};
+
+
+// joint group. NOTE: any joints in the group that have their world destroyed
+// will have their world pointer set to 0.
+
+struct dxJointGroup : public dBase
+{
+    dxJointGroup(): m_num(0), m_stack() {}
+
+    template<class T>
+    T *alloc(dWorldID w)
+    {
+        T *j = (T *)m_stack.alloc(sizeof(T));
+        if (j != NULL) {
+            ++m_num;
+            new(j) T(w);
+            j->flags |= dJOINT_INGROUP;
+        }
+        return j;
+    }
+
+    sizeint getJointCount() const { return m_num; }
+    sizeint exportJoints(dxJoint **jlist);
+
+    void *beginEnum() { return m_stack.rewind(); }
+    void *continueEnum(sizeint num_bytes) { return m_stack.next(num_bytes); }
+
+    void freeAll();
+
+private:
+    sizeint m_num;        // number of joints on the stack
+    dObStack m_stack; // a stack of (possibly differently sized) dxJoint objects.
+};
+
+// common limit and motor information for a single joint axis of movement
+struct dxJointLimitMotor
+{
+    dReal vel, fmax;        // powered joint: velocity, max force
+    dReal lostop, histop;   // joint limits, relative to initial position
+    dReal fudge_factor;     // when powering away from joint limits
+    dReal normal_cfm;       // cfm to use when not at a stop
+    dReal stop_erp, stop_cfm; // erp and cfm for when at joint limit
+    dReal bounce;           // restitution factor
+    // variables used between getInfo1() and getInfo2()
+    int limit;          // 0=free, 1=at lo limit, 2=at hi limit
+    dReal limit_err;    // if at limit, amount over limit
+
+    void init( dxWorld * );
+    void set( int num, dReal value );
+    dReal get( int num ) const;
+    bool testRotationalLimit( dReal angle );
+
+    enum
+    {
+        GI2__JL_MIN = dxJoint::GI2__JL_MIN,
+        GI2__JA_MIN = dxJoint::GI2__JA_MIN,
+        GI2_JAX = dxJoint::GI2_JAX,
+        GI2_JAY = dxJoint::GI2_JAY,
+        GI2_JAZ = dxJoint::GI2_JAZ,
+        GI2_RHS = dxJoint::GI2_RHS,
+        GI2_CFM = dxJoint::GI2_CFM,
+        GI2_LO = dxJoint::GI2_LO,
+        GI2_HI = dxJoint::GI2_HI,
+    };
+
+    bool addLimot( dxJoint *joint, dReal fps, 
+        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+        const dVector3 ax1, int rotational );
+    bool addTwoPointLimot( dxJoint *joint, dReal fps,
+        dReal *J1, dReal *J2, dReal *pairRhsCfm, dReal *pairLoHi,
+        const dVector3 ax1, const dVector3 pt1, const dVector3 pt2 );
+};
+
+
+#endif
+
+
+// Local Variables:
+// mode:c++
+// c-basic-offset:4
+// End: