resubmitting ode

1 files changed, 346 insertions, 0 deletions

diff --git a/libraries/ode-0.9/ode/src/joint.h b/libraries/ode-0.9/ode/src/joint.h
new file mode 100644
index 0000000..27c013e
--- /dev/null
+++ b/libraries/ode-0.9/ode/src/joint.h
@@ -0,0 +1,346 @@
+/*************************************************************************
+ *                                                                       *
+ * 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 "objects.h"
+#include <ode/contact.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
+};
+
+
+// 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 {
+    int m,nub;
+  };
+
+  // info returned by getInfo2 function
+
+  struct Info2 {
+    // integrator parameters: frames per second (1/stepsize), default error
+    // reduction parameter (0..1).
+    dReal fps,erp;
+
+    // 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 *J1l,*J1a,*J2l,*J2a;
+
+    // elements to jump from one row to the next in J's
+    int rowskip;
+
+    // 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 *c,*cfm;
+
+    // lo and hi limits for variables (set to -/+ infinity on entry).
+    dReal *lo,*hi;
+
+    // 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;
+  };
+
+  // virtual function table: size of the joint structure, function pointers.
+  // we do it this way instead of using C++ virtual functions because
+  // sometimes we need to allocate joints ourself within a memory pool.
+
+  typedef void init_fn (dxJoint *joint);
+  typedef void getInfo1_fn (dxJoint *joint, Info1 *info);
+  typedef void getInfo2_fn (dxJoint *joint, Info2 *info);
+  struct Vtable {
+    int size;
+    init_fn *init;
+    getInfo1_fn *getInfo1;
+    getInfo2_fn *getInfo2;
+    int typenum;                // a dJointTypeXXX type number
+  };
+
+  Vtable *vtable;               // virtual function table
+  int 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
+};
+
+
+// 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 {
+  int num;              // number of joints on the stack
+  dObStack 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);
+  int testRotationalLimit (dReal angle);
+  int addLimot (dxJoint *joint, dxJoint::Info2 *info, int row,
+                dVector3 ax1, int rotational);
+};
+
+
+// ball and socket
+
+struct dxJointBall : public dxJoint {
+  dVector3 anchor1;             // anchor w.r.t first body
+  dVector3 anchor2;             // anchor w.r.t second body
+  dReal erp;                    // error reduction
+  dReal cfm;                    // constraint force mix in
+  void set (int num, dReal value);
+  dReal get (int num);
+};
+extern struct dxJoint::Vtable __dball_vtable;
+
+
+// hinge
+
+struct dxJointHinge : public dxJoint {
+  dVector3 anchor1;             // anchor w.r.t first body
+  dVector3 anchor2;             // anchor w.r.t second body
+  dVector3 axis1;               // axis w.r.t first body
+  dVector3 axis2;               // axis w.r.t second body
+  dQuaternion qrel;             // initial relative rotation body1 -> body2
+  dxJointLimitMotor limot;      // limit and motor information
+};
+extern struct dxJoint::Vtable __dhinge_vtable;
+
+
+// universal
+
+struct dxJointUniversal : public dxJoint {
+  dVector3 anchor1;             // anchor w.r.t first body
+  dVector3 anchor2;             // anchor w.r.t second body
+  dVector3 axis1;               // axis w.r.t first body
+  dVector3 axis2;               // axis w.r.t second body
+  dQuaternion qrel1;    // initial relative rotation body1 -> virtual cross piece
+  dQuaternion qrel2;    // initial relative rotation virtual cross piece -> body2
+  dxJointLimitMotor limot1;     // limit and motor information for axis1
+  dxJointLimitMotor limot2;     // limit and motor information for axis2
+};
+extern struct dxJoint::Vtable __duniversal_vtable;
+
+
+/**
+ * The axisP must be perpendicular to axis2
+ * <PRE>
+ *                                        +-------------+
+ *                                        |      x      |
+ *                                        +------------\+
+ * Prismatic articulation                   ..     ..
+ *                       |                ..     ..
+ *                      \/              ..      ..
+ * +--------------+    --|        __..      ..  anchor2
+ * |      x       | .....|.......(__)     ..
+ * +--------------+    --|         ^     <
+ *        |----------------------->|
+ *            Offset               |--- Rotoide articulation
+ * </PRE>
+ */
+struct dxJointPR : public dxJoint {
+
+  dVector3 anchor2;         ///< @brief Position of the rotoide articulation
+                            ///<        w.r.t second body.
+                            ///< @note Position of body 2 in world frame +
+                            ///< anchor2 in world frame give the position
+                            ///< of the rotoide articulation
+  dVector3 axisR1;          ///< axis of the rotoide articulation w.r.t first body.
+                            ///< @note This is considered as axis1 from the parameter
+                            ///< view.
+  dVector3 axisR2;          ///< axis of the rotoide articulation w.r.t second body.
+                            ///< @note This is considered also as axis1 from the
+                            ///< parameter view
+  dVector3 axisP1;          ///< axis for the prismatic articulation w.r.t first body.
+                            ///< @note This is considered as axis2 in from the parameter
+                            ///< view
+  dQuaternion qrel;         ///< initial relative rotation body1 -> body2.
+  dVector3 offset;          ///< @brief vector between the body1 and the rotoide
+                            ///< articulation.
+                            ///<
+                            ///< Going from the first to the second in the frame
+                            ///<  of body1.
+                            ///< That should be aligned with body1 center along axisP
+                            ///< This is calculated whe the axis are set.
+  dxJointLimitMotor limotR; ///< limit and motor information for the rotoide articulation.
+  dxJointLimitMotor limotP; ///< limit and motor information for the prismatic articulation.
+};
+extern struct dxJoint::Vtable __dPR_vtable;
+
+
+
+// slider. if body2 is 0 then qrel is the absolute rotation of body1 and
+// offset is the position of body1 center along axis1.
+
+struct dxJointSlider : public dxJoint {
+  dVector3 axis1;               // axis w.r.t first body
+  dQuaternion qrel;             // initial relative rotation body1 -> body2
+  dVector3 offset;              // point relative to body2 that should be
+                                // aligned with body1 center along axis1
+  dxJointLimitMotor limot;      // limit and motor information
+};
+extern struct dxJoint::Vtable __dslider_vtable;
+
+
+// contact
+
+struct dxJointContact : public dxJoint {
+  int the_m;                    // number of rows computed by getInfo1
+  dContact contact;
+};
+extern struct dxJoint::Vtable __dcontact_vtable;
+
+
+// hinge 2
+
+struct dxJointHinge2 : public dxJoint {
+  dVector3 anchor1;             // anchor w.r.t first body
+  dVector3 anchor2;             // anchor w.r.t second body
+  dVector3 axis1;               // axis 1 w.r.t first body
+  dVector3 axis2;               // axis 2 w.r.t second body
+  dReal c0,s0;                  // cos,sin of desired angle between axis 1,2
+  dVector3 v1,v2;               // angle ref vectors embedded in first body
+  dxJointLimitMotor limot1;     // limit+motor info for axis 1
+  dxJointLimitMotor limot2;     // limit+motor info for axis 2
+  dReal susp_erp,susp_cfm;      // suspension parameters (erp,cfm)
+};
+extern struct dxJoint::Vtable __dhinge2_vtable;
+
+
+// angular motor
+
+struct dxJointAMotor : public dxJoint {
+  int num;                      // number of axes (0..3)
+  int mode;                     // a dAMotorXXX constant
+  int rel[3];                   // what the axes are relative to (global,b1,b2)
+  dVector3 axis[3];             // three axes
+  dxJointLimitMotor limot[3];   // limit+motor info for axes
+  dReal angle[3];               // user-supplied angles for axes
+  // these vectors are used for calculating euler angles
+  dVector3 reference1;          // original axis[2], relative to body 1
+  dVector3 reference2;          // original axis[0], relative to body 2
+};
+extern struct dxJoint::Vtable __damotor_vtable;
+
+
+struct dxJointLMotor : public dxJoint {
+  int num;
+  int rel[3];
+  dVector3 axis[3];
+  dxJointLimitMotor limot[3];
+};
+
+extern struct dxJoint::Vtable __dlmotor_vtable;
+
+
+// 2d joint, constrains to z == 0
+
+struct dxJointPlane2D : public dxJoint
+{
+    int                 row_motor_x;
+    int                 row_motor_y;
+    int                 row_motor_angle;
+    dxJointLimitMotor   motor_x;
+    dxJointLimitMotor   motor_y;
+    dxJointLimitMotor   motor_angle;
+};
+
+extern struct dxJoint::Vtable __dplane2d_vtable;
+
+
+// fixed
+
+struct dxJointFixed : public dxJoint {
+  dQuaternion qrel;             // initial relative rotation body1 -> body2
+  dVector3 offset;              // relative offset between the bodies
+  dReal erp;                    // error reduction parameter
+  dReal cfm;                    // constraint force mix-in
+  void set (int num, dReal value);
+  dReal get (int num);
+};
+extern struct dxJoint::Vtable __dfixed_vtable;
+
+
+// null joint, for testing only
+
+struct dxJointNull : public dxJoint {
+};
+extern struct dxJoint::Vtable __dnull_vtable;
+
+
+#endif