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/contact.cpp | 361 +++++++++++++++++++++++++++++ 1 file changed, 361 insertions(+) create mode 100644 libs/ode-0.16.1/ode/src/joints/contact.cpp (limited to 'libs/ode-0.16.1/ode/src/joints/contact.cpp') diff --git a/libs/ode-0.16.1/ode/src/joints/contact.cpp b/libs/ode-0.16.1/ode/src/joints/contact.cpp new file mode 100644 index 0000000..5ab3482 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/joints/contact.cpp @@ -0,0 +1,361 @@ +/************************************************************************* + * * + * 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 "contact.h" +#include "joint_internal.h" + + + + //**************************************************************************** + // contact + +dxJointContact::dxJointContact(dxWorld *w) : + dxJoint(w) +{ +} + + +void +dxJointContact::getSureMaxInfo(SureMaxInfo* info) +{ + // ...as the actual m is very likely to hit the maximum + info->max_m = (contact.surface.mode&dContactRolling) ? 6 : 3; +} + + +void +dxJointContact::getInfo1(dxJoint::Info1 *info) +{ + // make sure mu's >= 0, then calculate number of constraint rows and number + // of unbounded rows. + int m = 1, nub = 0; + + // Anisotropic sliding and rolling and spinning friction + if (contact.surface.mode & dContactAxisDep) { + if (contact.surface.mu < 0) { + contact.surface.mu = 0; + } + else if (contact.surface.mu > 0) { + if (contact.surface.mu == dInfinity) { nub++; } + m++; + } + + if (contact.surface.mu2 < 0) { + contact.surface.mu2 = 0; + } + else if (contact.surface.mu2 > 0) { + if (contact.surface.mu2 == dInfinity) { nub++; } + m++; + } + + if ((contact.surface.mode & dContactRolling) != 0) { + if (contact.surface.rho < 0) { + contact.surface.rho = 0; + } + else { + if (contact.surface.rho == dInfinity) { nub++; } + m++; + } + + if (contact.surface.rho2 < 0) { + contact.surface.rho2 = 0; + } + else { + if (contact.surface.rho2 == dInfinity) { nub++; } + m++; + } + + if (contact.surface.rhoN < 0) { + contact.surface.rhoN = 0; + } + else { + if (contact.surface.rhoN == dInfinity) { nub++; } + m++; + } + } + } + else { + if (contact.surface.mu < 0) { + contact.surface.mu = 0; + } + else if (contact.surface.mu > 0) { + if (contact.surface.mu == dInfinity) { nub += 2; } + m += 2; + } + + if ((contact.surface.mode & dContactRolling) != 0) { + if (contact.surface.rho < 0) { + contact.surface.rho = 0; + } + else { + if (contact.surface.rho == dInfinity) { nub += 3; } + m += 3; + } + } + } + + the_m = m; + info->m = m; + info->nub = nub; +} + + +void +dxJointContact::getInfo2(dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex) +{ + enum + { + ROW_NORMAL, + + ROW__OPTIONAL_MIN, + }; + + const int surface_mode = contact.surface.mode; + + // set right hand side and cfm value for normal + dReal erp = (surface_mode & dContactSoftERP) != 0 ? contact.surface.soft_erp : worldERP; + dReal k = worldFPS * erp; + + dReal depth = contact.geom.depth - world->contactp.min_depth; + if (depth < 0) depth = 0; + + dReal motionN = (surface_mode & dContactMotionN) != 0 ? contact.surface.motionN : REAL(0.0); + const dReal pushout = k * depth + motionN; + + bool apply_bounce = (surface_mode & dContactBounce) != 0 && contact.surface.bounce_vel >= 0; + dReal outgoing = 0; + + // note: this cap should not limit bounce velocity + const dReal maxvel = world->contactp.max_vel; + dReal c = pushout > maxvel ? maxvel : pushout; + + // c1,c2 = contact points with respect to body PORs + dVector3 c1, c2 = { 0, }; + + // get normal, with sign adjusted for body1/body2 polarity + dVector3 normal; + if ((flags & dJOINT_REVERSE) != 0) { + dCopyNegatedVector3(normal, contact.geom.normal); + } + else { + dCopyVector3(normal, contact.geom.normal); + } + + dxBody *b1 = node[1].body; + if (b1) { + dSubtractVectors3(c2, contact.geom.pos, b1->posr.pos); + // set Jacobian for b1 normal + dCopyNegatedVector3(J2 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal); + dCalcVectorCross3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, normal, c2); //== dCalcVectorCross3( J2 + GI2__JA_MIN, c2, normal ); dNegateVector3( J2 + GI2__JA_MIN ); + if (apply_bounce) { + outgoing /*+*/= dCalcVectorDot3(J2 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[1].body->avel) + - dCalcVectorDot3(normal, node[1].body->lvel); + } + } + + dxBody *b0 = node[0].body; + dSubtractVectors3(c1, contact.geom.pos, b0->posr.pos); + // set Jacobian for b0 normal + dCopyVector3(J1 + ROW_NORMAL * rowskip + GI2__JL_MIN, normal); + dCalcVectorCross3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, c1, normal); + if (apply_bounce) { + // calculate outgoing velocity (-ve for incoming contact) + outgoing += dCalcVectorDot3(J1 + ROW_NORMAL * rowskip + GI2__JA_MIN, node[0].body->avel) + + dCalcVectorDot3(normal, node[0].body->lvel); + } + + // deal with bounce + if (apply_bounce) { + dReal negated_outgoing = motionN - outgoing; + // only apply bounce if the outgoing velocity is greater than the + // threshold, and if the resulting c[rowNormal] exceeds what we already have. + dIASSERT(contact.surface.bounce_vel >= 0); + if (/*contact.surface.bounce_vel >= 0 &&*/ + negated_outgoing > contact.surface.bounce_vel) { + const dReal newc = contact.surface.bounce * negated_outgoing + motionN; + if (newc > c) { c = newc; } + } + } + + pairRhsCfm[ROW_NORMAL * pairskip + GI2_RHS] = c; + + if ((surface_mode & dContactSoftCFM) != 0) { + pairRhsCfm[ROW_NORMAL * pairskip + GI2_CFM] = contact.surface.soft_cfm; + } + + // set LCP limits for normal + pairLoHi[ROW_NORMAL * pairskip + GI2_LO] = 0; + pairLoHi[ROW_NORMAL * pairskip + GI2_HI] = dInfinity; + + + if (the_m > 1) { // if no friction, there is nothing else to do + // now do jacobian for tangential forces + dVector3 t1, t2; // two vectors tangential to normal + + if ((surface_mode & dContactFDir1) != 0) { // use fdir1 ? + dCopyVector3(t1, contact.fdir1); + dCalcVectorCross3(t2, normal, t1); + } + else { + dPlaneSpace(normal, t1, t2); + } + + int row = ROW__OPTIONAL_MIN; + int currRowSkip = row * rowskip, currPairSkip = row * pairskip; + + // first friction direction + const dReal mu = contact.surface.mu; + + if (mu > 0) { + dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t1); + dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t1); + + if (node[1].body) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t1); + dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t1, c2); //== dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, c2, t1 ); dNegateVector3( J2 + rowskip + GI2__JA_MIN ); + } + + // set right hand side + if ((surface_mode & dContactMotion1) != 0) { + pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion1; + } + // set slip (constraint force mixing) + if ((surface_mode & dContactSlip1) != 0) { + pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip1; + } + + // set LCP bounds and friction index. this depends on the approximation + // mode + pairLoHi[currPairSkip + GI2_LO] = -mu; + pairLoHi[currPairSkip + GI2_HI] = mu; + + if ((surface_mode & dContactApprox1_1) != 0) { + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // second friction direction + const dReal mu2 = (surface_mode & dContactMu2) != 0 ? contact.surface.mu2 : mu; + + if (mu2 > 0) { + dCopyVector3(J1 + currRowSkip + GI2__JL_MIN, t2); + dCalcVectorCross3(J1 + currRowSkip + GI2__JA_MIN, c1, t2); + + if (node[1].body) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JL_MIN, t2); + dCalcVectorCross3(J2 + currRowSkip + GI2__JA_MIN, t2, c2); //== dCalcVectorCross3( J2 + currRowSkip + GI2__JA_MIN, c2, t2 ); dNegateVector3( J2 + currRowSkip + GI2__JA_MIN ); + } + + // set right hand side + if ((surface_mode & dContactMotion2) != 0) { + pairRhsCfm[currPairSkip + GI2_RHS] = contact.surface.motion2; + } + // set slip (constraint force mixing) + if ((surface_mode & dContactSlip2) != 0) { + pairRhsCfm[currPairSkip + GI2_CFM] = contact.surface.slip2; + } + + // set LCP bounds and friction index. this depends on the approximation + // mode + pairLoHi[currPairSkip + GI2_LO] = -mu2; + pairLoHi[currPairSkip + GI2_HI] = mu2; + + if ((surface_mode & dContactApprox1_2) != 0) { + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + + // Handle rolling/spinning friction + if ((surface_mode & dContactRolling) != 0) { + + const dReal *const ax[3] = { + t1, // Rolling around t1 creates movement parallel to t2 + t2, + normal // Spinning axis + }; + + const int approx_bits[3] = { dContactApprox1_1, dContactApprox1_2, dContactApprox1_N }; + + // Get the coefficients + dReal rho[3]; + rho[0] = contact.surface.rho; + if ((surface_mode & dContactAxisDep) != 0) { + rho[1] = contact.surface.rho2; + rho[2] = contact.surface.rhoN; + } + else { + rho[1] = rho[0]; + rho[2] = rho[0]; + } + + for (int i = 0; i != 3; ++i) { + if (rho[i] > 0) { + // Set the angular axis + dCopyVector3(J1 + currRowSkip + GI2__JA_MIN, ax[i]); + + if (b1) { + dCopyNegatedVector3(J2 + currRowSkip + GI2__JA_MIN, ax[i]); + } + + // Set the lcp limits + pairLoHi[currPairSkip + GI2_LO] = -rho[i]; + pairLoHi[currPairSkip + GI2_HI] = rho[i]; + + // Should we use proportional force? + if ((surface_mode & approx_bits[i]) != 0) { + // Make limits proportional to normal force + findex[row] = 0; + } + + ++row; + currRowSkip += rowskip; currPairSkip += pairskip; + } + } + } + } +} + +dJointType +dxJointContact::type() const +{ + return dJointTypeContact; +} + + +sizeint +dxJointContact::size() const +{ + return sizeof(*this); +} + -- cgit v1.2.1