diff -r 000000000000 -r 2f259fa3e83a ode/inc/joint.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ode/inc/joint.h Tue Feb 02 01:00:49 2010 +0200 @@ -0,0 +1,338 @@ +/************************************************************************* + * * + * 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 "object.h" +#include +#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, + const 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 +}; +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 + * 
+ *                                        +-------------+
+ *                                        |      x      |
+ *                                        +------------\+
+ * Prismatic articulation                   ..     ..
+ *                       |                ..     ..
+ *                      \/              ..      ..
+ * +--------------+    --|        __..      ..  anchor2
+ * |      x       | .....|.......(__)     ..
+ * +--------------+    --|         ^     <
+ *        |----------------------->|
+ *            Offset               |--- Rotoide articulation
+ *
+ */ +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 +}; +extern struct dxJoint::Vtable __dfixed_vtable; + + +// null joint, for testing only + +struct dxJointNull : public dxJoint { +}; +extern struct dxJoint::Vtable __dnull_vtable; + + +#endif