/************************************************************************* * * * 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 #include "dball.h" #include "joint_internal.h" /* * Double Ball joint: tries to maintain a fixed distance between two anchor * points. */ dxJointDBall::dxJointDBall(dxWorld *w) : dxJoint(w) { dSetZero(anchor1, 3); dSetZero(anchor2, 3); targetDistance = 0; erp = world->global_erp; cfm = world->global_cfm; } void dxJointDBall::getSureMaxInfo( SureMaxInfo* info ) { info->max_m = 1; } void dxJointDBall::getInfo1( dxJoint::Info1 *info ) { info->m = 1; info->nub = 1; } void dxJointDBall::getInfo2( dReal worldFPS, dReal /*worldERP*/, int rowskip, dReal *J1, dReal *J2, int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, int *findex ) { dVector3 globalA1, globalA2; dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], globalA1); if (node[1].body) { dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], globalA2); } else { dCopyVector3(globalA2, anchor2); } dVector3 q; dSubtractVectors3(q, globalA1, globalA2); #ifdef dSINGLE const dReal MIN_LENGTH = REAL(1e-7); #else const dReal MIN_LENGTH = REAL(1e-12); #endif if (dCalcVectorLength3(q) < MIN_LENGTH) { // too small, let's choose an arbitrary direction // heuristic: difference in velocities at anchors dVector3 v1, v2; dBodyGetPointVel(node[0].body, globalA1[0], globalA1[1], globalA1[2], v1); if (node[1].body) { dBodyGetPointVel(node[1].body, globalA2[0], globalA2[1], globalA2[2], v2); } else { dZeroVector3(v2); } dSubtractVectors3(q, v1, v2); if (dCalcVectorLength3(q) < MIN_LENGTH) { // this direction is as good as any dAssignVector3(q, 1, 0, 0); } } dNormalize3(q); dCopyVector3(J1 + GI2__JL_MIN, q); dVector3 relA1; dBodyVectorToWorld(node[0].body, anchor1[0], anchor1[1], anchor1[2], relA1); dMatrix3 a1m; dZeroMatrix3(a1m); dSetCrossMatrixMinus(a1m, relA1, 4); dMultiply1_331(J1 + GI2__JA_MIN, a1m, q); if (node[1].body) { dCopyNegatedVector3(J2 + GI2__JL_MIN, q); dVector3 relA2; dBodyVectorToWorld(node[1].body, anchor2[0], anchor2[1], anchor2[2], relA2); dMatrix3 a2m; dZeroMatrix3(a2m); dSetCrossMatrixPlus(a2m, relA2, 4); dMultiply1_331(J2 + GI2__JA_MIN, a2m, q); } const dReal k = worldFPS * this->erp; pairRhsCfm[GI2_RHS] = k * (targetDistance - dCalcPointsDistance3(globalA1, globalA2)); pairRhsCfm[GI2_CFM] = this->cfm; } void dxJointDBall::updateTargetDistance() { dVector3 p1, p2; if (node[0].body) dBodyGetRelPointPos(node[0].body, anchor1[0], anchor1[1], anchor1[2], p1); else dCopyVector3(p1, anchor1); if (node[1].body) dBodyGetRelPointPos(node[1].body, anchor2[0], anchor2[1], anchor2[2], p2); else dCopyVector3(p2, anchor2); targetDistance = dCalcPointsDistance3(p1, p2); } void dJointSetDBallAnchor1( dJointID j, dReal x, dReal y, dReal z ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); if ( joint->flags & dJOINT_REVERSE ) { if (joint->node[1].body) dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2); else { joint->anchor2[0] = x; joint->anchor2[1] = y; joint->anchor2[2] = z; } } else { if (joint->node[0].body) dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1); else { joint->anchor1[0] = x; joint->anchor1[1] = y; joint->anchor1[2] = z; } } joint->updateTargetDistance(); } void dJointSetDBallAnchor2( dJointID j, dReal x, dReal y, dReal z ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); if ( joint->flags & dJOINT_REVERSE ) { if (joint->node[0].body) dBodyGetPosRelPoint(joint->node[0].body, x, y, z, joint->anchor1); else { joint->anchor1[0] = x; joint->anchor1[1] = y; joint->anchor1[2] = z; } } else { if (joint->node[1].body) dBodyGetPosRelPoint(joint->node[1].body, x, y, z, joint->anchor2); else { joint->anchor2[0] = x; joint->anchor2[1] = y; joint->anchor2[2] = z; } } joint->updateTargetDistance(); } dReal dJointGetDBallDistance(dJointID j) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); return joint->targetDistance; } void dJointSetDBallDistance(dJointID j, dReal dist) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); dUASSERT( dist>=0, "target distance must be non-negative" ); joint->targetDistance = dist; } void dJointGetDBallAnchor1( dJointID j, dVector3 result ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); if ( joint->flags & dJOINT_REVERSE ) { if (joint->node[1].body) dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result); else dCopyVector3(result, joint->anchor2); } else { if (joint->node[0].body) dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result); else dCopyVector3(result, joint->anchor1); } } void dJointGetDBallAnchor2( dJointID j, dVector3 result ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); dUASSERT( result, "bad result argument" ); if ( joint->flags & dJOINT_REVERSE ) { if (joint->node[0].body) dBodyGetRelPointPos(joint->node[0].body, joint->anchor1[0], joint->anchor1[1], joint->anchor1[2], result); else dCopyVector3(result, joint->anchor1); } else { if (joint->node[1].body) dBodyGetRelPointPos(joint->node[1].body, joint->anchor2[0], joint->anchor2[1], joint->anchor2[2], result); else dCopyVector3(result, joint->anchor2); } } void dJointSetDBallParam( dJointID j, int parameter, dReal value ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); switch ( parameter ) { case dParamCFM: joint->cfm = value; break; case dParamERP: joint->erp = value; break; } } dReal dJointGetDBallParam( dJointID j, int parameter ) { dxJointDBall* joint = static_cast(j); dUASSERT( joint, "bad joint argument" ); switch ( parameter ) { case dParamCFM: return joint->cfm; case dParamERP: return joint->erp; default: return 0; } } dJointType dxJointDBall::type() const { return dJointTypeDBall; } sizeint dxJointDBall::size() const { return sizeof( *this ); } void dxJointDBall::setRelativeValues() { updateTargetDistance(); }