From c5fc66ee58f2c60f2d226868bb1cf5b91badaf53 Mon Sep 17 00:00:00 2001 From: sanine Date: Sat, 1 Oct 2022 20:59:36 -0500 Subject: add ode --- libs/ode-0.16.1/ode/src/joints/fixed.cpp | 216 +++++++++++++++++++++++++++++++ 1 file changed, 216 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/joints/fixed.cpp (limited to 'libs/ode-0.16.1/ode/src/joints/fixed.cpp') diff --git a/libs/ode-0.16.1/ode/src/joints/fixed.cpp b/libs/ode-0.16.1/ode/src/joints/fixed.cpp new file mode 100644 index 0000000..527bf48 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/fixed.cpp @@ -0,0 +1,216 @@ +/************************************************************************* + * * + * 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 "config.h" +#include "fixed.h" +#include "joint_internal.h" + + + +//**************************************************************************** +// fixed joint + +dxJointFixed::dxJointFixed ( dxWorld *w ) : + dxJoint ( w ) +{ + dSetZero ( offset, 4 ); + dSetZero ( qrel, 4 ); + erp = world->global_erp; + cfm = world->global_cfm; +} + + +void +dxJointFixed::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + +void +dxJointFixed::getInfo1 ( dxJoint::Info1 *info ) +{ + info->m = 6; + info->nub = 6; +} + + +void +dxJointFixed::getInfo2 ( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + // Three rows for orientation + setFixedOrientation ( this, worldFPS, worldERP, + rowskip, J1 + dSA__MAX * rowskip, J2 + dSA__MAX * rowskip, + pairskip, pairRhsCfm + dSA__MAX * pairskip, qrel ); + + // Three rows for position. + // set Jacobian + J1[GI2_JLX] = 1; + J1[rowskip + GI2_JLY] = 1; + J1[2 * rowskip + GI2_JLZ] = 1; + + dReal k = worldFPS * this->erp; + dxBody *b0 = node[0].body, *b1 = node[1].body; + + dVector3 ofs; + dMultiply0_331 ( ofs, b0->posr.R, offset ); + + if ( b1 ) { + dSetCrossMatrixPlus( J1 + GI2__JA_MIN, ofs, rowskip ); + + J2[GI2_JLX] = -1; + J2[rowskip + GI2_JLY] = -1; + J2[2 * rowskip + GI2_JLZ] = -1; + } + + // set right hand side for the first three rows (linear) + if ( b1 ) { + for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) { + pairRhsCfm[currPairSkip + GI2_RHS] = k * ( b1->posr.pos[j] - b0->posr.pos[j] + ofs[j] ); + } + } else { + for ( int j = 0, currPairSkip = 0; j < 3; currPairSkip += pairskip, ++j ) { + pairRhsCfm[currPairSkip + GI2_RHS] = k * ( offset[j] - b0->posr.pos[j] ); + } + } + + dReal cfm = this->cfm; + pairRhsCfm[GI2_CFM] = cfm; + pairRhsCfm[pairskip + GI2_CFM] = cfm; + pairRhsCfm[2 * pairskip + GI2_CFM] = cfm; +} + + +void dJointSetFixed ( dJointID j ) +{ + dxJointFixed* joint = ( dxJointFixed* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Fixed ); + int i; + + // This code is taken from dJointSetSliderAxis(), 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 ) + { + dReal ofs[4]; + for ( i = 0; i < 4; i++ ) + ofs[i] = joint->node[0].body->posr.pos[i] - joint->node[1].body->posr.pos[i]; + dMultiply1_331 ( joint->offset, joint->node[0].body->posr.R, ofs ); + } + else + { + joint->offset[0] = joint->node[0].body->posr.pos[0]; + joint->offset[1] = joint->node[0].body->posr.pos[1]; + joint->offset[2] = joint->node[0].body->posr.pos[2]; + } + } + + joint->computeInitialRelativeRotation(); +} + +void dxJointFixed::set ( int num, dReal value ) +{ + switch ( num ) + { + case dParamCFM: + cfm = value; + break; + case dParamERP: + erp = value; + break; + } +} + + +dReal dxJointFixed::get ( int num ) +{ + switch ( num ) + { + case dParamCFM: + return cfm; + case dParamERP: + return erp; + default: + return 0; + } +} + + +void dJointSetFixedParam ( dJointID j, int parameter, dReal value ) +{ + dxJointFixed* joint = ( dxJointFixed* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Fixed ); + joint->set ( parameter, value ); +} + + +dReal dJointGetFixedParam ( dJointID j, int parameter ) +{ + dxJointFixed* joint = ( dxJointFixed* ) j; + dUASSERT ( joint, "bad joint argument" ); + checktype ( joint, Fixed ); + return joint->get ( parameter ); +} + + +dJointType +dxJointFixed::type() const +{ + return dJointTypeFixed; +} + + +sizeint +dxJointFixed::size() const +{ + return sizeof ( *this ); +} + +void +dxJointFixed::computeInitialRelativeRotation() +{ + if (node[0].body ) + { + if (node[1].body ) + { + dQMultiply1 (qrel, node[0].body->q, node[1].body->q ); + } + else + { + // set qrel to the transpose of the first body q + qrel[0] = node[0].body->q[0]; + qrel[1] = -node[0].body->q[1]; + qrel[2] = -node[0].body->q[2]; + qrel[3] = -node[0].body->q[3]; + } + } +} + -- cgit v1.2.1