[ODE] Universal Spring etc.
Simon Bovet
bovet at ifi.unizh.ch
Wed Aug 28 10:54:10 2002
This is a multi-part message in MIME format.
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: text/plain;
charset="iso-8859-1"
Content-Transfer-Encoding: 7bit
Hi,
I'm using ODE since a few weeks... and I'm already quite confident about its
future! So here is my small contribution in the direction of simulating
"flexible or deformable bodies"...
- Universal Spring: like universal joint, expanded with a linear spring that
keeps the bodies together and an angular spring that keeps the bodies
aligned. The procedures dJointSet(Get)UniversalSpring...
* ...Anchor, ...Axis1/2 are similar to universal joint
* ...MainAxis: specifies the axis on which the bodies should be aligned
* ...Constant/...Damping: the spring const and damping for the linear
spring
* ...AngularConstant/...AngularDamping: the same for the angular spring
* ...PrecessionDampingCFM: CFM that prevents the bodies from precessing
around the main axis
- I also needed to compute the force and torque applied on a body by a
joint, so I also wrote the procedures dJointGetForce/Torque.
All the modifications I made are bracketted by the comments:
// bovet+
[new code]
// bovet-
If you could update the latest CVS version with my new lines (I hope there
isn't too much bugs...), that would just be great! You'll also find an
example (test_univspring.cpp) of how this kind of joint can be used to
simulate some deformable beam...
Well, just let me know if you have any question! (There's probably a bit too
few comments in the code...)
Sincerely,
Simon
------------------------------------------------------
Simon Bovet
Artificial Intelligence Laboratory
Department of Information Technology
University of Zurich
Winterthurerstrasse 190
8057 Zurich, Switzerland
Tel: +41 1 635 43 42
http://www.ifi.unizh.ch/~bovet/
------------------------------------------------------
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="common.h"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="common.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_COMMON_H_
#define _ODE_COMMON_H_
#include <ode/config.h>
#include <ode/error.h>
#ifdef __cplusplus
extern "C" {
#endif
/* configuration stuff */
/* the efficient alignment. most platforms align data structures to some
* number of bytes, but this is not always the most efficient alignment.
* for example, many x86 compilers align to 4 bytes, but on a pentium it
* is important to align doubles to 8 byte boundaries (for speed), and
* the 4 floats in a SIMD register to 16 byte boundaries. many other
* platforms have similar behavior. setting a larger alignment can waste
* a (very) small amount of memory. NOTE: this number must be a power of
* two. this is set to 16 by default.
*/
#define EFFICIENT_ALIGNMENT 16
/* constants */
/* pi and 1/sqrt(2) are defined here if necessary because they don't get
* defined in <math.h> on some platforms (like MS-Windows)
*/
#ifndef M_PI
#define M_PI REAL(3.1415926535897932384626433832795029)
#endif
#ifndef M_SQRT1_2
#define M_SQRT1_2 REAL(0.7071067811865475244008443621048490)
#endif
/* debugging:
* IASSERT is an internal assertion, i.e. a consistency check. if it =
fails
* we want to know where.
* UASSERT is a user assertion, i.e. if it fails a nice error message
* should be printed for the user.
* AASSERT is an arguments assertion, i.e. if it fails "bad =
argument(s)"
* is printed.
* DEBUGMSG just prints out a message
*/
#ifndef dNODEBUG
#ifdef __GNUC__
#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s() [%s]",__FUNCTION__,__FILE__);
#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
msg " in %s()", __FUNCTION__);
#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
msg " in %s()", __FUNCTION__);
#else
#define dIASSERT(a) if (!(a)) dDebug (d_ERR_IASSERT, \
"assertion \"" #a "\" failed in %s:%d",__FILE__,__LINE__);
#define dUASSERT(a,msg) if (!(a)) dDebug (d_ERR_UASSERT, \
msg " (%s:%d)", __FILE__,__LINE__);
#define dDEBUGMSG(msg) dMessage (d_ERR_UASSERT, \
msg " (%s:%d)", __FILE__,__LINE__);
#endif
#else
#define dIASSERT(a) ;
#define dUASSERT(a,msg) ;
#define dDEBUGMSG(msg) ;
#endif
#define dAASSERT(a) dUASSERT(a,"Bad argument(s)")
/* floating point data type, vector, matrix and quaternion types */
#if defined(dSINGLE)
typedef float dReal;
#elif defined(dDOUBLE)
typedef double dReal;
#else
#error You must #define dSINGLE or dDOUBLE
#endif
/* round an integer up to a multiple of 4, except that 0 and 1 are =
unmodified
* (used to compute matrix leading dimensions)
*/
#define dPAD(a) (((a) > 1) ? ((((a)-1)|3)+1) : (a))
/* these types are mainly just used in headers */
typedef dReal dVector3[4];
typedef dReal dVector4[4];
typedef dReal dMatrix3[4*3];
typedef dReal dMatrix4[4*4];
typedef dReal dMatrix6[8*6];
typedef dReal dQuaternion[4];
/* precision dependent scalar math functions */
#if defined(dSINGLE)
#define REAL(x) (x ## f) /* form a constant */
#define dRecip(x) ((float)(1.0f/(x))) /* reciprocal */
#define dSqrt(x) ((float)sqrt(x)) /* square root */
#define dRecipSqrt(x) ((float)(1.0f/sqrt(x))) /* reciprocal square root =
*/
#define dSin(x) ((float)sin(x)) /* sine */
#define dCos(x) ((float)cos(x)) /* cosine */
#define dFabs(x) ((float)fabs(x)) /* absolute value */
#define dAtan2(y,x) ((float)atan2((y),(x))) /* arc tangent with 2 args =
*/
#elif defined(dDOUBLE)
#define REAL(x) (x)
#define dRecip(x) (1.0/(x))
#define dSqrt(x) sqrt(x)
#define dRecipSqrt(x) (1.0/sqrt(x))
#define dSin(x) sin(x)
#define dCos(x) cos(x)
#define dFabs(x) fabs(x)
#define dAtan2(y,x) atan2((y),(x))
#else
#error You must #define dSINGLE or dDOUBLE
#endif
/* utility */
/* round something up to be a multiple of the EFFICIENT_ALIGNMENT */
#define dEFFICIENT_SIZE(x) ((((x)-1)|(EFFICIENT_ALIGNMENT-1))+1)
/* alloca aligned to the EFFICIENT_ALIGNMENT. note that this can waste
* up to 15 bytes per allocation, depending on what alloca() returns.
*/
#define dALLOCA16(n) \
((char*)dEFFICIENT_SIZE(((int)(alloca((n)+(EFFICIENT_ALIGNMENT-1))))))
/* internal object types (all prefixed with `dx') */
struct dxWorld; /* dynamics world */
struct dxSpace; /* collision space */
struct dxBody; /* rigid body (dynamics object) */
struct dxGeom; /* geometry (collision object) */
struct dxJoint;
struct dxJointNode;
struct dxJointGroup;
typedef struct dxWorld *dWorldID;
typedef struct dxSpace *dSpaceID;
typedef struct dxBody *dBodyID;
typedef struct dxGeom *dGeomID;
typedef struct dxJoint *dJointID;
typedef struct dxJointGroup *dJointGroupID;
/* error numbers */
enum {
d_ERR_UNKNOWN =3D 0, /* unknown error */
d_ERR_IASSERT, /* internal assertion failed */
d_ERR_UASSERT, /* user assertion failed */
d_ERR_LCP /* user assertion failed */
};
/* joint type numbers */
enum {
dJointTypeNone =3D 0, /* or "unknown" */
dJointTypeBall,
dJointTypeHinge,
dJointTypeSlider,
dJointTypeContact,
dJointTypeUniversal,
dJointTypeHinge2,
dJointTypeFixed,
dJointTypeNull,
dJointTypeAMotor
// bovet+
, dJointTypeUniversalSpring
// bovet-
};
/* an alternative way of setting joint parameters, using joint parameter
* structures and member constants. we don't actually do this yet.
*/
/*
typedef struct dLimot {
int mode;
dReal lostop, histop;
dReal vel, fmax;
dReal fudge_factor;
dReal bounce, soft;
dReal suspension_erp, suspension_cfm;
} dLimot;
enum {
dLimotLoStop =3D 0x0001,
dLimotHiStop =3D 0x0002,
dLimotVel =3D 0x0004,
dLimotFMax =3D 0x0008,
dLimotFudgeFactor =3D 0x0010,
dLimotBounce =3D 0x0020,
dLimotSoft =3D 0x0040
};
*/
/* standard joint parameter names. why are these here? - because we =
don't want
* to include all the joint function definitions in joint.cpp. hmmmm.
* MSVC complains if we call D_ALL_PARAM_NAMES_X with a blank second =
argument,
* which is why we have the D_ALL_PARAM_NAMES macro as well. please copy =
and
* paste between these two.
*/
#define D_ALL_PARAM_NAMES(start) \
/* parameters for limits and motors */ \
dParamLoStop =3D start, \
dParamHiStop, \
dParamVel, \
dParamFMax, \
dParamFudgeFactor, \
dParamBounce, \
dParamCFM, \
dParamStopERP, \
dParamStopCFM, \
/* parameters for suspension */ \
dParamSuspensionERP, \
dParamSuspensionCFM,
#define D_ALL_PARAM_NAMES_X(start,x) \
/* parameters for limits and motors */ \
dParamLoStop ## x =3D start, \
dParamHiStop ## x, \
dParamVel ## x, \
dParamFMax ## x, \
dParamFudgeFactor ## x, \
dParamBounce ## x, \
dParamCFM ## x, \
dParamStopERP ## x, \
dParamStopCFM ## x, \
/* parameters for suspension */ \
dParamSuspensionERP ## x, \
dParamSuspensionCFM ## x,
enum {
D_ALL_PARAM_NAMES(0)
D_ALL_PARAM_NAMES_X(0x100,2)
D_ALL_PARAM_NAMES_X(0x200,3)
/* add a multiple of this constant to the basic parameter numbers to =
get
* the parameters for the second, third etc axes.
*/
dParamGroup=3D0x100
};
/* angular motor mode numbers */
enum{
dAMotorUser =3D 0,
dAMotorEuler =3D 1
};
#ifdef __cplusplus
}
#endif
#endif
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="joint.h"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="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 =3D 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 =3D 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 =3D 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 =
(<=3D6).
// i.e. that is the total number of rows in the jacobian. `nub' is the
// number of unbounded variables (which have lo,hi =3D -/+ 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 =3D 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=20
};
Vtable *vtable; // virtual function table
int flags; // dJOINT_xxx flags
dxJointNode node[2]; // connections to bodies. node[1].body can be 0
// bovet+
dReal force[16]; // force & torque applied to bodies at last step
// bovet-
};
// 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=3Dfree, 1=3Dat lo limit, 2=3Dat 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
};
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
};
extern struct dxJoint::Vtable __duniversal_vtable;
// bovet+
// universal spring
struct dxJointUniversalSpring : public dxJointUniversal {
dVector3 mainAxis1;
dVector3 mainAxis2;
dReal kp; // spring constant
dReal kd; // damping constant
dReal akp; // angular spring constant
dReal akd; // angular damping constant
dReal dcfm; // damping cfm for precession
dReal lfps;
dReal leps;
dReal lcfm;
dReal aeps;
dReal acfm;
};
extern struct dxJoint::Vtable __duniversalspring_vtable;
// bovet-
// 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;
// fixed
struct dxJointFixed : public dxJoint {
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
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="step.cpp"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="step.cpp"
/************************************************************************=
*
* =
*
* 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. =
*
* =
*
=
*************************************************************************=
/
#include "objects.h"
#include "joint.h"
#include <ode/config.h>
#include <ode/odemath.h>
#include <ode/rotation.h>
#include <ode/timer.h>
#include <ode/error.h>
#include <ode/matrix.h>
#include "lcp.h"
//***********************************************************************=
*****
// misc defines
#define FAST_FACTOR
//#define TIMING
#define ALLOCA dALLOCA16
//***********************************************************************=
*****
// debugging - comparison of various vectors and matrices produced by =
the
// slow and fast versions of the stepper.
//#define COMPARE_METHODS
#ifdef COMPARE_METHODS
#include "testing.h"
dMatrixComparison comparator;
#endif
//***********************************************************************=
*****
// special matrix multipliers
// this assumes the 4th and 8th rows of B and C are zero.
static void Multiply2_p8r (dReal *A, dReal *B, dReal *C,
int p, int r, int Askip)
{
int i,j;
dReal sum,*bb,*cc;
dIASSERT (p>0 && r>0 && A && B && C);
bb =3D B;
for (i=3Dp; i; i--) {
cc =3D C;
for (j=3Dr; j; j--) {
sum =3D bb[0]*cc[0];
sum +=3D bb[1]*cc[1];
sum +=3D bb[2]*cc[2];
sum +=3D bb[4]*cc[4];
sum +=3D bb[5]*cc[5];
sum +=3D bb[6]*cc[6];
*(A++) =3D sum;=20
cc +=3D 8;
}
A +=3D Askip - r;
bb +=3D 8;
}
}
// this assumes the 4th and 8th rows of B and C are zero.
static void MultiplyAdd2_p8r (dReal *A, dReal *B, dReal *C,
int p, int r, int Askip)
{
int i,j;
dReal sum,*bb,*cc;
dIASSERT (p>0 && r>0 && A && B && C);
bb =3D B;
for (i=3Dp; i; i--) {
cc =3D C;
for (j=3Dr; j; j--) {
sum =3D bb[0]*cc[0];
sum +=3D bb[1]*cc[1];
sum +=3D bb[2]*cc[2];
sum +=3D bb[4]*cc[4];
sum +=3D bb[5]*cc[5];
sum +=3D bb[6]*cc[6];
*(A++) +=3D sum;=20
cc +=3D 8;
}
A +=3D Askip - r;
bb +=3D 8;
}
}
// this assumes the 4th and 8th rows of B are zero.
static void Multiply0_p81 (dReal *A, dReal *B, dReal *C, int p)
{
int i;
dIASSERT (p>0 && A && B && C);
dReal sum;
for (i=3Dp; i; i--) {
sum =3D B[0]*C[0];
sum +=3D B[1]*C[1];
sum +=3D B[2]*C[2];
sum +=3D B[4]*C[4];
sum +=3D B[5]*C[5];
sum +=3D B[6]*C[6];
*(A++) =3D sum;
B +=3D 8;
}
}
// this assumes the 4th and 8th rows of B are zero.
static void MultiplyAdd0_p81 (dReal *A, dReal *B, dReal *C, int p)
{
int i;
dIASSERT (p>0 && A && B && C);
dReal sum;
for (i=3Dp; i; i--) {
sum =3D B[0]*C[0];
sum +=3D B[1]*C[1];
sum +=3D B[2]*C[2];
sum +=3D B[4]*C[4];
sum +=3D B[5]*C[5];
sum +=3D B[6]*C[6];
*(A++) +=3D sum;
B +=3D 8;
}
}
// this assumes the 4th and 8th rows of B are zero.
static void MultiplyAdd1_8q1 (dReal *A, dReal *B, dReal *C, int q)
{
int k;
dReal sum;
dIASSERT (q>0 && A && B && C);
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[k*8] * C[k];
A[0] +=3D sum;
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[1+k*8] * C[k];
A[1] +=3D sum;
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[2+k*8] * C[k];
A[2] +=3D sum;
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[4+k*8] * C[k];
A[4] +=3D sum;
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[5+k*8] * C[k];
A[5] +=3D sum;
sum =3D 0;
for (k=3D0; k<q; k++) sum +=3D B[6+k*8] * C[k];
A[6] +=3D sum;
}
//***********************************************************************=
*****
// body rotation
// return sin(x)/x. this has a singularity at 0 so special handling is =
needed
// for small arguments.
static inline dReal sinc (dReal x)
{
// if |x| < 1e-4 then use a taylor series expansion. this two term =
expansion
// is actually accurate to one LS bit within this range if double =
precision
// is being used - so don't worry!
if (dFabs(x) < 1.0e-4) return REAL(1.0) - =
x*x*REAL(0.166666666666666666667);
else return dSin(x)/x;
}
// given a body b, apply its linear and angular rotation over the time
// interval h, thereby adjusting its position and orientation.
static inline void moveAndRotateBody (dxBody *b, dReal h)
{
int j;
// handle linear velocity
for (j=3D0; j<3; j++) b->pos[j] +=3D h * b->lvel[j];
if (b->flags & dxBodyFlagFiniteRotation) {
dVector3 irv; // infitesimal rotation vector
dQuaternion q; // quaternion for finite rotation
if (b->flags & dxBodyFlagFiniteRotationAxis) {
// split the angular velocity vector into a component along the =
finite
// rotation axis, and a component orthogonal to it.
dVector3 frv,irv; // finite rotation vector
dReal k =3D dDOT (b->finite_rot_axis,b->avel);
frv[0] =3D b->finite_rot_axis[0] * k;
frv[1] =3D b->finite_rot_axis[1] * k;
frv[2] =3D b->finite_rot_axis[2] * k;
irv[0] =3D b->avel[0] - frv[0];
irv[1] =3D b->avel[1] - frv[1];
irv[2] =3D b->avel[2] - frv[2];
// make a rotation quaternion q that corresponds to frv * h.
// compare this with the full-finite-rotation case below.
h *=3D REAL(0.5);
dReal theta =3D k * h;
q[0] =3D dCos(theta);
dReal s =3D sinc(theta) * h;
q[1] =3D frv[0] * s;
q[2] =3D frv[1] * s;
q[3] =3D frv[2] * s;
}
else {
// make a rotation quaternion q that corresponds to w * h
dReal wlen =3D dSqrt (b->avel[0]*b->avel[0] + =
b->avel[1]*b->avel[1] +
b->avel[2]*b->avel[2]);
h *=3D REAL(0.5);
dReal theta =3D wlen * h;
q[0] =3D dCos(theta);
dReal s =3D sinc(theta) * h;
q[1] =3D b->avel[0] * s;
q[2] =3D b->avel[1] * s;
q[3] =3D b->avel[2] * s;
}
// do the finite rotation
dQuaternion q2;
dQMultiply0 (q2,q,b->q);
for (j=3D0; j<4; j++) b->q[j] =3D q2[j];
// do the infitesimal rotation if required
if (b->flags & dxBodyFlagFiniteRotationAxis) {
dReal dq[4];
dWtoDQ (irv,b->q,dq);
for (j=3D0; j<4; j++) b->q[j] +=3D h * dq[j];
}
}
else {
// the normal way - do an infitesimal rotation
dReal dq[4];
dWtoDQ (b->avel,b->q,dq);
for (j=3D0; j<4; j++) b->q[j] +=3D h * dq[j];
}
// normalize the quaternion and convert it to a rotation matrix
dNormalize4 (b->q);
dQtoR (b->q,b->R);
}
//***********************************************************************=
*****
// the slow, but sure way
// given lists of bodies and joints that form an island, perform a first
// order timestep.
//
// `body' is the body array, `nb' is the size of the array.
// `_joint' is the body array, `nj' is the size of the array.
void dInternalStepIsland_x1 (dxWorld *world, dxBody * const *body, int =
nb,
dxJoint * const *_joint, int nj, dReal stepsize)
{
int i,j,k;
int n6 =3D 6*nb;
# ifdef TIMING
dTimerStart("preprocessing");
# endif
// number all bodies in the body list - set their tag values
for (i=3D0; i<nb; i++) body[i]->tag =3D i;
// make a local copy of the joint array, because we might want to =
modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the =
joints
// but not the joint array, because the caller might need it =
unchanged).
dxJoint **joint =3D (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
memcpy (joint,_joint,nj * sizeof(dxJoint*));
// for all bodies, compute the inertia tensor and its inverse in the =
global
// frame, and compute the rotational force and add it to the torque
// accumulator.
// @@@ check computation of rotational force.
dReal *I =3D (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
dReal *invI =3D (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
dSetZero (I,3*nb*4);
dSetZero (invI,3*nb*4);
for (i=3D0; i<nb; i++) {
dReal tmp[12];
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
// compute rotational force
dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
dCROSS (body[i]->tacc,-=3D,body[i]->avel,tmp);
}
// add the gravity force to all bodies
for (i=3D0; i<nb; i++) {
if ((body[i]->flags & dxBodyNoGravity)=3D=3D0) {
body[i]->facc[0] +=3D body[i]->mass.mass * world->gravity[0];
body[i]->facc[1] +=3D body[i]->mass.mass * world->gravity[1];
body[i]->facc[2] +=3D body[i]->mass.mass * world->gravity[2];
}
}
// get m =3D total constraint dimension, nub =3D number of unbounded =
variables.
// create constraint offset array and number-of-rows array for all =
joints.
// the constraints are re-ordered as follows: the purely unbounded
// constraints, the mixed unbounded + LCP constraints, and last the =
purely
// LCP constraints.
//
// joints with m=3D0 are inactive and are removed from the joints =
array
// entirely, so that the code that follows does not consider them.
int m =3D 0;
dxJoint::Info1 *info =3D (dxJoint::Info1*) ALLOCA =
(nj*sizeof(dxJoint::Info1));
int *ofs =3D (int*) ALLOCA (nj*sizeof(int));
for (i=3D0, j=3D0; j<nj; j++) { // i=3Ddest, j=3Dsrc
joint[j]->vtable->getInfo1 (joint[j],info+i);
dIASSERT (info[i].m >=3D 0 && info[i].m <=3D 6 &&
info[i].nub >=3D 0 && info[i].nub <=3D info[i].m);
if (info[i].m > 0) {
joint[i] =3D joint[j];
i++;
}
}
nj =3D i;
// the purely unbounded constraints
for (i=3D0; i<nj; i++) if (info[i].nub =3D=3D info[i].m) {
ofs[i] =3D m;
m +=3D info[i].m;
}
int nub =3D m;
// the mixed unbounded + LCP constraints
for (i=3D0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) =
{
ofs[i] =3D m;
m +=3D info[i].m;
}
// the purely LCP constraints
for (i=3D0; i<nj; i++) if (info[i].nub =3D=3D 0) {
ofs[i] =3D m;
m +=3D info[i].m;
}
// create (6*nb,6*nb) inverse mass matrix `invM', and fill it with =
mass
// parameters
# ifdef TIMING
dTimerNow ("create mass matrix");
# endif
int nskip =3D dPAD (n6);
dReal *invM =3D (dReal*) ALLOCA (n6*nskip*sizeof(dReal));
dSetZero (invM,n6*nskip);
for (i=3D0; i<nb; i++) {
dReal *MM =3D invM+(i*6)*nskip+(i*6);
MM[0] =3D body[i]->invMass;
MM[nskip+1] =3D body[i]->invMass;
MM[2*nskip+2] =3D body[i]->invMass;
MM +=3D 3*nskip+3;
for (j=3D0; j<3; j++) for (k=3D0; k<3; k++) {
MM[j*nskip+k] =3D invI[i*12+j*4+k];
}
}
// assemble some body vectors: fe =3D external forces, v =3D =
velocities
dReal *fe =3D (dReal*) ALLOCA (n6 * sizeof(dReal));
dReal *v =3D (dReal*) ALLOCA (n6 * sizeof(dReal));
dSetZero (fe,n6);
dSetZero (v,n6);
for (i=3D0; i<nb; i++) {
for (j=3D0; j<3; j++) fe[i*6+j] =3D body[i]->facc[j];
for (j=3D0; j<3; j++) fe[i*6+3+j] =3D body[i]->tacc[j];
for (j=3D0; j<3; j++) v[i*6+j] =3D body[i]->lvel[j];
for (j=3D0; j<3; j++) v[i*6+3+j] =3D body[i]->avel[j];
}
// this will be set to the velocity update
dReal *vnew =3D (dReal*) ALLOCA (n6 * sizeof(dReal));
dSetZero (vnew,n6);
// if there are constraints, compute cforce
if (m > 0) {
// create a constraint equation right hand side vector `c', a =
constraint
// force mixing vector `cfm', and LCP low and high bound vectors, =
and an
// 'findex' vector.
dReal *c =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *cfm =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *lo =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *hi =3D (dReal*) ALLOCA (m*sizeof(dReal));
int *findex =3D (int*) alloca (m*sizeof(int));
dSetZero (c,m);
dSetValue (cfm,m,world->global_cfm);
dSetValue (lo,m,-dInfinity);
dSetValue (hi,m, dInfinity);
for (i=3D0; i<m; i++) findex[i] =3D -1;
// create (m,6*nb) jacobian mass matrix `J', and fill it with =
constraint
// data. also fill the c vector.
# ifdef TIMING
dTimerNow ("create J");
# endif
dReal *J =3D (dReal*) ALLOCA (m*nskip*sizeof(dReal));
dSetZero (J,m*nskip);
dxJoint::Info2 Jinfo;
Jinfo.rowskip =3D nskip;
Jinfo.fps =3D dRecip(stepsize);
Jinfo.erp =3D world->global_erp;
for (i=3D0; i<nj; i++) {
Jinfo.J1l =3D J + nskip*ofs[i] + 6*joint[i]->node[0].body->tag;
Jinfo.J1a =3D Jinfo.J1l + 3;
if (joint[i]->node[1].body) {
Jinfo.J2l =3D J + nskip*ofs[i] + 6*joint[i]->node[1].body->tag;
Jinfo.J2a =3D Jinfo.J2l + 3;
}
else {
Jinfo.J2l =3D 0;
Jinfo.J2a =3D 0;
}
Jinfo.c =3D c + ofs[i];
Jinfo.cfm =3D cfm + ofs[i];
Jinfo.lo =3D lo + ofs[i];
Jinfo.hi =3D hi + ofs[i];
Jinfo.findex =3D findex + ofs[i];
joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
// adjust returned findex values for global index numbering
for (j=3D0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >=3D 0) findex[ofs[i] + j] +=3D ofs[i];
}
}
// compute A =3D J*invM*J'
# ifdef TIMING
dTimerNow ("compute A");
# endif
dReal *JinvM =3D (dReal*) ALLOCA (m*nskip*sizeof(dReal));
dSetZero (JinvM,m*nskip);
dMultiply0 (JinvM,J,invM,m,n6,n6);
int mskip =3D dPAD(m);
dReal *A =3D (dReal*) ALLOCA (m*mskip*sizeof(dReal));
dSetZero (A,m*mskip);
dMultiply2 (A,JinvM,J,m,n6,m);
// add cfm to the diagonal of A
for (i=3D0; i<m; i++) A[i*mskip+i] +=3D cfm[i] * Jinfo.fps;
# ifdef COMPARE_METHODS
comparator.nextMatrix (A,m,m,1,"A");
# endif
// compute `rhs', the right hand side of the equation J*a=3Dc
# ifdef TIMING
dTimerNow ("compute rhs");
# endif
dReal *tmp1 =3D (dReal*) ALLOCA (n6 * sizeof(dReal));
dSetZero (tmp1,n6);
dMultiply0 (tmp1,invM,fe,n6,n6,1);
for (i=3D0; i<n6; i++) tmp1[i] +=3D v[i]/stepsize;
dReal *rhs =3D (dReal*) ALLOCA (m * sizeof(dReal));
dSetZero (rhs,m);
dMultiply0 (rhs,J,tmp1,m,n6,1);
for (i=3D0; i<m; i++) rhs[i] =3D c[i]/stepsize - rhs[i];
# ifdef COMPARE_METHODS
comparator.nextMatrix (c,m,1,0,"c");
comparator.nextMatrix (rhs,m,1,0,"rhs");
# endif
// solve the LCP problem and get lambda.
// this will destroy A but that's okay
# ifdef TIMING
dTimerNow ("solving LCP problem");
# endif
dReal *lambda =3D (dReal*) ALLOCA (m * sizeof(dReal));
dReal *residual =3D (dReal*) ALLOCA (m * sizeof(dReal));
dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
// OLD WAY - direct factor and solve
//
// // factorize A (L*L'=3DA)
//# ifdef TIMING
// dTimerNow ("factorize A");
//# endif
// dReal *L =3D (dReal*) ALLOCA (m*mskip*sizeof(dReal));
// memcpy (L,A,m*mskip*sizeof(dReal));
// if (dFactorCholesky (L,m)=3D=3D0) dDebug (0,"A is not positive =
definite");
//
// // compute lambda
//# ifdef TIMING
// dTimerNow ("compute lambda");
//# endif
// dReal *lambda =3D (dReal*) ALLOCA (m * sizeof(dReal));
// memcpy (lambda,rhs,m * sizeof(dReal));
// dSolveCholesky (L,lambda,m);
# ifdef COMPARE_METHODS
comparator.nextMatrix (lambda,m,1,0,"lambda");
# endif
// compute the velocity update `vnew'
# ifdef TIMING
dTimerNow ("compute velocity update");
# endif
dMultiply1 (tmp1,J,lambda,n6,m,1);
for (i=3D0; i<n6; i++) tmp1[i] +=3D fe[i];
dMultiply0 (vnew,invM,tmp1,n6,n6,1);
for (i=3D0; i<n6; i++) vnew[i] =3D v[i] + stepsize*vnew[i];
// see if the constraint has worked: compute J*vnew and make sure it =
equals
// `c' (to within a certain tolerance).
# ifdef TIMING
dTimerNow ("verify constraint equation");
# endif
dMultiply0 (tmp1,J,vnew,m,n6,1);
dReal err =3D 0;
for (i=3D0; i<m; i++) err +=3D dFabs(tmp1[i]-c[i]);
printf ("%.6e\n",err);
}
else {
// no constraints
dMultiply0 (vnew,invM,fe,n6,n6,1);
for (i=3D0; i<n6; i++) vnew[i] =3D v[i] + stepsize*vnew[i];
}
# ifdef COMPARE_METHODS
comparator.nextMatrix (vnew,n6,1,0,"vnew");
# endif
// apply the velocity update to the bodies
# ifdef TIMING
dTimerNow ("update velocity");
# endif
for (i=3D0; i<nb; i++) {
for (j=3D0; j<3; j++) body[i]->lvel[j] =3D vnew[i*6+j];
for (j=3D0; j<3; j++) body[i]->avel[j] =3D vnew[i*6+3+j];
}
// update the position and orientation from the new linear/angular =
velocity
// (over the given timestep)
# ifdef TIMING
dTimerNow ("update position");
# endif
for (i=3D0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
# ifdef TIMING
dTimerNow ("tidy up");
# endif
// zero all force accumulators
for (i=3D0; i<nb; i++) {
body[i]->facc[0] =3D 0;
body[i]->facc[1] =3D 0;
body[i]->facc[2] =3D 0;
body[i]->facc[3] =3D 0;
body[i]->tacc[0] =3D 0;
body[i]->tacc[1] =3D 0;
body[i]->tacc[2] =3D 0;
body[i]->tacc[3] =3D 0;
}
# ifdef TIMING
dTimerEnd();
if (m > 0) dTimerReport (stdout,1);
# endif
}
//***********************************************************************=
*****
// an optimized version of dInternalStepIsland1()
void dInternalStepIsland_x2 (dxWorld *world, dxBody * const *body, int =
nb,
dxJoint * const *_joint, int nj, dReal stepsize)
{
int i,j,k;
# ifdef TIMING
dTimerStart("preprocessing");
# endif
dReal stepsize1 =3D dRecip(stepsize);
// number all bodies in the body list - set their tag values
for (i=3D0; i<nb; i++) body[i]->tag =3D i;
// make a local copy of the joint array, because we might want to =
modify it.
// (the "dxJoint *const*" declaration says we're allowed to modify the =
joints
// but not the joint array, because the caller might need it =
unchanged).
dxJoint **joint =3D (dxJoint**) ALLOCA (nj * sizeof(dxJoint*));
memcpy (joint,_joint,nj * sizeof(dxJoint*));
// for all bodies, compute the inertia tensor and its inverse in the =
global
// frame, and compute the rotational force and add it to the torque
// accumulator. I and invI are vertically stacked 3x4 matrices, one =
per body.
// @@@ check computation of rotational force.
dReal *I =3D (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
dReal *invI =3D (dReal*) ALLOCA (3*nb*4 * sizeof(dReal));
dSetZero (I,3*nb*4);
dSetZero (invI,3*nb*4);
for (i=3D0; i<nb; i++) {
dReal tmp[12];
// compute inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->mass.I,body[i]->R);
dMULTIPLY0_333 (I+i*12,body[i]->R,tmp);
// compute inverse inertia tensor in global frame
dMULTIPLY2_333 (tmp,body[i]->invI,body[i]->R);
dMULTIPLY0_333 (invI+i*12,body[i]->R,tmp);
// compute rotational force
dMULTIPLY0_331 (tmp,I+i*12,body[i]->avel);
dCROSS (body[i]->tacc,-=3D,body[i]->avel,tmp);
}
// add the gravity force to all bodies
for (i=3D0; i<nb; i++) {
if ((body[i]->flags & dxBodyNoGravity)=3D=3D0) {
body[i]->facc[0] +=3D body[i]->mass.mass * world->gravity[0];
body[i]->facc[1] +=3D body[i]->mass.mass * world->gravity[1];
body[i]->facc[2] +=3D body[i]->mass.mass * world->gravity[2];
}
}
// get m =3D total constraint dimension, nub =3D number of unbounded =
variables.
// create constraint offset array and number-of-rows array for all =
joints.
// the constraints are re-ordered as follows: the purely unbounded
// constraints, the mixed unbounded + LCP constraints, and last the =
purely
// LCP constraints. this assists the LCP solver to put all unbounded
// variables at the start for a quick factorization.
//
// joints with m=3D0 are inactive and are removed from the joints =
array
// entirely, so that the code that follows does not consider them.
// also number all active joints in the joint list (set their tag =
values).
// inactive joints receive a tag value of -1.
int m =3D 0;
dxJoint::Info1 *info =3D (dxJoint::Info1*) ALLOCA =
(nj*sizeof(dxJoint::Info1));
int *ofs =3D (int*) ALLOCA (nj*sizeof(int));
for (i=3D0, j=3D0; j<nj; j++) { // i=3Ddest, j=3Dsrc
joint[j]->vtable->getInfo1 (joint[j],info+i);
dIASSERT (info[i].m >=3D 0 && info[i].m <=3D 6 &&
info[i].nub >=3D 0 && info[i].nub <=3D info[i].m);
if (info[i].m > 0) {
joint[i] =3D joint[j];
joint[i]->tag =3D i;
i++;
}
else {
joint[j]->tag =3D -1;
}
}
nj =3D i;
// the purely unbounded constraints
for (i=3D0; i<nj; i++) if (info[i].nub =3D=3D info[i].m) {
ofs[i] =3D m;
m +=3D info[i].m;
}
int nub =3D m;
// the mixed unbounded + LCP constraints
for (i=3D0; i<nj; i++) if (info[i].nub > 0 && info[i].nub < info[i].m) =
{
ofs[i] =3D m;
m +=3D info[i].m;
}
// the purely LCP constraints
for (i=3D0; i<nj; i++) if (info[i].nub =3D=3D 0) {
ofs[i] =3D m;
m +=3D info[i].m;
}
// this will be set to the force due to the constraints
dReal *cforce =3D (dReal*) ALLOCA (nb*8 * sizeof(dReal));
dSetZero (cforce,nb*8);
// if there are constraints, compute cforce
if (m > 0) {
// create a constraint equation right hand side vector `c', a =
constraint
// force mixing vector `cfm', and LCP low and high bound vectors, =
and an
// 'findex' vector.
dReal *c =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *cfm =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *lo =3D (dReal*) ALLOCA (m*sizeof(dReal));
dReal *hi =3D (dReal*) ALLOCA (m*sizeof(dReal));
int *findex =3D (int*) alloca (m*sizeof(int));
dSetZero (c,m);
dSetValue (cfm,m,world->global_cfm);
dSetValue (lo,m,-dInfinity);
dSetValue (hi,m, dInfinity);
for (i=3D0; i<m; i++) findex[i] =3D -1;
// get jacobian data from constraints. a (2*m)x8 matrix will be =
created
// to store the two jacobian blocks from each constraint. it has =
this
// format:
//
// l l l 0 a a a 0 \=20
// l l l 0 a a a 0 }-- jacobian body 1 block for joint 0 (3 =
rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 \=20
// l l l 0 a a a 0 }-- jacobian body 2 block for joint 0 (3 =
rows)
// l l l 0 a a a 0 /
// l l l 0 a a a 0 }--- jacobian body 1 block for joint 1 (1 row)
// l l l 0 a a a 0 }--- jacobian body 2 block for joint 1 (1 row)
// etc...
//
// (lll) =3D linear jacobian data
// (aaa) =3D angular jacobian data
//
# ifdef TIMING
dTimerNow ("create J");
# endif
dReal *J =3D (dReal*) ALLOCA (2*m*8*sizeof(dReal));
dSetZero (J,2*m*8);
dxJoint::Info2 Jinfo;
Jinfo.rowskip =3D 8;
Jinfo.fps =3D stepsize1;
Jinfo.erp =3D world->global_erp;
for (i=3D0; i<nj; i++) {
Jinfo.J1l =3D J + 2*8*ofs[i];
Jinfo.J1a =3D Jinfo.J1l + 4;
Jinfo.J2l =3D Jinfo.J1l + 8*info[i].m;
Jinfo.J2a =3D Jinfo.J2l + 4;
Jinfo.c =3D c + ofs[i];
Jinfo.cfm =3D cfm + ofs[i];
Jinfo.lo =3D lo + ofs[i];
Jinfo.hi =3D hi + ofs[i];
Jinfo.findex =3D findex + ofs[i];
joint[i]->vtable->getInfo2 (joint[i],&Jinfo);
// adjust returned findex values for global index numbering
for (j=3D0; j<info[i].m; j++) {
if (findex[ofs[i] + j] >=3D 0) findex[ofs[i] + j] +=3D ofs[i];
}
}
// compute A =3D J*invM*J'. first compute JinvM =3D J*invM. this has =
the same
// format as J so we just go through the constraints in J =
multiplying by
// the appropriate scalars and matrices.
# ifdef TIMING
dTimerNow ("compute A");
# endif
dReal *JinvM =3D (dReal*) ALLOCA (2*m*8*sizeof(dReal));
dSetZero (JinvM,2*m*8);
for (i=3D0; i<nj; i++) {
int b =3D joint[i]->node[0].body->tag;
dReal body_invMass =3D body[b]->invMass;
dReal *body_invI =3D invI + b*12;
dReal *Jsrc =3D J + 2*8*ofs[i];
dReal *Jdst =3D JinvM + 2*8*ofs[i];
for (j=3Dinfo[i].m-1; j>=3D0; j--) {
for (k=3D0; k<3; k++) Jdst[k] =3D Jsrc[k] * body_invMass;
dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
Jsrc +=3D 8;
Jdst +=3D 8;
}
if (joint[i]->node[1].body) {
b =3D joint[i]->node[1].body->tag;
body_invMass =3D body[b]->invMass;
body_invI =3D invI + b*12;
for (j=3Dinfo[i].m-1; j>=3D0; j--) {
for (k=3D0; k<3; k++) Jdst[k] =3D Jsrc[k] * body_invMass;
dMULTIPLY0_133 (Jdst+4,Jsrc+4,body_invI);
Jsrc +=3D 8;
Jdst +=3D 8;
}
}
}
// now compute A =3D JinvM * J'. A's rows and columns are grouped by =
joint,
// i.e. in the same way as the rows of J. block (i,j) of A is only =
nonzero
// if joints i and j have at least one body in common. this fact =
suggests
// the algorithm used to fill A:
//
// for b =3D all bodies
// n =3D number of joints attached to body b
// for i =3D 1..n
// for j =3D i+1..n
// ii =3D actual joint number for i
// jj =3D actual joint number for j
// // (ii,jj) will be set to all pairs of joints around =
body b
// compute blockwise: A(ii,jj) +=3D JinvM(ii) * J(jj)'
//
// this algorithm catches all pairs of joints that have at least one =
body
// in common. it does not compute the diagonal blocks of A however -
// another similar algorithm does that.
int mskip =3D dPAD(m);
dReal *A =3D (dReal*) ALLOCA (m*mskip*sizeof(dReal));
dSetZero (A,m*mskip);
for (i=3D0; i<nb; i++) {
for (dxJointNode *n1=3Dbody[i]->firstjoint; n1; n1=3Dn1->next) {
for (dxJointNode *n2=3Dn1->next; n2; n2=3Dn2->next) {
// get joint numbers and ensure ofs[j1] >=3D ofs[j2]
int j1 =3D n1->joint->tag;
int j2 =3D n2->joint->tag;
if (ofs[j1] < ofs[j2]) {
int tmp =3D j1;
j1 =3D j2;
j2 =3D tmp;
}
// if either joint was tagged as -1 then it is an inactive (m=3D0)
// joint that should not be considered
if (j1=3D=3D-1 || j2=3D=3D-1) continue;
// determine if body i is the 1st or 2nd body of joints j1 and j2
int jb1 =3D (joint[j1]->node[1].body =3D=3D body[i]);
int jb2 =3D (joint[j2]->node[1].body =3D=3D body[i]);
// jb1/jb2 must be 0 for joints with only one body
dIASSERT(joint[j1]->node[1].body || jb1=3D=3D0);
dIASSERT(joint[j2]->node[1].body || jb2=3D=3D0);
// set block of A
MultiplyAdd2_p8r (A + ofs[j1]*mskip + ofs[j2],
JinvM + 2*8*ofs[j1] + jb1*8*info[j1].m,
J + 2*8*ofs[j2] + jb2*8*info[j2].m,
info[j1].m,info[j2].m, mskip);
}
}
}
// compute diagonal blocks of A
for (i=3D0; i<nj; i++) {
Multiply2_p8r (A + ofs[i]*(mskip+1),
JinvM + 2*8*ofs[i],
J + 2*8*ofs[i],
info[i].m,info[i].m, mskip);
if (joint[i]->node[1].body) {
MultiplyAdd2_p8r (A + ofs[i]*(mskip+1),
JinvM + 2*8*ofs[i] + 8*info[i].m,
J + 2*8*ofs[i] + 8*info[i].m,
info[i].m,info[i].m, mskip);
}
}
// add cfm to the diagonal of A
for (i=3D0; i<m; i++) A[i*mskip+i] +=3D cfm[i] * stepsize1;
# ifdef COMPARE_METHODS
comparator.nextMatrix (A,m,m,1,"A");
# endif
// compute the right hand side `rhs'
# ifdef TIMING
dTimerNow ("compute rhs");
# endif
dReal *tmp1 =3D (dReal*) ALLOCA (nb*8 * sizeof(dReal));
dSetZero (tmp1,nb*8);
// put v/h + invM*fe into tmp1
for (i=3D0; i<nb; i++) {
dReal body_invMass =3D body[i]->invMass;
dReal *body_invI =3D invI + i*12;
for (j=3D0; j<3; j++) tmp1[i*8+j] =3D body[i]->facc[j] * =
body_invMass +
body[i]->lvel[j] * stepsize1;
dMULTIPLY0_331 (tmp1 + i*8 + 4,body_invI,body[i]->tacc);
for (j=3D0; j<3; j++) tmp1[i*8+4+j] +=3D body[i]->avel[j] * =
stepsize1;
}
// put J*tmp1 into rhs
dReal *rhs =3D (dReal*) ALLOCA (m * sizeof(dReal));
dSetZero (rhs,m);
for (i=3D0; i<nj; i++) {
dReal *JJ =3D J + 2*8*ofs[i];
Multiply0_p81 (rhs+ofs[i],JJ,
tmp1 + 8*joint[i]->node[0].body->tag, info[i].m);
if (joint[i]->node[1].body) {
MultiplyAdd0_p81 (rhs+ofs[i],JJ + 8*info[i].m,
tmp1 + 8*joint[i]->node[1].body->tag, info[i].m);
}
}
// complete rhs
for (i=3D0; i<m; i++) rhs[i] =3D c[i]*stepsize1 - rhs[i];
# ifdef COMPARE_METHODS
comparator.nextMatrix (c,m,1,0,"c");
comparator.nextMatrix (rhs,m,1,0,"rhs");
# endif
// solve the LCP problem and get lambda.
// this will destroy A but that's okay
# ifdef TIMING
dTimerNow ("solving LCP problem");
# endif
dReal *lambda =3D (dReal*) ALLOCA (m * sizeof(dReal));
dReal *residual =3D (dReal*) ALLOCA (m * sizeof(dReal));
dSolveLCP (m,A,lambda,rhs,residual,nub,lo,hi,findex);
// OLD WAY - direct factor and solve
//
// // factorize A (L*L'=3DA)
//# ifdef TIMING
// dTimerNow ("factorize A");
//# endif
// dReal *L =3D (dReal*) ALLOCA (m*mskip*sizeof(dReal));
// memcpy (L,A,m*mskip*sizeof(dReal));
//# ifdef FAST_FACTOR
// dFastFactorCholesky (L,m); // does not report non positive =
definiteness
//# else
// if (dFactorCholesky (L,m)=3D=3D0) dDebug (0,"A is not positive =
definite");
//# endif
//
// // compute lambda
//# ifdef TIMING
// dTimerNow ("compute lambda");
//# endif
// dReal *lambda =3D (dReal*) ALLOCA (m * sizeof(dReal));
// memcpy (lambda,rhs,m * sizeof(dReal));
// dSolveCholesky (L,lambda,m);
# ifdef COMPARE_METHODS
comparator.nextMatrix (lambda,m,1,0,"lambda");
# endif
// compute the constraint force `cforce'
# ifdef TIMING
dTimerNow ("compute constraint force");
# endif
// compute cforce =3D J'*lambda
for (i=3D0; i<nj; i++) {
#if 1
// bovet+
dReal *JJ =3D J + 2*8*ofs[i];
dReal *f1 =3D joint[i]->force;
dReal *f2 =3D f1 + 8;
for (j =3D 0; j < 8; j++) f1[j]=3D 0, f2[j] =3D 0;
dReal *c =3D cforce + 8*joint[i]->node[0].body->tag;
MultiplyAdd1_8q1 (f1,JJ,
lambda+ofs[i], info[i].m);
for (j =3D 0; j < 8; j++) c[j] +=3D f1[j];
if (joint[i]->node[1].body) {
c =3D cforce + 8*joint[i]->node[1].body->tag;
MultiplyAdd1_8q1 (f2,
JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
for (j =3D 0; j < 8; j++) c[j] +=3D f2[j];
}
#else // OLD:
dReal *JJ =3D J + 2*8*ofs[i];
MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[0].body->tag,JJ,
lambda+ofs[i], info[i].m);
if (joint[i]->node[1].body) {
MultiplyAdd1_8q1 (cforce + 8*joint[i]->node[1].body->tag,
JJ + 8*info[i].m, lambda+ofs[i], info[i].m);
}
#endif
// bovet-
}
}
// compute the velocity update
# ifdef TIMING
dTimerNow ("compute velocity update");
# endif
// add fe to cforce
for (i=3D0; i<nb; i++) {
for (j=3D0; j<3; j++) cforce[i*8+j] +=3D body[i]->facc[j];
for (j=3D0; j<3; j++) cforce[i*8+4+j] +=3D body[i]->tacc[j];
}
// multiply cforce by stepsize
for (i=3D0; i < nb*8; i++) cforce[i] *=3D stepsize;
// add invM * cforce to the body velocity
for (i=3D0; i<nb; i++) {
dReal body_invMass =3D body[i]->invMass;
dReal *body_invI =3D invI + i*12;
for (j=3D0; j<3; j++) body[i]->lvel[j] +=3D body_invMass * =
cforce[i*8+j];
dMULTIPLYADD0_331 (body[i]->avel,body_invI,cforce+i*8+4);
}
// update the position and orientation from the new linear/angular =
velocity
// (over the given timestep)
# ifdef TIMING
dTimerNow ("update position");
# endif
for (i=3D0; i<nb; i++) moveAndRotateBody (body[i],stepsize);
# ifdef COMPARE_METHODS
dReal *tmp_vnew =3D (dReal*) ALLOCA (nb*6*sizeof(dReal));
for (i=3D0; i<nb; i++) {
for (j=3D0; j<3; j++) tmp_vnew[i*6+j] =3D body[i]->lvel[j];
for (j=3D0; j<3; j++) tmp_vnew[i*6+3+j] =3D body[i]->avel[j];
}
comparator.nextMatrix (tmp_vnew,nb*6,1,0,"vnew");
# endif
# ifdef TIMING
dTimerNow ("tidy up");
# endif
// zero all force accumulators
for (i=3D0; i<nb; i++) {
body[i]->facc[0] =3D 0;
body[i]->facc[1] =3D 0;
body[i]->facc[2] =3D 0;
body[i]->facc[3] =3D 0;
body[i]->tacc[0] =3D 0;
body[i]->tacc[1] =3D 0;
body[i]->tacc[2] =3D 0;
body[i]->tacc[3] =3D 0;
}
# ifdef TIMING
dTimerEnd();
if (m > 0) dTimerReport (stdout,1);
# endif
}
//***********************************************************************=
*****
void dInternalStepIsland (dxWorld *world, dxBody * const *body, int nb,
dxJoint * const *joint, int nj, dReal stepsize)
{
# ifndef COMPARE_METHODS
dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
# endif
# ifdef COMPARE_METHODS
int i;
// save body state
dxBody *state =3D (dxBody*) ALLOCA (nb*sizeof(dxBody));
for (i=3D0; i<nb; i++) memcpy (state+i,body[i],sizeof(dxBody));
// take slow step
comparator.reset();
dInternalStepIsland_x1 (world,body,nb,joint,nj,stepsize);
comparator.end();
// restore state
for (i=3D0; i<nb; i++) memcpy (body[i],state+i,sizeof(dxBody));
// take fast step
dInternalStepIsland_x2 (world,body,nb,joint,nj,stepsize);
comparator.end();
//comparator.dump();
//_exit (1);
# endif
}
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="ode.cpp"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="ode.cpp"
/************************************************************************=
*
* =
*
* 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. =
*
* =
*
=
*************************************************************************=
/
// this source file is mostly concerned with the data structures, not =
the
// numerics.
#include "objects.h"
#include <ode/ode.h>
#include "joint.h"
#include <ode/odemath.h>
#include <ode/matrix.h>
#include "step.h"
#include <ode/memory.h>
#include <ode/error.h>
// misc defines
#define ALLOCA dALLOCA16
//***********************************************************************=
*****
// utility
static inline void initObject (dObject *obj, dxWorld *w)
{
obj->world =3D w;
obj->next =3D 0;
obj->tome =3D 0;
obj->userdata =3D 0;
obj->tag =3D 0;
}
// add an object `obj' to the list who's head pointer is pointed to by =
`first'.
static inline void addObjectToList (dObject *obj, dObject **first)
{
obj->next =3D *first;
obj->tome =3D first;
if (*first) (*first)->tome =3D &obj->next;
(*first) =3D obj;
}
// remove the object from the linked list
static inline void removeObjectFromList (dObject *obj)
{
if (obj->next) obj->next->tome =3D obj->tome;
*(obj->tome) =3D obj->next;
// safeguard
obj->next =3D 0;
obj->tome =3D 0;
}
// remove the joint from neighbour lists of all connected bodies
static void removeJointReferencesFromAttachedBodies (dxJoint *j)
{
for (int i=3D0; i<2; i++) {
dxBody *body =3D j->node[i].body;
if (body) {
dxJointNode *n =3D body->firstjoint;
dxJointNode *last =3D 0;
while (n) {
if (n->joint =3D=3D j) {
if (last) last->next =3D n->next;
else body->firstjoint =3D n->next;
break;
}
last =3D n;
n =3D n->next;
}
}
}
j->node[0].body =3D 0;
j->node[0].next =3D 0;
j->node[1].body =3D 0;
j->node[1].next =3D 0;
}
//***********************************************************************=
*****
// island processing
// this groups all joints and bodies in a world into islands. all =
objects
// in an island are reachable by going through connected bodies and =
joints.
// each island can be simulated separately.
// note that joints that are not attached to anything will not be =
included
// in any island, an so they do not affect the simulation.
//
// this function starts new island from unvisited bodies. however, it =
will
// never start a new islands from a disabled body. thus islands of =
disabled
// bodies will not be included in the simulation. disabled bodies are
// re-enabled if they are found to be part of an active island.
static void processIslands (dxWorld *world, dReal stepsize)
{
dxBody *b,*bb,**body;
dxJoint *j,**joint;
// nothing to do if no bodies
if (world->nb <=3D 0) return;
// make arrays for body and joint lists (for a single island) to go =
into
body =3D (dxBody**) ALLOCA (world->nb * sizeof(dxBody*));
joint =3D (dxJoint**) ALLOCA (world->nj * sizeof(dxJoint*));
int bcount =3D 0; // number of bodies in `body'
int jcount =3D 0; // number of joints in `joint'
// set all body/joint tags to 0
for (b=3Dworld->firstbody; b; b=3D(dxBody*)b->next) b->tag =3D 0;
for (j=3Dworld->firstjoint; j; j=3D(dxJoint*)j->next) j->tag =3D 0;
// allocate a stack of unvisited bodies in the island. the maximum =
size of
// the stack can be the lesser of the number of bodies or joints, =
because
// new bodies are only ever added to the stack by going through =
untagged
// joints. all the bodies in the stack must be tagged!
int stackalloc =3D (world->nj < world->nb) ? world->nj : world->nb;
dxBody **stack =3D (dxBody**) ALLOCA (stackalloc * sizeof(dxBody*));
for (bb=3Dworld->firstbody; bb; bb=3D(dxBody*)bb->next) {
// get bb =3D the next enabled, untagged body, and tag it
if (bb->tag || (bb->flags & dxBodyDisabled)) continue;
bb->tag =3D 1;
// tag all bodies and joints starting from bb.
int stacksize =3D 0;
b =3D bb;
body[0] =3D bb;
bcount =3D 1;
jcount =3D 0;
goto quickstart;
while (stacksize > 0) {
b =3D stack[--stacksize]; // pop body off stack
body[bcount++] =3D b; // put body on body list
quickstart:
// traverse and tag all body's joints, add untagged connected =
bodies
// to stack
for (dxJointNode *n=3Db->firstjoint; n; n=3Dn->next) {
if (!n->joint->tag) {
n->joint->tag =3D 1;
joint[jcount++] =3D n->joint;
if (n->body && !n->body->tag) {
n->body->tag =3D 1;
stack[stacksize++] =3D n->body;
}
}
}
dIASSERT(stacksize <=3D world->nb);
dIASSERT(stacksize <=3D world->nj);
}
// now do something with body and joint lists
dInternalStepIsland (world,body,bcount,joint,jcount,stepsize);
// what we've just done may have altered the body/joint tag values.
// we must make sure that these tags are nonzero.
// also make sure all bodies are in the enabled state.
int i;
for (i=3D0; i<bcount; i++) {
body[i]->tag =3D 1;
body[i]->flags &=3D ~dxBodyDisabled;
}
for (i=3D0; i<jcount; i++) joint[i]->tag =3D 1;
}
// if debugging, check that all objects (except for disabled bodies,
// unconnected joints, and joints that are connected to disabled =
bodies)
// were tagged.
# ifndef dNODEBUG
for (b=3Dworld->firstbody; b; b=3D(dxBody*)b->next) {
if (b->flags & dxBodyDisabled) {
if (b->tag) dDebug (0,"disabled body tagged");
}
else {
if (!b->tag) dDebug (0,"enabled body not tagged");
}
}
for (j=3Dworld->firstjoint; j; j=3D(dxJoint*)j->next) {
if ((j->node[0].body && (j->node[0].body->flags & =
dxBodyDisabled)=3D=3D0) ||
(j->node[1].body && (j->node[1].body->flags & dxBodyDisabled)=3D=3D0)) =
{
if (!j->tag) dDebug (0,"attached enabled joint not tagged");
}
else {
if (j->tag) dDebug (0,"unattached or disabled joint tagged");
}
}
# endif
}
//***********************************************************************=
*****
// debugging
// see if an object list loops on itself (if so, it's bad).
static int listHasLoops (dObject *first)
{
if (first=3D=3D0 || first->next=3D=3D0) return 0;
dObject *a=3Dfirst,*b=3Dfirst->next;
int skip=3D0;
while (b) {
if (a=3D=3Db) return 1;
b =3D b->next;
if (skip) a =3D a->next;
skip ^=3D 1;
}
return 0;
}
// check the validity of the world data structures
static void checkWorld (dxWorld *w)
{
dxBody *b;
dxJoint *j;
// check there are no loops
if (listHasLoops (w->firstbody)) dDebug (0,"body list has loops");
if (listHasLoops (w->firstjoint)) dDebug (0,"joint list has loops");
// check lists are well formed (check `tome' pointers)
for (b=3Dw->firstbody; b; b=3D(dxBody*)b->next) {
if (b->next && b->next->tome !=3D &b->next)
dDebug (0,"bad tome pointer in body list");
}
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) {
if (j->next && j->next->tome !=3D &j->next)
dDebug (0,"bad tome pointer in joint list");
}
// check counts
int n =3D 0;
for (b=3Dw->firstbody; b; b=3D(dxBody*)b->next) n++;
if (w->nb !=3D n) dDebug (0,"body count incorrect");
n =3D 0;
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) n++;
if (w->nj !=3D n) dDebug (0,"joint count incorrect");
// set all tag values to a known value
static int count =3D 0;
count++;
for (b=3Dw->firstbody; b; b=3D(dxBody*)b->next) b->tag =3D count;
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) j->tag =3D count;
// check all body/joint world pointers are ok
for (b=3Dw->firstbody; b; b=3D(dxBody*)b->next) if (b->world !=3D w)
dDebug (0,"bad world pointer in body list");
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) if (j->world !=3D w)
dDebug (0,"bad world pointer in joint list");
/*
// check for half-connected joints - actually now these are valid
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) {
if (j->node[0].body || j->node[1].body) {
if (!(j->node[0].body && j->node[1].body))
dDebug (0,"half connected joint found");
}
}
*/
// check that every joint node appears in the joint lists of both =
bodies it
// attaches
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) {
for (int i=3D0; i<2; i++) {
if (j->node[i].body) {
int ok =3D 0;
for (dxJointNode *n=3Dj->node[i].body->firstjoint; n; n=3Dn->next) {
if (n->joint =3D=3D j) ok =3D 1;
}
if (ok=3D=3D0) dDebug (0,"joint not in joint list of attached body");
}
}
}
// check all body joint lists (correct body ptrs)
for (b=3Dw->firstbody; b; b=3D(dxBody*)b->next) {
for (dxJointNode *n=3Db->firstjoint; n; n=3Dn->next) {
if (&n->joint->node[0] =3D=3D n) {
if (n->joint->node[1].body !=3D b)
dDebug (0,"bad body pointer in joint node of body list (1)");
}
else {
if (n->joint->node[0].body !=3D b)
dDebug (0,"bad body pointer in joint node of body list (2)");
}
if (n->joint->tag !=3D count) dDebug (0,"bad joint node pointer in =
body");
}
}
// check all body pointers in joints, check they are distinct
for (j=3Dw->firstjoint; j; j=3D(dxJoint*)j->next) {
if (j->node[0].body && (j->node[0].body =3D=3D j->node[1].body))
dDebug (0,"non-distinct body pointers in joint");
if ((j->node[0].body && j->node[0].body->tag !=3D count) ||
(j->node[1].body && j->node[1].body->tag !=3D count))
dDebug (0,"bad body pointer in joint");
}
}
void dWorldCheck (dxWorld *w)
{
checkWorld (w);
}
//***********************************************************************=
*****
// body
dxBody *dBodyCreate (dxWorld *w)
{
dAASSERT (w);
dxBody *b =3D new dxBody;
initObject (b,w);
b->firstjoint =3D 0;
b->flags =3D 0;
dMassSetParameters (&b->mass,1,0,0,0,1,1,1,0,0,0);
dSetZero (b->invI,4*3);
b->invI[0] =3D 1;
b->invI[5] =3D 1;
b->invI[10] =3D 1;
b->invMass =3D 1;
dSetZero (b->pos,4);
dSetZero (b->q,4);
b->q[0] =3D 1;
dRSetIdentity (b->R);
dSetZero (b->lvel,4);
dSetZero (b->avel,4);
dSetZero (b->facc,4);
dSetZero (b->tacc,4);
dSetZero (b->finite_rot_axis,4);
addObjectToList (b,(dObject **) &w->firstbody);
w->nb++;
return b;
}
void dBodyDestroy (dxBody *b)
{
dAASSERT (b);
// detach all neighbouring joints, then delete this body.
dxJointNode *n =3D b->firstjoint;
while (n) {
// sneaky trick to speed up removal of joint references (black =
magic)
n->joint->node[(n =3D=3D n->joint->node)].body =3D 0;
dxJointNode *next =3D n->next;
n->next =3D 0;
removeJointReferencesFromAttachedBodies (n->joint);
n =3D next;
}
removeObjectFromList (b);
b->world->nb--;
delete b;
}
void dBodySetData (dBodyID b, void *data)
{
dAASSERT (b);
b->userdata =3D data;
}
void *dBodyGetData (dBodyID b)
{
dAASSERT (b);
return b->userdata;
}
void dBodySetPosition (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->pos[0] =3D x;
b->pos[1] =3D y;
b->pos[2] =3D z;
}
void dBodySetRotation (dBodyID b, const dMatrix3 R)
{
dAASSERT (b && R);
dQuaternion q;
dRtoQ (R,q);
dNormalize4 (q);
b->q[0] =3D q[0];
b->q[1] =3D q[1];
b->q[2] =3D q[2];
b->q[3] =3D q[3];
dQtoR (b->q,b->R);
}
void dBodySetQuaternion (dBodyID b, const dQuaternion q)
{
dAASSERT (b && q);
b->q[0] =3D q[0];
b->q[1] =3D q[1];
b->q[2] =3D q[2];
b->q[3] =3D q[3];
dNormalize4 (b->q);
dQtoR (b->q,b->R);
}
void dBodySetLinearVel (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->lvel[0] =3D x;
b->lvel[1] =3D y;
b->lvel[2] =3D z;
}
void dBodySetAngularVel (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->avel[0] =3D x;
b->avel[1] =3D y;
b->avel[2] =3D z;
}
const dReal * dBodyGetPosition (dBodyID b)
{
dAASSERT (b);
return b->pos;
}
const dReal * dBodyGetRotation (dBodyID b)
{
dAASSERT (b);
return b->R;
}
const dReal * dBodyGetQuaternion (dBodyID b)
{
dAASSERT (b);
return b->q;
}
const dReal * dBodyGetLinearVel (dBodyID b)
{
dAASSERT (b);
return b->lvel;
}
const dReal * dBodyGetAngularVel (dBodyID b)
{
dAASSERT (b);
return b->avel;
}
void dBodySetMass (dBodyID b, const dMass *mass)
{
dAASSERT (b && mass);
memcpy (&b->mass,mass,sizeof(dMass));
if (dInvertPDMatrix (b->mass.I,b->invI,3)=3D=3D0) {
dDEBUGMSG ("inertia must be positive definite");
dRSetIdentity (b->invI);
}
b->invMass =3D dRecip(b->mass.mass);
}
void dBodyGetMass (dBodyID b, dMass *mass)
{
dAASSERT (b && mass);
memcpy (mass,&b->mass,sizeof(dMass));
}
void dBodyAddForce (dBodyID b, dReal fx, dReal fy, dReal fz)
{
dAASSERT (b);
b->facc[0] +=3D fx;
b->facc[1] +=3D fy;
b->facc[2] +=3D fz;
}
void dBodyAddTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
{
dAASSERT (b);
b->tacc[0] +=3D fx;
b->tacc[1] +=3D fy;
b->tacc[2] +=3D fz;
}
void dBodyAddRelForce (dBodyID b, dReal fx, dReal fy, dReal fz)
{
dAASSERT (b);
dVector3 t1,t2;
t1[0] =3D fx;
t1[1] =3D fy;
t1[2] =3D fz;
t1[3] =3D 0;
dMULTIPLY0_331 (t2,b->R,t1);
b->facc[0] +=3D t2[0];
b->facc[1] +=3D t2[1];
b->facc[2] +=3D t2[2];
}
void dBodyAddRelTorque (dBodyID b, dReal fx, dReal fy, dReal fz)
{
dAASSERT (b);
dVector3 t1,t2;
t1[0] =3D fx;
t1[1] =3D fy;
t1[2] =3D fz;
t1[3] =3D 0;
dMULTIPLY0_331 (t2,b->R,t1);
b->tacc[0] +=3D t2[0];
b->tacc[1] +=3D t2[1];
b->tacc[2] +=3D t2[2];
}
void dBodyAddForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz)
{
dAASSERT (b);
b->facc[0] +=3D fx;
b->facc[1] +=3D fy;
b->facc[2] +=3D fz;
dVector3 f,q;
f[0] =3D fx;
f[1] =3D fy;
f[2] =3D fz;
q[0] =3D px - b->pos[0];
q[1] =3D py - b->pos[1];
q[2] =3D pz - b->pos[2];
dCROSS (b->tacc,+=3D,q,f);
}
void dBodyAddForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 prel,f,p;
f[0] =3D fx;
f[1] =3D fy;
f[2] =3D fz;
f[3] =3D 0;
prel[0] =3D px;
prel[1] =3D py;
prel[2] =3D pz;
prel[3] =3D 0;
dMULTIPLY0_331 (p,b->R,prel);
b->facc[0] +=3D f[0];
b->facc[1] +=3D f[1];
b->facc[2] +=3D f[2];
dCROSS (b->tacc,+=3D,p,f);
}
void dBodyAddRelForceAtPos (dBodyID b, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 frel,f;
frel[0] =3D fx;
frel[1] =3D fy;
frel[2] =3D fz;
frel[3] =3D 0;
dMULTIPLY0_331 (f,b->R,frel);
b->facc[0] +=3D f[0];
b->facc[1] +=3D f[1];
b->facc[2] +=3D f[2];
dVector3 q;
q[0] =3D px - b->pos[0];
q[1] =3D py - b->pos[1];
q[2] =3D pz - b->pos[2];
dCROSS (b->tacc,+=3D,q,f);
}
void dBodyAddRelForceAtRelPos (dBodyID b, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz)
{
dAASSERT (b);
dVector3 frel,prel,f,p;
frel[0] =3D fx;
frel[1] =3D fy;
frel[2] =3D fz;
frel[3] =3D 0;
prel[0] =3D px;
prel[1] =3D py;
prel[2] =3D pz;
prel[3] =3D 0;
dMULTIPLY0_331 (f,b->R,frel);
dMULTIPLY0_331 (p,b->R,prel);
b->facc[0] +=3D f[0];
b->facc[1] +=3D f[1];
b->facc[2] +=3D f[2];
dCROSS (b->tacc,+=3D,p,f);
}
const dReal * dBodyGetForce (dBodyID b)
{
dAASSERT (b);
return b->facc;
}
const dReal * dBodyGetTorque (dBodyID b)
{
dAASSERT (b);
return b->tacc;
}
void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->facc[0] =3D x;
b->facc[1] =3D y;
b->facc[2] =3D z;
}
void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->tacc[0] =3D x;
b->tacc[1] =3D y;
b->tacc[2] =3D z;
}
void dBodyGetRelPointPos (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 prel,p;
prel[0] =3D px;
prel[1] =3D py;
prel[2] =3D pz;
prel[3] =3D 0;
dMULTIPLY0_331 (p,b->R,prel);
result[0] =3D p[0] + b->pos[0];
result[1] =3D p[1] + b->pos[1];
result[2] =3D p[2] + b->pos[2];
}
void dBodyGetRelPointVel (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 prel,p;
prel[0] =3D px;
prel[1] =3D py;
prel[2] =3D pz;
prel[3] =3D 0;
dMULTIPLY0_331 (p,b->R,prel);
result[0] =3D b->lvel[0];
result[1] =3D b->lvel[1];
result[2] =3D b->lvel[2];
dCROSS (result,+=3D,b->avel,p);
}
void dBodyGetPointVel (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 p;
p[0] =3D px - b->pos[0];
p[1] =3D py - b->pos[1];
p[2] =3D pz - b->pos[2];
p[3] =3D 0;
result[0] =3D b->lvel[0];
result[1] =3D b->lvel[1];
result[2] =3D b->lvel[2];
dCROSS (result,+=3D,b->avel,p);
}
void dBodyGetPosRelPoint (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 prel;
prel[0] =3D px - b->pos[0];
prel[1] =3D py - b->pos[1];
prel[2] =3D pz - b->pos[2];
prel[3] =3D 0;
dMULTIPLY1_331 (result,b->R,prel);
}
void dBodyVectorToWorld (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 p;
p[0] =3D px;
p[1] =3D py;
p[2] =3D pz;
p[3] =3D 0;
dMULTIPLY0_331 (result,b->R,p);
}
void dBodyVectorFromWorld (dBodyID b, dReal px, dReal py, dReal pz,
dVector3 result)
{
dAASSERT (b);
dVector3 p;
p[0] =3D px;
p[1] =3D py;
p[2] =3D pz;
p[3] =3D 0;
dMULTIPLY1_331 (result,b->R,p);
}
void dBodySetFiniteRotationMode (dBodyID b, int mode)
{
dAASSERT (b);
b->flags &=3D ~(dxBodyFlagFiniteRotation | =
dxBodyFlagFiniteRotationAxis);
if (mode) {
b->flags |=3D dxBodyFlagFiniteRotation;
if (b->finite_rot_axis[0] !=3D 0 || b->finite_rot_axis[1] !=3D 0 ||
b->finite_rot_axis[2] !=3D 0) {
b->flags |=3D dxBodyFlagFiniteRotationAxis;
}
}
}
void dBodySetFiniteRotationAxis (dBodyID b, dReal x, dReal y, dReal z)
{
dAASSERT (b);
b->finite_rot_axis[0] =3D x;
b->finite_rot_axis[1] =3D y;
b->finite_rot_axis[2] =3D z;
if (x !=3D 0 || y !=3D 0 || z !=3D 0) {
dNormalize3 (b->finite_rot_axis);
b->flags |=3D dxBodyFlagFiniteRotationAxis;
}
else {
b->flags &=3D ~dxBodyFlagFiniteRotationAxis;
}
}
int dBodyGetFiniteRotationMode (dBodyID b)
{
dAASSERT (b);
return ((b->flags & dxBodyFlagFiniteRotation) !=3D 0);
}
void dBodyGetFiniteRotationAxis (dBodyID b, dVector3 result)
{
dAASSERT (b);
result[0] =3D b->finite_rot_axis[0];
result[1] =3D b->finite_rot_axis[1];
result[2] =3D b->finite_rot_axis[2];
}
int dBodyGetNumJoints (dBodyID b)
{
dAASSERT (b);
int count=3D0;
for (dxJointNode *n=3Db->firstjoint; n; n=3Dn->next, count++);
return count;
}
dJointID dBodyGetJoint (dBodyID b, int index)
{
dAASSERT (b);
int i=3D0;
for (dxJointNode *n=3Db->firstjoint; n; n=3Dn->next, i++) {
if (i =3D=3D index) return n->joint;
}
return 0;
}
void dBodyEnable (dBodyID b)
{
dAASSERT (b);
b->flags &=3D ~dxBodyDisabled;
}
void dBodyDisable (dBodyID b)
{
dAASSERT (b);
b->flags |=3D dxBodyDisabled;
}
int dBodyIsEnabled (dBodyID b)
{
dAASSERT (b);
return ((b->flags & dxBodyDisabled) =3D=3D 0);
}
void dBodySetGravityMode (dBodyID b, int mode)
{
dAASSERT (b);
if (mode) b->flags &=3D ~dxBodyNoGravity;
else b->flags |=3D dxBodyNoGravity;
}
int dBodyGetGravityMode (dBodyID b)
{
dAASSERT (b);
return ((b->flags & dxBodyNoGravity) !=3D 0);
}
//***********************************************************************=
*****
// joints
static void dJointInit (dxWorld *w, dxJoint *j)
{
dIASSERT (w && j);
initObject (j,w);
j->vtable =3D 0;
j->flags =3D 0;
j->node[0].joint =3D j;
j->node[0].body =3D 0;
j->node[0].next =3D 0;
j->node[1].joint =3D j;
j->node[1].body =3D 0;
j->node[1].next =3D 0;
addObjectToList (j,(dObject **) &w->firstjoint);
w->nj++;
}
static dxJoint *createJoint (dWorldID w, dJointGroupID group,
dxJoint::Vtable *vtable)
{
dIASSERT (w && vtable);
dxJoint *j;
if (group) {
j =3D (dxJoint*) group->stack.alloc (vtable->size);
group->num++;
}
else j =3D (dxJoint*) dAlloc (vtable->size);
dJointInit (w,j);
j->vtable =3D vtable;
if (group) j->flags |=3D dJOINT_INGROUP;
if (vtable->init) vtable->init (j);
return j;
}
dxJoint * dJointCreateBall (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dball_vtable);
}
dxJoint * dJointCreateHinge (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dhinge_vtable);
}
dxJoint * dJointCreateSlider (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dslider_vtable);
}
dxJoint * dJointCreateContact (dWorldID w, dJointGroupID group,
const dContact *c)
{
dAASSERT (w && c);
dxJointContact *j =3D (dxJointContact *)
createJoint (w,group,&__dcontact_vtable);
j->contact =3D *c;
return j;
}
dxJoint * dJointCreateHinge2 (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dhinge2_vtable);
}
dxJoint * dJointCreateUniversal (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__duniversal_vtable);
}
// bovet+
dxJoint * dJointCreateUniversalSpring (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__duniversalspring_vtable);
}
// bovet-
dxJoint * dJointCreateFixed (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dfixed_vtable);
}
dxJoint * dJointCreateNull (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__dnull_vtable);
}
dxJoint * dJointCreateAMotor (dWorldID w, dJointGroupID group)
{
dAASSERT (w);
return createJoint (w,group,&__damotor_vtable);
}
void dJointDestroy (dxJoint *j)
{
dAASSERT (j);
if (j->flags & dJOINT_INGROUP) return;
removeJointReferencesFromAttachedBodies (j);
removeObjectFromList (j);
j->world->nj--;
dFree (j,j->vtable->size);
}
dJointGroupID dJointGroupCreate (int max_size)
{
// not any more ... dUASSERT (max_size > 0,"max size must be > 0");
dxJointGroup *group =3D new dxJointGroup;
group->num =3D 0;
return group;
}
void dJointGroupDestroy (dJointGroupID group)
{
dAASSERT (group);
dJointGroupEmpty (group);
delete group;
}
void dJointGroupEmpty (dJointGroupID group)
{
// the joints in this group are detached starting from the most =
recently
// added (at the top of the stack). this helps ensure that the various
// linked lists are not traversed too much, as the joints will =
hopefully
// be at the start of those lists.
// if any group joints have their world pointer set to 0, their world =
was
// previously destroyed. no special handling is required for these =
joints.
dAASSERT (group);
int i;
dxJoint **jlist =3D (dxJoint**) ALLOCA (group->num * =
sizeof(dxJoint*));
dxJoint *j =3D (dxJoint*) group->stack.rewind();
for (i=3D0; i < group->num; i++) {
jlist[i] =3D j;
j =3D (dxJoint*) (group->stack.next (j->vtable->size));
}
for (i=3Dgroup->num-1; i >=3D 0; i--) {
if (jlist[i]->world) {
removeJointReferencesFromAttachedBodies (jlist[i]);
removeObjectFromList (jlist[i]);
jlist[i]->world->nj--;
}
}
group->num =3D 0;
group->stack.freeAll();
}
void dJointAttach (dxJoint *joint, dxBody *body1, dxBody *body2)
{
// check arguments
dUASSERT (joint,"bad joint argument");
dUASSERT (body1 =3D=3D 0 || body1 !=3D body2,"can't have =
body1=3D=3Dbody2");
dxWorld *world =3D joint->world;
dUASSERT ( (!body1 || body1->world =3D=3D world) &&
(!body2 || body2->world =3D=3D world),
"joint and bodies must be in same world");
// check if the joint can not be attached to just one body
dUASSERT (!((joint->flags & dJOINT_TWOBODIES) &&
((body1 !=3D 0) ^ (body2 !=3D 0))),
"joint can not be attached to just one body");
// remove any existing body attachments
if (joint->node[0].body || joint->node[1].body) {
removeJointReferencesFromAttachedBodies (joint);
}
// if a body is zero, make sure that it is body2, so 0 --> =
node[1].body
if (body1=3D=3D0) {
body1 =3D body2;
body2 =3D 0;
joint->flags &=3D (~dJOINT_REVERSE);
}
else {
joint->flags |=3D dJOINT_REVERSE;
}
// attach to new bodies
joint->node[0].body =3D body1;
joint->node[1].body =3D body2;
if (body1) {
joint->node[1].next =3D body1->firstjoint;
body1->firstjoint =3D &joint->node[1];
}
else joint->node[1].next =3D 0;
if (body2) {
joint->node[0].next =3D body2->firstjoint;
body2->firstjoint =3D &joint->node[0];
}
else {
joint->node[0].next =3D 0;
}
}
void dJointSetData (dxJoint *joint, void *data)
{
dAASSERT (joint);
joint->userdata =3D data;
}
void *dJointGetData (dxJoint *joint)
{
dAASSERT (joint);
return joint->userdata;
}
int dJointGetType (dxJoint *joint)
{
dAASSERT (joint);
return joint->vtable->typenum;
}
dBodyID dJointGetBody (dxJoint *joint, int index)
{
dAASSERT (joint);
if (index >=3D 0 && index < 2) return joint->node[index].body;
else return 0;
}
const dReal *dJointGetForce(dxJoint *joint, int index)
{
dAASSERT(joint && index >=3D 0 && index < 2);
return joint->force + 8*index;
}
const dReal *dJointGetTorque(dxJoint *joint, int index)
{
dAASSERT(joint && index >=3D 0 && index < 2);
return joint->force + 8*index + 4;
}
int dAreConnected (dBodyID b1, dBodyID b2)
{
dAASSERT (b1 && b2);
// look through b1's neighbour list for b2
for (dxJointNode *n=3Db1->firstjoint; n; n=3Dn->next) {
if (n->body =3D=3D b2) return 1;
}
return 0;
}
//***********************************************************************=
*****
// world
dxWorld * dWorldCreate()
{
dxWorld *w =3D new dxWorld;
w->firstbody =3D 0;
w->firstjoint =3D 0;
w->nb =3D 0;
w->nj =3D 0;
dSetZero (w->gravity,4);
w->global_erp =3D REAL(0.2);
#if defined(dSINGLE)
w->global_cfm =3D 1e-5f;
#elif defined(dDOUBLE)
w->global_cfm =3D 1e-10;
#else
#error dSINGLE or dDOUBLE must be defined
#endif
return w;
}
void dWorldDestroy (dxWorld *w)
{
// delete all bodies and joints
dAASSERT (w);
dxBody *nextb, *b =3D w->firstbody;
while (b) {
nextb =3D (dxBody*) b->next;
delete b;
b =3D nextb;
}
dxJoint *nextj, *j =3D w->firstjoint;
while (j) {
nextj =3D (dxJoint*)j->next;
if (j->flags & dJOINT_INGROUP) {
// the joint is part of a group, so "deactivate" it instead
j->world =3D 0;
j->node[0].body =3D 0;
j->node[0].next =3D 0;
j->node[1].body =3D 0;
j->node[1].next =3D 0;
dMessage (0,"warning: destroying world containing grouped =
joints");
}
else {
dFree (j,j->vtable->size);
}
j =3D nextj;
}
delete w;
}
void dWorldSetGravity (dWorldID w, dReal x, dReal y, dReal z)
{
dAASSERT (w);
w->gravity[0] =3D x;
w->gravity[1] =3D y;
w->gravity[2] =3D z;
}
void dWorldGetGravity (dWorldID w, dVector3 g)
{
dAASSERT (w);
g[0] =3D w->gravity[0];
g[1] =3D w->gravity[1];
g[2] =3D w->gravity[2];
}
void dWorldSetERP (dWorldID w, dReal erp)
{
dAASSERT (w);
w->global_erp =3D erp;
}
dReal dWorldGetERP (dWorldID w)
{
dAASSERT (w);
return w->global_erp;
}
void dWorldSetCFM (dWorldID w, dReal cfm)
{
dAASSERT (w);
w->global_cfm =3D cfm;
}
dReal dWorldGetCFM (dWorldID w)
{
dAASSERT (w);
return w->global_cfm;
}
void dWorldStep (dWorldID w, dReal stepsize)
{
dUASSERT (w,"bad world argument");
dUASSERT (stepsize > 0,"stepsize must be > 0");
processIslands (w,stepsize);
}
void dWorldImpulseToForce (dWorldID w, dReal stepsize,
dReal ix, dReal iy, dReal iz,
dVector3 force)
{
dAASSERT (w);
stepsize =3D dRecip(stepsize);
force[0] =3D stepsize * ix;
force[1] =3D stepsize * iy;
force[2] =3D stepsize * iz;
// @@@ force[3] =3D 0;
}
//***********************************************************************=
*****
// testing
#define NUM 100
#define DO(x)
extern "C" void dTestDataStructures()
{
int i;
DO(printf ("testDynamicsStuff()\n"));
dBodyID body [NUM];
int nb =3D 0;
dJointID joint [NUM];
int nj =3D 0;
for (i=3D0; i<NUM; i++) body[i] =3D 0;
for (i=3D0; i<NUM; i++) joint[i] =3D 0;
DO(printf ("creating world\n"));
dWorldID w =3D dWorldCreate();
checkWorld (w);
for (;;) {
if (nb < NUM && dRandReal() > 0.5) {
DO(printf ("creating body\n"));
body[nb] =3D dBodyCreate (w);
DO(printf ("\t--> %p\n",body[nb]));
nb++;
checkWorld (w);
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
if (nj < NUM && nb > 2 && dRandReal() > 0.5) {
dBodyID b1 =3D body [dRand() % nb];
dBodyID b2 =3D body [dRand() % nb];
if (b1 !=3D b2) {
DO(printf ("creating joint, attaching to %p,%p\n",b1,b2));
joint[nj] =3D dJointCreateBall (w,0);
DO(printf ("\t-->%p\n",joint[nj]));
checkWorld (w);
dJointAttach (joint[nj],b1,b2);
nj++;
checkWorld (w);
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
}
if (nj > 0 && nb > 2 && dRandReal() > 0.5) {
dBodyID b1 =3D body [dRand() % nb];
dBodyID b2 =3D body [dRand() % nb];
if (b1 !=3D b2) {
int k =3D dRand() % nj;
DO(printf ("reattaching joint %p\n",joint[k]));
dJointAttach (joint[k],b1,b2);
checkWorld (w);
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
}
if (nb > 0 && dRandReal() > 0.5) {
int k =3D dRand() % nb;
DO(printf ("destroying body %p\n",body[k]));
dBodyDestroy (body[k]);
checkWorld (w);
for (; k < (NUM-1); k++) body[k] =3D body[k+1];
nb--;
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
if (nj > 0 && dRandReal() > 0.5) {
int k =3D dRand() % nj;
DO(printf ("destroying joint %p\n",joint[k]));
dJointDestroy (joint[k]);
checkWorld (w);
for (; k < (NUM-1); k++) joint[k] =3D joint[k+1];
nj--;
DO(printf ("%d BODIES, %d JOINTS\n",nb,nj));
}
}
/*
printf ("creating world\n");
dWorldID w =3D dWorldCreate();
checkWorld (w);
printf ("creating body\n");
dBodyID b1 =3D dBodyCreate (w);
checkWorld (w);
printf ("creating body\n");
dBodyID b2 =3D dBodyCreate (w);
checkWorld (w);
printf ("creating joint\n");
dJointID j =3D dJointCreateBall (w);
checkWorld (w);
printf ("attaching joint\n");
dJointAttach (j,b1,b2);
checkWorld (w);
printf ("destroying joint\n");
dJointDestroy (j);
checkWorld (w);
printf ("destroying body\n");
dBodyDestroy (b1);
checkWorld (w);
printf ("destroying body\n");
dBodyDestroy (b2);
checkWorld (w);
printf ("destroying world\n");
dWorldDestroy (w);
*/
}
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="joint.cpp"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="joint.cpp"
/************************************************************************=
*
* =
*
* 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. =
*
* =
*
=
*************************************************************************=
/
/*
design note: the general principle for giving a joint the option of =
connecting
to the static environment (i.e. the absolute frame) is to check the =
second
body (joint->node[1].body), and if it is zero then behave as if its body
transform is the identity.
*/
#include <ode/odemath.h>
#include <ode/rotation.h>
#include <ode/matrix.h>
#include "joint.h"
//***********************************************************************=
*****
// externs
extern "C" void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
extern "C" void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
//***********************************************************************=
*****
// utility
// set three "ball-and-socket" rows in the constraint equation, and the
// corresponding right hand side.
static inline void setBall (dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2)
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1,a2;
int s =3D info->rowskip;
// set jacobian
info->J1l[0] =3D 1;
info->J1l[s+1] =3D 1;
info->J1l[2*s+2] =3D 1;
dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
dCROSSMAT (info->J1a,a1,s,-,+);
if (joint->node[1].body) {
info->J2l[0] =3D -1;
info->J2l[s+1] =3D -1;
info->J2l[2*s+2] =3D -1;
dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
dCROSSMAT (info->J2a,a2,s,+,-);
}
// set right hand side
dReal k =3D info->fps * info->erp;
if (joint->node[1].body) {
for (int j=3D0; j<3; j++) {
info->c[j] =3D k * (a2[j] + joint->node[1].body->pos[j] -
a1[j] - joint->node[0].body->pos[j]);
}
}
else {
for (int j=3D0; j<3; j++) {
info->c[j] =3D k * (anchor2[j] - a1[j] -
joint->node[0].body->pos[j]);
}
}
}
// this is like setBall(), except that `axis' is a unit length vector
// (in global coordinates) that should be used for the first jacobian
// position row (the other two row vectors will be derived from this).
// `erp1' is the erp value to use along the axis.
static inline void setBall2 (dxJoint *joint, dxJoint::Info2 *info,
dVector3 anchor1, dVector3 anchor2,
dVector3 axis, dReal erp1)
{
// anchor points in global coordinates with respect to body PORs.
dVector3 a1,a2;
int i,s =3D info->rowskip;
// get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0],
// [0 1 0] and [0 0 1], which makes everything much easier.
dVector3 q1,q2;
dPlaneSpace (axis,q1,q2);
// set jacobian
for (i=3D0; i<3; i++) info->J1l[i] =3D axis[i];
for (i=3D0; i<3; i++) info->J1l[s+i] =3D q1[i];
for (i=3D0; i<3; i++) info->J1l[2*s+i] =3D q2[i];
dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1);
dCROSS (info->J1a,=3D,a1,axis);
dCROSS (info->J1a+s,=3D,a1,q1);
dCROSS (info->J1a+2*s,=3D,a1,q2);
if (joint->node[1].body) {
for (i=3D0; i<3; i++) info->J2l[i] =3D -axis[i];
for (i=3D0; i<3; i++) info->J2l[s+i] =3D -q1[i];
for (i=3D0; i<3; i++) info->J2l[2*s+i] =3D -q2[i];
dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2);
dCROSS (info->J2a,=3D -,a2,axis);
dCROSS (info->J2a+s,=3D -,a2,q1);
dCROSS (info->J2a+2*s,=3D -,a2,q2);
}
// set right hand side - measure error along (axis,q1,q2)
dReal k1 =3D info->fps * erp1;
dReal k =3D info->fps * info->erp;
for (i=3D0; i<3; i++) a1[i] +=3D joint->node[0].body->pos[i];
if (joint->node[1].body) {
for (i=3D0; i<3; i++) a2[i] +=3D joint->node[1].body->pos[i];
info->c[0] =3D k1 * (dDOT(axis,a2) - dDOT(axis,a1));
info->c[1] =3D k * (dDOT(q1,a2) - dDOT(q1,a1));
info->c[2] =3D k * (dDOT(q2,a2) - dDOT(q2,a1));
}
else {
info->c[0] =3D k1 * (dDOT(axis,anchor2) - dDOT(axis,a1));
info->c[1] =3D k * (dDOT(q1,anchor2) - dDOT(q1,a1));
info->c[2] =3D k * (dDOT(q2,anchor2) - dDOT(q2,a1));
}
}
// compute anchor points relative to bodies
static void setAnchors (dxJoint *j, dReal x, dReal y, dReal z,
dVector3 anchor1, dVector3 anchor2)
{
if (j->node[0].body) {
dReal q[4];
q[0] =3D x - j->node[0].body->pos[0];
q[1] =3D y - j->node[0].body->pos[1];
q[2] =3D z - j->node[0].body->pos[2];
q[3] =3D 0;
dMULTIPLY1_331 (anchor1,j->node[0].body->R,q);
if (j->node[1].body) {
q[0] =3D x - j->node[1].body->pos[0];
q[1] =3D y - j->node[1].body->pos[1];
q[2] =3D z - j->node[1].body->pos[2];
q[3] =3D 0;
dMULTIPLY1_331 (anchor2,j->node[1].body->R,q);
}
else {
anchor2[0] =3D x;
anchor2[1] =3D y;
anchor2[2] =3D z;
}
}
anchor1[3] =3D 0;
anchor2[3] =3D 0;
}
// compute axes relative to bodies. axis2 can be 0
static void setAxes (dxJoint *j, dReal x, dReal y, dReal z,
dVector3 axis1, dVector3 axis2)
{
if (j->node[0].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (axis1,j->node[0].body->R,q);
if (axis2) {
if (j->node[1].body) {
dMULTIPLY1_331 (axis2,j->node[1].body->R,q);
}
else {
axis2[0] =3D x;
axis2[1] =3D y;
axis2[2] =3D z;
}
axis2[3] =3D 0;
}
}
axis1[3] =3D 0;
}
static void getAnchor (dxJoint *j, dVector3 result, dVector3 anchor1)
{
if (j->node[0].body) {
dMULTIPLY0_331 (result,j->node[0].body->R,anchor1);
result[0] +=3D j->node[0].body->pos[0];
result[1] +=3D j->node[0].body->pos[1];
result[2] +=3D j->node[0].body->pos[2];
}
}
static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1)
{
if (j->node[0].body) {
dMULTIPLY0_331 (result,j->node[0].body->R,axis1);
}
}
// given two bodies (body1,body2), the hinge axis that they are =
connected by
// w.r.t. body1 (axis), and the initial relative orientation between =
them
// (q_initial), return the relative rotation angle. the initial relative
// orientation corresponds to an angle of zero. if body2 is 0 then =
measure the
// angle between body1 and the static frame.
//
// this will not return the correct angle if the bodies rotate along any =
axis
// other than the given hinge axis.
static dReal getHingeAngle (dxBody *body1, dxBody *body2, dVector3 axis,
dQuaternion q_initial)
{
// the angle between the two bodies is extracted from the quaternion =
that
// represents the relative rotation between them. recall that a =
quaternion
// q is:
// [s,v] =3D [ cos(theta/2) , sin(theta/2) * u ]
// where s is a scalar and v is a 3-vector. u is a unit length axis =
and
// theta is a rotation along that axis. we can get theta/2 by:
// theta/2 =3D atan2 ( sin(theta/2) , cos(theta/2) )
// but we can't get sin(theta/2) directly, only its absolute value, =
i.e.:
// |v| =3D |sin(theta/2)| * |u|
// =3D |sin(theta/2)|
// using this value will have a strange effect. recall that there are =
two
// quaternion representations of a given rotation, q and -q. typically =
as
// a body rotates along the axis it will go through a complete cycle =
using
// one representation and then the next cycle will use the other
// representation. this corresponds to u pointing in the direction of =
the
// hinge axis and then in the opposite direction. the result is that =
theta
// will appear to go "backwards" every other cycle. here is a fix: if =
u
// points "away" from the direction of the hinge (motor) axis (i.e. =
more
// than 90 degrees) then use -q instead of q. this represents the same
// rotation, but results in the cos(theta/2) value being sign =
inverted.
// get qrel =3D relative rotation between the two bodies
dQuaternion qrel;
if (body2) {
dQuaternion qq;
dQMultiply1 (qq,body1->q,body2->q);
dQMultiply2 (qrel,qq,q_initial);
}
else {
// pretend body2->q is the identity
dQMultiply3 (qrel,body1->q,q_initial);
}
// extract the angle from the quaternion. cost2 =3D cos(theta/2),
// sint2 =3D |sin(theta/2)|
dReal cost2 =3D qrel[0];
dReal sint2 =3D dSqrt =
(qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]);
dReal theta =3D (dDOT(qrel+1,axis) >=3D 0) ? // @@@ padding =
assumptions
(2 * dAtan2(sint2,cost2)) : // if u points in direction of axis
(2 * dAtan2(sint2,-cost2)); // if u points in opposite direction
// the angle we get will be between 0..2*pi, but we want to return =
angles
// between -pi..pi
if (theta > M_PI) theta -=3D 2*M_PI;
// the angle we've just extracted has the wrong sign
theta =3D -theta;
return theta;
}
//***********************************************************************=
*****
// dxJointLimitMotor
void dxJointLimitMotor::init (dxWorld *world)
{
vel =3D 0;
fmax =3D 0;
lostop =3D -dInfinity;
histop =3D dInfinity;
fudge_factor =3D 1;
normal_cfm =3D world->global_cfm;
stop_erp =3D world->global_erp;
stop_cfm =3D world->global_cfm;
bounce =3D 0;
limit =3D 0;
limit_err =3D 0;
}
void dxJointLimitMotor::set (int num, dReal value)
{
switch (num) {
case dParamLoStop:
if (value <=3D histop) lostop =3D value;
break;
case dParamHiStop:
if (value >=3D lostop) histop =3D value;
break;
case dParamVel:
vel =3D value;
break;
case dParamFMax:
if (value >=3D 0) fmax =3D value;
break;
case dParamFudgeFactor:
if (value >=3D 0 && value <=3D 1) fudge_factor =3D value;
break;
case dParamBounce:
bounce =3D value;
break;
case dParamCFM:
normal_cfm =3D value;
break;
case dParamStopERP:
stop_erp =3D value;
break;
case dParamStopCFM:
stop_cfm =3D value;
break;
}
}
dReal dxJointLimitMotor::get (int num)
{
switch (num) {
case dParamLoStop: return lostop;
case dParamHiStop: return histop;
case dParamVel: return vel;
case dParamFMax: return fmax;
case dParamFudgeFactor: return fudge_factor;
case dParamBounce: return bounce;
case dParamCFM: return normal_cfm;
case dParamStopERP: return stop_erp;
case dParamStopCFM: return stop_cfm;
default: return 0;
}
}
int dxJointLimitMotor::testRotationalLimit (dReal angle)
{
if (angle <=3D lostop) {
limit =3D 1;
limit_err =3D angle - lostop;
return 1;
}
else if (angle >=3D histop) {
limit =3D 2;
limit_err =3D angle - histop;
return 1;
}
else {
limit =3D 0;
return 0;
}
}
int dxJointLimitMotor::addLimot (dxJoint *joint,
dxJoint::Info2 *info, int row,
dVector3 ax1, int rotational)
{
int srow =3D row * info->rowskip;
// if the joint is powered, or has joint limits, add in the extra row
int powered =3D fmax > 0;
if (powered || limit) {
dReal *J1 =3D rotational ? info->J1a : info->J1l;
dReal *J2 =3D rotational ? info->J2a : info->J2l;
J1[srow+0] =3D ax1[0];
J1[srow+1] =3D ax1[1];
J1[srow+2] =3D ax1[2];
if (joint->node[1].body) {
J2[srow+0] =3D -ax1[0];
J2[srow+1] =3D -ax1[1];
J2[srow+2] =3D -ax1[2];
}
// if we're limited low and high simultaneously, the joint motor is
// ineffective
if (limit && (lostop =3D=3D histop)) powered =3D 0;
if (powered) {
info->cfm[row] =3D normal_cfm;
if (! limit) {
info->c[row] =3D vel;
info->lo[row] =3D -fmax;
info->hi[row] =3D fmax;
}
else {
// the joint is at a limit, AND is being powered. if the joint is
// being powered into the limit then we apply the maximum motor force
// in that direction, because the motor is working against the
// immovable limit. if the joint is being powered away from the limit
// then we have problems because actually we need *two* lcp
// constraints to handle this case. so we fake it and apply some
// fraction of the maximum force. the fraction to use can be set as
// a fudge factor.
dReal fm =3D fmax;
if (vel > 0) fm =3D -fm;
// if we're powering away from the limit, apply the fudge factor
if ((limit=3D=3D1 && vel > 0) || (limit=3D=3D2 && vel < 0)) fm *=3D =
fudge_factor;
if (rotational) {
dBodyAddTorque (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],
-fm*ax1[2]);
if (joint->node[1].body)
dBodyAddTorque (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
}
else {
dBodyAddForce (joint->node[0].body,-fm*ax1[0],-fm*ax1[1],-fm*ax1[2]);
if (joint->node[1].body)
dBodyAddForce (joint->node[1].body,fm*ax1[0],fm*ax1[1],fm*ax1[2]);
}
}
}
if (limit) {
dReal k =3D info->fps * stop_erp;
info->c[row] =3D -k * limit_err;
info->cfm[row] =3D stop_cfm;
if (lostop =3D=3D histop) {
// limited low and high simultaneously
info->lo[row] =3D -dInfinity;
info->hi[row] =3D dInfinity;
}
else {
if (limit =3D=3D 1) {
// low limit
info->lo[row] =3D 0;
info->hi[row] =3D dInfinity;
}
else {
// high limit
info->lo[row] =3D -dInfinity;
info->hi[row] =3D 0;
}
// deal with bounce
if (bounce > 0) {
// calculate joint velocity
dReal vel;
if (rotational) {
vel =3D dDOT(joint->node[0].body->avel,ax1);
if (joint->node[1].body)
vel -=3D dDOT(joint->node[1].body->avel,ax1);
}
else {
vel =3D dDOT(joint->node[0].body->lvel,ax1);
if (joint->node[1].body)
vel -=3D dDOT(joint->node[1].body->lvel,ax1);
}
// only apply bounce if the velocity is incoming, and if the
// resulting c[] exceeds what we already have.
if (limit =3D=3D 1) {
// low limit
if (vel < 0) {
dReal newc =3D -bounce * vel;
if (newc > info->c[row]) info->c[row] =3D newc;
}
}
else {
// high limit - all those computations are reversed
if (vel > 0) {
dReal newc =3D -bounce * vel;
if (newc < info->c[row]) info->c[row] =3D newc;
}
}
}
}
}
return 1;
}
else return 0;
}
//***********************************************************************=
*****
// ball and socket
static void ballInit (dxJointBall *j)
{
dSetZero (j->anchor1,4);
dSetZero (j->anchor2,4);
}
static void ballGetInfo1 (dxJointBall *j, dxJoint::Info1 *info)
{
info->m =3D 3;
info->nub =3D 3;
}
static void ballGetInfo2 (dxJointBall *joint, dxJoint::Info2 *info)
{
setBall (joint,info,joint->anchor1,joint->anchor2);
}
extern "C" void dJointSetBallAnchor (dxJointBall *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dball_vtable,"joint is not a ball");
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
}
extern "C" void dJointGetBallAnchor (dxJointBall *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dball_vtable,"joint is not a ball");
getAnchor (joint,result,joint->anchor1);
}
dxJoint::Vtable __dball_vtable =3D {
sizeof(dxJointBall),
(dxJoint::init_fn*) ballInit,
(dxJoint::getInfo1_fn*) ballGetInfo1,
(dxJoint::getInfo2_fn*) ballGetInfo2,
dJointTypeBall};
//***********************************************************************=
*****
// hinge
static void hingeInit (dxJointHinge *j)
{
dSetZero (j->anchor1,4);
dSetZero (j->anchor2,4);
dSetZero (j->axis1,4);
j->axis1[0] =3D 1;
dSetZero (j->axis2,4);
j->axis2[0] =3D 1;
dSetZero (j->qrel,4);
j->limot.init (j->world);
}
static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info)
{
info->nub =3D 5; =20
// see if joint is powered
if (j->limot.fmax > 0)
info->m =3D 6; // powered hinge needs an extra constraint row
else info->m =3D 5;
// see if we're at a joint limit.
if ((j->limot.lostop >=3D -M_PI || j->limot.histop <=3D M_PI) &&
j->limot.lostop <=3D j->limot.histop) {
dReal angle =3D getHingeAngle =
(j->node[0].body,j->node[1].body,j->axis1,
j->qrel);
if (j->limot.testRotationalLimit (angle)) info->m =3D 6;
}
}
static void hingeGetInfo2 (dxJointHinge *joint, dxJoint::Info2 *info)
{
// set the three ball-and-socket rows
setBall (joint,info,joint->anchor1,joint->anchor2);
// set the two hinge rows. the hinge axis should be the only =
unconstrained
// rotational axis, the angular velocity of the two bodies =
perpendicular to
// the hinge axis should be equal. thus the constraint equations are
// p*w1 - p*w2 =3D 0
// q*w1 - q*w2 =3D 0
// where p and q are unit vectors normal to the hinge axis, and w1 and =
w2
// are the angular velocity vectors of the two bodies.
dVector3 ax1; // length 1 joint axis in global coordinates, from 1st =
body
dVector3 p,q; // plane space vectors for ax1
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
dPlaneSpace (ax1,p,q);
int s3=3D3*info->rowskip;
int s4=3D4*info->rowskip;
info->J1a[s3+0] =3D p[0];
info->J1a[s3+1] =3D p[1];
info->J1a[s3+2] =3D p[2];
info->J1a[s4+0] =3D q[0];
info->J1a[s4+1] =3D q[1];
info->J1a[s4+2] =3D q[2];
if (joint->node[1].body) {
info->J2a[s3+0] =3D -p[0];
info->J2a[s3+1] =3D -p[1];
info->J2a[s3+2] =3D -p[2];
info->J2a[s4+0] =3D -q[0];
info->J2a[s4+1] =3D -q[1];
info->J2a[s4+2] =3D -q[2];
}
// compute the right hand side of the constraint equation. set =
relative
// body velocities along p and q to bring the hinge back into =
alignment.
// if ax1,ax2 are the unit length hinge axes as computed from body1 =
and
// body2, we need to rotate both bodies along the axis u =3D (ax1 x =
ax2).
// if `theta' is the angle between ax1 and ax2, we need an angular =
velocity
// along u to cover angle erp*theta in one step :
// |angular_velocity| =3D angle/time =3D erp*theta / stepsize
// =3D (erp*fps) * theta
// angular_velocity =3D |angular_velocity| * (ax1 x ax2) / |ax1 x =
ax2|
// =3D (erp*fps) * theta * (ax1 x ax2) / =
sin(theta)
// ...as ax1 and ax2 are unit length. if theta is smallish,
// theta ~=3D sin(theta), so
// angular_velocity =3D (erp*fps) * (ax1 x ax2)
// ax1 x ax2 is in the plane space of ax1, so we project the angular
// velocity to p and q to find the right hand side.
dVector3 ax2,b;
if (joint->node[1].body) {
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
}
else {
ax2[0] =3D joint->axis2[0];
ax2[1] =3D joint->axis2[1];
ax2[2] =3D joint->axis2[2];
}
dCROSS (b,=3D,ax1,ax2);
dReal k =3D info->fps * info->erp;
info->c[3] =3D k * dDOT(b,p);
info->c[4] =3D k * dDOT(b,q);
// if the hinge is powered, or has joint limits, add in the stuff
joint->limot.addLimot (joint,info,5,ax1,1);
}
// compute initial relative rotation body1 -> body2, or env -> body1
static void hingeComputeInitialRelativeRotation (dxJointHinge *joint)
{
if (joint->node[0].body) {
if (joint->node[1].body) {
dQMultiply1 =
(joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
}
else {
// set joint->qrel to the transpose of the first body q
joint->qrel[0] =3D joint->node[0].body->q[0];
for (int i=3D1; i<4; i++) joint->qrel[i] =3D =
-joint->node[0].body->q[i];
}
}
}
extern "C" void dJointSetHingeAnchor (dxJointHinge *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
hingeComputeInitialRelativeRotation (joint);
}
extern "C" void dJointSetHingeAxis (dxJointHinge *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
setAxes (joint,x,y,z,joint->axis1,joint->axis2);
hingeComputeInitialRelativeRotation (joint);
}
extern "C" void dJointGetHingeAnchor (dxJointHinge *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
getAnchor (joint,result,joint->anchor1);
}
extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
getAxis (joint,result,joint->axis1);
}
extern "C" void dJointSetHingeParam (dxJointHinge *joint,
int parameter, dReal value)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
joint->limot.set (parameter,value);
}
extern "C" dReal dJointGetHingeParam (dxJointHinge *joint, int =
parameter)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
return joint->limot.get (parameter);
}
extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
hinge");
if (joint->node[0].body) {
return getHingeAngle =
(joint->node[0].body,joint->node[1].body,joint->axis1,
joint->qrel);
}
else return 0;
}
extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__dhinge_vtable,"joint is not a =
Hinge");
if (joint->node[0].body) {
dVector3 axis;
dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
dReal rate =3D dDOT(axis,joint->node[0].body->avel);
if (joint->node[1].body) rate -=3D =
dDOT(axis,joint->node[1].body->avel);
return rate;
}
else return 0;
}
dxJoint::Vtable __dhinge_vtable =3D {
sizeof(dxJointHinge),
(dxJoint::init_fn*) hingeInit,
(dxJoint::getInfo1_fn*) hingeGetInfo1,
(dxJoint::getInfo2_fn*) hingeGetInfo2,
dJointTypeHinge};
//***********************************************************************=
*****
// slider
static void sliderInit (dxJointSlider *j)
{
dSetZero (j->axis1,4);
j->axis1[0] =3D 1;
dSetZero (j->qrel,4);
dSetZero (j->offset,4);
j->limot.init (j->world);
}
extern "C" dReal dJointGetSliderPosition (dxJointSlider *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
// get axis1 in global coordinates
dVector3 ax1,q;
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
if (joint->node[1].body) {
// get body2 + offset point in global coordinates
dMULTIPLY0_331 (q,joint->node[1].body->R,joint->offset);
for (int i=3D0; i<3; i++) q[i] =3D joint->node[0].body->pos[i] - =
q[i] -=20
joint->node[1].body->pos[i];
}
else {
for (int i=3D0; i<3; i++) q[i] =3D joint->node[0].body->pos[i] -
joint->offset[i];
=20
}
return dDOT(ax1,q);
}
extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
// get axis1 in global coordinates
dVector3 ax1;
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
if (joint->node[1].body) {
return dDOT(ax1,joint->node[0].body->lvel) -
dDOT(ax1,joint->node[1].body->lvel);
}
else {
return dDOT(ax1,joint->node[0].body->lvel);
}
}
static void sliderGetInfo1 (dxJointSlider *j, dxJoint::Info1 *info)
{
info->nub =3D 5;
// see if joint is powered
if (j->limot.fmax > 0)
info->m =3D 6; // powered slider needs an extra constraint row
else info->m =3D 5;
// see if we're at a joint limit.
j->limot.limit =3D 0;
if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) &&
j->limot.lostop <=3D j->limot.histop) {
// measure joint position
dReal pos =3D dJointGetSliderPosition (j);
if (pos <=3D j->limot.lostop) {
j->limot.limit =3D 1;
j->limot.limit_err =3D pos - j->limot.lostop;
info->m =3D 6;
}
else if (pos >=3D j->limot.histop) {
j->limot.limit =3D 2;
j->limot.limit_err =3D pos - j->limot.histop;
info->m =3D 6;
}
}
}
static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info)
{
int i,s =3D info->rowskip;
int s2=3D2*s,s3=3D3*s,s4=3D4*s;
// pull out pos and R for both bodies. also get the `connection'
// vector pos2-pos1.
dReal *pos1,*pos2,*R1,*R2;
dVector3 c;
pos1 =3D joint->node[0].body->pos;
R1 =3D joint->node[0].body->R;
if (joint->node[1].body) {
pos2 =3D joint->node[1].body->pos;
R2 =3D joint->node[1].body->R;
for (i=3D0; i<3; i++) c[i] =3D pos2[i] - pos1[i];
}
else {
pos2 =3D 0;
R2 =3D 0;
}
// 3 rows to make body rotations equal
info->J1a[0] =3D 1;
info->J1a[s+1] =3D 1;
info->J1a[s2+2] =3D 1;
if (joint->node[1].body) {
info->J2a[0] =3D -1;
info->J2a[s+1] =3D -1;
info->J2a[s2+2] =3D -1;
}
// remaining two rows. we want: vel2 =3D vel1 + w1 x c ... but this =
would
// result in three equations, so we project along the planespace =
vectors
// so that sliding along the slider axis is disregarded. for symmetry =
we
// also substitute (w1+w2)/2 for w1, as w1 is supposed to equal w2.
dVector3 ax1; // joint axis in global coordinates (unit length)
dVector3 p,q; // plane space of ax1
dMULTIPLY0_331 (ax1,R1,joint->axis1);
dPlaneSpace (ax1,p,q);
if (joint->node[1].body) {
dVector3 tmp;
dCROSS (tmp, =3D REAL(0.5) * ,c,p);
for (i=3D0; i<3; i++) info->J2a[s3+i] =3D tmp[i];
for (i=3D0; i<3; i++) info->J2a[s3+i] =3D tmp[i];
dCROSS (tmp, =3D REAL(0.5) * ,c,q);
for (i=3D0; i<3; i++) info->J2a[s4+i] =3D tmp[i];
for (i=3D0; i<3; i++) info->J2a[s4+i] =3D tmp[i];
for (i=3D0; i<3; i++) info->J2l[s3+i] =3D -p[i];
for (i=3D0; i<3; i++) info->J2l[s4+i] =3D -q[i];
}
for (i=3D0; i<3; i++) info->J1l[s3+i] =3D p[i];
for (i=3D0; i<3; i++) info->J1l[s4+i] =3D q[i];
// compute the right hand side. the first three elements will result =
in
// relative angular velocity of the two bodies - this is set to bring =
them
// back into alignment. the correcting angular velocity is=20
// |angular_velocity| =3D angle/time =3D erp*theta / stepsize
// =3D (erp*fps) * theta
// angular_velocity =3D |angular_velocity| * u
// =3D (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's =
frame
// to qrel with respect to body 1's frame. using a small angle =
approximation
// for sin(), this gives
// angular_velocity =3D (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two =
bodies is
// q =3D [cos(theta/2) sin(theta/2)*u] =3D [s v]
// get qerr =3D relative rotation (rotation error) between two bodies
dQuaternion qerr,e;
if (joint->node[1].body) {
dQuaternion qq;
dQMultiply1 (qq,joint->node[0].body->q,joint->node[1].body->q);
dQMultiply2 (qerr,qq,joint->qrel);
}
else {
dQMultiply3 (qerr,joint->node[0].body->q,joint->qrel);
}
if (qerr[0] < 0) {
qerr[1] =3D -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] =3D -qerr[2];
qerr[3] =3D -qerr[3];
}
dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD =
padding!
dReal k =3D info->fps * info->erp;
info->c[0] =3D 2*k * e[0];
info->c[1] =3D 2*k * e[1];
info->c[2] =3D 2*k * e[2];
// compute last two elements of right hand side. we want to align the =
offset
// point (in body 2's frame) with the center of body 1.
if (joint->node[1].body) {
dVector3 ofs; // offset point in global coordinates
dMULTIPLY0_331 (ofs,R2,joint->offset);
for (i=3D0; i<3; i++) c[i] +=3D ofs[i];
info->c[3] =3D k * dDOT(p,c);
info->c[4] =3D k * dDOT(q,c);
}
else {
dVector3 ofs; // offset point in global coordinates
for (i=3D0; i<3; i++) ofs[i] =3D joint->offset[i] - pos1[i];
info->c[3] =3D k * dDOT(p,ofs);
info->c[4] =3D k * dDOT(q,ofs);
}
// if the slider is powered, or has joint limits, add in the extra row
joint->limot.addLimot (joint,info,5,ax1,0);
}
extern "C" void dJointSetSliderAxis (dxJointSlider *joint,
dReal x, dReal y, dReal z)
{
int i;
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
setAxes (joint,x,y,z,joint->axis1,0);
// compute initial relative rotation body1 -> body2, or env -> body1
// also compute center of body1 w.r.t body 2
if (joint->node[1].body) {
dQMultiply1 =
(joint->qrel,joint->node[0].body->q,joint->node[1].body->q);
dVector3 c;
for (i=3D0; i<3; i++)
c[i] =3D joint->node[0].body->pos[i] - =
joint->node[1].body->pos[i];
dMULTIPLY1_331 (joint->offset,joint->node[1].body->R,c);
}
else {
// set joint->qrel to the transpose of the first body's q
joint->qrel[0] =3D joint->node[0].body->q[0];
for (i=3D1; i<4; i++) joint->qrel[i] =3D -joint->node[0].body->q[i];
for (i=3D0; i<3; i++) joint->offset[i] =3D =
joint->node[0].body->pos[i];
}
}
extern "C" void dJointGetSliderAxis (dxJointSlider *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
getAxis (joint,result,joint->axis1);
}
extern "C" void dJointSetSliderParam (dxJointSlider *joint,
int parameter, dReal value)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
joint->limot.set (parameter,value);
}
extern "C" dReal dJointGetSliderParam (dxJointSlider *joint, int =
parameter)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dslider_vtable,"joint is not a =
slider");
return joint->limot.get (parameter);
}
dxJoint::Vtable __dslider_vtable =3D {
sizeof(dxJointSlider),
(dxJoint::init_fn*) sliderInit,
(dxJoint::getInfo1_fn*) sliderGetInfo1,
(dxJoint::getInfo2_fn*) sliderGetInfo2,
dJointTypeSlider};
//***********************************************************************=
*****
// contact
static void contactInit (dxJointContact *j)
{
// default frictionless contact. hmmm, this info gets overwritten =
straight
// away anyway, so why bother?
j->contact.surface.mode =3D 0;
j->contact.surface.mu =3D 0;
dSetZero (j->contact.geom.pos,4);
dSetZero (j->contact.geom.normal,4);
j->contact.geom.depth =3D 0;
}
static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info)
{
// make sure mu's >=3D 0, then calculate number of constraint rows and =
number
// of unbounded rows.
int m =3D 1, nub=3D0;
if (j->contact.surface.mu < 0) j->contact.surface.mu =3D 0;
if (j->contact.surface.mode & dContactMu2) {
if (j->contact.surface.mu > 0) m++;
if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 =3D 0;
if (j->contact.surface.mu2 > 0) m++;
if (j->contact.surface.mu =3D=3D dInfinity) nub ++;
if (j->contact.surface.mu2 =3D=3D dInfinity) nub ++;
}
else {
if (j->contact.surface.mu > 0) m +=3D 2;
if (j->contact.surface.mu =3D=3D dInfinity) nub +=3D 2;
}
j->the_m =3D m;
info->m =3D m;
info->nub =3D nub;
}
static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info)
{
int i,s =3D info->rowskip;
int s2 =3D 2*s;
// get normal, with sign adjusted for body1/body2 polarity
dVector3 normal;
if (j->flags & dJOINT_REVERSE) {
normal[0] =3D j->contact.geom.normal[0];
normal[1] =3D j->contact.geom.normal[1];
normal[2] =3D j->contact.geom.normal[2];
}
else {
normal[0] =3D - j->contact.geom.normal[0];
normal[1] =3D - j->contact.geom.normal[1];
normal[2] =3D - j->contact.geom.normal[2];
}
normal[3] =3D 0; // @@@ hmmm
// c1,c2 =3D contact points with respect to body PORs
dVector3 c1,c2;
for (i=3D0; i<3; i++) c1[i] =3D j->contact.geom.pos[i] - =
j->node[0].body->pos[i];
// set jacobian for normal
info->J1l[0] =3D normal[0];
info->J1l[1] =3D normal[1];
info->J1l[2] =3D normal[2];
dCROSS (info->J1a,=3D,c1,normal);
if (j->node[1].body) {
for (i=3D0; i<3; i++) c2[i] =3D j->contact.geom.pos[i] -
j->node[1].body->pos[i];
info->J2l[0] =3D -normal[0];
info->J2l[1] =3D -normal[1];
info->J2l[2] =3D -normal[2];
dCROSS (info->J2a,=3D -,c2,normal);
}
// set right hand side and cfm value for normal
dReal erp =3D info->erp;
if (j->contact.surface.mode & dContactSoftERP)
erp =3D j->contact.surface.soft_erp;
dReal k =3D info->fps * erp;
info->c[0] =3D k*j->contact.geom.depth;
if (j->contact.surface.mode & dContactSoftCFM)
info->cfm[0] =3D j->contact.surface.soft_cfm;
// deal with bounce
if (j->contact.surface.mode & dContactBounce) {
// calculate outgoing velocity (-ve for incoming contact)
dReal outgoing =3D dDOT(info->J1l,j->node[0].body->lvel) +
dDOT(info->J1a,j->node[0].body->avel);
if (j->node[1].body) {
outgoing +=3D dDOT(info->J2l,j->node[1].body->lvel) +
dDOT(info->J2a,j->node[1].body->avel);
}
// only apply bounce if the outgoing velocity is greater than the
// threshold, and if the resulting c[0] exceeds what we already =
have.
if (j->contact.surface.bounce_vel >=3D 0 &&
(-outgoing) > j->contact.surface.bounce_vel) {
dReal newc =3D - j->contact.surface.bounce * outgoing;
if (newc > info->c[0]) info->c[0] =3D newc;
}
}
// set LCP limits for normal
info->lo[0] =3D 0;
info->hi[0] =3D dInfinity;
// now do jacobian for tangential forces
dVector3 t1,t2; // two vectors tangential to normal
// first friction direction
if (j->the_m >=3D 2) {
if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ?
t1[0] =3D j->contact.fdir1[0];
t1[1] =3D j->contact.fdir1[1];
t1[2] =3D j->contact.fdir1[2];
dCROSS (t2,=3D,normal,t1);
}
else {
dPlaneSpace (normal,t1,t2);
}
info->J1l[s+0] =3D t1[0];
info->J1l[s+1] =3D t1[1];
info->J1l[s+2] =3D t1[2];
dCROSS (info->J1a+s,=3D,c1,t1);
if (j->node[1].body) {
info->J2l[s+0] =3D -t1[0];
info->J2l[s+1] =3D -t1[1];
info->J2l[s+2] =3D -t1[2];
dCROSS (info->J2a+s,=3D -,c2,t1);
}
// set right hand side
if (j->contact.surface.mode & dContactMotion1) {
info->c[1] =3D j->contact.surface.motion1;
}
// set LCP bounds and friction index. this depends on the =
approximation
// mode
info->lo[1] =3D -j->contact.surface.mu;
info->hi[1] =3D j->contact.surface.mu;
if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] =3D =
0;
// set slip (constraint force mixing)
if (j->contact.surface.mode & dContactSlip1)
info->cfm[1] =3D j->contact.surface.slip1;
}
// second friction direction
if (j->the_m >=3D 3) {
info->J1l[s2+0] =3D t2[0];
info->J1l[s2+1] =3D t2[1];
info->J1l[s2+2] =3D t2[2];
dCROSS (info->J1a+s2,=3D,c1,t2);
if (j->node[1].body) {
info->J2l[s2+0] =3D -t2[0];
info->J2l[s2+1] =3D -t2[1];
info->J2l[s2+2] =3D -t2[2];
dCROSS (info->J2a+s2,=3D -,c2,t2);
}
// set right hand side
if (j->contact.surface.mode & dContactMotion2) {
info->c[2] =3D j->contact.surface.motion2;
}
// set LCP bounds and friction index. this depends on the =
approximation
// mode
if (j->contact.surface.mode & dContactMu2) {
info->lo[2] =3D -j->contact.surface.mu2;
info->hi[2] =3D j->contact.surface.mu2;
}
else {
info->lo[2] =3D -j->contact.surface.mu;
info->hi[2] =3D j->contact.surface.mu;
}
if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] =3D =
0;
// set slip (constraint force mixing)
if (j->contact.surface.mode & dContactSlip2)
info->cfm[2] =3D j->contact.surface.slip2;
}
}
dxJoint::Vtable __dcontact_vtable =3D {
sizeof(dxJointContact),
(dxJoint::init_fn*) contactInit,
(dxJoint::getInfo1_fn*) contactGetInfo1,
(dxJoint::getInfo2_fn*) contactGetInfo2,
dJointTypeContact};
//***********************************************************************=
*****
// hinge 2. note that this joint must be attached to two bodies for it =
to work
static dReal measureHinge2Angle (dxJointHinge2 *joint)
{
dVector3 a1,a2;
dMULTIPLY0_331 (a1,joint->node[1].body->R,joint->axis2);
dMULTIPLY1_331 (a2,joint->node[0].body->R,a1);
dReal x =3D dDOT(joint->v1,a2);
dReal y =3D dDOT(joint->v2,a2);
return -dAtan2 (y,x);
}
static void hinge2Init (dxJointHinge2 *j)
{
dSetZero (j->anchor1,4);
dSetZero (j->anchor2,4);
dSetZero (j->axis1,4);
j->axis1[0] =3D 1;
dSetZero (j->axis2,4);
j->axis2[1] =3D 1;
j->c0 =3D 0;
j->s0 =3D 0;
dSetZero (j->v1,4);
j->v1[0] =3D 1;
dSetZero (j->v2,4);
j->v2[1] =3D 1;
j->limot1.init (j->world);
j->limot2.init (j->world);
j->susp_erp =3D j->world->global_erp;
j->susp_cfm =3D j->world->global_cfm;
j->flags |=3D dJOINT_TWOBODIES;
}
static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info)
{
info->m =3D 4;
info->nub =3D 4;
// see if we're powered or at a joint limit for axis 1
int atlimit=3D0;
if ((j->limot1.lostop >=3D -M_PI || j->limot1.histop <=3D M_PI) &&
j->limot1.lostop <=3D j->limot1.histop) {
dReal angle =3D measureHinge2Angle (j);
if (j->limot1.testRotationalLimit (angle)) atlimit =3D 1;
}
if (atlimit || j->limot1.fmax > 0) info->m++;
// see if we're powering axis 2 (we currently never limit this axis)
j->limot2.limit =3D 0;
if (j->limot2.fmax > 0) info->m++;
}
// macro that computes ax1,ax2 =3D axis 1 and 2 in global coordinates =
(they are
// relative to body 1 and 2 initially) and then computes the constrained
// rotational axis as the cross product of ax1 and ax2.
// the sin and cos of the angle between axis 1 and 2 is computed, this =
comes
// from dot and cross product rules.
#define HINGE2_GET_AXIS_INFO(axis,sin_angle,cos_angle) \
dVector3 ax1,ax2; \
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1); \
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2); \
dCROSS (axis,=3D,ax1,ax2); \
sin_angle =3D dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + =
axis[2]*axis[2]); \
cos_angle =3D dDOT (ax1,ax2);
static void hinge2GetInfo2 (dxJointHinge2 *joint, dxJoint::Info2 *info)
{
// get information we need to set the hinge row
dReal s,c;
dVector3 q;
HINGE2_GET_AXIS_INFO (q,s,c);
dNormalize3 (q); // @@@ quicker: divide q by s ?
// set the three ball-and-socket rows (aligned to the suspension axis =
ax1)
setBall2 =
(joint,info,joint->anchor1,joint->anchor2,ax1,joint->susp_erp);
// set the hinge row
int s3=3D3*info->rowskip;
info->J1a[s3+0] =3D q[0];
info->J1a[s3+1] =3D q[1];
info->J1a[s3+2] =3D q[2];
if (joint->node[1].body) {
info->J2a[s3+0] =3D -q[0];
info->J2a[s3+1] =3D -q[1];
info->J2a[s3+2] =3D -q[2];
}
// compute the right hand side for the constrained rotational DOF.
// axis 1 and axis 2 are separated by an angle `theta'. the desired
// separation angle is theta0. sin(theta0) and cos(theta0) are =
recorded
// in the joint structure. the correcting angular velocity is:
// |angular_velocity| =3D angle/time =3D erp*(theta0-theta) / =
stepsize
// =3D (erp*fps) * (theta0-theta)
// (theta0-theta) can be computed using the following =
small-angle-difference
// approximation:
// theta0-theta ~=3D tan(theta0-theta)
// =3D sin(theta0-theta)/cos(theta0-theta)
// =3D (c*s0 - s*c0) / (c*c0 + s*s0)
// =3D c*s0 - s*c0 assuming c*c0 + s*s0 ~=3D 1
// where c =3D cos(theta), s =3D sin(theta)
// c0 =3D cos(theta0), s0 =3D sin(theta0)
dReal k =3D info->fps * info->erp;
info->c[3] =3D k * (joint->c0 * s - joint->s0 * c);
// if the axis1 hinge is powered, or has joint limits, add in more =
stuff
int row =3D 4 + joint->limot1.addLimot (joint,info,4,ax1,1);
// if the axis2 hinge is powered, add in more stuff
joint->limot2.addLimot (joint,info,row,ax2,1);
// set parameter for the suspension
info->cfm[0] =3D joint->susp_cfm;
}
// compute vectors v1 and v2 (embedded in body1), used to measure angle
// between body 1 and body 2
static void makeHinge2V1andV2 (dxJointHinge2 *joint)
{
if (joint->node[0].body) {
// get axis 1 and 2 in global coords
dVector3 ax1,ax2,v;
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
// don't do anything if the axis1 or axis2 vectors are zero or the =
same
if ((ax1[0]=3D=3D0 && ax1[1]=3D=3D0 && ax1[2]=3D=3D0) ||
(ax2[0]=3D=3D0 && ax2[1]=3D=3D0 && ax2[2]=3D=3D0) ||
(ax1[0]=3D=3Dax2[0] && ax1[1]=3D=3Dax2[1] && ax1[2]=3D=3Dax2[2])) =
return;
// modify axis 2 so it's perpendicular to axis 1
dReal k =3D dDOT(ax1,ax2);
for (int i=3D0; i<3; i++) ax2[i] -=3D k*ax1[i];
dNormalize3 (ax2);
// make v1 =3D modified axis2, v2 =3D axis1 x (modified axis2)
dCROSS (v,=3D,ax1,ax2);
dMULTIPLY1_331 (joint->v1,joint->node[0].body->R,ax2);
dMULTIPLY1_331 (joint->v2,joint->node[0].body->R,v);
}
}
extern "C" void dJointSetHinge2Anchor (dxJointHinge2 *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
makeHinge2V1andV2 (joint);
}
extern "C" void dJointSetHinge2Axis1 (dxJointHinge2 *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[0].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
joint->axis1[3] =3D 0;
// compute the sin and cos of the angle between axis 1 and axis 2
dVector3 ax;
HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
}
makeHinge2V1andV2 (joint);
}
extern "C" void dJointSetHinge2Axis2 (dxJointHinge2 *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[1].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
joint->axis1[3] =3D 0;
// compute the sin and cos of the angle between axis 1 and axis 2
dVector3 ax;
HINGE2_GET_AXIS_INFO(ax,joint->s0,joint->c0);
}
makeHinge2V1andV2 (joint);
}
extern "C" void dJointSetHinge2Param (dxJointHinge2 *joint,
int parameter, dReal value)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if ((parameter & 0xff00) =3D=3D 0x100) {
joint->limot2.set (parameter & 0xff,value);
}
else {
if (parameter =3D=3D dParamSuspensionERP) joint->susp_erp =3D value;
else if (parameter =3D=3D dParamSuspensionCFM) joint->susp_cfm =3D =
value;
else joint->limot1.set (parameter,value);
}
}
extern "C" void dJointGetHinge2Anchor (dxJointHinge2 *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
getAnchor (joint,result,joint->anchor1);
}
extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[0].body) {
dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis1);
}
}
extern "C" void dJointGetHinge2Axis2 (dxJointHinge2 *joint, dVector3 =
result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[1].body) {
dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis2);
}
}
extern "C" dReal dJointGetHinge2Param (dxJointHinge2 *joint, int =
parameter)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if ((parameter & 0xff00) =3D=3D 0x100) {
return joint->limot2.get (parameter & 0xff);
}
else {
if (parameter =3D=3D dParamSuspensionERP) return joint->susp_erp;
else if (parameter =3D=3D dParamSuspensionCFM) return =
joint->susp_cfm;
else return joint->limot1.get (parameter);
}
}
extern "C" dReal dJointGetHinge2Angle1 (dxJointHinge2 *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[0].body) return measureHinge2Angle (joint);
else return 0;
}
extern "C" dReal dJointGetHinge2Angle1Rate (dxJointHinge2 *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[0].body) {
dVector3 axis;
dMULTIPLY0_331 (axis,joint->node[0].body->R,joint->axis1);
dReal rate =3D dDOT(axis,joint->node[0].body->avel);
if (joint->node[1].body) rate -=3D =
dDOT(axis,joint->node[1].body->avel);
return rate;
}
else return 0;
}
extern "C" dReal dJointGetHinge2Angle2Rate (dxJointHinge2 *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dhinge2_vtable,"joint is not a =
hinge2");
if (joint->node[0].body && joint->node[1].body) {
dVector3 axis;
dMULTIPLY0_331 (axis,joint->node[1].body->R,joint->axis2);
dReal rate =3D dDOT(axis,joint->node[0].body->avel);
if (joint->node[1].body) rate -=3D =
dDOT(axis,joint->node[1].body->avel);
return rate;
}
else return 0;
}
dxJoint::Vtable __dhinge2_vtable =3D {
sizeof(dxJointHinge2),
(dxJoint::init_fn*) hinge2Init,
(dxJoint::getInfo1_fn*) hinge2GetInfo1,
(dxJoint::getInfo2_fn*) hinge2GetInfo2,
dJointTypeHinge2};
//***********************************************************************=
*****
// universal
static void universalInit (dxJointUniversal *j)
{
dSetZero (j->anchor1,4);
dSetZero (j->anchor2,4);
dSetZero (j->axis1,4);
j->axis1[0] =3D 1;
dSetZero (j->axis2,4);
j->axis2[1] =3D 1;
}
static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 =
*info)
{
info->nub =3D 4;
info->m =3D 4;
}
static void universalGetInfo2 (dxJointUniversal *joint, dxJoint::Info2 =
*info)
{
// set the three ball-and-socket rows
setBall (joint,info,joint->anchor1,joint->anchor2);
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the =
constraint
// equation is
// p*w1 - p*w2 =3D 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
// length 1 vector perpendicular to ax1 and ax2. Neither body can =
rotate
// about this.
dVector3 p;
=20
// This says "ax1 =3D joint->node[0].body->R * joint->axis1"
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
if (joint->node[1].body) {
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
}
else {
ax2[0] =3D joint->axis2[0];
ax2[1] =3D joint->axis2[1];
ax2[2] =3D joint->axis2[2];
}
// if ax1 and ax2 are almost parallel, p won't be perpendicular to =
them.
// Is there some more robust way to do this?
dCROSS(p, =3D, ax1, ax2);
dNormalize3(p);
=20
int s3=3D3*info->rowskip;
info->J1a[s3+0] =3D p[0];
info->J1a[s3+1] =3D p[1];
info->J1a[s3+2] =3D p[2];
if (joint->node[1].body) {
info->J2a[s3+0] =3D -p[0];
info->J2a[s3+1] =3D -p[1];
info->J2a[s3+2] =3D -p[2];
}
// compute the right hand side of the constraint equation. set =
relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| =3D angle/time =3D erp*(theta - Pi/2) / =
stepsize
// =3D (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,=20
// theta - Pi/2 ~=3D cos(theta), so
// |angular_velocity| =3D (erp*fps) * (ax1 dot ax2)
info->c[3] =3D info->fps * info->erp * - dDOT(ax1, ax2);
}
extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
}
extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
if (joint->node[0].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
}
joint->axis1[3] =3D 0;
}
extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
if (joint->node[1].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
}
joint->axis2[3] =3D 0;
}
extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
getAnchor (joint,result,joint->anchor1);
}
extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
if (joint->node[0].body) {
dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1);
}
}
extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversal_vtable,"joint is not a =
universal");
if (joint->node[1].body) {
dMULTIPLY0_331 (result, joint->node[1].body->R, joint->axis2);
}
}
dxJoint::Vtable __duniversal_vtable =3D {
sizeof(dxJointUniversal),
(dxJoint::init_fn*) universalInit,
(dxJoint::getInfo1_fn*) universalGetInfo1,
(dxJoint::getInfo2_fn*) universalGetInfo2,
dJointTypeUniversal};
// bovet+
//***********************************************************************=
*****
// universal spring
static void universalSpringInit (dxJointUniversalSpring *j)
{
universalInit(j);
j->kp =3D j->akp =3D 0;
j->kd =3D j->akd =3D 0;
j->dcfm =3D 1;
j->lfps =3D -1;
}
static void universalSpringGetInfo1 (dxJointUniversalSpring *j, =
dxJoint::Info1 *info)
{
info->nub =3D info->m =3D 6;
}
static void universalSpringGetInfo2 (dxJointUniversalSpring *joint, =
dxJoint::Info2 *info)
{
// set the three ball-and-socket rows
setBall (joint,info,joint->anchor1,joint->anchor2);
// set the universal joint row. the angular velocity about an axis
// perpendicular to both joint axes should be equal. thus the =
constraint
// equation is
// p*w1 - p*w2 =3D 0
// where p is a vector normal to both joint axes, and w1 and w2
// are the angular velocity vectors of the two bodies.
// length 1 joint axis in global coordinates, from each body
dVector3 ax1, ax2;
// length 1 vector perpendicular to ax1 and ax2. Neither body can =
rotate
// about this.
dVector3 p;
=20
// This says "ax1 =3D joint->node[0].body->R * joint->axis1"
dMULTIPLY0_331 (ax1,joint->node[0].body->R,joint->axis1);
if (joint->node[1].body) {
dMULTIPLY0_331 (ax2,joint->node[1].body->R,joint->axis2);
}
else {
ax2[0] =3D joint->axis2[0];
ax2[1] =3D joint->axis2[1];
ax2[2] =3D joint->axis2[2];
}
// if ax1 and ax2 are almost parallel, p won't be perpendicular to =
them.
// Is there some more robust way to do this?
dCROSS(p, =3D, ax1, ax2);
dNormalize3(p);
=20
int s3=3D3*info->rowskip;
info->J1a[s3+0] =3D p[0];
info->J1a[s3+1] =3D p[1];
info->J1a[s3+2] =3D p[2];
if (joint->node[1].body) {
info->J2a[s3+0] =3D -p[0];
info->J2a[s3+1] =3D -p[1];
info->J2a[s3+2] =3D -p[2];
}
// compute the right hand side of the constraint equation. set =
relative
// body velocities along p to bring the axes back to perpendicular.
// If ax1, ax2 are unit length joint axes as computed from body1 and
// body2, we need to rotate both bodies along the axis p. If theta
// is the angle between ax1 and ax2, we need an angular velocity
// along p to cover the angle erp * (theta - Pi/2) in one step:
//
// |angular_velocity| =3D angle/time =3D erp*(theta - Pi/2) / =
stepsize
// =3D (erp*fps) * (theta - Pi/2)
//
// if theta is close to Pi/2,=20
// theta - Pi/2 ~=3D cos(theta), so
// |angular_velocity| =3D (erp*fps) * (ax1 dot ax2)
=20
if (info->fps !=3D joint->lfps) {
dReal h =3D 1 / info->fps;
if ((joint->kp =3D=3D 0) && (joint->kd =3D=3D 0)) {
joint->leps =3D info->erp;
joint->lcfm =3D info->cfm[3];
} else {
joint->leps =3D h * joint->kp / (h * joint->kp + joint->kd);
joint->lcfm =3D 1 / (h * joint->kp + joint->kd);
}
joint->leps *=3D info->fps;
=20
if ((joint->akp =3D=3D 0) && (joint->akd =3D=3D 0)) {
joint->aeps =3D info->erp;
joint->acfm =3D info->cfm[4];
} else {
joint->aeps =3D h * joint->akp / (h * joint->akp + joint->akd);
joint->acfm =3D 1 / (h * joint->akp + joint->akd);
}
joint->aeps *=3D info->fps;
=20
=20
joint->lfps =3D info->fps;
}
info->c[3] =3D joint->leps * - dDOT(ax1, ax2);
info->cfm[3] =3D joint->lcfm;
=20
// angular spring
// again q*w1 - q*w2 =3D 0 but now with q =3D a1 x a2 where ai are the =
main axes
int s4 =3D 4*info->rowskip;
dVector3 a1, a2, q;
dReal qn;
dMULTIPLY0_331 (a1,joint->node[0].body->R,joint->mainAxis1);
if(joint->node[1].body)
dMULTIPLY0_331 (a2,joint->node[1].body->R,joint->mainAxis2);
else {
a2[0] =3D joint->mainAxis2[0];
a2[1] =3D joint->mainAxis2[1];
a2[2] =3D joint->mainAxis2[2];
}
// dNormalize3(a1);
// dNormalize3(a2);
dVector3 va, vb;
dCROSS(va, =3D, a1, a2);
qn =3D dSqrt(dDOT(va, va));
dCROSS(vb, =3D, p, va);
dCROSS(q, =3D, vb, p);
dNormalize3(q);
info->J1a[s4+0] =3D q[0];
info->J1a[s4+1] =3D q[1];
info->J1a[s4+2] =3D q[2];
if (joint->node[1].body) {
info->J2a[s4+0] =3D -q[0];
info->J2a[s4+1] =3D -q[1];
info->J2a[s4+2] =3D -q[2];
}
// right hand side
// bring axes a1 and a2 back to parallel
info->c[4] =3D joint->aeps * qn;
info->cfm[4] =3D joint->acfm;
=20
// "dissipative term"
// r*w1 - r*w2 =3D 0 with r =3D p x q
dVector3 r;
dCROSS(r, =3D, p, q);
dNormalize3(r);
=20
int s5 =3D 5*info->rowskip;
info->J1a[s5+0] =3D r[0];
info->J1a[s5+1] =3D r[1];
info->J1a[s5+2] =3D r[2];
if (joint->node[1].body) {
info->J2a[s5+0] =3D -r[0];
info->J2a[s5+1] =3D -r[1];
info->J2a[s5+2] =3D -r[2];
}
info->cfm[5] =3D joint->dcfm;
info->c[5] =3D 0;
}
extern "C" void dJointSetUniversalSpringAnchor (dxJointUniversalSpring =
*joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2);
}
extern "C" void dJointSetUniversalSpringMainAxis (dxJointUniversalSpring =
*joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
if (joint->node[0].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->mainAxis1,joint->node[0].body->R,q);
if (joint->node[1].body)
dMULTIPLY1_331 (joint->mainAxis2,joint->node[1].body->R,q);
else {
joint->mainAxis2[0] =3D x;
joint->mainAxis2[1] =3D y;
joint->mainAxis2[2] =3D z;
}
}
joint->mainAxis1[3] =3D joint->mainAxis2[3] =3D 0;
}
extern "C" void dJointGetUniversalSpringMainAxis (dxJointUniversalSpring =
*joint,
int index, dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
dUASSERT(index =3D=3D 0 || index =3D=3D 1, "bad index argument");
if (index =3D=3D 0) {
result[0] =3D joint->mainAxis1[0];
result[1] =3D joint->mainAxis1[1];
result[2] =3D joint->mainAxis1[2];
} else {
result[0] =3D joint->mainAxis2[0];
result[1] =3D joint->mainAxis2[1];
result[2] =3D joint->mainAxis2[2];
}
}
extern "C" void dJointSetUniversalSpringAxis1 (dxJointUniversalSpring =
*joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
if (joint->node[0].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q);
}
joint->axis1[3] =3D 0;
}
extern "C" void dJointSetUniversalSpringAxis2 (dxJointUniversalSpring =
*joint,
dReal x, dReal y, dReal z)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
if (joint->node[1].body) {
dReal q[4];
q[0] =3D x;
q[1] =3D y;
q[2] =3D z;
q[3] =3D 0;
dNormalize3 (q);
dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q);
}
joint->axis2[3] =3D 0;
}
extern "C" void dJointSetUniversalSpringConstant (dxJointUniversalSpring =
*joint, dReal kp)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
joint->kp =3D kp;
}
extern "C" void dJointSetUniversalSpringDamping (dxJointUniversalSpring =
*joint, dReal kd)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
joint->kd =3D kd;
}
extern "C" void dJointSetUniversalSpringAngularConstant =
(dxJointUniversalSpring *joint, dReal kp)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
joint->akp =3D kp;
}
extern "C" void dJointSetUniversalSpringAngularDamping =
(dxJointUniversalSpring *joint, dReal kd)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
joint->akd =3D kd;
}
extern "C" void dJointSetUniversalSpringPrecessionDampingCFM =
(dxJointUniversalSpring *joint,
dReal cfm)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
joint->dcfm =3D cfm;
}
extern "C" void dJointGetUniversalSpringAnchor (dxJointUniversalSpring =
*joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
getAnchor (joint,result,joint->anchor1);
}
extern "C" void dJointGetUniversalSpringAxis1 (dxJointUniversalSpring =
*joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
if (joint->node[0].body) {
dMULTIPLY0_331 (result, joint->node[0].body->R, joint->axis1);
}
}
extern "C" void dJointGetUniversalSpringAxis2 (dxJointUniversalSpring =
*joint,
dVector3 result)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(result,"bad result argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
if (joint->node[1].body) {
dMULTIPLY0_331 (result, joint->node[1].body->R, joint->axis2);
}
}
extern "C" dReal dJointGetUniversalSpringConstant =
(dxJointUniversalSpring *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
return joint->kp;
}
extern "C" dReal dJointGetUniversalSpringDamping (dxJointUniversalSpring =
*joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
return joint->kd;
}
extern "C" dReal dJointGetUniversalSpringAngularConstant =
(dxJointUniversalSpring *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
return joint->akp;
}
extern "C" dReal dJointGetUniversalSpringAngularDamping =
(dxJointUniversalSpring *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
return joint->akd;
}
extern "C" dReal dJointGetUniversalSpringPrecessionDampingCFM =
(dxJointUniversalSpring *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__duniversalspring_vtable,"joint is not =
a universal spring");
return joint->dcfm;
}
dxJoint::Vtable __duniversalspring_vtable =3D {
sizeof(dxJointUniversalSpring),
(dxJoint::init_fn*) universalSpringInit,
(dxJoint::getInfo1_fn*) universalSpringGetInfo1,
(dxJoint::getInfo2_fn*) universalSpringGetInfo2,
dJointTypeUniversalSpring};
// bovet-
//***********************************************************************=
*****
// angular motor
static void amotorInit (dxJointAMotor *j)
{
int i;
j->num =3D 0;
j->mode =3D dAMotorUser;
for (i=3D0; i<3; i++) {
j->rel[i] =3D 0;
dSetZero (j->axis[i],4);
j->limot[i].init (j->world);
j->angle[i] =3D 0;
}
dSetZero (j->reference1,4);
dSetZero (j->reference2,4);
j->flags |=3D dJOINT_TWOBODIES;
}
// compute the 3 axes in global coordinates
static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 =
ax[3])
{
if (joint->mode =3D=3D dAMotorEuler) {
// special handling for euler mode
dMULTIPLY0_331 (ax[0],joint->node[0].body->R,joint->axis[0]);
dMULTIPLY0_331 (ax[2],joint->node[1].body->R,joint->axis[2]);
dCROSS (ax[1],=3D,ax[2],ax[0]);
dNormalize3 (ax[1]);
}
else {
for (int i=3D0; i < joint->num; i++) {
if (joint->rel[i] =3D=3D 1) {
// relative to b1
dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]);
}
if (joint->rel[i] =3D=3D 2) {
// relative to b2
dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]);
}
else {
// global - just copy it
ax[i][0] =3D joint->axis[i][0];
ax[i][1] =3D joint->axis[i][1];
ax[i][2] =3D joint->axis[i][2];
}
}
}
}
static void amotorComputeEulerAngles (dxJointAMotor *joint, dVector3 =
ax[3])
{
// assumptions:
// global axes already calculated --> ax
// axis[0] is relative to body 1 --> global ax[0]
// axis[2] is relative to body 2 --> global ax[2]
// ax[1] =3D ax[2] x ax[0]
// original ax[0] and ax[2] are perpendicular
// reference1 is perpendicular to ax[0] (in body 1 frame)
// reference2 is perpendicular to ax[2] (in body 2 frame)
// all ax[] and reference vectors are unit length
// calculate references in global frame
dVector3 ref1,ref2;
dMULTIPLY0_331 (ref1,joint->node[0].body->R,joint->reference1);
dMULTIPLY0_331 (ref2,joint->node[1].body->R,joint->reference2);
// get q perpendicular to both ax[0] and ref1, get first euler angle
dVector3 q;
dCROSS (q,=3D,ax[0],ref1);
joint->angle[0] =3D -dAtan2 (dDOT(ax[2],q),dDOT(ax[2],ref1));
// get q perpendicular to both ax[0] and ax[1], get second euler angle
dCROSS (q,=3D,ax[0],ax[1]);
joint->angle[1] =3D -dAtan2 (dDOT(ax[2],ax[0]),dDOT(ax[2],q));
// get q perpendicular to both ax[1] and ax[2], get third euler angle
dCROSS (q,=3D,ax[1],ax[2]);
joint->angle[2] =3D -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q));
}
// set the reference vectors as follows:
// * reference1 =3D current axis[2] relative to body 1
// * reference2 =3D current axis[0] relative to body 2
// this assumes that:
// * axis[0] is relative to body 1
// * axis[2] is relative to body 2
static void amotorSetEulerReferenceVectors (dxJointAMotor *j)
{
if (j->node[0].body && j->node[1].body) {
dVector3 r; // axis[2] and axis[0] in global coordinates
dMULTIPLY0_331 (r,j->node[1].body->R,j->axis[2]);
dMULTIPLY1_331 (j->reference1,j->node[0].body->R,r);
dMULTIPLY0_331 (r,j->node[0].body->R,j->axis[0]);
dMULTIPLY1_331 (j->reference2,j->node[1].body->R,r);
}
}
static void amotorGetInfo1 (dxJointAMotor *j, dxJoint::Info1 *info)
{
info->m =3D 0;
info->nub =3D 0;
// compute the axes and angles, if in euler mode
if (j->mode =3D=3D dAMotorEuler) {
dVector3 ax[3];
amotorComputeGlobalAxes (j,ax);
amotorComputeEulerAngles (j,ax);
}
// see if we're powered or at a joint limit for each axis
for (int i=3D0; i < j->num; i++) {
if (j->limot[i].testRotationalLimit (j->angle[i]) ||
j->limot[i].fmax > 0) {
info->m++;
}
}
}
static void amotorGetInfo2 (dxJointAMotor *joint, dxJoint::Info2 *info)
{
int i;
// compute the axes (if not global)
dVector3 ax[3];
amotorComputeGlobalAxes (joint,ax);
// in euler angle mode we do not actually constrain the angular =
velocity
// along the axes axis[0] and axis[2] (although we do use axis[1]) :
//
// to get constrain w2-w1 along ...not
// ------ --------------------- ------
// d(angle[0])/dt =3D 0 ax[1] x ax[2] ax[0]
// d(angle[1])/dt =3D 0 ax[1]
// d(angle[2])/dt =3D 0 ax[0] x ax[1] ax[2]
//
// constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=3D0.
// to prove the result for angle[0], write the expression for angle[0] =
from
// GetInfo1 then take the derivative. to prove this for angle[2] it is
// easier to take the euler rate expression for d(angle[2])/dt with =
respect
// to the components of w and set that to 0.
dVector3 *axptr[3];
axptr[0] =3D &ax[0];
axptr[1] =3D &ax[1];
axptr[2] =3D &ax[2];
dVector3 ax0_cross_ax1;
dVector3 ax1_cross_ax2;
if (joint->mode =3D=3D dAMotorEuler) {
dCROSS (ax0_cross_ax1,=3D,ax[0],ax[1]);
axptr[2] =3D &ax0_cross_ax1;
dCROSS (ax1_cross_ax2,=3D,ax[1],ax[2]);
axptr[0] =3D &ax1_cross_ax2;
}
int row=3D0;
for (i=3D0; i < joint->num; i++) {
row +=3D joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1);
}
}
extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num)
{
dAASSERT(joint && num >=3D 0 && num <=3D 3);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (joint->mode =3D=3D dAMotorEuler) {
joint->num =3D 3;
}
else {
if (num < 0) num =3D 0;
if (num > 3) num =3D 3;
joint->num =3D num;
}
}
extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int =
rel,
dReal x, dReal y, dReal z)
{
dAASSERT(joint && anum >=3D 0 && anum <=3D 2 && rel >=3D 0 && rel <=3D =
2);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (anum < 0) anum =3D 0;
if (anum > 2) anum =3D 2;
joint->rel[anum] =3D rel;
// x,y,z is always in global coordinates regardless of rel, so we may =
have
// to convert it to be relative to a body
dVector3 r;
r[0] =3D x;
r[1] =3D y;
r[2] =3D z;
r[3] =3D 0;
if (rel > 0) {
if (rel=3D=3D1) {
dMULTIPLY1_331 (joint->axis[anum],joint->node[0].body->R,r);
}
else {
dMULTIPLY1_331 (joint->axis[anum],joint->node[1].body->R,r);
}
}
else {
joint->axis[anum][0] =3D r[0];
joint->axis[anum][1] =3D r[1];
joint->axis[anum][2] =3D r[2];
}
dNormalize3 (joint->axis[anum]);
if (joint->mode =3D=3D dAMotorEuler) amotorSetEulerReferenceVectors =
(joint);
}
extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum,
dReal angle)
{
dAASSERT(joint && anum >=3D 0 && anum < 3);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (joint->mode =3D=3D dAMotorUser) {
if (anum < 0) anum =3D 0;
if (anum > 3) anum =3D 3;
joint->angle[anum] =3D angle;
}
}
extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int =
parameter,
dReal value)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
int anum =3D parameter >> 8;
if (anum < 0) anum =3D 0;
if (anum > 2) anum =3D 2;
parameter &=3D 0xff;
joint->limot[anum].set (parameter, value);
}
extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
joint->mode =3D mode;
if (joint->mode =3D=3D dAMotorEuler) {
joint->num =3D 3;
amotorSetEulerReferenceVectors (joint);
}
}
extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
return joint->num;
}
extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum,
dVector3 result)
{
dAASSERT(joint && anum >=3D 0 && anum < 3);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (anum < 0) anum =3D 0;
if (anum > 2) anum =3D 2;
if (joint->rel[anum] > 0) {
if (joint->rel[anum]=3D=3D1) {
dMULTIPLY0_331 (result,joint->node[0].body->R,joint->axis[anum]);
}
else {
dMULTIPLY0_331 (result,joint->node[1].body->R,joint->axis[anum]);
}
}
else {
result[0] =3D joint->axis[anum][0];
result[1] =3D joint->axis[anum][1];
result[2] =3D joint->axis[anum][2];
}
}
extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum)
{
dAASSERT(joint && anum >=3D 0 && anum < 3);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (anum < 0) anum =3D 0;
if (anum > 2) anum =3D 2;
return joint->rel[anum];
}
extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum)
{
dAASSERT(joint && anum >=3D 0 && anum < 3);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
if (anum < 0) anum =3D 0;
if (anum > 3) anum =3D 3;
return joint->angle[anum];
}
extern "C" dReal dJointGetAMotorAngleRate (dxJointAMotor *joint, int =
anum)
{
// @@@
dDebug (0,"not yet implemented");
return 0;
}
extern "C" dReal dJointGetAMotorParam (dxJointAMotor *joint, int =
parameter)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
int anum =3D parameter >> 8;
if (anum < 0) anum =3D 0;
if (anum > 2) anum =3D 2;
parameter &=3D 0xff;
return joint->limot[anum].get (parameter);
}
extern "C" int dJointGetAMotorMode (dxJointAMotor *joint)
{
dAASSERT(joint);
dUASSERT(joint->vtable =3D=3D &__damotor_vtable,"joint is not an =
amotor");
return joint->mode;
}
dxJoint::Vtable __damotor_vtable =3D {
sizeof(dxJointAMotor),
(dxJoint::init_fn*) amotorInit,
(dxJoint::getInfo1_fn*) amotorGetInfo1,
(dxJoint::getInfo2_fn*) amotorGetInfo2,
dJointTypeAMotor};
//***********************************************************************=
*****
// fixed joint
static void fixedInit (dxJointFixed *j)
{
dSetZero (j->offset,4);
}
static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info)
{
info->m =3D 6;
info->nub =3D 6;
}
static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info)
{
int s =3D info->rowskip;
// set jacobian
info->J1l[0] =3D 1;
info->J1l[s+1] =3D 1;
info->J1l[2*s+2] =3D 1;
info->J1a[3*s] =3D 1;
info->J1a[4*s+1] =3D 1;
info->J1a[5*s+2] =3D 1;
dVector3 ofs;
if (joint->node[1].body) {
dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset);
dCROSSMAT (info->J1a,ofs,s,+,-);
info->J2l[0] =3D -1;
info->J2l[s+1] =3D -1;
info->J2l[2*s+2] =3D -1;
info->J2a[3*s] =3D -1;
info->J2a[4*s+1] =3D -1;
info->J2a[5*s+2] =3D -1;
}
// set right hand side for the first three rows (linear)
dReal k =3D info->fps * info->erp;
if (joint->node[1].body) {
for (int j=3D0; j<3; j++)
info->c[j] =3D k * (joint->node[1].body->pos[j] -
joint->node[0].body->pos[j] + ofs[j]);
}
else {
for (int j=3D0; j<3; j++)
info->c[j] =3D k * (joint->offset[j] - =
joint->node[0].body->pos[j]);
}
// set right hand side for the next three rows (angular). this code is
// borrowed from the slider, so look at the comments there.
// @@@ make a function common to both the slider and this joint !!!
// get qerr =3D relative rotation (rotation error) between two bodies
dQuaternion qerr,e;
if (joint->node[1].body) {
dQMultiply1 (qerr,joint->node[0].body->q,joint->node[1].body->q);
}
else {
qerr[0] =3D joint->node[0].body->q[0];
for (int i=3D1; i<4; i++) qerr[i] =3D -joint->node[0].body->q[i];
}
if (qerr[0] < 0) {
qerr[1] =3D -qerr[1]; // adjust sign of qerr to make theta small
qerr[2] =3D -qerr[2];
qerr[3] =3D -qerr[3];
}
dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD =
padding!
info->c[3] =3D 2*k * e[0];
info->c[4] =3D 2*k * e[1];
info->c[5] =3D 2*k * e[2];
}
extern "C" void dJointSetFixed (dxJointFixed *joint)
{
dUASSERT(joint,"bad joint argument");
dUASSERT(joint->vtable =3D=3D &__dfixed_vtable,"joint is not fixed");
int i;
// compute the offset between the bodies
if (joint->node[0].body) {
if (joint->node[1].body) {
dReal ofs[4];
for (i=3D0; i<4; i++) ofs[i] =3D joint->node[0].body->pos[i];
for (i=3D0; i<4; i++) ofs[i] -=3D joint->node[1].body->pos[i];
dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs);
}
else {
for (i=3D0; i<4; i++) joint->offset[i] =3D =
joint->node[0].body->pos[i];
}
}
}
dxJoint::Vtable __dfixed_vtable =3D {
sizeof(dxJointFixed),
(dxJoint::init_fn*) fixedInit,
(dxJoint::getInfo1_fn*) fixedGetInfo1,
(dxJoint::getInfo2_fn*) fixedGetInfo2,
dJointTypeFixed};
//***********************************************************************=
*****
// null joint
static void nullGetInfo1 (dxJointNull *j, dxJoint::Info1 *info)
{
info->m =3D 0;
info->nub =3D 0;
}
static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info)
{
dDebug (0,"this should never get called");
}
dxJoint::Vtable __dnull_vtable =3D {
sizeof(dxJointNull),
(dxJoint::init_fn*) 0,
(dxJoint::getInfo1_fn*) nullGetInfo1,
(dxJoint::getInfo2_fn*) nullGetInfo2,
dJointTypeNull};
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="objects.h"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="objects.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_OBJECTS_H_
#define _ODE_OBJECTS_H_
#include <ode/common.h>
#include <ode/mass.h>
#ifdef __cplusplus
extern "C" {
#endif
/* world */
dWorldID dWorldCreate();
void dWorldDestroy (dWorldID);
void dWorldSetGravity (dWorldID, dReal x, dReal y, dReal z);
void dWorldGetGravity (dWorldID, dVector3 gravity);
void dWorldSetERP (dWorldID, dReal erp);
dReal dWorldGetERP (dWorldID);
void dWorldSetCFM (dWorldID, dReal cfm);
dReal dWorldGetCFM (dWorldID);
void dWorldStep (dWorldID, dReal stepsize);
void dWorldImpulseToForce (dWorldID, dReal stepsize,
dReal ix, dReal iy, dReal iz, dVector3 force);
/* bodies */
dBodyID dBodyCreate (dWorldID);
void dBodyDestroy (dBodyID);
void dBodySetData (dBodyID, void *data);
void *dBodyGetData (dBodyID);
void dBodySetPosition (dBodyID, dReal x, dReal y, dReal z);
void dBodySetRotation (dBodyID, const dMatrix3 R);
void dBodySetQuaternion (dBodyID, const dQuaternion q);
void dBodySetLinearVel (dBodyID, dReal x, dReal y, dReal z);
void dBodySetAngularVel (dBodyID, dReal x, dReal y, dReal z);
const dReal * dBodyGetPosition (dBodyID);
const dReal * dBodyGetRotation (dBodyID); /* ptr to 4x3 rot matrix */
const dReal * dBodyGetQuaternion (dBodyID);
const dReal * dBodyGetLinearVel (dBodyID);
const dReal * dBodyGetAngularVel (dBodyID);
void dBodySetMass (dBodyID, const dMass *mass);
void dBodyGetMass (dBodyID, dMass *mass);
void dBodyAddForce (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddTorque (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddRelForce (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddRelTorque (dBodyID, dReal fx, dReal fy, dReal fz);
void dBodyAddForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz);
void dBodyAddForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz);
void dBodyAddRelForceAtPos (dBodyID, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz);
void dBodyAddRelForceAtRelPos (dBodyID, dReal fx, dReal fy, dReal fz,
dReal px, dReal py, dReal pz);
const dReal * dBodyGetForce (dBodyID);
const dReal * dBodyGetTorque (dBodyID);
void dBodySetForce (dBodyID b, dReal x, dReal y, dReal z);
void dBodySetTorque (dBodyID b, dReal x, dReal y, dReal z);
void dBodyGetRelPointPos (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodyGetRelPointVel (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodyGetPointVel (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodyGetPosRelPoint (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodyVectorToWorld (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodyVectorFromWorld (dBodyID, dReal px, dReal py, dReal pz,
dVector3 result);
void dBodySetFiniteRotationMode (dBodyID, int mode);
void dBodySetFiniteRotationAxis (dBodyID, dReal x, dReal y, dReal z);
int dBodyGetFiniteRotationMode (dBodyID);
void dBodyGetFiniteRotationAxis (dBodyID, dVector3 result);
int dBodyGetNumJoints (dBodyID b);
dJointID dBodyGetJoint (dBodyID, int index);
void dBodyEnable (dBodyID);
void dBodyDisable (dBodyID);
int dBodyIsEnabled (dBodyID);
void dBodySetGravityMode (dBodyID b, int mode);
int dBodyGetGravityMode (dBodyID b);
/* joints */
dJointID dJointCreateBall (dWorldID, dJointGroupID);
// bovet+
dJointID dJointCreateUniversalSpring (dWorldID, dJointGroupID);
// bovet-
dJointID dJointCreateHinge (dWorldID, dJointGroupID);
dJointID dJointCreateSlider (dWorldID, dJointGroupID);
dJointID dJointCreateContact (dWorldID, dJointGroupID, const dContact =
*);
dJointID dJointCreateHinge2 (dWorldID, dJointGroupID);
dJointID dJointCreateUniversal (dWorldID, dJointGroupID);
dJointID dJointCreateFixed (dWorldID, dJointGroupID);
dJointID dJointCreateNull (dWorldID, dJointGroupID);
dJointID dJointCreateAMotor (dWorldID, dJointGroupID);
void dJointDestroy (dJointID);
dJointGroupID dJointGroupCreate (int max_size);
void dJointGroupDestroy (dJointGroupID);
void dJointGroupEmpty (dJointGroupID);
void dJointAttach (dJointID, dBodyID body1, dBodyID body2);
void dJointSetData (dJointID, void *data);
void *dJointGetData (dJointID);
int dJointGetType (dJointID);
dBodyID dJointGetBody (dJointID, int index);
// bovet+
const dReal *dJointGetForce(dJointID, int index);
const dReal *dJointGetTorque(dJointID, int index);
// bovet-
void dJointSetBallAnchor (dJointID, dReal x, dReal y, dReal z);
// bovet+
void dJointSetUniversalSpringAnchor (dJointID, dReal x, dReal y, dReal =
z);
void dJointSetUniversalSpringMainAxis (dJointID, dReal x, dReal y, dReal =
z);
void dJointSetUniversalSpringAxis1 (dJointID, dReal x, dReal y, dReal =
z);
void dJointSetUniversalSpringAxis2 (dJointID, dReal x, dReal y, dReal =
z);
void dJointSetUniversalSpringConstant (dJointID, dReal kp);
void dJointSetUniversalSpringDamping (dJointID, dReal kd);
void dJointSetUniversalSpringAngularConstant (dJointID, dReal kp);
void dJointSetUniversalSpringAngularDamping (dJointID, dReal kd);
void dJointSetUniversalSpringPrecessionDampingCFM (dJointID, dReal cfm);
// bovet-
void dJointSetHingeAnchor (dJointID, dReal x, dReal y, dReal z);
void dJointSetHingeAxis (dJointID, dReal x, dReal y, dReal z);
void dJointSetHingeParam (dJointID, int parameter, dReal value);
void dJointSetSliderAxis (dJointID, dReal x, dReal y, dReal z);
void dJointSetSliderParam (dJointID, int parameter, dReal value);
void dJointSetHinge2Anchor (dJointID, dReal x, dReal y, dReal z);
void dJointSetHinge2Axis1 (dJointID, dReal x, dReal y, dReal z);
void dJointSetHinge2Axis2 (dJointID, dReal x, dReal y, dReal z);
void dJointSetHinge2Param (dJointID, int parameter, dReal value);
void dJointSetUniversalAnchor (dJointID, dReal x, dReal y, dReal z);
void dJointSetUniversalAxis1 (dJointID, dReal x, dReal y, dReal z);
void dJointSetUniversalAxis2 (dJointID, dReal x, dReal y, dReal z);
void dJointSetFixed (dJointID);
void dJointSetAMotorNumAxes (dJointID, int num);
void dJointSetAMotorAxis (dJointID, int anum, int rel,
dReal x, dReal y, dReal z);
void dJointSetAMotorAngle (dJointID, int anum, dReal angle);
void dJointSetAMotorParam (dJointID, int parameter, dReal value);
void dJointSetAMotorMode (dJointID, int mode);
void dJointGetBallAnchor (dJointID, dVector3 result);
// bovet+
void dJointGetUniversalSpringAnchor (dJointID, dVector3 result);
void dJointGetUniversalSpringMainAxis (dJointID, int index, dVector3 =
result);
void dJointGetUniversalSpringAxis1 (dJointID, dVector3 result);
void dJointGetUniversalSpringAxis2 (dJointID, dVector3 result);
dReal dJointGetUniversalSpringConstant (dJointID);
dReal dJointGetUniversalSpringDamping (dJointID);
dReal dJointGetUniversalSpringAngularConstant (dJointID);
dReal dJointGetUniversalSpringAngularDamping (dJointID);
dReal dJointGetUniversalSpringPrecessionDampingCFM (dJointID);
// bovet-
void dJointGetHingeAnchor (dJointID, dVector3 result);
void dJointGetHingeAxis (dJointID, dVector3 result);
dReal dJointGetHingeParam (dJointID, int parameter);
dReal dJointGetHingeAngle (dJointID);
dReal dJointGetHingeAngleRate (dJointID);
dReal dJointGetSliderPosition (dJointID);
dReal dJointGetSliderPositionRate (dJointID);
void dJointGetSliderAxis (dJointID, dVector3 result);
dReal dJointGetSliderParam (dJointID, int parameter);
void dJointGetHinge2Anchor (dJointID, dVector3 result);
void dJointGetHinge2Axis1 (dJointID, dVector3 result);
void dJointGetHinge2Axis2 (dJointID, dVector3 result);
dReal dJointGetHinge2Param (dJointID, int parameter);
dReal dJointGetHinge2Angle1 (dJointID);
dReal dJointGetHinge2Angle1Rate (dJointID);
dReal dJointGetHinge2Angle2Rate (dJointID);
void dJointGetUniversalAnchor (dJointID, dVector3 result);
void dJointGetUniversalAxis1 (dJointID, dVector3 result);
void dJointGetUniversalAxis2 (dJointID, dVector3 result);
int dJointGetAMotorNumAxes (dJointID);
void dJointGetAMotorAxis (dJointID, int anum, dVector3 result);
int dJointGetAMotorAxisRel (dJointID, int anum);
dReal dJointGetAMotorAngle (dJointID, int anum);
dReal dJointGetAMotorAngleRate (dJointID, int anum);
dReal dJointGetAMotorParam (dJointID, int parameter);
int dJointGetAMotorMode (dJointID);
int dAreConnected (dBodyID, dBodyID);
#ifdef __cplusplus
}
#endif
#endif
------=_NextPart_000_0017_01C24E8D.DE21AF60
Content-Type: application/octet-stream;
name="test_univspring.cpp"
Content-Transfer-Encoding: quoted-printable
Content-Disposition: attachment;
filename="test_univspring.cpp"
/************************************************************************=
*
* =
*
* 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. =
*
* =
*
=
*************************************************************************=
/
#include <ode/ode.h>
#include <drawstuff/drawstuff.h>
#ifdef MSVC
#pragma warning(disable:4244 4305) // for VC++, no precision loss =
complaints
#endif
// select correct drawing functions
#ifdef dDOUBLE
#define dsDrawBox dsDrawBoxD
#define dsDrawSphere dsDrawSphereD
#define dsDrawCylinder dsDrawCylinderD
#define dsDrawCappedCylinder dsDrawCappedCylinderD
#endif
// some constants
#define NUM 20 // max number of objects
#define DENSITY (1.0) // density of all objects
#define GPB 3 // maximum number of geometries per body
#define ELEMS 10 // number of elements for the beam
// dynamics and collision objects
struct MyObject {
dBodyID body; // the body
dGeomID geom[GPB]; // geometries representing this body
};
static int num=3D0; // number of objects in simulation
static int nextobj=3D0; // next object to recycle if num=3D=3DNUM
static dWorldID world;
static dSpaceID space;
static MyObject obj[NUM];
static dJointGroupID contactgroup;
static int selected =3D -1; // selected object
struct MyBeamObject {
dBodyID body;
dGeomID geom;
dJointID joint;
};
static MyBeamObject elem[ELEMS]; // beam elements
static void* beamData =3D (void*) 1000;
// this is called by dSpaceCollide when two objects in space are
// potentially colliding.
static void nearCallback (void *data, dGeomID o1, dGeomID o2)
{
int i;
// if (o1->body && o2->body) return;
// exit without doing anything if the two bodies are connected by a =
joint
dBodyID b1 =3D dGeomGetBody(o1);
dBodyID b2 =3D dGeomGetBody(o2);
if (b1 && b2 && dAreConnected (b1,b2)) return;
if (b1 && b2 && (dBodyGetData(b1) =3D=3D beamData) && =
(dBodyGetData(b2) =3D=3D beamData)) return;
dContact contact[3]; // up to 3 contacts per box
for (i=3D0; i<3; i++) {
contact[i].surface.mode =3D dContactBounce; //dContactMu2;
contact[i].surface.mu =3D dInfinity;
contact[i].surface.mu2 =3D 0;
contact[i].surface.bounce =3D 0.5;
contact[i].surface.bounce_vel =3D 0.1;
}
if (int numc =3D dCollide (o1,o2,3,&contact[0].geom,sizeof(dContact))) =
{
// dMatrix3 RI;
// dRSetIdentity (RI);
// const dReal ss[3] =3D {0.02,0.02,0.02};
for (i=3D0; i<numc; i++) {
dJointID c =3D dJointCreateContact (world,contactgroup,contact+i);
dJointAttach (c,b1,b2);
// dsDrawBox (contact[i].geom.pos,RI,ss);
}
}
}
// start simulation - set viewpoint
static void start()
{
static float xyz[3] =3D {4.0f,-2.0f,2.0f};
static float hpr[3] =3D {125.5000f,-17.0000f,0.0000f};
dsSetViewpoint (xyz,hpr);
printf ("To drop another object, press:\n");
printf (" b for box.\n");
printf (" s for sphere.\n");
printf (" c for cylinder.\n");
printf (" x for a composite object.\n");
printf ("To select an object, press space.\n");
printf ("To disable the selected object, press d.\n");
printf ("To enable the selected object, press e.\n");
}
char locase (char c)
{
if (c >=3D 'A' && c <=3D 'Z') return c - ('a'-'A');
else return c;
}
// called when a key pressed
static void command (int cmd)
{
int i,j,k;
dReal sides[3];
dMass m;
cmd =3D locase (cmd);
if (cmd =3D=3D 'b' || cmd =3D=3D 's' || cmd =3D=3D 'c' || cmd =3D=3D =
'x') {
if (num < NUM) {
i =3D num;
num++;
}
else {
i =3D nextobj;
nextobj++;
if (nextobj >=3D num) nextobj =3D 0;
// destroy the body and geoms for slot i
dBodyDestroy (obj[i].body);
for (k=3D0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomDestroy (obj[i].geom[k]);
}
memset (&obj[i],0,sizeof(obj[i]));
}
obj[i].body =3D dBodyCreate (world);
for (k=3D0; k<3; k++) sides[k] =3D dRandReal()*0.5+0.1;
dBodySetPosition (obj[i].body,
dRandReal()*2.0+1.0,dRandReal()*0.5-0.25,dRandReal()+2);
dMatrix3 R;
dRFromAxisAndAngle (R,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dBodySetRotation (obj[i].body,R);
dBodySetData (obj[i].body,(void*) i);
if (cmd =3D=3D 'b') {
dMassSetBox (&m,DENSITY,sides[0],sides[1],sides[2]);
obj[i].geom[0] =3D dCreateBox (space,sides[0],sides[1],sides[2]);
}
else if (cmd =3D=3D 'c') {
sides[0] *=3D 0.5;
dMassSetCappedCylinder (&m,DENSITY,3,sides[0],sides[1]);
obj[i].geom[0] =3D dCreateCCylinder (space,sides[0],sides[1]);
}
else if (cmd =3D=3D 's') {
sides[0] *=3D 0.5;
dMassSetSphere (&m,DENSITY,sides[0]);
obj[i].geom[0] =3D dCreateSphere (space,sides[0]);
}
else if (cmd =3D=3D 'x') {
dGeomID g2[GPB]; // encapsulated geometries
dReal dpos[GPB][3]; // delta-positions for encapsulated geometries
// start accumulating masses for the encapsulated geometries
dMass m2;
dMassSetZero (&m);
// set random delta positions
for (j=3D0; j<GPB; j++) {
for (k=3D0; k<3; k++) dpos[j][k] =3D dRandReal()*0.3-0.15;
}
for (k=3D0; k<3; k++) {
obj[i].geom[k] =3D dCreateGeomTransform (space);
dGeomTransformSetCleanup (obj[i].geom[k],1);
if (k=3D=3D0) {
dReal radius =3D dRandReal()*0.25+0.05;
g2[k] =3D dCreateSphere (0,radius);
dMassSetSphere (&m2,DENSITY,radius);
}
else if (k=3D=3D1) {
g2[k] =3D dCreateBox (0,sides[0],sides[1],sides[2]);
dMassSetBox (&m2,DENSITY,sides[0],sides[1],sides[2]);
}
else {
dReal radius =3D dRandReal()*0.1+0.05;
dReal length =3D dRandReal()*1.0+0.1;
g2[k] =3D dCreateCCylinder (0,radius,length);
dMassSetCappedCylinder (&m2,DENSITY,3,radius,length);
}
dGeomTransformSetGeom (obj[i].geom[k],g2[k]);
// set the transformation (adjust the mass too)
dGeomSetPosition (g2[k],dpos[k][0],dpos[k][1],dpos[k][2]);
dMassTranslate (&m2,dpos[k][0],dpos[k][1],dpos[k][2]);
dMatrix3 Rtx;
dRFromAxisAndAngle (Rtx,dRandReal()*2.0-1.0,dRandReal()*2.0-1.0,
dRandReal()*2.0-1.0,dRandReal()*10.0-5.0);
dGeomSetRotation (g2[k],Rtx);
dMassRotate (&m2,Rtx);
// add to the total mass
dMassAdd (&m,&m2);
}
// move all encapsulated objects so that the center of mass is =
(0,0,0)
for (k=3D0; k<2; k++) {
dGeomSetPosition (g2[k],
dpos[k][0]-m.c[0],
dpos[k][1]-m.c[1],
dpos[k][2]-m.c[2]);
}
dMassTranslate (&m,-m.c[0],-m.c[1],-m.c[2]);
}
for (k=3D0; k < GPB; k++) {
if (obj[i].geom[k]) dGeomSetBody (obj[i].geom[k],obj[i].body);
}
dBodySetMass (obj[i].body,&m);
}
if (cmd =3D=3D ' ') {
selected++;
if (selected >=3D num) selected =3D 0;
if (selected < 0) selected =3D 0;
}
else if (cmd =3D=3D 'd' && selected >=3D 0 && selected < num) {
dBodyDisable (obj[selected].body);
}
else if (cmd =3D=3D 'e' && selected >=3D 0 && selected < num) {
dBodyEnable (obj[selected].body);
}
}
// draw a geom
void drawGeom (dGeomID g, const dReal *pos, const dReal *R)
{
if (!g) return;
if (!pos) pos =3D dGeomGetPosition (g);
if (!R) R =3D dGeomGetRotation (g);
int type =3D dGeomGetClass (g);
if (type =3D=3D dBoxClass) {
dVector3 sides;
dGeomBoxGetLengths (g,sides);
dsDrawBox (pos,R,sides);
}
else if (type =3D=3D dSphereClass) {
dsDrawSphere (pos,R,dGeomSphereGetRadius (g));
}
else if (type =3D=3D dCCylinderClass) {
dReal radius,length;
dGeomCCylinderGetParams (g,&radius,&length);
dsDrawCappedCylinder (pos,R,length,radius);
}
else if (type =3D=3D dGeomTransformClass) {
dGeomID g2 =3D dGeomTransformGetGeom (g);
const dReal *pos2 =3D dGeomGetPosition (g2);
const dReal *R2 =3D dGeomGetRotation (g2);
dVector3 actual_pos;
dMatrix3 actual_R;
dMULTIPLY0_331 (actual_pos,R,pos2);
actual_pos[0] +=3D pos[0];
actual_pos[1] +=3D pos[1];
actual_pos[2] +=3D pos[2];
dMULTIPLY0_333 (actual_R,R,R2);
drawGeom (g2,actual_pos,actual_R);
}
}
// simulation loop
static void simLoop (int pause)
{
dsSetColor (0,0,2);
dSpaceCollide (space,0,&nearCallback);
if (!pause) dWorldStep (world,0.05);
// remove all contact joints
dJointGroupEmpty (contactgroup);
dsSetColor (1,1,0);
dsSetTexture (DS_WOOD);
for (int i=3D0; i<num; i++) {
int color_changed =3D 0;
if (i=3D=3Dselected) {
dsSetColor (0,0.7,1);
color_changed =3D 1;
}
else if (! dBodyIsEnabled (obj[i].body)) {
dsSetColor (1,0,0);
color_changed =3D 1;
}
for (int j=3D0; j < GPB; j++) drawGeom (obj[i].geom[j],0,0);
if (color_changed) dsSetColor (1,1,0);
}
for (int j =3D 0; j < ELEMS; j++) drawGeom(elem[j].geom,0,0);
}
void createBeam()
{
dMass m;
// beam elem size
const dReal dx =3D 0.3;
const dReal w =3D 0.5;
const dReal h =3D 0.5;
// beam density
const dReal rho =3D 0.1;
// beam pos
const dReal x0 =3D 0;
const dReal y =3D 0;
const dReal z =3D 1;
const dReal sides[3] =3D {dx, w, h};
for (unsigned i =3D 0; i < ELEMS; i++) {
elem[i].body =3D dBodyCreate(world);
dBodySetPosition(elem[i].body, x0+i*dx, y, z);
dMassSetBox(&m, rho, sides[0], sides[1], sides[2]);
elem[i].geom =3D dCreateBox(space, sides[0], sides[1], sides[2]);
dGeomSetBody(elem[i].geom, elem[i].body);
dBodySetMass(elem[i].body, &m);
dBodySetData(elem[i].body, beamData);
if (i =3D=3D 0) {
elem[i].joint =3D dJointCreateFixed(world, 0);
dJointAttach(elem[i].joint, elem[i].body, 0);
dJointSetFixed(elem[i].joint);
} else {
elem[i].joint =3D dJointCreateUniversalSpring(world, 0);
dJointAttach(elem[i].joint, elem[i-1].body, elem[i].body);
dJointSetUniversalSpringAnchor(elem[i].joint, x0+(i-0.5)*dx, y, z);
dJointSetUniversalSpringMainAxis(elem[i].joint, 1, 0, 0);
dJointSetUniversalSpringAxis1(elem[i].joint, 0, 1, 0);
dJointSetUniversalSpringAxis2(elem[i].joint, 0, 0, 1);
=09
// Elastic properties
dJointSetUniversalSpringAngularConstant(elem[i].joint, 3.0);
dJointSetUniversalSpringAngularDamping(elem[i].joint, 0.2);
dJointSetUniversalSpringPrecessionDampingCFM(elem[i].joint, 1e-4);
}
}
}
int main (int argc, char **argv)
{
// setup pointers to drawstuff callback functions
dsFunctions fn;
fn.version =3D DS_VERSION;
fn.start =3D &start;
fn.step =3D &simLoop;
fn.command =3D &command;
fn.stop =3D 0;
fn.path_to_textures =3D "../../drawstuff/textures";
// create world
world =3D dWorldCreate();
space =3D dHashSpaceCreate();
contactgroup =3D dJointGroupCreate (0);
dWorldSetGravity (world,0,0,-0.5);
dWorldSetCFM (world,1e-5);
dCreatePlane (space,0,0,1,0);
memset (obj,0,sizeof(obj));
createBeam();
// run simulation
dsSimulationLoop (argc,argv,352,288,&fn);
dJointGroupDestroy (contactgroup);
dSpaceDestroy (space);
dWorldDestroy (world);
return 0;
}
------=_NextPart_000_0017_01C24E8D.DE21AF60--