From fca74b0bf0a0833f5701e9c0de7b3bc15b2233dd Mon Sep 17 00:00:00 2001 From: dan miller Date: Fri, 19 Oct 2007 05:20:07 +0000 Subject: dont ask --- .../ode-0.9/contrib/BreakableJoints/joint.cpp | 2803 -------------------- 1 file changed, 2803 deletions(-) delete mode 100644 libraries/ode-0.9/contrib/BreakableJoints/joint.cpp (limited to 'libraries/ode-0.9/contrib/BreakableJoints/joint.cpp') diff --git a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp b/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp deleted file mode 100644 index 2c724f8..0000000 --- a/libraries/ode-0.9/contrib/BreakableJoints/joint.cpp +++ /dev/null @@ -1,2803 +0,0 @@ -/************************************************************************* - * * - * 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 -#include -#include -#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 = info->rowskip; - - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); - dCROSSMAT (info->J1a,a1,s,-,+); - if (joint->node[1].body) { - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); - dCROSSMAT (info->J2a,a2,s,+,-); - } - - // set right hand side - dReal k = info->fps * info->erp; - if (joint->node[1].body) { - for (int j=0; j<3; j++) { - info->c[j] = k * (a2[j] + joint->node[1].body->pos[j] - - a1[j] - joint->node[0].body->pos[j]); - } - } - else { - for (int j=0; j<3; j++) { - info->c[j] = 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 = 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=0; i<3; i++) info->J1l[i] = axis[i]; - for (i=0; i<3; i++) info->J1l[s+i] = q1[i]; - for (i=0; i<3; i++) info->J1l[2*s+i] = q2[i]; - dMULTIPLY0_331 (a1,joint->node[0].body->R,anchor1); - dCROSS (info->J1a,=,a1,axis); - dCROSS (info->J1a+s,=,a1,q1); - dCROSS (info->J1a+2*s,=,a1,q2); - if (joint->node[1].body) { - for (i=0; i<3; i++) info->J2l[i] = -axis[i]; - for (i=0; i<3; i++) info->J2l[s+i] = -q1[i]; - for (i=0; i<3; i++) info->J2l[2*s+i] = -q2[i]; - dMULTIPLY0_331 (a2,joint->node[1].body->R,anchor2); - dCROSS (info->J2a,= -,a2,axis); - dCROSS (info->J2a+s,= -,a2,q1); - dCROSS (info->J2a+2*s,= -,a2,q2); - } - - // set right hand side - measure error along (axis,q1,q2) - dReal k1 = info->fps * erp1; - dReal k = info->fps * info->erp; - - for (i=0; i<3; i++) a1[i] += joint->node[0].body->pos[i]; - if (joint->node[1].body) { - for (i=0; i<3; i++) a2[i] += joint->node[1].body->pos[i]; - info->c[0] = k1 * (dDOT(axis,a2) - dDOT(axis,a1)); - info->c[1] = k * (dDOT(q1,a2) - dDOT(q1,a1)); - info->c[2] = k * (dDOT(q2,a2) - dDOT(q2,a1)); - } - else { - info->c[0] = k1 * (dDOT(axis,anchor2) - dDOT(axis,a1)); - info->c[1] = k * (dDOT(q1,anchor2) - dDOT(q1,a1)); - info->c[2] = k * (dDOT(q2,anchor2) - dDOT(q2,a1)); - } -} - - -// set three orientation rows in the constraint equation, and the -// corresponding right hand side. - -static void setFixedOrientation(dxJoint *joint, dxJoint::Info2 *info, dQuaternion qrel, int start_row) -{ - int s = info->rowskip; - int start_index = start_row * s; - - // 3 rows to make body rotations equal - info->J1a[start_index] = 1; - info->J1a[start_index + s + 1] = 1; - info->J1a[start_index + s*2+2] = 1; - if (joint->node[1].body) { - info->J2a[start_index] = -1; - info->J2a[start_index + s+1] = -1; - info->J2a[start_index + s*2+2] = -1; - } - - // 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 - // |angular_velocity| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * u - // = (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 = (erp*fps) * 2 * v - // where the quaternion of the relative rotation between the two bodies is - // q = [cos(theta/2) sin(theta/2)*u] = [s v] - - // get qerr = 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,qrel); - } - else { - dQMultiply3 (qerr,joint->node[0].body->q,qrel); - } - if (qerr[0] < 0) { - qerr[1] = -qerr[1]; // adjust sign of qerr to make theta small - qerr[2] = -qerr[2]; - qerr[3] = -qerr[3]; - } - dMULTIPLY0_331 (e,joint->node[0].body->R,qerr+1); // @@@ bad SIMD padding! - dReal k = info->fps * info->erp; - info->c[start_row] = 2*k * e[0]; - info->c[start_row+1] = 2*k * e[1]; - info->c[start_row+2] = 2*k * e[2]; -} - - -// 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] = x - j->node[0].body->pos[0]; - q[1] = y - j->node[0].body->pos[1]; - q[2] = z - j->node[0].body->pos[2]; - q[3] = 0; - dMULTIPLY1_331 (anchor1,j->node[0].body->R,q); - if (j->node[1].body) { - q[0] = x - j->node[1].body->pos[0]; - q[1] = y - j->node[1].body->pos[1]; - q[2] = z - j->node[1].body->pos[2]; - q[3] = 0; - dMULTIPLY1_331 (anchor2,j->node[1].body->R,q); - } - else { - anchor2[0] = x; - anchor2[1] = y; - anchor2[2] = z; - } - } - anchor1[3] = 0; - anchor2[3] = 0; -} - - -// compute axes relative to bodies. either axis1 or 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] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - if (axis1) { - dMULTIPLY1_331 (axis1,j->node[0].body->R,q); - axis1[3] = 0; - } - if (axis2) { - if (j->node[1].body) { - dMULTIPLY1_331 (axis2,j->node[1].body->R,q); - } - else { - axis2[0] = x; - axis2[1] = y; - axis2[2] = z; - } - axis2[3] = 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] += j->node[0].body->pos[0]; - result[1] += j->node[0].body->pos[1]; - result[2] += j->node[0].body->pos[2]; - } -} - - -static void getAnchor2 (dxJoint *j, dVector3 result, dVector3 anchor2) -{ - if (j->node[1].body) { - dMULTIPLY0_331 (result,j->node[1].body->R,anchor2); - result[0] += j->node[1].body->pos[0]; - result[1] += j->node[1].body->pos[1]; - result[2] += j->node[1].body->pos[2]; - } - else { - result[0] = anchor2[0]; - result[1] = anchor2[1]; - result[2] = anchor2[2]; - } -} - - -static void getAxis (dxJoint *j, dVector3 result, dVector3 axis1) -{ - if (j->node[0].body) { - dMULTIPLY0_331 (result,j->node[0].body->R,axis1); - } -} - - -static void getAxis2 (dxJoint *j, dVector3 result, dVector3 axis2) -{ - if (j->node[1].body) { - dMULTIPLY0_331 (result,j->node[1].body->R,axis2); - } - else { - result[0] = axis2[0]; - result[1] = axis2[1]; - result[2] = axis2[2]; - } -} - - -static dReal getHingeAngleFromRelativeQuat (dQuaternion qrel, dVector3 axis) -{ - // 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] = [ 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 = atan2 ( sin(theta/2) , cos(theta/2) ) - // but we can't get sin(theta/2) directly, only its absolute value, i.e.: - // |v| = |sin(theta/2)| * |u| - // = |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. - - // extract the angle from the quaternion. cost2 = cos(theta/2), - // sint2 = |sin(theta/2)| - dReal cost2 = qrel[0]; - dReal sint2 = dSqrt (qrel[1]*qrel[1]+qrel[2]*qrel[2]+qrel[3]*qrel[3]); - dReal theta = (dDOT(qrel+1,axis) >= 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 -= 2*M_PI; - - // the angle we've just extracted has the wrong sign - theta = -theta; - - return theta; -} - - -// 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) -{ - // get qrel = 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); - } - - return getHingeAngleFromRelativeQuat (qrel,axis); -} - -//**************************************************************************** -// dxJointLimitMotor - -void dxJointLimitMotor::init (dxWorld *world) -{ - vel = 0; - fmax = 0; - lostop = -dInfinity; - histop = dInfinity; - fudge_factor = 1; - normal_cfm = world->global_cfm; - stop_erp = world->global_erp; - stop_cfm = world->global_cfm; - bounce = 0; - limit = 0; - limit_err = 0; -} - - -void dxJointLimitMotor::set (int num, dReal value) -{ - switch (num) { - case dParamLoStop: - if (value <= histop) lostop = value; - break; - case dParamHiStop: - if (value >= lostop) histop = value; - break; - case dParamVel: - vel = value; - break; - case dParamFMax: - if (value >= 0) fmax = value; - break; - case dParamFudgeFactor: - if (value >= 0 && value <= 1) fudge_factor = value; - break; - case dParamBounce: - bounce = value; - break; - case dParamCFM: - normal_cfm = value; - break; - case dParamStopERP: - stop_erp = value; - break; - case dParamStopCFM: - stop_cfm = 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 <= lostop) { - limit = 1; - limit_err = angle - lostop; - return 1; - } - else if (angle >= histop) { - limit = 2; - limit_err = angle - histop; - return 1; - } - else { - limit = 0; - return 0; - } -} - - -int dxJointLimitMotor::addLimot (dxJoint *joint, - dxJoint::Info2 *info, int row, - dVector3 ax1, int rotational) -{ - int srow = row * info->rowskip; - - // if the joint is powered, or has joint limits, add in the extra row - int powered = fmax > 0; - if (powered || limit) { - dReal *J1 = rotational ? info->J1a : info->J1l; - dReal *J2 = rotational ? info->J2a : info->J2l; - - J1[srow+0] = ax1[0]; - J1[srow+1] = ax1[1]; - J1[srow+2] = ax1[2]; - if (joint->node[1].body) { - J2[srow+0] = -ax1[0]; - J2[srow+1] = -ax1[1]; - J2[srow+2] = -ax1[2]; - } - - // linear limot torque decoupling step: - // - // if this is a linear limot (e.g. from a slider), we have to be careful - // that the linear constraint forces (+/- ax1) applied to the two bodies - // do not create a torque couple. in other words, the points that the - // constraint force is applied at must lie along the same ax1 axis. - // a torque couple will result in powered or limited slider-jointed free - // bodies from gaining angular momentum. - // the solution used here is to apply the constraint forces at the point - // halfway between the body centers. there is no penalty (other than an - // extra tiny bit of computation) in doing this adjustment. note that we - // only need to do this if the constraint connects two bodies. - - dVector3 ltd; // Linear Torque Decoupling vector (a torque) - if (!rotational && joint->node[1].body) { - dVector3 c; - c[0]=REAL(0.5)*(joint->node[1].body->pos[0]-joint->node[0].body->pos[0]); - c[1]=REAL(0.5)*(joint->node[1].body->pos[1]-joint->node[0].body->pos[1]); - c[2]=REAL(0.5)*(joint->node[1].body->pos[2]-joint->node[0].body->pos[2]); - dCROSS (ltd,=,c,ax1); - info->J1a[srow+0] = ltd[0]; - info->J1a[srow+1] = ltd[1]; - info->J1a[srow+2] = ltd[2]; - info->J2a[srow+0] = ltd[0]; - info->J2a[srow+1] = ltd[1]; - info->J2a[srow+2] = ltd[2]; - } - - // if we're limited low and high simultaneously, the joint motor is - // ineffective - if (limit && (lostop == histop)) powered = 0; - - if (powered) { - info->cfm[row] = normal_cfm; - if (! limit) { - info->c[row] = vel; - info->lo[row] = -fmax; - info->hi[row] = 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 = fmax; - if (vel > 0) fm = -fm; - - // if we're powering away from the limit, apply the fudge factor - if ((limit==1 && vel > 0) || (limit==2 && vel < 0)) fm *= 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]); - - // linear limot torque decoupling step: refer to above discussion - dBodyAddTorque (joint->node[0].body,-fm*ltd[0],-fm*ltd[1], - -fm*ltd[2]); - dBodyAddTorque (joint->node[1].body,-fm*ltd[0],-fm*ltd[1], - -fm*ltd[2]); - } - } - } - } - - if (limit) { - dReal k = info->fps * stop_erp; - info->c[row] = -k * limit_err; - info->cfm[row] = stop_cfm; - - if (lostop == histop) { - // limited low and high simultaneously - info->lo[row] = -dInfinity; - info->hi[row] = dInfinity; - } - else { - if (limit == 1) { - // low limit - info->lo[row] = 0; - info->hi[row] = dInfinity; - } - else { - // high limit - info->lo[row] = -dInfinity; - info->hi[row] = 0; - } - - // deal with bounce - if (bounce > 0) { - // calculate joint velocity - dReal vel; - if (rotational) { - vel = dDOT(joint->node[0].body->avel,ax1); - if (joint->node[1].body) - vel -= dDOT(joint->node[1].body->avel,ax1); - } - else { - vel = dDOT(joint->node[0].body->lvel,ax1); - if (joint->node[1].body) - vel -= 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 == 1) { - // low limit - if (vel < 0) { - dReal newc = -bounce * vel; - if (newc > info->c[row]) info->c[row] = newc; - } - } - else { - // high limit - all those computations are reversed - if (vel > 0) { - dReal newc = -bounce * vel; - if (newc < info->c[row]) info->c[row] = 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 = 3; - info->nub = 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 == &__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 == &__dball_vtable,"joint is not a ball"); - if (joint->flags & dJOINT_REVERSE) - getAnchor2 (joint,result,joint->anchor2); - else - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetBallAnchor2 (dxJointBall *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dball_vtable,"joint is not a ball"); - if (joint->flags & dJOINT_REVERSE) - getAnchor (joint,result,joint->anchor1); - else - getAnchor2 (joint,result,joint->anchor2); -} - - -dxJoint::Vtable __dball_vtable = { - 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] = 1; - dSetZero (j->axis2,4); - j->axis2[0] = 1; - dSetZero (j->qrel,4); - j->limot.init (j->world); -} - - -static void hingeGetInfo1 (dxJointHinge *j, dxJoint::Info1 *info) -{ - info->nub = 5; - - // see if joint is powered - if (j->limot.fmax > 0) - info->m = 6; // powered hinge needs an extra constraint row - else info->m = 5; - - // see if we're at a joint limit. - if ((j->limot.lostop >= -M_PI || j->limot.histop <= M_PI) && - j->limot.lostop <= j->limot.histop) { - dReal angle = getHingeAngle (j->node[0].body,j->node[1].body,j->axis1, - j->qrel); - if (j->limot.testRotationalLimit (angle)) info->m = 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 = 0 - // q*w1 - q*w2 = 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=3*info->rowskip; - int s4=4*info->rowskip; - - info->J1a[s3+0] = p[0]; - info->J1a[s3+1] = p[1]; - info->J1a[s3+2] = p[2]; - info->J1a[s4+0] = q[0]; - info->J1a[s4+1] = q[1]; - info->J1a[s4+2] = q[2]; - - if (joint->node[1].body) { - info->J2a[s3+0] = -p[0]; - info->J2a[s3+1] = -p[1]; - info->J2a[s3+2] = -p[2]; - info->J2a[s4+0] = -q[0]; - info->J2a[s4+1] = -q[1]; - info->J2a[s4+2] = -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 = (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| = angle/time = erp*theta / stepsize - // = (erp*fps) * theta - // angular_velocity = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2| - // = (erp*fps) * theta * (ax1 x ax2) / sin(theta) - // ...as ax1 and ax2 are unit length. if theta is smallish, - // theta ~= sin(theta), so - // angular_velocity = (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] = joint->axis2[0]; - ax2[1] = joint->axis2[1]; - ax2[2] = joint->axis2[2]; - } - dCROSS (b,=,ax1,ax2); - dReal k = info->fps * info->erp; - info->c[3] = k * dDOT(b,p); - info->c[4] = 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] = joint->node[0].body->q[0]; - for (int i=1; i<4; i++) joint->qrel[i] = -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 == &__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 == &__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 == &__dhinge_vtable,"joint is not a hinge"); - if (joint->flags & dJOINT_REVERSE) - getAnchor2 (joint,result,joint->anchor2); - else - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetHingeAnchor2 (dxJointHinge *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - if (joint->flags & dJOINT_REVERSE) - getAnchor (joint,result,joint->anchor1); - else - getAnchor2 (joint,result,joint->anchor2); -} - - -extern "C" void dJointGetHingeAxis (dxJointHinge *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__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 == &__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 == &__dhinge_vtable,"joint is not a hinge"); - return joint->limot.get (parameter); -} - - -extern "C" dReal dJointGetHingeAngle (dxJointHinge *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a hinge"); - if (joint->node[0].body) { - dReal ang = getHingeAngle (joint->node[0].body,joint->node[1].body,joint->axis1, - joint->qrel); - if (joint->flags & dJOINT_REVERSE) - return -ang; - else - return ang; - } - else return 0; -} - - -extern "C" dReal dJointGetHingeAngleRate (dxJointHinge *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__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 = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); - if (joint->flags & dJOINT_REVERSE) rate = - rate; - return rate; - } - else return 0; -} - - -extern "C" void dJointAddHingeTorque (dxJointHinge *joint, dReal torque) -{ - dVector3 axis; - dAASSERT(joint); - dUASSERT(joint->vtable == &__dhinge_vtable,"joint is not a Hinge"); - - if (joint->flags & dJOINT_REVERSE) - torque = -torque; - - getAxis (joint,axis,joint->axis1); - axis[0] *= torque; - axis[1] *= torque; - axis[2] *= torque; - - if (joint->node[0].body != 0) - dBodyAddTorque (joint->node[0].body, axis[0], axis[1], axis[2]); - if (joint->node[1].body != 0) - dBodyAddTorque(joint->node[1].body, -axis[0], -axis[1], -axis[2]); -} - - -dxJoint::Vtable __dhinge_vtable = { - 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] = 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 == &__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=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - q[i] - - joint->node[1].body->pos[i]; - } - else { - for (int i=0; i<3; i++) q[i] = joint->node[0].body->pos[i] - - joint->offset[i]; - - } - return dDOT(ax1,q); -} - - -extern "C" dReal dJointGetSliderPositionRate (dxJointSlider *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__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 = 5; - - // see if joint is powered - if (j->limot.fmax > 0) - info->m = 6; // powered slider needs an extra constraint row - else info->m = 5; - - // see if we're at a joint limit. - j->limot.limit = 0; - if ((j->limot.lostop > -dInfinity || j->limot.histop < dInfinity) && - j->limot.lostop <= j->limot.histop) { - // measure joint position - dReal pos = dJointGetSliderPosition (j); - if (pos <= j->limot.lostop) { - j->limot.limit = 1; - j->limot.limit_err = pos - j->limot.lostop; - info->m = 6; - } - else if (pos >= j->limot.histop) { - j->limot.limit = 2; - j->limot.limit_err = pos - j->limot.histop; - info->m = 6; - } - } -} - - -static void sliderGetInfo2 (dxJointSlider *joint, dxJoint::Info2 *info) -{ - int i,s = info->rowskip; - int s2=2*s,s3=3*s,s4=4*s; - - // pull out pos and R for both bodies. also get the `connection' - // vector pos2-pos1. - - dReal *pos1,*pos2,*R1,*R2; - dVector3 c; - pos1 = joint->node[0].body->pos; - R1 = joint->node[0].body->R; - if (joint->node[1].body) { - pos2 = joint->node[1].body->pos; - R2 = joint->node[1].body->R; - for (i=0; i<3; i++) c[i] = pos2[i] - pos1[i]; - } - else { - pos2 = 0; - R2 = 0; - } - - // 3 rows to make body rotations equal - setFixedOrientation(joint, info, joint->qrel, 0); - - // remaining two rows. we want: vel2 = 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, = REAL(0.5) * ,c,p); - for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s3+i] = tmp[i]; - dCROSS (tmp, = REAL(0.5) * ,c,q); - for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; - for (i=0; i<3; i++) info->J2a[s4+i] = tmp[i]; - for (i=0; i<3; i++) info->J2l[s3+i] = -p[i]; - for (i=0; i<3; i++) info->J2l[s4+i] = -q[i]; - } - for (i=0; i<3; i++) info->J1l[s3+i] = p[i]; - for (i=0; i<3; i++) info->J1l[s4+i] = q[i]; - - // 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. - dReal k = info->fps * info->erp; - if (joint->node[1].body) { - dVector3 ofs; // offset point in global coordinates - dMULTIPLY0_331 (ofs,R2,joint->offset); - for (i=0; i<3; i++) c[i] += ofs[i]; - info->c[3] = k * dDOT(p,c); - info->c[4] = k * dDOT(q,c); - } - else { - dVector3 ofs; // offset point in global coordinates - for (i=0; i<3; i++) ofs[i] = joint->offset[i] - pos1[i]; - info->c[3] = k * dDOT(p,ofs); - info->c[4] = 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 == &__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=0; i<3; i++) - c[i] = 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] = joint->node[0].body->q[0]; - for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - for (i=0; i<3; i++) joint->offset[i] = 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 == &__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 == &__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 == &__dslider_vtable,"joint is not a slider"); - return joint->limot.get (parameter); -} - - -extern "C" void dJointAddSliderForce (dxJointSlider *joint, dReal force) -{ - dVector3 axis; - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dslider_vtable,"joint is not a slider"); - - if (joint->flags & dJOINT_REVERSE) - force -= force; - - getAxis (joint,axis,joint->axis1); - axis[0] *= force; - axis[1] *= force; - axis[2] *= force; - - if (joint->node[0].body != 0) - dBodyAddForce (joint->node[0].body,axis[0],axis[1],axis[2]); - if (joint->node[1].body != 0) - dBodyAddForce(joint->node[1].body, -axis[0], -axis[1], -axis[2]); -} - - -dxJoint::Vtable __dslider_vtable = { - 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? -#if 0 /* so don't bother ;) */ - j->contact.surface.mode = 0; - j->contact.surface.mu = 0; - dSetZero (j->contact.geom.pos,4); - dSetZero (j->contact.geom.normal,4); - j->contact.geom.depth = 0; -#endif -} - - -static void contactGetInfo1 (dxJointContact *j, dxJoint::Info1 *info) -{ - // make sure mu's >= 0, then calculate number of constraint rows and number - // of unbounded rows. - int m = 1, nub=0; - if (j->contact.surface.mu < 0) j->contact.surface.mu = 0; - if (j->contact.surface.mode & dContactMu2) { - if (j->contact.surface.mu > 0) m++; - if (j->contact.surface.mu2 < 0) j->contact.surface.mu2 = 0; - if (j->contact.surface.mu2 > 0) m++; - if (j->contact.surface.mu == dInfinity) nub ++; - if (j->contact.surface.mu2 == dInfinity) nub ++; - } - else { - if (j->contact.surface.mu > 0) m += 2; - if (j->contact.surface.mu == dInfinity) nub += 2; - } - - j->the_m = m; - info->m = m; - info->nub = nub; -} - - -static void contactGetInfo2 (dxJointContact *j, dxJoint::Info2 *info) -{ - int i,s = info->rowskip; - int s2 = 2*s; - - // get normal, with sign adjusted for body1/body2 polarity - dVector3 normal; - if (j->flags & dJOINT_REVERSE) { - normal[0] = - j->contact.geom.normal[0]; - normal[1] = - j->contact.geom.normal[1]; - normal[2] = - j->contact.geom.normal[2]; - } - else { - normal[0] = j->contact.geom.normal[0]; - normal[1] = j->contact.geom.normal[1]; - normal[2] = j->contact.geom.normal[2]; - } - normal[3] = 0; // @@@ hmmm - - // c1,c2 = contact points with respect to body PORs - dVector3 c1,c2; - for (i=0; i<3; i++) c1[i] = j->contact.geom.pos[i] - j->node[0].body->pos[i]; - - // set jacobian for normal - info->J1l[0] = normal[0]; - info->J1l[1] = normal[1]; - info->J1l[2] = normal[2]; - dCROSS (info->J1a,=,c1,normal); - if (j->node[1].body) { - for (i=0; i<3; i++) c2[i] = j->contact.geom.pos[i] - - j->node[1].body->pos[i]; - info->J2l[0] = -normal[0]; - info->J2l[1] = -normal[1]; - info->J2l[2] = -normal[2]; - dCROSS (info->J2a,= -,c2,normal); - } - - // set right hand side and cfm value for normal - dReal erp = info->erp; - if (j->contact.surface.mode & dContactSoftERP) - erp = j->contact.surface.soft_erp; - dReal k = info->fps * erp; - info->c[0] = k*j->contact.geom.depth; - if (j->contact.surface.mode & dContactSoftCFM) - info->cfm[0] = j->contact.surface.soft_cfm; - - // deal with bounce - if (j->contact.surface.mode & dContactBounce) { - // calculate outgoing velocity (-ve for incoming contact) - dReal outgoing = dDOT(info->J1l,j->node[0].body->lvel) + - dDOT(info->J1a,j->node[0].body->avel); - if (j->node[1].body) { - outgoing += 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 >= 0 && - (-outgoing) > j->contact.surface.bounce_vel) { - dReal newc = - j->contact.surface.bounce * outgoing; - if (newc > info->c[0]) info->c[0] = newc; - } - } - - // set LCP limits for normal - info->lo[0] = 0; - info->hi[0] = dInfinity; - - // now do jacobian for tangential forces - dVector3 t1,t2; // two vectors tangential to normal - - // first friction direction - if (j->the_m >= 2) { - if (j->contact.surface.mode & dContactFDir1) { // use fdir1 ? - t1[0] = j->contact.fdir1[0]; - t1[1] = j->contact.fdir1[1]; - t1[2] = j->contact.fdir1[2]; - dCROSS (t2,=,normal,t1); - } - else { - dPlaneSpace (normal,t1,t2); - } - info->J1l[s+0] = t1[0]; - info->J1l[s+1] = t1[1]; - info->J1l[s+2] = t1[2]; - dCROSS (info->J1a+s,=,c1,t1); - if (j->node[1].body) { - info->J2l[s+0] = -t1[0]; - info->J2l[s+1] = -t1[1]; - info->J2l[s+2] = -t1[2]; - dCROSS (info->J2a+s,= -,c2,t1); - } - // set right hand side - if (j->contact.surface.mode & dContactMotion1) { - info->c[1] = j->contact.surface.motion1; - } - // set LCP bounds and friction index. this depends on the approximation - // mode - info->lo[1] = -j->contact.surface.mu; - info->hi[1] = j->contact.surface.mu; - if (j->contact.surface.mode & dContactApprox1_1) info->findex[1] = 0; - - // set slip (constraint force mixing) - if (j->contact.surface.mode & dContactSlip1) - info->cfm[1] = j->contact.surface.slip1; - } - - // second friction direction - if (j->the_m >= 3) { - info->J1l[s2+0] = t2[0]; - info->J1l[s2+1] = t2[1]; - info->J1l[s2+2] = t2[2]; - dCROSS (info->J1a+s2,=,c1,t2); - if (j->node[1].body) { - info->J2l[s2+0] = -t2[0]; - info->J2l[s2+1] = -t2[1]; - info->J2l[s2+2] = -t2[2]; - dCROSS (info->J2a+s2,= -,c2,t2); - } - // set right hand side - if (j->contact.surface.mode & dContactMotion2) { - info->c[2] = 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] = -j->contact.surface.mu2; - info->hi[2] = j->contact.surface.mu2; - } - else { - info->lo[2] = -j->contact.surface.mu; - info->hi[2] = j->contact.surface.mu; - } - if (j->contact.surface.mode & dContactApprox1_2) info->findex[2] = 0; - - // set slip (constraint force mixing) - if (j->contact.surface.mode & dContactSlip2) - info->cfm[2] = j->contact.surface.slip2; - } -} - - -dxJoint::Vtable __dcontact_vtable = { - 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 = dDOT(joint->v1,a2); - dReal y = 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] = 1; - dSetZero (j->axis2,4); - j->axis2[1] = 1; - j->c0 = 0; - j->s0 = 0; - - dSetZero (j->v1,4); - j->v1[0] = 1; - dSetZero (j->v2,4); - j->v2[1] = 1; - - j->limot1.init (j->world); - j->limot2.init (j->world); - - j->susp_erp = j->world->global_erp; - j->susp_cfm = j->world->global_cfm; - - j->flags |= dJOINT_TWOBODIES; -} - - -static void hinge2GetInfo1 (dxJointHinge2 *j, dxJoint::Info1 *info) -{ - info->m = 4; - info->nub = 4; - - // see if we're powered or at a joint limit for axis 1 - int atlimit=0; - if ((j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && - j->limot1.lostop <= j->limot1.histop) { - dReal angle = measureHinge2Angle (j); - if (j->limot1.testRotationalLimit (angle)) atlimit = 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 = 0; - if (j->limot2.fmax > 0) info->m++; -} - - -// macro that computes ax1,ax2 = 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,=,ax1,ax2); \ - sin_angle = dSqrt (axis[0]*axis[0] + axis[1]*axis[1] + axis[2]*axis[2]); \ - cos_angle = 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=3*info->rowskip; - info->J1a[s3+0] = q[0]; - info->J1a[s3+1] = q[1]; - info->J1a[s3+2] = q[2]; - if (joint->node[1].body) { - info->J2a[s3+0] = -q[0]; - info->J2a[s3+1] = -q[1]; - info->J2a[s3+2] = -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| = angle/time = erp*(theta0-theta) / stepsize - // = (erp*fps) * (theta0-theta) - // (theta0-theta) can be computed using the following small-angle-difference - // approximation: - // theta0-theta ~= tan(theta0-theta) - // = sin(theta0-theta)/cos(theta0-theta) - // = (c*s0 - s*c0) / (c*c0 + s*s0) - // = c*s0 - s*c0 assuming c*c0 + s*s0 ~= 1 - // where c = cos(theta), s = sin(theta) - // c0 = cos(theta0), s0 = sin(theta0) - - dReal k = info->fps * info->erp; - info->c[3] = k * (joint->c0 * s - joint->s0 * c); - - // if the axis1 hinge is powered, or has joint limits, add in more stuff - int row = 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] = 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]==0 && ax1[1]==0 && ax1[2]==0) || - (ax2[0]==0 && ax2[1]==0 && ax2[2]==0) || - (ax1[0]==ax2[0] && ax1[1]==ax2[1] && ax1[2]==ax2[2])) return; - - // modify axis 2 so it's perpendicular to axis 1 - dReal k = dDOT(ax1,ax2); - for (int i=0; i<3; i++) ax2[i] -= k*ax1[i]; - dNormalize3 (ax2); - - // make v1 = modified axis2, v2 = axis1 x (modified axis2) - dCROSS (v,=,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 == &__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 == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[0].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis1,joint->node[0].body->R,q); - joint->axis1[3] = 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 == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->node[1].body) { - dReal q[4]; - q[0] = x; - q[1] = y; - q[2] = z; - q[3] = 0; - dNormalize3 (q); - dMULTIPLY1_331 (joint->axis2,joint->node[1].body->R,q); - joint->axis1[3] = 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 == &__dhinge2_vtable,"joint is not a hinge2"); - if ((parameter & 0xff00) == 0x100) { - joint->limot2.set (parameter & 0xff,value); - } - else { - if (parameter == dParamSuspensionERP) joint->susp_erp = value; - else if (parameter == dParamSuspensionCFM) joint->susp_cfm = 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 == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->flags & dJOINT_REVERSE) - getAnchor2 (joint,result,joint->anchor2); - else - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetHinge2Anchor2 (dxJointHinge2 *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - if (joint->flags & dJOINT_REVERSE) - getAnchor (joint,result,joint->anchor1); - else - getAnchor2 (joint,result,joint->anchor2); -} - - -extern "C" void dJointGetHinge2Axis1 (dxJointHinge2 *joint, dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__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 == &__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 == &__dhinge2_vtable,"joint is not a hinge2"); - if ((parameter & 0xff00) == 0x100) { - return joint->limot2.get (parameter & 0xff); - } - else { - if (parameter == dParamSuspensionERP) return joint->susp_erp; - else if (parameter == 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 == &__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 == &__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 = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= 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 == &__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 = dDOT(axis,joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis,joint->node[1].body->avel); - return rate; - } - else return 0; -} - - -extern "C" void dJointAddHinge2Torques (dxJointHinge2 *joint, dReal torque1, dReal torque2) -{ - dVector3 axis1, axis2; - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dhinge2_vtable,"joint is not a hinge2"); - - if (joint->node[0].body && joint->node[1].body) { - dMULTIPLY0_331 (axis1,joint->node[0].body->R,joint->axis1); - dMULTIPLY0_331 (axis2,joint->node[1].body->R,joint->axis2); - axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; - axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; - axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; - dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); - dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); - } -} - - -dxJoint::Vtable __dhinge2_vtable = { - sizeof(dxJointHinge2), - (dxJoint::init_fn*) hinge2Init, - (dxJoint::getInfo1_fn*) hinge2GetInfo1, - (dxJoint::getInfo2_fn*) hinge2GetInfo2, - dJointTypeHinge2}; - -//**************************************************************************** -// universal - -// I just realized that the universal joint is equivalent to a hinge 2 joint with -// perfectly stiff suspension. By comparing the hinge 2 implementation to -// the universal implementation, you may be able to improve this -// implementation (or, less likely, the hinge2 implementation). - -static void universalInit (dxJointUniversal *j) -{ - dSetZero (j->anchor1,4); - dSetZero (j->anchor2,4); - dSetZero (j->axis1,4); - j->axis1[0] = 1; - dSetZero (j->axis2,4); - j->axis2[1] = 1; - dSetZero(j->qrel1,4); - dSetZero(j->qrel2,4); - j->limot1.init (j->world); - j->limot2.init (j->world); -} - - -static void getUniversalAxes(dxJointUniversal *joint, dVector3 ax1, dVector3 ax2) -{ - // This says "ax1 = 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] = joint->axis2[0]; - ax2[1] = joint->axis2[1]; - ax2[2] = joint->axis2[2]; - } -} - - -static dReal getUniversalAngle1(dxJointUniversal *joint) -{ - if (joint->node[0].body) { - // length 1 joint axis in global coordinates, from each body - dVector3 ax1, ax2; - dMatrix3 R; - dQuaternion qcross, qq, qrel; - - getUniversalAxes (joint,ax1,ax2); - - // It should be possible to get both angles without explicitly - // constructing the rotation matrix of the cross. Basically, - // orientation of the cross about axis1 comes from body 2, - // about axis 2 comes from body 1, and the perpendicular - // axis can come from the two bodies somehow. (We don't really - // want to assume it's 90 degrees, because in general the - // constraints won't be perfectly satisfied, or even very well - // satisfied.) - // - // However, we'd need a version of getHingeAngleFromRElativeQuat() - // that CAN handle when its relative quat is rotated along a direction - // other than the given axis. What I have here works, - // although it's probably much slower than need be. - - dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); - dRtoQ (R,qcross); - - // This code is essential the same as getHingeAngle(), see the comments - // there for details. - - // get qrel = relative rotation between node[0] and the cross - dQMultiply1 (qq,joint->node[0].body->q,qcross); - dQMultiply2 (qrel,qq,joint->qrel1); - - return getHingeAngleFromRelativeQuat(qrel, joint->axis1); - } - return 0; -} - - -static dReal getUniversalAngle2(dxJointUniversal *joint) -{ - if (joint->node[0].body) { - // length 1 joint axis in global coordinates, from each body - dVector3 ax1, ax2; - dMatrix3 R; - dQuaternion qcross, qq, qrel; - - getUniversalAxes (joint,ax1,ax2); - - // It should be possible to get both angles without explicitly - // constructing the rotation matrix of the cross. Basically, - // orientation of the cross about axis1 comes from body 2, - // about axis 2 comes from body 1, and the perpendicular - // axis can come from the two bodies somehow. (We don't really - // want to assume it's 90 degrees, because in general the - // constraints won't be perfectly satisfied, or even very well - // satisfied.) - // - // However, we'd need a version of getHingeAngleFromRElativeQuat() - // that CAN handle when its relative quat is rotated along a direction - // other than the given axis. What I have here works, - // although it's probably much slower than need be. - - dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); - dRtoQ(R, qcross); - - if (joint->node[1].body) { - dQMultiply1 (qq, joint->node[1].body->q, qcross); - dQMultiply2 (qrel,qq,joint->qrel2); - } - else { - // pretend joint->node[1].body->q is the identity - dQMultiply2 (qrel,qcross, joint->qrel2); - } - - return - getHingeAngleFromRelativeQuat(qrel, joint->axis2); - } - return 0; -} - - -static void universalGetInfo1 (dxJointUniversal *j, dxJoint::Info1 *info) -{ - info->nub = 4; - info->m = 4; - - // see if we're powered or at a joint limit. - bool constraint1 = j->limot1.fmax > 0; - bool constraint2 = j->limot2.fmax > 0; - - bool limiting1 = (j->limot1.lostop >= -M_PI || j->limot1.histop <= M_PI) && - j->limot1.lostop <= j->limot1.histop; - bool limiting2 = (j->limot2.lostop >= -M_PI || j->limot2.histop <= M_PI) && - j->limot2.lostop <= j->limot2.histop; - - // We need to call testRotationLimit() even if we're motored, since it - // records the result. - if (limiting1 || limiting2) { - dReal angle1, angle2; - angle1 = getUniversalAngle1(j); - angle2 = getUniversalAngle2(j); - if (limiting1 && j->limot1.testRotationalLimit (angle1)) constraint1 = true; - if (limiting2 && j->limot2.testRotationalLimit (angle2)) constraint2 = true; - } - if (constraint1) - info->m++; - if (constraint2) - info->m++; -} - - -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 = 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; - dVector3 ax2_temp; - // length 1 vector perpendicular to ax1 and ax2. Neither body can rotate - // about this. - dVector3 p; - dReal k; - - getUniversalAxes(joint, ax1, ax2); - k = dDOT(ax1, ax2); - ax2_temp[0] = ax2[0] - k*ax1[0]; - ax2_temp[1] = ax2[1] - k*ax1[1]; - ax2_temp[2] = ax2[2] - k*ax1[2]; - dCROSS(p, =, ax1, ax2_temp); - dNormalize3(p); - - int s3=3*info->rowskip; - - info->J1a[s3+0] = p[0]; - info->J1a[s3+1] = p[1]; - info->J1a[s3+2] = p[2]; - - if (joint->node[1].body) { - info->J2a[s3+0] = -p[0]; - info->J2a[s3+1] = -p[1]; - info->J2a[s3+2] = -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| = angle/time = erp*(theta - Pi/2) / stepsize - // = (erp*fps) * (theta - Pi/2) - // - // if theta is close to Pi/2, - // theta - Pi/2 ~= cos(theta), so - // |angular_velocity| ~= (erp*fps) * (ax1 dot ax2) - - info->c[3] = info->fps * info->erp * - dDOT(ax1, ax2); - - // if the first angle is powered, or has joint limits, add in the stuff - int row = 4 + joint->limot1.addLimot (joint,info,4,ax1,1); - - // if the second angle is powered, or has joint limits, add in more stuff - joint->limot2.addLimot (joint,info,row,ax2,1); -} - - -static void universalComputeInitialRelativeRotations (dxJointUniversal *joint) -{ - if (joint->node[0].body) { - dVector3 ax1, ax2; - dMatrix3 R; - dQuaternion qcross; - - getUniversalAxes(joint, ax1, ax2); - - // Axis 1. - dRFrom2Axes(R, ax1[0], ax1[1], ax1[2], ax2[0], ax2[1], ax2[2]); - dRtoQ(R, qcross); - dQMultiply1 (joint->qrel1, joint->node[0].body->q, qcross); - - // Axis 2. - dRFrom2Axes(R, ax2[0], ax2[1], ax2[2], ax1[0], ax1[1], ax1[2]); - dRtoQ(R, qcross); - if (joint->node[1].body) { - dQMultiply1 (joint->qrel2, joint->node[1].body->q, qcross); - } - else { - // set joint->qrel to qcross - for (int i=0; i<4; i++) joint->qrel2[i] = qcross[i]; - } - } -} - - -extern "C" void dJointSetUniversalAnchor (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - setAnchors (joint,x,y,z,joint->anchor1,joint->anchor2); - universalComputeInitialRelativeRotations(joint); -} - - -extern "C" void dJointSetUniversalAxis1 (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - setAxes (joint,x,y,z,NULL,joint->axis2); - else - setAxes (joint,x,y,z,joint->axis1,NULL); - universalComputeInitialRelativeRotations(joint); -} - - -extern "C" void dJointSetUniversalAxis2 (dxJointUniversal *joint, - dReal x, dReal y, dReal z) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - setAxes (joint,x,y,z,joint->axis1,NULL); - else - setAxes (joint,x,y,z,NULL,joint->axis2); - universalComputeInitialRelativeRotations(joint); -} - - -extern "C" void dJointGetUniversalAnchor (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - getAnchor2 (joint,result,joint->anchor2); - else - getAnchor (joint,result,joint->anchor1); -} - - -extern "C" void dJointGetUniversalAnchor2 (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - getAnchor (joint,result,joint->anchor1); - else - getAnchor2 (joint,result,joint->anchor2); -} - - -extern "C" void dJointGetUniversalAxis1 (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - getAxis2 (joint,result,joint->axis2); - else - getAxis (joint,result,joint->axis1); -} - - -extern "C" void dJointGetUniversalAxis2 (dxJointUniversal *joint, - dVector3 result) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(result,"bad result argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - getAxis (joint,result,joint->axis1); - else - getAxis2 (joint,result,joint->axis2); -} - - -extern "C" void dJointSetUniversalParam (dxJointUniversal *joint, - int parameter, dReal value) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if ((parameter & 0xff00) == 0x100) { - joint->limot2.set (parameter & 0xff,value); - } - else { - joint->limot1.set (parameter,value); - } -} - - -extern "C" dReal dJointGetUniversalParam (dxJointUniversal *joint, int parameter) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if ((parameter & 0xff00) == 0x100) { - return joint->limot2.get (parameter & 0xff); - } - else { - return joint->limot1.get (parameter); - } -} - - -extern "C" dReal dJointGetUniversalAngle1 (dxJointUniversal *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - return getUniversalAngle2 (joint); - else - return getUniversalAngle1 (joint); -} - - -extern "C" dReal dJointGetUniversalAngle2 (dxJointUniversal *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - if (joint->flags & dJOINT_REVERSE) - return getUniversalAngle1 (joint); - else - return getUniversalAngle2 (joint); -} - - -extern "C" dReal dJointGetUniversalAngle1Rate (dxJointUniversal *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - - if (joint->node[0].body) { - dVector3 axis; - - if (joint->flags & dJOINT_REVERSE) - getAxis2 (joint,axis,joint->axis2); - else - getAxis (joint,axis,joint->axis1); - - dReal rate = dDOT(axis, joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); - return rate; - } - return 0; -} - - -extern "C" dReal dJointGetUniversalAngle2Rate (dxJointUniversal *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - - if (joint->node[0].body) { - dVector3 axis; - - if (joint->flags & dJOINT_REVERSE) - getAxis (joint,axis,joint->axis1); - else - getAxis2 (joint,axis,joint->axis2); - - dReal rate = dDOT(axis, joint->node[0].body->avel); - if (joint->node[1].body) rate -= dDOT(axis, joint->node[1].body->avel); - return rate; - } - return 0; -} - - -extern "C" void dJointAddUniversalTorques (dxJointUniversal *joint, dReal torque1, dReal torque2) -{ - dVector3 axis1, axis2; - dAASSERT(joint); - dUASSERT(joint->vtable == &__duniversal_vtable,"joint is not a universal"); - - if (joint->flags & dJOINT_REVERSE) { - dReal temp = torque1; - torque1 = - torque2; - torque2 = - temp; - } - - getAxis (joint,axis1,joint->axis1); - getAxis2 (joint,axis2,joint->axis2); - axis1[0] = axis1[0] * torque1 + axis2[0] * torque2; - axis1[1] = axis1[1] * torque1 + axis2[1] * torque2; - axis1[2] = axis1[2] * torque1 + axis2[2] * torque2; - - if (joint->node[0].body != 0) - dBodyAddTorque (joint->node[0].body,axis1[0],axis1[1],axis1[2]); - if (joint->node[1].body != 0) - dBodyAddTorque(joint->node[1].body, -axis1[0], -axis1[1], -axis1[2]); -} - - - - - -dxJoint::Vtable __duniversal_vtable = { - sizeof(dxJointUniversal), - (dxJoint::init_fn*) universalInit, - (dxJoint::getInfo1_fn*) universalGetInfo1, - (dxJoint::getInfo2_fn*) universalGetInfo2, - dJointTypeUniversal}; - -//**************************************************************************** -// angular motor - -static void amotorInit (dxJointAMotor *j) -{ - int i; - j->num = 0; - j->mode = dAMotorUser; - for (i=0; i<3; i++) { - j->rel[i] = 0; - dSetZero (j->axis[i],4); - j->limot[i].init (j->world); - j->angle[i] = 0; - } - dSetZero (j->reference1,4); - dSetZero (j->reference2,4); - - j->flags |= dJOINT_TWOBODIES; -} - - -// compute the 3 axes in global coordinates - -static void amotorComputeGlobalAxes (dxJointAMotor *joint, dVector3 ax[3]) -{ - if (joint->mode == 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],=,ax[2],ax[0]); - dNormalize3 (ax[1]); - } - else { - for (int i=0; i < joint->num; i++) { - if (joint->rel[i] == 1) { - // relative to b1 - dMULTIPLY0_331 (ax[i],joint->node[0].body->R,joint->axis[i]); - } - if (joint->rel[i] == 2) { - // relative to b2 - dMULTIPLY0_331 (ax[i],joint->node[1].body->R,joint->axis[i]); - } - else { - // global - just copy it - ax[i][0] = joint->axis[i][0]; - ax[i][1] = joint->axis[i][1]; - ax[i][2] = 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] = 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,=,ax[0],ref1); - joint->angle[0] = -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,=,ax[0],ax[1]); - joint->angle[1] = -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,=,ax[1],ax[2]); - joint->angle[2] = -dAtan2 (dDOT(ref2,ax[1]), dDOT(ref2,q)); -} - - -// set the reference vectors as follows: -// * reference1 = current axis[2] relative to body 1 -// * reference2 = 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 = 0; - info->nub = 0; - - // compute the axes and angles, if in euler mode - if (j->mode == 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=0; 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 = 0 ax[1] x ax[2] ax[0] - // d(angle[1])/dt = 0 ax[1] - // d(angle[2])/dt = 0 ax[0] x ax[1] ax[2] - // - // constraining w2-w1 along an axis 'a' means that a'*(w2-w1)=0. - // 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] = &ax[0]; - axptr[1] = &ax[1]; - axptr[2] = &ax[2]; - - dVector3 ax0_cross_ax1; - dVector3 ax1_cross_ax2; - if (joint->mode == dAMotorEuler) { - dCROSS (ax0_cross_ax1,=,ax[0],ax[1]); - axptr[2] = &ax0_cross_ax1; - dCROSS (ax1_cross_ax2,=,ax[1],ax[2]); - axptr[0] = &ax1_cross_ax2; - } - - int row=0; - for (i=0; i < joint->num; i++) { - row += joint->limot[i].addLimot (joint,info,row,*(axptr[i]),1); - } -} - - -extern "C" void dJointSetAMotorNumAxes (dxJointAMotor *joint, int num) -{ - dAASSERT(joint && num >= 0 && num <= 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (joint->mode == dAMotorEuler) { - joint->num = 3; - } - else { - if (num < 0) num = 0; - if (num > 3) num = 3; - joint->num = num; - } -} - - -extern "C" void dJointSetAMotorAxis (dxJointAMotor *joint, int anum, int rel, - dReal x, dReal y, dReal z) -{ - dAASSERT(joint && anum >= 0 && anum <= 2 && rel >= 0 && rel <= 2); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - joint->rel[anum] = 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] = x; - r[1] = y; - r[2] = z; - r[3] = 0; - if (rel > 0) { - if (rel==1) { - 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] = r[0]; - joint->axis[anum][1] = r[1]; - joint->axis[anum][2] = r[2]; - } - dNormalize3 (joint->axis[anum]); - if (joint->mode == dAMotorEuler) amotorSetEulerReferenceVectors (joint); -} - - -extern "C" void dJointSetAMotorAngle (dxJointAMotor *joint, int anum, - dReal angle) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (joint->mode == dAMotorUser) { - if (anum < 0) anum = 0; - if (anum > 3) anum = 3; - joint->angle[anum] = angle; - } -} - - -extern "C" void dJointSetAMotorParam (dxJointAMotor *joint, int parameter, - dReal value) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - int anum = parameter >> 8; - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - parameter &= 0xff; - joint->limot[anum].set (parameter, value); -} - - -extern "C" void dJointSetAMotorMode (dxJointAMotor *joint, int mode) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - joint->mode = mode; - if (joint->mode == dAMotorEuler) { - joint->num = 3; - amotorSetEulerReferenceVectors (joint); - } -} - - -extern "C" int dJointGetAMotorNumAxes (dxJointAMotor *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - return joint->num; -} - - -extern "C" void dJointGetAMotorAxis (dxJointAMotor *joint, int anum, - dVector3 result) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - if (joint->rel[anum] > 0) { - if (joint->rel[anum]==1) { - 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] = joint->axis[anum][0]; - result[1] = joint->axis[anum][1]; - result[2] = joint->axis[anum][2]; - } -} - - -extern "C" int dJointGetAMotorAxisRel (dxJointAMotor *joint, int anum) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - return joint->rel[anum]; -} - - -extern "C" dReal dJointGetAMotorAngle (dxJointAMotor *joint, int anum) -{ - dAASSERT(joint && anum >= 0 && anum < 3); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - if (anum < 0) anum = 0; - if (anum > 3) anum = 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 == &__damotor_vtable,"joint is not an amotor"); - int anum = parameter >> 8; - if (anum < 0) anum = 0; - if (anum > 2) anum = 2; - parameter &= 0xff; - return joint->limot[anum].get (parameter); -} - - -extern "C" int dJointGetAMotorMode (dxJointAMotor *joint) -{ - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - return joint->mode; -} - - -extern "C" void dJointAddAMotorTorques (dxJointAMotor *joint, dReal torque1, dReal torque2, dReal torque3) -{ - dVector3 axes[3]; - dAASSERT(joint); - dUASSERT(joint->vtable == &__damotor_vtable,"joint is not an amotor"); - - if (joint->num == 0) - return; - dUASSERT((joint->flags & dJOINT_REVERSE) == 0, "dJointAddAMotorTorques not yet implemented for reverse AMotor joints"); - - amotorComputeGlobalAxes (joint,axes); - axes[0][0] *= torque1; - axes[0][1] *= torque1; - axes[0][2] *= torque1; - if (joint->num >= 2) { - axes[0][0] += axes[1][0] * torque2; - axes[0][1] += axes[1][0] * torque2; - axes[0][2] += axes[1][0] * torque2; - if (joint->num >= 3) { - axes[0][0] += axes[2][0] * torque3; - axes[0][1] += axes[2][0] * torque3; - axes[0][2] += axes[2][0] * torque3; - } - } - - if (joint->node[0].body != 0) - dBodyAddTorque (joint->node[0].body,axes[0][0],axes[0][1],axes[0][2]); - if (joint->node[1].body != 0) - dBodyAddTorque(joint->node[1].body, -axes[0][0], -axes[0][1], -axes[0][2]); -} - - -dxJoint::Vtable __damotor_vtable = { - 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); - dSetZero (j->qrel,4); -} - - -static void fixedGetInfo1 (dxJointFixed *j, dxJoint::Info1 *info) -{ - info->m = 6; - info->nub = 6; -} - - -static void fixedGetInfo2 (dxJointFixed *joint, dxJoint::Info2 *info) -{ - int s = info->rowskip; - - // Three rows for orientation - setFixedOrientation(joint, info, joint->qrel, 3); - - // Three rows for position. - // set jacobian - info->J1l[0] = 1; - info->J1l[s+1] = 1; - info->J1l[2*s+2] = 1; - - dVector3 ofs; - dMULTIPLY0_331 (ofs,joint->node[0].body->R,joint->offset); - if (joint->node[1].body) { - dCROSSMAT (info->J1a,ofs,s,+,-); - info->J2l[0] = -1; - info->J2l[s+1] = -1; - info->J2l[2*s+2] = -1; - } - - // set right hand side for the first three rows (linear) - dReal k = info->fps * info->erp; - if (joint->node[1].body) { - for (int j=0; j<3; j++) - info->c[j] = k * (joint->node[1].body->pos[j] - - joint->node[0].body->pos[j] + ofs[j]); - } - else { - for (int j=0; j<3; j++) - info->c[j] = k * (joint->offset[j] - joint->node[0].body->pos[j]); - } -} - - -extern "C" void dJointSetFixed (dxJointFixed *joint) -{ - dUASSERT(joint,"bad joint argument"); - dUASSERT(joint->vtable == &__dfixed_vtable,"joint is not fixed"); - int i; - - // This code is taken from sJointSetSliderAxis(), we should really put the - // common code in its own function. - // compute the offset between the bodies - if (joint->node[0].body) { - if (joint->node[1].body) { - dQMultiply1 (joint->qrel,joint->node[0].body->q,joint->node[1].body->q); - dReal ofs[4]; - for (i=0; i<4; i++) ofs[i] = joint->node[0].body->pos[i]; - for (i=0; i<4; i++) ofs[i] -= joint->node[1].body->pos[i]; - dMULTIPLY1_331 (joint->offset,joint->node[0].body->R,ofs); - } - else { - // set joint->qrel to the transpose of the first body's q - joint->qrel[0] = joint->node[0].body->q[0]; - for (i=1; i<4; i++) joint->qrel[i] = -joint->node[0].body->q[i]; - for (i=0; i<4; i++) joint->offset[i] = joint->node[0].body->pos[i]; - } - } -} - - -dxJoint::Vtable __dfixed_vtable = { - 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 = 0; - info->nub = 0; -} - - -static void nullGetInfo2 (dxJointNull *joint, dxJoint::Info2 *info) -{ - dDebug (0,"this should never get called"); -} - - -dxJoint::Vtable __dnull_vtable = { - sizeof(dxJointNull), - (dxJoint::init_fn*) 0, - (dxJoint::getInfo1_fn*) nullGetInfo1, - (dxJoint::getInfo2_fn*) nullGetInfo2, - dJointTypeNull}; - -/******************** breakable joint contribution ***********************/ -extern "C" void dJointSetBreakable (dxJoint *joint, int b) { - dAASSERT(joint); - if (b) { - // we want this joint to be breakable but we must first check if it - // was already breakable - if (!joint->breakInfo) { - // allocate a dxJointBreakInfo struct - joint->breakInfo = new dxJointBreakInfo; - joint->breakInfo->flags = 0; - for (int i = 0; i < 3; i++) { - joint->breakInfo->b1MaxF[0] = 0; - joint->breakInfo->b1MaxT[0] = 0; - joint->breakInfo->b2MaxF[0] = 0; - joint->breakInfo->b2MaxT[0] = 0; - } - joint->breakInfo->callback = 0; - } - else { - // the joint was already breakable - return; - } - } - else { - // we want this joint to be unbreakable mut we must first check if - // it is alreay unbreakable - if (joint->breakInfo) { - // deallocate the dxJointBreakInfo struct - delete joint->breakInfo; - joint->breakInfo = 0; - } - else { - // the joint was already unbreakable - return; - } - } -} - -extern "C" void dJointSetBreakCallback (dxJoint *joint, dJointBreakCallback *callbackFunc) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointSetBreakCallback called on unbreakable joint"); - } -# endif - joint->breakInfo->callback = callbackFunc; -} - -extern "C" void dJointSetBreakMode (dxJoint *joint, int mode) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointSetBreakMode called on unbreakable joint"); - } -# endif - joint->breakInfo->flags = mode; -} - -extern "C" int dJointGetBreakMode (dxJoint *joint) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointGetBreakMode called on unbreakable joint"); - } -# endif - return joint->breakInfo->flags; -} - -extern "C" void dJointSetBreakForce (dxJoint *joint, int body, dReal x, dReal y, dReal z) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointSetBreakForce called on unbreakable joint"); - } -# endif - if (body) { - joint->breakInfo->b2MaxF[0] = x; - joint->breakInfo->b2MaxF[1] = y; - joint->breakInfo->b2MaxF[2] = z; - } - else { - joint->breakInfo->b1MaxF[0] = x; - joint->breakInfo->b1MaxF[1] = y; - joint->breakInfo->b1MaxF[2] = z; - } -} - -extern "C" void dJointSetBreakTorque (dxJoint *joint, int body, dReal x, dReal y, dReal z) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointSetBreakTorque called on unbreakable joint"); - } -# endif - if (body) { - joint->breakInfo->b2MaxT[0] = x; - joint->breakInfo->b2MaxT[1] = y; - joint->breakInfo->b2MaxT[2] = z; - } - else { - joint->breakInfo->b1MaxT[0] = x; - joint->breakInfo->b1MaxT[1] = y; - joint->breakInfo->b1MaxT[2] = z; - } -} - -extern "C" int dJointIsBreakable (dxJoint *joint) { - dAASSERT(joint); - return joint->breakInfo != 0; -} - -extern "C" void dJointGetBreakForce (dxJoint *joint, int body, dReal *force) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointGetBreakForce called on unbreakable joint"); - } -# endif - if (body) - for (int i=0; i<3; i++) force[i]=joint->breakInfo->b2MaxF[i]; - else - for (int i=0; i<3; i++) force[i]=joint->breakInfo->b1MaxF[i]; -} - -extern "C" void dJointGetBreakTorque (dxJoint *joint, int body, dReal *torque) { - dAASSERT(joint); -# ifndef dNODEBUG - // only works for a breakable joint - if (!joint->breakInfo) { - dDebug (0, "dJointGetBreakTorque called on unbreakable joint"); - } -# endif - if (body) - for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b2MaxT[i]; - else - for (int i=0; i<3; i++) torque[i]=joint->breakInfo->b1MaxT[i]; -} -/*************************************************************************/ -- cgit v1.1