path: root/libraries/ode-0.9/ode/src/joint.h

/*************************************************************************
 *                                                                       *
 * 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