text: /*************************************************************************
   *                                                                       *
   * 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 "../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
  };
  
  
  // 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()
          int8 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;
  
          // jacobians for joint damping
          // angular ones are implemented first
          dReal *J1ld, *J1ad, *J2ld, *J2ad;
  
          // 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;
          dReal *c_v_max;
  
          // 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;
      };
  
      // 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.
  
        int8 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
      dReal lambda_erp[6];            // lambda generated by last step
  
  
      dxJoint( dxWorld *w );
      virtual ~dxJoint();
  
      virtual void getInfo1( Info1* info ) = 0;
      virtual void getInfo2( Info2* info ) = 0;
      // This call quickly!!! estimates maximum value of "m" that could be returned by getInfo1()
  	// See comments at definition of SureMaxInfo for defails.
      virtual void getSureMaxInfo( SureMaxInfo* info ) = 0;
      virtual dJointType type() const = 0;
      virtual size_t 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;
  
      // to use joint damping set use_damping to true
      // and set damping_coefficient to the desired value
      bool use_damping;
      dReal damping_coefficient;
  
      // Moved from ball.h and fixed.h by OSRF
      dReal erp;          // error reduction
      dReal cfm;          // constraint force mix in
  };
  
  
  // 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 );
  };
  
  
  
  
  
  
  
  
  
  
  #endif
  
  
  // Local Variables:
  // mode:c++
  // c-basic-offset:4
  // End:
  