From 385fb61e01d9d077ff786d1d5cafef6ba7ee1ec5 Mon Sep 17 00:00:00 2001 From: Oleh Derevenko Date: Sat, 24 Jun 2023 19:03:58 +0300 Subject: [PATCH] Fixed: Three corrections by Tilmann * Fixed a missing break in a switch statement in demo_jointPU.cpp while handling gravity switching request. * Fixed wrong face index being returned in convex-trimesh libCCD collision check routine. * Fixed use of potentially outdated AABBs in GIIMPACT cylinder-trimesh collision check routine. --- CHANGELOG.txt | 8 + ode/demo/demo_jointPU.cpp | 3 +- ode/src/collision_cylinder_trimesh.cpp | 2343 ++++++++++++++++---------------- ode/src/collision_libccd.cpp | 2160 ++++++++++++++--------------- ode/src/joints/pu.cpp | 1520 +++++++++++---------- 5 files changed, 3021 insertions(+), 3013 deletions(-) diff --git a/CHANGELOG.txt b/CHANGELOG.txt index 21d4d73b..df208f02 100644 --- a/CHANGELOG.txt +++ b/CHANGELOG.txt @@ -8,6 +8,14 @@ the rules for this file: * keep the format consistent (79 char width, M/D/Y date format). ------------------------------------------------------------------------------ +06/24/2023 Oleh Derevenko (by Tilmann) + * Fixed a missing break in a switch statement in demo_jointPU.cpp while + handling gravity switching request. + * Fixed wrong face index being returned in convex-trimesh libCCD + collision check routine. + * Fixed use of potentially outdated AABBs in GIIMPACT cylinder-trimesh + collision check routine. + 11/08/2020 Steve Peters * dGeomBoxPointDepth implementation was fixed for locations outside of the box. diff --git a/ode/demo/demo_jointPU.cpp b/ode/demo/demo_jointPU.cpp index 60227877..874dd054 100644 --- a/ode/demo/demo_jointPU.cpp +++ b/ode/demo/demo_jointPU.cpp @@ -347,8 +347,9 @@ case 'h' : case 'H' : case '?' : dWorldSetGravity(world, 0, 0, -0.5); } + break; -case 'p' :case 'P' : { + case 'p' :case 'P' : { switch (joint->getType() ) { case dJointTypeSlider : { dSliderJoint *sj = reinterpret_cast (joint); diff --git a/ode/src/collision_cylinder_trimesh.cpp b/ode/src/collision_cylinder_trimesh.cpp index fd22e1a7..4a316c54 100644 --- a/ode/src/collision_cylinder_trimesh.cpp +++ b/ode/src/collision_cylinder_trimesh.cpp @@ -1,1171 +1,1172 @@ -/************************************************************************* - * * - * Open Dynamics Engine, Copyright (C) 2001-2003 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. * - * * - *************************************************************************/ - -/* - * Cylinder-trimesh collider by Alen Ladavac - * Ported to ODE by Nguyen Binh - */ - - -#include -#include -#include "config.h" -#include "matrix.h" -#include "odemath.h" -#include "collision_util.h" -#include "collision_trimesh_internal.h" -#include "util.h" - -#if dTRIMESH_ENABLED - -#define MAX_REAL dInfinity -static const int nCYLINDER_AXIS = 2; -static const int nCYLINDER_CIRCLE_SEGMENTS = 8; -static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; - -#define OPTIMIZE_CONTACTS 1 - -// Local contacts data -typedef struct _sLocalContactData -{ - dVector3 vPos; - dVector3 vNormal; - dReal fDepth; - int triIndex; - int nFlags; // 0 = filtered out, 1 = OK -}sLocalContactData; - -struct sCylinderTrimeshColliderData -{ - sCylinderTrimeshColliderData(int flags, int skip): m_iFlags(flags), m_iSkip(skip), m_nContacts(0), m_gLocalContacts(NULL) {} - -#ifdef OPTIMIZE_CONTACTS - void _OptimizeLocalContacts(); -#endif - void _InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh); - int _ProcessLocalContacts(dContactGeom *contact, dxGeom *Cylinder, dxTriMesh *Trimesh); - - bool _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, - dVector3& vAxis, int iAxis, bool bNoFlip = false); - bool _cldTestCircleToEdgeAxis( - const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, - const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, - const dVector3 &vVx0, const dVector3 &vVx1, int iAxis); - bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); - bool _cldClipCylinderEdgeToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); - void _cldClipCylinderToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); - void TestOneTriangleVsCylinder(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, - const bool bDoubleSided); - int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], - bool &bOutFinishSearching); - - // cylinder data - dMatrix3 m_mCylinderRot; - dQuaternion m_qCylinderRot; - dQuaternion m_qInvCylinderRot; - dVector3 m_vCylinderPos; - dVector3 m_vCylinderAxis; - dReal m_fCylinderRadius; - dReal m_fCylinderSize; - dVector3 m_avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; - - // mesh data - dQuaternion m_qTrimeshRot; - dQuaternion m_qInvTrimeshRot; - dMatrix3 m_mTrimeshRot; - dVector3 m_vTrimeshPos; - - // global collider data - dVector3 m_vBestPoint; - dReal m_fBestDepth; - dReal m_fBestCenter; - dReal m_fBestrt; - int m_iBestAxis; - dVector3 m_vContactNormal; - dVector3 m_vNormal; - dVector3 m_vE0; - dVector3 m_vE1; - dVector3 m_vE2; - - // ODE stuff - int m_iFlags; - int m_iSkip; - int m_nContacts;// = 0; - sLocalContactData* m_gLocalContacts; -}; - - -#ifdef OPTIMIZE_CONTACTS - -// Use to classify contacts to be "near" in position -static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 -// Use to classify contacts to be "near" in normal direction -static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 - -// If this two contact can be classified as "near" -inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) -{ - int bPosNear = 0; - int bSameDir = 0; - dVector3 vDiff; - - // First check if they are "near" in position - dVector3Subtract(c1.vPos,c2.vPos,vDiff); - if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) - &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) - &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) - { - bPosNear = 1; - } - - // Second check if they are "near" in normal direction - dVector3Subtract(c1.vNormal,c2.vNormal,vDiff); - if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) - &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) - &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) - { - bSameDir = 1; - } - - // Will be "near" if position and normal direction are "near" - return (bPosNear && bSameDir); -} - -inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) -{ - // The not better will be throw away - // You can change the selection criteria here - return (c1.fDepth > c2.fDepth); -} - -// iterate through gLocalContacts and filtered out "near contact" -void sCylinderTrimeshColliderData::_OptimizeLocalContacts() -{ - int nContacts = m_nContacts; - - for (int i = 0; i < nContacts-1; i++) - { - for (int j = i+1; j < nContacts; j++) - { - if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j])) - { - // If they are seem to be the same then filtered - // out the least penetrate one - if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i])) - { - m_gLocalContacts[i].nFlags = 0; // filtered 1st contact - } - else - { - m_gLocalContacts[j].nFlags = 0; // filtered 2nd contact - } - - // NOTE - // There is other way is to add two depth together but - // it not work so well. Why??? - } - } - } -} -#endif // OPTIMIZE_CONTACTS - -int sCylinderTrimeshColliderData::_ProcessLocalContacts(dContactGeom *contact, - dxGeom *Cylinder, dxTriMesh *Trimesh) -{ -#ifdef OPTIMIZE_CONTACTS - if (m_nContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) - { - // Can be optimized... - _OptimizeLocalContacts(); - } -#endif - - int iContact = 0; - dContactGeom* Contact = 0; - - int nFinalContact = 0; - - for (iContact = 0; iContact < m_nContacts; iContact ++) - { - if (1 == m_gLocalContacts[iContact].nFlags) - { - Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iSkip); - Contact->depth = m_gLocalContacts[iContact].fDepth; - dVector3Copy(m_gLocalContacts[iContact].vNormal,Contact->normal); - dVector3Copy(m_gLocalContacts[iContact].vPos,Contact->pos); - Contact->g1 = Cylinder; - Contact->g2 = Trimesh; - Contact->side1 = -1; - Contact->side2 = m_gLocalContacts[iContact].triIndex; - dVector3Inv(Contact->normal); - - nFinalContact++; - } - } - // debug - //if (nFinalContact != m_nContacts) - //{ - // printf("[Info] %d contacts generated,%d filtered.\n",m_nContacts,m_nContacts-nFinalContact); - //} - - return nFinalContact; -} - - -bool sCylinderTrimeshColliderData::_cldTestAxis( - const dVector3 &v0, - const dVector3 &v1, - const dVector3 &v2, - dVector3& vAxis, - int iAxis, - bool bNoFlip/* = false*/) -{ - - // calculate length of separating axis vector - dReal fL = dVector3Length(vAxis); - // if not long enough - if ( fL < REAL(1e-5) ) - { - // do nothing - return true; - } - - // otherwise normalize it - vAxis[0] /= fL; - vAxis[1] /= fL; - vAxis[2] /= fL; - - dReal fdot1 = dVector3Dot(m_vCylinderAxis,vAxis); - // project capsule on vAxis - dReal frc; - - if (dFabs(fdot1) > REAL(1.0) ) - { - // fdot1 = REAL(1.0); - frc = dFabs(m_fCylinderSize* REAL(0.5)); - } - else - { - frc = dFabs((m_fCylinderSize* REAL(0.5)) * fdot1) - + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); - } - - dVector3 vV0; - dVector3Subtract(v0,m_vCylinderPos,vV0); - dVector3 vV1; - dVector3Subtract(v1,m_vCylinderPos,vV1); - dVector3 vV2; - dVector3Subtract(v2,m_vCylinderPos,vV2); - - // project triangle on vAxis - dReal afv[3]; - afv[0] = dVector3Dot( vV0 , vAxis ); - afv[1] = dVector3Dot( vV1 , vAxis ); - afv[2] = dVector3Dot( vV2 , vAxis ); - - dReal fMin = MAX_REAL; - dReal fMax = -MAX_REAL; - - // for each vertex - for(int i = 0; i < 3; i++) - { - // find minimum - if (afv[i]fMax) - { - fMax = afv[i]; - } - } - - // find capsule's center of interval on axis - dReal fCenter = (fMin+fMax)* REAL(0.5); - // calculate triangles halfinterval - dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); - - // if they do not overlap, - if( dFabs(fCenter) > (frc+fTriangleRadius) ) - { - // exit, we have no intersection - return false; - } - - // calculate depth - dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); - - // if greater then best found so far - if ( fDepth < m_fBestDepth ) - { - // remember depth - m_fBestDepth = fDepth; - m_fBestCenter = fCenter; - m_fBestrt = frc; - dVector3Copy(vAxis,m_vContactNormal); - m_iBestAxis = iAxis; - - // flip normal if interval is wrong faced - if ( fCenter< REAL(0.0) && !bNoFlip) - { - dVector3Inv(m_vContactNormal); - m_fBestCenter = -fCenter; - } - } - - return true; -} - -// intersection test between edge and circle -bool sCylinderTrimeshColliderData::_cldTestCircleToEdgeAxis( - const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, - const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, - const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) -{ - // calculate direction of edge - dVector3 vkl; - dVector3Subtract( vVx1 , vVx0 , vkl); - dNormalize3(vkl); - // starting point of edge - dVector3 vol; - dVector3Copy(vVx0,vol); - - // calculate angle cosine between cylinder axis and edge - dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); - - // if edge is perpendicular to cylinder axis - if(dFabs(fdot2) so save some cycles here - dVector3Subtract(v0 ,v2 , m_vE2); - - // calculate caps centers in absolute space - dVector3 vCp0; - vCp0[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); - vCp0[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); - vCp0[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); - -#if 0 - dVector3 vCp1; - vCp1[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); - vCp1[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); - vCp1[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); -#endif - - // reset best axis - m_iBestAxis = 0; - dVector3 vAxis; - - // axis m_vNormal - //vAxis = -m_vNormal; - vAxis[0] = -m_vNormal[0]; - vAxis[1] = -m_vNormal[1]; - vAxis[2] = -m_vNormal[2]; - if (!_cldTestAxis(v0, v1, v2, vAxis, 1, true)) - { - return false; - } - - // axis CxE0 - // vAxis = ( m_vCylinderAxis cross m_vE0 ); - dVector3Cross(m_vCylinderAxis, m_vE0,vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) - { - return false; - } - - // axis CxE1 - // vAxis = ( m_vCylinderAxis cross m_vE1 ); - dVector3Cross(m_vCylinderAxis, m_vE1,vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) - { - return false; - } - - // axis CxE2 - // vAxis = ( m_vCylinderAxis cross m_vE2 ); - dVector3Cross(m_vCylinderAxis, m_vE2,vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) - { - return false; - } - - // first vertex on triangle - // axis ((V0-Cp0) x C) x C - //vAxis = ( ( v0-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; - _CalculateAxis(v0 , vCp0 , m_vCylinderAxis , vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) - { - return false; - } - - // second vertex on triangle - // axis ((V1-Cp0) x C) x C - // vAxis = ( ( v1-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; - _CalculateAxis(v1 , vCp0 , m_vCylinderAxis , vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) - { - return false; - } - - // third vertex on triangle - // axis ((V2-Cp0) x C) x C - //vAxis = ( ( v2-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; - _CalculateAxis(v2 , vCp0 , m_vCylinderAxis , vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) - { - return false; - } - - // test cylinder axis - // vAxis = m_vCylinderAxis; - dVector3Copy(m_vCylinderAxis , vAxis); - if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) - { - return false; - } - - // Test top and bottom circle ring of cylinder for separation - dVector3 vccATop; - vccATop[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); - vccATop[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); - vccATop[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); - - dVector3 vccABottom; - vccABottom[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); - vccABottom[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); - vccABottom[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); - - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v1, 15)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v1, v2, 16)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v2, 17)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v1, 18)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v1, v2, 19)) - { - return false; - } - - if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v2, 20)) - { - return false; - } - - return true; -} - -bool sCylinderTrimeshColliderData::_cldClipCylinderEdgeToTriangle( - const dVector3 &v0, const dVector3 &/*v1*/, const dVector3 &/*v2*/) -{ - // translate cylinder - dReal fTemp = dVector3Dot(m_vCylinderAxis , m_vContactNormal); - dVector3 vN2; - vN2[0] = m_vContactNormal[0] - m_vCylinderAxis[0]*fTemp; - vN2[1] = m_vContactNormal[1] - m_vCylinderAxis[1]*fTemp; - vN2[2] = m_vContactNormal[2] - m_vCylinderAxis[2]*fTemp; - - fTemp = dVector3Length(vN2); - if (fTemp < REAL(1e-5)) - { - return false; - } - - // Normalize it - vN2[0] /= fTemp; - vN2[1] /= fTemp; - vN2[2] /= fTemp; - - // calculate caps centers in absolute space - dVector3 vCposTrans; - vCposTrans[0] = m_vCylinderPos[0] + vN2[0]*m_fCylinderRadius; - vCposTrans[1] = m_vCylinderPos[1] + vN2[1]*m_fCylinderRadius; - vCposTrans[2] = m_vCylinderPos[2] + vN2[2]*m_fCylinderRadius; - - dVector3 vCEdgePoint0; - vCEdgePoint0[0] = vCposTrans[0] + m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); - vCEdgePoint0[1] = vCposTrans[1] + m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); - vCEdgePoint0[2] = vCposTrans[2] + m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); - - dVector3 vCEdgePoint1; - vCEdgePoint1[0] = vCposTrans[0] - m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); - vCEdgePoint1[1] = vCposTrans[1] - m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); - vCEdgePoint1[2] = vCposTrans[2] - m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); - - // transform cylinder edge points into triangle space - vCEdgePoint0[0] -= v0[0]; - vCEdgePoint0[1] -= v0[1]; - vCEdgePoint0[2] -= v0[2]; - - vCEdgePoint1[0] -= v0[0]; - vCEdgePoint1[1] -= v0[1]; - vCEdgePoint1[2] -= v0[2]; - - dVector4 plPlane; - dVector3 vPlaneNormal; - - // triangle plane - //plPlane = Plane4f( -m_vNormal, 0); - vPlaneNormal[0] = -m_vNormal[0]; - vPlaneNormal[1] = -m_vNormal[1]; - vPlaneNormal[2] = -m_vNormal[2]; - dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 0 - //plPlane = Plane4f( ( m_vNormal cross m_vE0 ), REAL(1e-5)); - dVector3Cross(m_vNormal,m_vE0,vPlaneNormal); - dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 1 - //dVector3 vTemp = ( m_vNormal cross m_vE1 ); - dVector3Cross(m_vNormal,m_vE1,vPlaneNormal); - fTemp = dVector3Dot(m_vE0 , vPlaneNormal) - REAL(1e-5); - //plPlane = Plane4f( vTemp, -(( m_vE0 dot vTemp )-REAL(1e-5))); - dConstructPlane(vPlaneNormal,-fTemp,plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // plane with edge 2 - // plPlane = Plane4f( ( m_vNormal cross m_vE2 ), REAL(1e-5)); - dVector3Cross(m_vNormal,m_vE2,vPlaneNormal); - dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); - if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) - { - return false; - } - - // return capsule edge points into absolute space - vCEdgePoint0[0] += v0[0]; - vCEdgePoint0[1] += v0[1]; - vCEdgePoint0[2] += v0[2]; - - vCEdgePoint1[0] += v0[0]; - vCEdgePoint1[1] += v0[1]; - vCEdgePoint1[2] += v0[2]; - - // calculate depths for both contact points - dVector3 vTemp; - dVector3Subtract(vCEdgePoint0,m_vCylinderPos, vTemp); - dReal fRestDepth0 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; - dVector3Subtract(vCEdgePoint1,m_vCylinderPos, vTemp); - dReal fRestDepth1 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; - - dReal fDepth0 = m_fBestDepth - (fRestDepth0); - dReal fDepth1 = m_fBestDepth - (fRestDepth1); - - // clamp depths to zero - if(fDepth0 < REAL(0.0) ) - { - fDepth0 = REAL(0.0); - } - - if(fDepth1= (m_iFlags & NUMC_MASK)) - return true; - } - - // Generate contact 1 - { - // generate contacts - m_gLocalContacts[m_nContacts].fDepth = fDepth1; - dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); - dVector3Copy(vCEdgePoint1,m_gLocalContacts[m_nContacts].vPos); - m_gLocalContacts[m_nContacts].nFlags = 1; - m_nContacts++; - } - - return true; -} - -void sCylinderTrimeshColliderData::_cldClipCylinderToTriangle( - const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) -{ - int i = 0; - dVector3 avPoints[3]; - dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; - dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; - - dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); - dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); - - // setup array of triangle vertices - dVector3Copy(v0,avPoints[0]); - dVector3Copy(v1,avPoints[1]); - dVector3Copy(v2,avPoints[2]); - - dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; - dSetZero(vCylinderCircleNormal_Rel,4); - // check which circle from cylinder we take for clipping - if ( dVector3Dot(m_vCylinderAxis , m_vContactNormal) > REAL(0.0)) - { - // get top circle - vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); - vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); - vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); - - vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); - } - else - { - // get bottom circle - vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); - vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); - vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); - - vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); - } - - dVector3 vTemp; - dQuatInv(m_qCylinderRot , m_qInvCylinderRot); - // transform triangle points to space of cylinder circle - for(i=0; i<3; i++) - { - dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); - dQuatTransform(m_qInvCylinderRot,vTemp,avPoints[i]); - } - - int iTmpCounter1 = 0; - int iTmpCounter2 = 0; - dVector4 plPlane; - - // plane of cylinder that contains circle for intersection - //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); - dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); - dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); - - // Body of base circle of Cylinder - int nCircleSegment = 0; - for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) - { - dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); - - if (0 == (nCircleSegment % 2)) - { - dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); - } - else - { - dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); - } - - dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); - dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); - } - - // back transform clipped points to absolute space - dReal ftmpdot; - dReal fTempDepth; - dVector3 vPoint; - - if (nCircleSegment %2) - { - for( i=0; i REAL(0.0)) - { - m_gLocalContacts[m_nContacts].fDepth = fTempDepth; - dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); - dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); - m_gLocalContacts[m_nContacts].nFlags = 1; - m_nContacts++; - if(m_nContacts >= (m_iFlags & NUMC_MASK)) - return;; - } - } - } - else - { - for( i=0; i REAL(0.0)) - { - m_gLocalContacts[m_nContacts].fDepth = fTempDepth; - dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); - dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); - m_gLocalContacts[m_nContacts].nFlags = 1; - m_nContacts++; - if(m_nContacts >= (m_iFlags & NUMC_MASK)) - return;; - } - } - } -} - -void sCylinderTrimeshColliderData::TestOneTriangleVsCylinder( - const dVector3 &v0, - const dVector3 &v1, - const dVector3 &v2, - const bool bDoubleSided) -{ - // calculate triangle normal - dVector3Subtract( v2 , v1 , m_vE1); - dVector3 vTemp; - dVector3Subtract( v0 , v1 ,vTemp); - dVector3Cross(m_vE1 , vTemp , m_vNormal ); - - // Even though all triangles might be initially valid, - // a triangle may degenerate into a segment after applying - // space transformation. - if (!dSafeNormalize3( m_vNormal)) - { - return; - } - - // create plane from triangle - //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); - dReal plDistance = -dVector3Dot(v0, m_vNormal); - dVector4 plTrianglePlane; - dConstructPlane( m_vNormal,plDistance,plTrianglePlane); - - // calculate sphere distance to plane - dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(m_vCylinderPos , plTrianglePlane); - - // Sphere must be over positive side of triangle - if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) - { - // if not don't generate contacts - return; - } - - dVector3 vPnt0; - dVector3 vPnt1; - dVector3 vPnt2; - - if (fDistanceCylinderCenterToPlane < REAL(0.0) ) - { - // flip it - dVector3Copy(v0 , vPnt0); - dVector3Copy(v1 , vPnt2); - dVector3Copy(v2 , vPnt1); - } - else - { - dVector3Copy(v0 , vPnt0); - dVector3Copy(v1 , vPnt1); - dVector3Copy(v2 , vPnt2); - } - - m_fBestDepth = MAX_REAL; - - // do intersection test and find best separating axis - if(!_cldTestSeparatingAxes(vPnt0, vPnt1, vPnt2) ) - { - // if not found do nothing - return; - } - - // if best separation axis is not found - if ( m_iBestAxis == 0 ) - { - // this should not happen (the function should have already returned in this case) - dIASSERT(false); - // do nothing - return; - } - - dReal fdot = dVector3Dot( m_vContactNormal , m_vCylinderAxis ); - - // choose which clipping method are we going to apply - if (dFabs(fdot) < REAL(0.9) ) - { - if (!_cldClipCylinderEdgeToTriangle(vPnt0, vPnt1, vPnt2)) - { - return; - } - } - else - { - _cldClipCylinderToTriangle(vPnt0, vPnt1, vPnt2); - } -} - -void sCylinderTrimeshColliderData::_InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh) -{ - // get cylinder information - // Rotation - const dReal* pRotCyc = dGeomGetRotation(Cylinder); - dMatrix3Copy(pRotCyc,m_mCylinderRot); - dGeomGetQuaternion(Cylinder,m_qCylinderRot); - - // Position - const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(Cylinder); - dVector3Copy(*pPosCyc,m_vCylinderPos); - // Cylinder axis - dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); - // get cylinder radius and size - dGeomCylinderGetParams(Cylinder,&m_fCylinderRadius,&m_fCylinderSize); - - // get trimesh position and orientation - const dReal* pRotTris = dGeomGetRotation(Trimesh); - dMatrix3Copy(pRotTris,m_mTrimeshRot); - dGeomGetQuaternion(Trimesh,m_qTrimeshRot); - - // Position - const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(Trimesh); - dVector3Copy(*pPosTris,m_vTrimeshPos); - - - // calculate basic angle for 8-gon - dReal fAngle = (dReal) (M_PI / nCYLINDER_CIRCLE_SEGMENTS); - // calculate angle increment - dReal fAngleIncrement = fAngle*REAL(2.0); - - // calculate plane normals - // axis dependant code - for(int i=0; i= (m_iFlags & NUMC_MASK)); - - return ctContacts0; -} - -// OPCODE version of cylinder to mesh collider -#if dTRIMESH_OPCODE -static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider, - sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh, - OBBCache &BoxCache) -{ - Matrix4x4 MeshMatrix; - const dVector3 vZeroVector3 = { REAL(0.0), }; - MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix); - - const dVector3 &vCylinderPos = cData.m_vCylinderPos; - const dMatrix3 &mCylinderRot = cData.m_mCylinderRot; - - dVector3 vCylinderOffsetPos; - dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos); - - const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5); - - OBB obbCylinder; - obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]); - obbCylinder.mExtents.Set( - 0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, - 1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, - 2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius); - obbCylinder.mRot.Set( - mCylinderRot[0], mCylinderRot[4], mCylinderRot[8], - mCylinderRot[1], mCylinderRot[5], mCylinderRot[9], - mCylinderRot[2], mCylinderRot[6], mCylinderRot[10]); - - // TC results - if (Trimesh->getDoTC(dxTriMesh::TTC_BOX)) - { - dxTriMesh::BoxTC* BoxTC = 0; - const int iBoxCacheSize = Trimesh->m_BoxTCCache.size(); - for (int i = 0; i != iBoxCacheSize; i++) - { - if (Trimesh->m_BoxTCCache[i].Geom == Cylinder) - { - BoxTC = &Trimesh->m_BoxTCCache[i]; - break; - } - } - if (!BoxTC) - { - Trimesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); - - BoxTC = &Trimesh->m_BoxTCCache[Trimesh->m_BoxTCCache.size() - 1]; - BoxTC->Geom = Cylinder; - BoxTC->FatCoeff = REAL(1.0); - } - - // Intersect - Collider.SetTemporalCoherence(true); - Collider.Collide(*BoxTC, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); - } - else - { - Collider.SetTemporalCoherence(false); - Collider.Collide(BoxCache, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); - } -} - -int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - dIASSERT( skip >= (int)sizeof( dContactGeom ) ); - dIASSERT( o1->type == dCylinderClass ); - dIASSERT( o2->type == dTriMeshClass ); - dIASSERT ((flags & NUMC_MASK) >= 1); - - int nContactCount = 0; - - dxGeom *Cylinder = o1; - dxTriMesh *Trimesh = (dxTriMesh *)o2; - - // Main data holder - sCylinderTrimeshColliderData cData(flags, skip); - cData._InitCylinderTrimeshData(Cylinder, Trimesh); - - const unsigned uiTLSKind = Trimesh->getParentSpaceTLSKind(); - dIASSERT(uiTLSKind == Cylinder->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method - TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); - OBBCollider& Collider = pccColliderCache->m_OBBCollider; - - dQueryCTLPotentialCollisionTriangles(Collider, cData, Cylinder, Trimesh, pccColliderCache->m_DefaultBoxCache); - - // Retrieve data - int TriCount = Collider.GetNbTouchedPrimitives(); - - if (TriCount != 0) - { - const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); - - if (Trimesh->m_ArrayCallback != NULL) - { - Trimesh->m_ArrayCallback(Trimesh, Cylinder, Triangles, TriCount); - } - - // allocate buffer for local contacts on stack - cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); - - int ctContacts0 = 0; - - // loop through all intersecting triangles - for (int i = 0; i < TriCount; i++) - { - const int Triint = Triangles[i]; - if (!Trimesh->invokeCallback(Cylinder, Triint)) continue; - - - dVector3 dv[3]; - Trimesh->fetchMeshTriangle(dv, Triint, cData.m_vTrimeshPos, cData.m_mTrimeshRot); - - bool bFinishSearching; - ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); - - if (bFinishSearching) - { - break; - } - } - - if (cData.m_nContacts != 0) - { - nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); - } - } - - return nContactCount; -} -#endif - -// GIMPACT version of cylinder to mesh collider -#if dTRIMESH_GIMPACT -int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - dIASSERT( skip >= (int)sizeof( dContactGeom ) ); - dIASSERT( o1->type == dCylinderClass ); - dIASSERT( o2->type == dTriMeshClass ); - dIASSERT ((flags & NUMC_MASK) >= 1); - - int nContactCount = 0; - - dxGeom *Cylinder = o1; - dxTriMesh *Trimesh = (dxTriMesh *)o2; - - // Main data holder - sCylinderTrimeshColliderData cData(flags, skip); - cData._InitCylinderTrimeshData(Cylinder, Trimesh); - - //*****at first , collide box aabb******// - - aabb3f test_aabb(o1->aabb[0], o1->aabb[1], o1->aabb[2], o1->aabb[3], o1->aabb[4], o1->aabb[5]); - - - GDYNAMIC_ARRAY collision_result; - GIM_CREATE_BOXQUERY_LIST(collision_result); - - gim_aabbset_box_collision(&test_aabb, &Trimesh->m_collision_trimesh.m_aabbset , &collision_result); - - if (collision_result.m_size != 0) - { - //*****Set globals for box collision******// - - int ctContacts0 = 0; - cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); - - GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); - GIM_TRIMESH * ptrimesh = &Trimesh->m_collision_trimesh; - - gim_trimesh_locks_work_data(ptrimesh); - - for(unsigned int i=0;i +#include +#include "config.h" +#include "matrix.h" +#include "odemath.h" +#include "collision_util.h" +#include "collision_trimesh_internal.h" +#include "util.h" + +#if dTRIMESH_ENABLED + +#define MAX_REAL dInfinity +static const int nCYLINDER_AXIS = 2; +static const int nCYLINDER_CIRCLE_SEGMENTS = 8; +static const int nMAX_CYLINDER_TRIANGLE_CLIP_POINTS = 12; + +#define OPTIMIZE_CONTACTS 1 + +// Local contacts data +typedef struct _sLocalContactData +{ + dVector3 vPos; + dVector3 vNormal; + dReal fDepth; + int triIndex; + int nFlags; // 0 = filtered out, 1 = OK +}sLocalContactData; + +struct sCylinderTrimeshColliderData +{ + sCylinderTrimeshColliderData(int flags, int skip): m_iFlags(flags), m_iSkip(skip), m_nContacts(0), m_gLocalContacts(NULL) {} + +#ifdef OPTIMIZE_CONTACTS + void _OptimizeLocalContacts(); +#endif + void _InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh); + int _ProcessLocalContacts(dContactGeom *contact, dxGeom *Cylinder, dxTriMesh *Trimesh); + + bool _cldTestAxis(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + dVector3& vAxis, int iAxis, bool bNoFlip = false); + bool _cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis); + bool _cldTestSeparatingAxes(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + bool _cldClipCylinderEdgeToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void _cldClipCylinderToTriangle(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2); + void TestOneTriangleVsCylinder(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const bool bDoubleSided); + int TestCollisionForSingleTriangle(int ctContacts0, int Triint, dVector3 dv[3], + bool &bOutFinishSearching); + + // cylinder data + dMatrix3 m_mCylinderRot; + dQuaternion m_qCylinderRot; + dQuaternion m_qInvCylinderRot; + dVector3 m_vCylinderPos; + dVector3 m_vCylinderAxis; + dReal m_fCylinderRadius; + dReal m_fCylinderSize; + dVector3 m_avCylinderNormals[nCYLINDER_CIRCLE_SEGMENTS]; + + // mesh data + dQuaternion m_qTrimeshRot; + dQuaternion m_qInvTrimeshRot; + dMatrix3 m_mTrimeshRot; + dVector3 m_vTrimeshPos; + + // global collider data + dVector3 m_vBestPoint; + dReal m_fBestDepth; + dReal m_fBestCenter; + dReal m_fBestrt; + int m_iBestAxis; + dVector3 m_vContactNormal; + dVector3 m_vNormal; + dVector3 m_vE0; + dVector3 m_vE1; + dVector3 m_vE2; + + // ODE stuff + int m_iFlags; + int m_iSkip; + int m_nContacts;// = 0; + sLocalContactData* m_gLocalContacts; +}; + + +#ifdef OPTIMIZE_CONTACTS + +// Use to classify contacts to be "near" in position +static const dReal fSameContactPositionEpsilon = REAL(0.0001); // 1e-4 +// Use to classify contacts to be "near" in normal direction +static const dReal fSameContactNormalEpsilon = REAL(0.0001); // 1e-4 + +// If this two contact can be classified as "near" +inline int _IsNearContacts(sLocalContactData& c1,sLocalContactData& c2) +{ + int bPosNear = 0; + int bSameDir = 0; + dVector3 vDiff; + + // First check if they are "near" in position + dVector3Subtract(c1.vPos,c2.vPos,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[1]) < fSameContactPositionEpsilon) + &&(dFabs(vDiff[2]) < fSameContactPositionEpsilon)) + { + bPosNear = 1; + } + + // Second check if they are "near" in normal direction + dVector3Subtract(c1.vNormal,c2.vNormal,vDiff); + if ( (dFabs(vDiff[0]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[1]) < fSameContactNormalEpsilon) + &&(dFabs(vDiff[2]) < fSameContactNormalEpsilon) ) + { + bSameDir = 1; + } + + // Will be "near" if position and normal direction are "near" + return (bPosNear && bSameDir); +} + +inline int _IsBetter(sLocalContactData& c1,sLocalContactData& c2) +{ + // The not better will be throw away + // You can change the selection criteria here + return (c1.fDepth > c2.fDepth); +} + +// iterate through gLocalContacts and filtered out "near contact" +void sCylinderTrimeshColliderData::_OptimizeLocalContacts() +{ + int nContacts = m_nContacts; + + for (int i = 0; i < nContacts-1; i++) + { + for (int j = i+1; j < nContacts; j++) + { + if (_IsNearContacts(m_gLocalContacts[i],m_gLocalContacts[j])) + { + // If they are seem to be the same then filtered + // out the least penetrate one + if (_IsBetter(m_gLocalContacts[j],m_gLocalContacts[i])) + { + m_gLocalContacts[i].nFlags = 0; // filtered 1st contact + } + else + { + m_gLocalContacts[j].nFlags = 0; // filtered 2nd contact + } + + // NOTE + // There is other way is to add two depth together but + // it not work so well. Why??? + } + } + } +} +#endif // OPTIMIZE_CONTACTS + +int sCylinderTrimeshColliderData::_ProcessLocalContacts(dContactGeom *contact, + dxGeom *Cylinder, dxTriMesh *Trimesh) +{ +#ifdef OPTIMIZE_CONTACTS + if (m_nContacts > 1 && !(m_iFlags & CONTACTS_UNIMPORTANT)) + { + // Can be optimized... + _OptimizeLocalContacts(); + } +#endif + + int iContact = 0; + dContactGeom* Contact = 0; + + int nFinalContact = 0; + + for (iContact = 0; iContact < m_nContacts; iContact ++) + { + if (1 == m_gLocalContacts[iContact].nFlags) + { + Contact = SAFECONTACT(m_iFlags, contact, nFinalContact, m_iSkip); + Contact->depth = m_gLocalContacts[iContact].fDepth; + dVector3Copy(m_gLocalContacts[iContact].vNormal,Contact->normal); + dVector3Copy(m_gLocalContacts[iContact].vPos,Contact->pos); + Contact->g1 = Cylinder; + Contact->g2 = Trimesh; + Contact->side1 = -1; + Contact->side2 = m_gLocalContacts[iContact].triIndex; + dVector3Inv(Contact->normal); + + nFinalContact++; + } + } + // debug + //if (nFinalContact != m_nContacts) + //{ + // printf("[Info] %d contacts generated,%d filtered.\n",m_nContacts,m_nContacts-nFinalContact); + //} + + return nFinalContact; +} + + +bool sCylinderTrimeshColliderData::_cldTestAxis( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + dVector3& vAxis, + int iAxis, + bool bNoFlip/* = false*/) +{ + + // calculate length of separating axis vector + dReal fL = dVector3Length(vAxis); + // if not long enough + if ( fL < REAL(1e-5) ) + { + // do nothing + return true; + } + + // otherwise normalize it + vAxis[0] /= fL; + vAxis[1] /= fL; + vAxis[2] /= fL; + + dReal fdot1 = dVector3Dot(m_vCylinderAxis,vAxis); + // project capsule on vAxis + dReal frc; + + if (dFabs(fdot1) > REAL(1.0) ) + { + // fdot1 = REAL(1.0); + frc = dFabs(m_fCylinderSize* REAL(0.5)); + } + else + { + frc = dFabs((m_fCylinderSize* REAL(0.5)) * fdot1) + + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); + } + + dVector3 vV0; + dVector3Subtract(v0,m_vCylinderPos,vV0); + dVector3 vV1; + dVector3Subtract(v1,m_vCylinderPos,vV1); + dVector3 vV2; + dVector3Subtract(v2,m_vCylinderPos,vV2); + + // project triangle on vAxis + dReal afv[3]; + afv[0] = dVector3Dot( vV0 , vAxis ); + afv[1] = dVector3Dot( vV1 , vAxis ); + afv[2] = dVector3Dot( vV2 , vAxis ); + + dReal fMin = MAX_REAL; + dReal fMax = -MAX_REAL; + + // for each vertex + for(int i = 0; i < 3; i++) + { + // find minimum + if (afv[i]fMax) + { + fMax = afv[i]; + } + } + + // find capsule's center of interval on axis + dReal fCenter = (fMin+fMax)* REAL(0.5); + // calculate triangles halfinterval + dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); + + // if they do not overlap, + if( dFabs(fCenter) > (frc+fTriangleRadius) ) + { + // exit, we have no intersection + return false; + } + + // calculate depth + dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); + + // if greater then best found so far + if ( fDepth < m_fBestDepth ) + { + // remember depth + m_fBestDepth = fDepth; + m_fBestCenter = fCenter; + m_fBestrt = frc; + dVector3Copy(vAxis,m_vContactNormal); + m_iBestAxis = iAxis; + + // flip normal if interval is wrong faced + if ( fCenter< REAL(0.0) && !bNoFlip) + { + dVector3Inv(m_vContactNormal); + m_fBestCenter = -fCenter; + } + } + + return true; +} + +// intersection test between edge and circle +bool sCylinderTrimeshColliderData::_cldTestCircleToEdgeAxis( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, + const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, + const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) +{ + // calculate direction of edge + dVector3 vkl; + dVector3Subtract( vVx1 , vVx0 , vkl); + dNormalize3(vkl); + // starting point of edge + dVector3 vol; + dVector3Copy(vVx0,vol); + + // calculate angle cosine between cylinder axis and edge + dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); + + // if edge is perpendicular to cylinder axis + if(dFabs(fdot2) so save some cycles here + dVector3Subtract(v0 ,v2 , m_vE2); + + // calculate caps centers in absolute space + dVector3 vCp0; + vCp0[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp0[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp0[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); + +#if 0 + dVector3 vCp1; + vCp1[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize* REAL(0.5)); + vCp1[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize* REAL(0.5)); + vCp1[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize* REAL(0.5)); +#endif + + // reset best axis + m_iBestAxis = 0; + dVector3 vAxis; + + // axis m_vNormal + //vAxis = -m_vNormal; + vAxis[0] = -m_vNormal[0]; + vAxis[1] = -m_vNormal[1]; + vAxis[2] = -m_vNormal[2]; + if (!_cldTestAxis(v0, v1, v2, vAxis, 1, true)) + { + return false; + } + + // axis CxE0 + // vAxis = ( m_vCylinderAxis cross m_vE0 ); + dVector3Cross(m_vCylinderAxis, m_vE0,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 2)) + { + return false; + } + + // axis CxE1 + // vAxis = ( m_vCylinderAxis cross m_vE1 ); + dVector3Cross(m_vCylinderAxis, m_vE1,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 3)) + { + return false; + } + + // axis CxE2 + // vAxis = ( m_vCylinderAxis cross m_vE2 ); + dVector3Cross(m_vCylinderAxis, m_vE2,vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 4)) + { + return false; + } + + // first vertex on triangle + // axis ((V0-Cp0) x C) x C + //vAxis = ( ( v0-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v0 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 11)) + { + return false; + } + + // second vertex on triangle + // axis ((V1-Cp0) x C) x C + // vAxis = ( ( v1-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v1 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 12)) + { + return false; + } + + // third vertex on triangle + // axis ((V2-Cp0) x C) x C + //vAxis = ( ( v2-vCp0 ) cross m_vCylinderAxis ) cross m_vCylinderAxis; + _CalculateAxis(v2 , vCp0 , m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 13)) + { + return false; + } + + // test cylinder axis + // vAxis = m_vCylinderAxis; + dVector3Copy(m_vCylinderAxis , vAxis); + if (!_cldTestAxis(v0, v1, v2, vAxis, 14)) + { + return false; + } + + // Test top and bottom circle ring of cylinder for separation + dVector3 vccATop; + vccATop[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccATop[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccATop[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + dVector3 vccABottom; + vccABottom[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize * REAL(0.5)); + vccABottom[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize * REAL(0.5)); + vccABottom[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize * REAL(0.5)); + + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v1, 15)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v1, v2, 16)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccATop, m_vCylinderAxis, v0, v2, 17)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v1, 18)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v1, v2, 19)) + { + return false; + } + + if (!_cldTestCircleToEdgeAxis(v0, v1, v2, vccABottom, m_vCylinderAxis, v0, v2, 20)) + { + return false; + } + + return true; +} + +bool sCylinderTrimeshColliderData::_cldClipCylinderEdgeToTriangle( + const dVector3 &v0, const dVector3 &/*v1*/, const dVector3 &/*v2*/) +{ + // translate cylinder + dReal fTemp = dVector3Dot(m_vCylinderAxis , m_vContactNormal); + dVector3 vN2; + vN2[0] = m_vContactNormal[0] - m_vCylinderAxis[0]*fTemp; + vN2[1] = m_vContactNormal[1] - m_vCylinderAxis[1]*fTemp; + vN2[2] = m_vContactNormal[2] - m_vCylinderAxis[2]*fTemp; + + fTemp = dVector3Length(vN2); + if (fTemp < REAL(1e-5)) + { + return false; + } + + // Normalize it + vN2[0] /= fTemp; + vN2[1] /= fTemp; + vN2[2] /= fTemp; + + // calculate caps centers in absolute space + dVector3 vCposTrans; + vCposTrans[0] = m_vCylinderPos[0] + vN2[0]*m_fCylinderRadius; + vCposTrans[1] = m_vCylinderPos[1] + vN2[1]*m_fCylinderRadius; + vCposTrans[2] = m_vCylinderPos[2] + vN2[2]*m_fCylinderRadius; + + dVector3 vCEdgePoint0; + vCEdgePoint0[0] = vCposTrans[0] + m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[1] = vCposTrans[1] + m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint0[2] = vCposTrans[2] + m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + dVector3 vCEdgePoint1; + vCEdgePoint1[0] = vCposTrans[0] - m_vCylinderAxis[0] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[1] = vCposTrans[1] - m_vCylinderAxis[1] * (m_fCylinderSize* REAL(0.5)); + vCEdgePoint1[2] = vCposTrans[2] - m_vCylinderAxis[2] * (m_fCylinderSize* REAL(0.5)); + + // transform cylinder edge points into triangle space + vCEdgePoint0[0] -= v0[0]; + vCEdgePoint0[1] -= v0[1]; + vCEdgePoint0[2] -= v0[2]; + + vCEdgePoint1[0] -= v0[0]; + vCEdgePoint1[1] -= v0[1]; + vCEdgePoint1[2] -= v0[2]; + + dVector4 plPlane; + dVector3 vPlaneNormal; + + // triangle plane + //plPlane = Plane4f( -m_vNormal, 0); + vPlaneNormal[0] = -m_vNormal[0]; + vPlaneNormal[1] = -m_vNormal[1]; + vPlaneNormal[2] = -m_vNormal[2]; + dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 0 + //plPlane = Plane4f( ( m_vNormal cross m_vE0 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE0,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 1 + //dVector3 vTemp = ( m_vNormal cross m_vE1 ); + dVector3Cross(m_vNormal,m_vE1,vPlaneNormal); + fTemp = dVector3Dot(m_vE0 , vPlaneNormal) - REAL(1e-5); + //plPlane = Plane4f( vTemp, -(( m_vE0 dot vTemp )-REAL(1e-5))); + dConstructPlane(vPlaneNormal,-fTemp,plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // plane with edge 2 + // plPlane = Plane4f( ( m_vNormal cross m_vE2 ), REAL(1e-5)); + dVector3Cross(m_vNormal,m_vE2,vPlaneNormal); + dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); + if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) + { + return false; + } + + // return capsule edge points into absolute space + vCEdgePoint0[0] += v0[0]; + vCEdgePoint0[1] += v0[1]; + vCEdgePoint0[2] += v0[2]; + + vCEdgePoint1[0] += v0[0]; + vCEdgePoint1[1] += v0[1]; + vCEdgePoint1[2] += v0[2]; + + // calculate depths for both contact points + dVector3 vTemp; + dVector3Subtract(vCEdgePoint0,m_vCylinderPos, vTemp); + dReal fRestDepth0 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + dVector3Subtract(vCEdgePoint1,m_vCylinderPos, vTemp); + dReal fRestDepth1 = -dVector3Dot(vTemp,m_vContactNormal) + m_fBestrt; + + dReal fDepth0 = m_fBestDepth - (fRestDepth0); + dReal fDepth1 = m_fBestDepth - (fRestDepth1); + + // clamp depths to zero + if(fDepth0 < REAL(0.0) ) + { + fDepth0 = REAL(0.0); + } + + if(fDepth1= (m_iFlags & NUMC_MASK)) + return true; + } + + // Generate contact 1 + { + // generate contacts + m_gLocalContacts[m_nContacts].fDepth = fDepth1; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vCEdgePoint1,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + } + + return true; +} + +void sCylinderTrimeshColliderData::_cldClipCylinderToTriangle( + const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) +{ + int i = 0; + dVector3 avPoints[3]; + dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; + + dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); + + // setup array of triangle vertices + dVector3Copy(v0,avPoints[0]); + dVector3Copy(v1,avPoints[1]); + dVector3Copy(v2,avPoints[2]); + + dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; + dSetZero(vCylinderCircleNormal_Rel,4); + // check which circle from cylinder we take for clipping + if ( dVector3Dot(m_vCylinderAxis , m_vContactNormal) > REAL(0.0)) + { + // get top circle + vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); + } + else + { + // get bottom circle + vCylinderCirclePos[0] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); + vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); + + vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); + } + + dVector3 vTemp; + dQuatInv(m_qCylinderRot , m_qInvCylinderRot); + // transform triangle points to space of cylinder circle + for(i=0; i<3; i++) + { + dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); + dQuatTransform(m_qInvCylinderRot,vTemp,avPoints[i]); + } + + int iTmpCounter1 = 0; + int iTmpCounter2 = 0; + dVector4 plPlane; + + // plane of cylinder that contains circle for intersection + //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); + dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); + dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); + + // Body of base circle of Cylinder + int nCircleSegment = 0; + for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; nCircleSegment++) + { + dConstructPlane(m_avCylinderNormals[nCircleSegment],m_fCylinderRadius,plPlane); + + if (0 == (nCircleSegment % 2)) + { + dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); + } + else + { + dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); + } + + dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); + } + + // back transform clipped points to absolute space + dReal ftmpdot; + dReal fTempDepth; + dVector3 vPoint; + + if (nCircleSegment %2) + { + for( i=0; i REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } + else + { + for( i=0; i REAL(0.0)) + { + m_gLocalContacts[m_nContacts].fDepth = fTempDepth; + dVector3Copy(m_vContactNormal,m_gLocalContacts[m_nContacts].vNormal); + dVector3Copy(vPoint,m_gLocalContacts[m_nContacts].vPos); + m_gLocalContacts[m_nContacts].nFlags = 1; + m_nContacts++; + if(m_nContacts >= (m_iFlags & NUMC_MASK)) + return;; + } + } + } +} + +void sCylinderTrimeshColliderData::TestOneTriangleVsCylinder( + const dVector3 &v0, + const dVector3 &v1, + const dVector3 &v2, + const bool bDoubleSided) +{ + // calculate triangle normal + dVector3Subtract( v2 , v1 , m_vE1); + dVector3 vTemp; + dVector3Subtract( v0 , v1 ,vTemp); + dVector3Cross(m_vE1 , vTemp , m_vNormal ); + + // Even though all triangles might be initially valid, + // a triangle may degenerate into a segment after applying + // space transformation. + if (!dSafeNormalize3( m_vNormal)) + { + return; + } + + // create plane from triangle + //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); + dReal plDistance = -dVector3Dot(v0, m_vNormal); + dVector4 plTrianglePlane; + dConstructPlane( m_vNormal,plDistance,plTrianglePlane); + + // calculate sphere distance to plane + dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(m_vCylinderPos , plTrianglePlane); + + // Sphere must be over positive side of triangle + if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) + { + // if not don't generate contacts + return; + } + + dVector3 vPnt0; + dVector3 vPnt1; + dVector3 vPnt2; + + if (fDistanceCylinderCenterToPlane < REAL(0.0) ) + { + // flip it + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt2); + dVector3Copy(v2 , vPnt1); + } + else + { + dVector3Copy(v0 , vPnt0); + dVector3Copy(v1 , vPnt1); + dVector3Copy(v2 , vPnt2); + } + + m_fBestDepth = MAX_REAL; + + // do intersection test and find best separating axis + if(!_cldTestSeparatingAxes(vPnt0, vPnt1, vPnt2) ) + { + // if not found do nothing + return; + } + + // if best separation axis is not found + if ( m_iBestAxis == 0 ) + { + // this should not happen (the function should have already returned in this case) + dIASSERT(false); + // do nothing + return; + } + + dReal fdot = dVector3Dot( m_vContactNormal , m_vCylinderAxis ); + + // choose which clipping method are we going to apply + if (dFabs(fdot) < REAL(0.9) ) + { + if (!_cldClipCylinderEdgeToTriangle(vPnt0, vPnt1, vPnt2)) + { + return; + } + } + else + { + _cldClipCylinderToTriangle(vPnt0, vPnt1, vPnt2); + } +} + +void sCylinderTrimeshColliderData::_InitCylinderTrimeshData(dxGeom *Cylinder, dxTriMesh *Trimesh) +{ + // get cylinder information + // Rotation + const dReal* pRotCyc = dGeomGetRotation(Cylinder); + dMatrix3Copy(pRotCyc,m_mCylinderRot); + dGeomGetQuaternion(Cylinder,m_qCylinderRot); + + // Position + const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(Cylinder); + dVector3Copy(*pPosCyc,m_vCylinderPos); + // Cylinder axis + dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); + // get cylinder radius and size + dGeomCylinderGetParams(Cylinder,&m_fCylinderRadius,&m_fCylinderSize); + + // get trimesh position and orientation + const dReal* pRotTris = dGeomGetRotation(Trimesh); + dMatrix3Copy(pRotTris,m_mTrimeshRot); + dGeomGetQuaternion(Trimesh,m_qTrimeshRot); + + // Position + const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(Trimesh); + dVector3Copy(*pPosTris,m_vTrimeshPos); + + + // calculate basic angle for 8-gon + dReal fAngle = (dReal) (M_PI / nCYLINDER_CIRCLE_SEGMENTS); + // calculate angle increment + dReal fAngleIncrement = fAngle*REAL(2.0); + + // calculate plane normals + // axis dependant code + for(int i=0; i= (m_iFlags & NUMC_MASK)); + + return ctContacts0; +} + +// OPCODE version of cylinder to mesh collider +#if dTRIMESH_OPCODE +static void dQueryCTLPotentialCollisionTriangles(OBBCollider &Collider, + sCylinderTrimeshColliderData &cData, dxGeom *Cylinder, dxTriMesh *Trimesh, + OBBCache &BoxCache) +{ + Matrix4x4 MeshMatrix; + const dVector3 vZeroVector3 = { REAL(0.0), }; + MakeMatrix(vZeroVector3, cData.m_mTrimeshRot, MeshMatrix); + + const dVector3 &vCylinderPos = cData.m_vCylinderPos; + const dMatrix3 &mCylinderRot = cData.m_mCylinderRot; + + dVector3 vCylinderOffsetPos; + dSubtractVectors3(vCylinderOffsetPos, vCylinderPos, cData.m_vTrimeshPos); + + const dReal fCylinderRadius = cData.m_fCylinderRadius, fCylinderHalfAxis = cData.m_fCylinderSize * REAL(0.5); + + OBB obbCylinder; + obbCylinder.mCenter.Set(vCylinderOffsetPos[0], vCylinderOffsetPos[1], vCylinderOffsetPos[2]); + obbCylinder.mExtents.Set( + 0 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 1 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius, + 2 == nCYLINDER_AXIS ? fCylinderHalfAxis : fCylinderRadius); + obbCylinder.mRot.Set( + mCylinderRot[0], mCylinderRot[4], mCylinderRot[8], + mCylinderRot[1], mCylinderRot[5], mCylinderRot[9], + mCylinderRot[2], mCylinderRot[6], mCylinderRot[10]); + + // TC results + if (Trimesh->getDoTC(dxTriMesh::TTC_BOX)) + { + dxTriMesh::BoxTC* BoxTC = 0; + const int iBoxCacheSize = Trimesh->m_BoxTCCache.size(); + for (int i = 0; i != iBoxCacheSize; i++) + { + if (Trimesh->m_BoxTCCache[i].Geom == Cylinder) + { + BoxTC = &Trimesh->m_BoxTCCache[i]; + break; + } + } + if (!BoxTC) + { + Trimesh->m_BoxTCCache.push(dxTriMesh::BoxTC()); + + BoxTC = &Trimesh->m_BoxTCCache[Trimesh->m_BoxTCCache.size() - 1]; + BoxTC->Geom = Cylinder; + BoxTC->FatCoeff = REAL(1.0); + } + + // Intersect + Collider.SetTemporalCoherence(true); + Collider.Collide(*BoxTC, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } + else + { + Collider.SetTemporalCoherence(false); + Collider.Collide(BoxCache, obbCylinder, Trimesh->retrieveMeshBVTreeRef(), null, &MeshMatrix); + } +} + +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + const unsigned uiTLSKind = Trimesh->getParentSpaceTLSKind(); + dIASSERT(uiTLSKind == Cylinder->getParentSpaceTLSKind()); // The colliding spaces must use matching cleanup method + TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(uiTLSKind); + OBBCollider& Collider = pccColliderCache->m_OBBCollider; + + dQueryCTLPotentialCollisionTriangles(Collider, cData, Cylinder, Trimesh, pccColliderCache->m_DefaultBoxCache); + + // Retrieve data + int TriCount = Collider.GetNbTouchedPrimitives(); + + if (TriCount != 0) + { + const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); + + if (Trimesh->m_ArrayCallback != NULL) + { + Trimesh->m_ArrayCallback(Trimesh, Cylinder, Triangles, TriCount); + } + + // allocate buffer for local contacts on stack + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + int ctContacts0 = 0; + + // loop through all intersecting triangles + for (int i = 0; i < TriCount; i++) + { + const int Triint = Triangles[i]; + if (!Trimesh->invokeCallback(Cylinder, Triint)) continue; + + + dVector3 dv[3]; + Trimesh->fetchMeshTriangle(dv, Triint, cData.m_vTrimeshPos, cData.m_mTrimeshRot); + + bool bFinishSearching; + ctContacts0 = cData.TestCollisionForSingleTriangle(ctContacts0, Triint, dv, bFinishSearching); + + if (bFinishSearching) + { + break; + } + } + + if (cData.m_nContacts != 0) + { + nContactCount = cData._ProcessLocalContacts(contact, Cylinder, Trimesh); + } + } + + return nContactCount; +} +#endif + +// GIMPACT version of cylinder to mesh collider +#if dTRIMESH_GIMPACT +int dCollideCylinderTrimesh(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + dIASSERT( skip >= (int)sizeof( dContactGeom ) ); + dIASSERT( o1->type == dCylinderClass ); + dIASSERT( o2->type == dTriMeshClass ); + dIASSERT ((flags & NUMC_MASK) >= 1); + + int nContactCount = 0; + + dxGeom *Cylinder = o1; + dxTriMesh *Trimesh = (dxTriMesh *)o2; + + GDYNAMIC_ARRAY collision_result; + GIM_CREATE_BOXQUERY_LIST(collision_result); + + Cylinder->recomputeAABB(); + Trimesh->recomputeAABB(); + + //*****at first , collide box aabb******// + + aabb3f test_aabb(Cylinder->aabb[0], Cylinder->aabb[1], Cylinder->aabb[2], Cylinder->aabb[3], Cylinder->aabb[4], Cylinder->aabb[5]); + gim_aabbset_box_collision(&test_aabb, &Trimesh->m_collision_trimesh.m_aabbset , &collision_result); + + if (collision_result.m_size != 0) + { + // Main data holder + sCylinderTrimeshColliderData cData(flags, skip); + cData._InitCylinderTrimeshData(Cylinder, Trimesh); + + //*****Set globals for box collision******// + + int ctContacts0 = 0; + cData.m_gLocalContacts = (sLocalContactData*)dALLOCA16(sizeof(sLocalContactData)*(cData.m_iFlags & NUMC_MASK)); + + GUINT32 * boxesresult = GIM_DYNARRAY_POINTER(GUINT32,collision_result); + GIM_TRIMESH * ptrimesh = &Trimesh->m_collision_trimesh; + + gim_trimesh_locks_work_data(ptrimesh); + + for(unsigned int i=0;i -#include -#include "ccdcustom/vec3.h" -#include "ccdcustom/quat.h" -#include "config.h" -#include "odemath.h" -#include "collision_libccd.h" -#include "collision_trimesh_internal.h" -#include "collision_std.h" -#include "collision_util.h" -#include "error.h" - - -struct _ccd_obj_t { - ccd_vec3_t pos; - ccd_quat_t rot, rot_inv; -}; -typedef struct _ccd_obj_t ccd_obj_t; - -struct _ccd_box_t { - ccd_obj_t o; - ccd_real_t dim[3]; -}; -typedef struct _ccd_box_t ccd_box_t; - -struct _ccd_cap_t { - ccd_obj_t o; - ccd_real_t radius; - ccd_vec3_t axis; - ccd_vec3_t p1; - ccd_vec3_t p2; -}; -typedef struct _ccd_cap_t ccd_cap_t; - -struct _ccd_cyl_t { - ccd_obj_t o; - ccd_real_t radius; - ccd_vec3_t axis; - ccd_vec3_t p1; - ccd_vec3_t p2; -}; -typedef struct _ccd_cyl_t ccd_cyl_t; - -struct _ccd_sphere_t { - ccd_obj_t o; - ccd_real_t radius; -}; -typedef struct _ccd_sphere_t ccd_sphere_t; - -struct _ccd_convex_t { - ccd_obj_t o; - dxConvex *convex; -}; -typedef struct _ccd_convex_t ccd_convex_t; - -struct _ccd_triangle_t { - ccd_obj_t o; - ccd_vec3_t vertices[3]; -}; -typedef struct _ccd_triangle_t ccd_triangle_t; - -/** Transforms geom to ccd struct */ -static void ccdGeomToObj(const dGeomID g, ccd_obj_t *); -static void ccdGeomToBox(const dGeomID g, ccd_box_t *); -static void ccdGeomToCap(const dGeomID g, ccd_cap_t *); -static void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *); -static void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *); -static void ccdGeomToConvex(const dGeomID g, ccd_convex_t *); - -/** Support functions */ -static void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); -static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); -static void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); -static void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); -static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); - -/** Center function */ -static void ccdCenter(const void *obj, ccd_vec3_t *c); - -/** General collide function */ -static int ccdCollide(dGeomID o1, dGeomID o2, int flags, - dContactGeom *contact, int skip, - void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, - void *obj2, ccd_support_fn supp2, ccd_center_fn cen2); - -static int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip); -static bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth); -// Adds a contact between 2 cylinders -static int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip); - -static unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, - const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, - ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount); -static bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices); -static unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip); -static void setObjPosToTriangleCenter(ccd_triangle_t *t); -static void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); - - -static -void ccdGeomToObj(const dGeomID g, ccd_obj_t *o) -{ - const dReal *ode_pos; - dQuaternion ode_rot; - - ode_pos = dGeomGetPosition(g); - dGeomGetQuaternion(g, ode_rot); - - ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]); - ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]); - - ccdQuatInvert2(&o->rot_inv, &o->rot); -} - -static -void ccdGeomToBox(const dGeomID g, ccd_box_t *box) -{ - dVector3 dim; - - ccdGeomToObj(g, (ccd_obj_t *)box); - - dGeomBoxGetLengths(g, dim); - box->dim[0] = (ccd_real_t)(dim[0] * 0.5); - box->dim[1] = (ccd_real_t)(dim[1] * 0.5); - box->dim[2] = (ccd_real_t)(dim[2] * 0.5); -} - -static -void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap) -{ - dReal r, h; - ccdGeomToObj(g, (ccd_obj_t *)cap); - - dGeomCapsuleGetParams(g, &r, &h); - cap->radius = r; - ccdVec3Set(&cap->axis, 0.0, 0.0, h / 2); - ccdQuatRotVec(&cap->axis, &cap->o.rot); - ccdVec3Copy(&cap->p1, &cap->axis); - ccdVec3Copy(&cap->p2, &cap->axis); - ccdVec3Scale(&cap->p2, -1.0); - ccdVec3Add(&cap->p1, &cap->o.pos); - ccdVec3Add(&cap->p2, &cap->o.pos); -} - -static -void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *cyl) -{ - dReal r, h; - ccdGeomToObj(g, (ccd_obj_t *)cyl); - - dGeomCylinderGetParams(g, &r, &h); - cyl->radius = r; - ccdVec3Set(&cyl->axis, 0.0, 0.0, h / 2); - ccdQuatRotVec(&cyl->axis, &cyl->o.rot); - ccdVec3Copy(&cyl->p1, &cyl->axis); - ccdVec3Copy(&cyl->p2, &cyl->axis); - int cylAxisNormalizationResult = ccdVec3SafeNormalize(&cyl->axis); - dUVERIFY(cylAxisNormalizationResult == 0, "Invalid cylinder has been passed"); - ccdVec3Scale(&cyl->p2, -1.0); - ccdVec3Add(&cyl->p1, &cyl->o.pos); - ccdVec3Add(&cyl->p2, &cyl->o.pos); -} - -static -void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *s) -{ - ccdGeomToObj(g, (ccd_obj_t *)s); - s->radius = dGeomSphereGetRadius(g); -} - -static -void ccdGeomToConvex(const dGeomID g, ccd_convex_t *c) -{ - ccdGeomToObj(g, (ccd_obj_t *)c); - c->convex = (dxConvex *)g; -} - - -static -void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_box_t *o = (const ccd_box_t *)obj; - ccd_vec3_t dir; - - ccdVec3Copy(&dir, _dir); - ccdQuatRotVec(&dir, &o->o.rot_inv); - - ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], - ccdSign(ccdVec3Y(&dir)) * o->dim[1], - ccdSign(ccdVec3Z(&dir)) * o->dim[2]); - - // transform support vertex - ccdQuatRotVec(v, &o->o.rot); - ccdVec3Add(v, &o->o.pos); -} - -static -void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_cap_t *o = (const ccd_cap_t *)obj; - - ccdVec3Copy(v, _dir); - ccdVec3Scale(v, o->radius); - - if (ccdVec3Dot(_dir, &o->axis) > 0.0){ - ccdVec3Add(v, &o->p1); - }else{ - ccdVec3Add(v, &o->p2); - } - -} - -static -void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj; - ccd_vec3_t dir; - ccd_real_t len; - - ccd_real_t dot = ccdVec3Dot(_dir, &cyl->axis); - if (dot > 0.0){ - ccdVec3Copy(v, &cyl->p1); - } else{ - ccdVec3Copy(v, &cyl->p2); - } - // project dir onto cylinder's 'top'/'bottom' plane - ccdVec3Copy(&dir, &cyl->axis); - ccdVec3Scale(&dir, -dot); - ccdVec3Add(&dir, _dir); - len = CCD_SQRT(ccdVec3Len2(&dir)); - if (!ccdIsZero(len)) { - ccdVec3Scale(&dir, cyl->radius / len); - ccdVec3Add(v, &dir); - } -} - -static -void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_sphere_t *s = (const ccd_sphere_t *)obj; - - ccdVec3Copy(v, _dir); - ccdVec3Scale(v, s->radius); - dIASSERT(dFabs(CCD_SQRT(ccdVec3Len2(_dir)) - REAL(1.0)) < 1e-6); // ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(_dir))); - - ccdVec3Add(v, &s->o.pos); -} - -static -void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_convex_t *c = (const ccd_convex_t *)obj; - ccd_vec3_t dir, p; - ccd_real_t maxdot, dot; - sizeint i; - const dReal *curp; - - ccdVec3Copy(&dir, _dir); - ccdQuatRotVec(&dir, &c->o.rot_inv); - - maxdot = -CCD_REAL_MAX; - curp = c->convex->points; - for (i = 0; i < c->convex->pointcount; i++, curp += 3){ - ccdVec3Set(&p, curp[0], curp[1], curp[2]); - dot = ccdVec3Dot(&dir, &p); - if (dot > maxdot){ - ccdVec3Copy(v, &p); - maxdot = dot; - } - } - - - // transform support vertex - ccdQuatRotVec(v, &c->o.rot); - ccdVec3Add(v, &c->o.pos); -} - -static -void ccdCenter(const void *obj, ccd_vec3_t *c) -{ - const ccd_obj_t *o = (const ccd_obj_t *)obj; - ccdVec3Copy(c, &o->pos); -} - -static -int ccdCollide( - dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, int skip, - void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, - void *obj2, ccd_support_fn supp2, ccd_center_fn cen2) -{ - ccd_t ccd; - int res; - ccd_real_t depth; - ccd_vec3_t dir, pos; - int max_contacts = (flags & NUMC_MASK); - - if (max_contacts < 1) - return 0; - - CCD_INIT(&ccd); - ccd.support1 = supp1; - ccd.support2 = supp2; - ccd.center1 = cen1; - ccd.center2 = cen2; - ccd.max_iterations = 500; - ccd.mpr_tolerance = (ccd_real_t)1E-6; - - - if (flags & CONTACTS_UNIMPORTANT){ - if (ccdMPRIntersect(obj1, obj2, &ccd)){ - return 1; - }else{ - return 0; - } - } - - res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); - if (res == 0){ - contact->g1 = o1; - contact->g2 = o2; - - contact->side1 = contact->side2 = -1; - - contact->depth = depth; - - contact->pos[0] = ccdVec3X(&pos); - contact->pos[1] = ccdVec3Y(&pos); - contact->pos[2] = ccdVec3Z(&pos); - - ccdVec3Scale(&dir, -1.); - contact->normal[0] = ccdVec3X(&dir); - contact->normal[1] = ccdVec3Y(&dir); - contact->normal[2] = ccdVec3Z(&dir); - - return 1; - } - - return 0; -} - -/*extern */ -int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_cyl_t cyl; - ccd_box_t box; - - ccdGeomToBox(o1, &box); - ccdGeomToCyl(o2, &cyl); - - return ccdCollide(o1, o2, flags, contact, skip, - &box, ccdSupportBox, ccdCenter, - &cyl, ccdSupportCyl, ccdCenter); -} - -/*extern */ -int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_cap_t cap; - ccd_cyl_t cyl; - - ccdGeomToCap(o1, &cap); - ccdGeomToCyl(o2, &cyl); - - return ccdCollide(o1, o2, flags, contact, skip, - &cap, ccdSupportCap, ccdCenter, - &cyl, ccdSupportCyl, ccdCenter); -} - -/*extern */ -int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_box_t box; - ccd_convex_t conv; - - ccdGeomToConvex(o1, &conv); - ccdGeomToBox(o2, &box); - - return ccdCollide(o1, o2, flags, contact, skip, - &conv, ccdSupportConvex, ccdCenter, - &box, ccdSupportBox, ccdCenter); -} - -/*extern */ -int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_cap_t cap; - ccd_convex_t conv; - - ccdGeomToConvex(o1, &conv); - ccdGeomToCap(o2, &cap); - - return ccdCollide(o1, o2, flags, contact, skip, - &conv, ccdSupportConvex, ccdCenter, - &cap, ccdSupportCap, ccdCenter); -} - -/*extern */ -int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_sphere_t sphere; - ccd_convex_t conv; - - ccdGeomToConvex(o1, &conv); - ccdGeomToSphere(o2, &sphere); - - return ccdCollide(o1, o2, flags, contact, skip, - &conv, ccdSupportConvex, ccdCenter, - &sphere, ccdSupportSphere, ccdCenter); -} - -/*extern */ -int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_cyl_t cyl; - ccd_convex_t conv; - - ccdGeomToConvex(o1, &conv); - ccdGeomToCyl(o2, &cyl); - - return ccdCollide(o1, o2, flags, contact, skip, - &conv, ccdSupportConvex, ccdCenter, - &cyl, ccdSupportCyl, ccdCenter); -} - -/*extern */ -int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_convex_t c1, c2; - - ccdGeomToConvex(o1, &c1); - ccdGeomToConvex(o2, &c2); - - return ccdCollide(o1, o2, flags, contact, skip, - &c1, ccdSupportConvex, ccdCenter, - &c2, ccdSupportConvex, ccdCenter); -} - - -/*extern */ -int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) -{ - ccd_cyl_t cyl1, cyl2; - - ccdGeomToCyl(o1, &cyl1); - ccdGeomToCyl(o2, &cyl2); - - int numContacts = collideCylCyl(o1, o2, &cyl1, &cyl2, flags, contact, skip); - if (numContacts < 0) { - numContacts = ccdCollide(o1, o2, flags, contact, skip, - &cyl1, ccdSupportCyl, ccdCenter, - &cyl2, ccdSupportCyl, ccdCenter); - } - return numContacts; -} - -static -int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip) -{ - int maxContacts = (flags & NUMC_MASK); - dAASSERT(maxContacts != 0); - - maxContacts = maxContacts > 8 ? 8 : maxContacts; - - dReal axesProd = dFabs(ccdVec3Dot(&cyl1->axis, &cyl2->axis)); - // Check if cylinders' axes are in line - if (REAL(1.0) - axesProd < 1e-3f) { - ccd_vec3_t p, proj; - dReal r1, l1; - dReal r2, l2; - dGeomCylinderGetParams(o1, &r1, &l1); - dGeomCylinderGetParams(o2, &r2, &l2); - l1 *= 0.5f; - l2 *= 0.5f; - - // Determine the cylinder with smaller radius (minCyl) and bigger radius (maxCyl) and their respective properties: radius, length - bool r1IsMin; - dReal rmin, rmax; - ccd_cyl_t *minCyl, *maxCyl; - if (r1 <= r2) { - rmin = r1; rmax = r2; - minCyl = cyl1; maxCyl = cyl2; - r1IsMin = true; - } - else { - rmin = r2; rmax = r1; - minCyl = cyl2; maxCyl = cyl1; - r1IsMin = false; - } - - dReal lSum = l1 + l2; - - ccdVec3Copy(&p, &minCyl->o.pos); - ccdVec3Sub(&p, &maxCyl->o.pos); - dReal dot = ccdVec3Dot(&p, &maxCyl->axis); - - // Maximum possible contact depth - dReal depth_v = lSum - dFabs(dot) + dSqrt(dMax(0, REAL(1.0) - axesProd * axesProd)) * rmin; - if (depth_v < 0) { - return 0; - } - - // Project the smaller cylinder's center onto the larger cylinder's plane - ccdVec3Copy(&proj, &maxCyl->axis); - ccdVec3Scale(&proj, -dot); - ccdVec3Add(&proj, &p); - dReal radiiDiff = (dReal)sqrt(ccdVec3Len2(&proj)); - dReal depth_h = r1 + r2 - radiiDiff; - - // Check the distance between cylinders' centers - if (depth_h < 0) { - return 0; - } - - // Check if "vertical" contact depth is less than "horizontal" contact depth - if (depth_v < depth_h) { - int contactCount = 0; - dReal dot2 = -ccdVec3Dot(&p, &minCyl->axis); - // lmin, lmax - distances from cylinders' centers to potential contact points relative to cylinders' axes - dReal lmax = r1IsMin ? l2 : l1; - dReal lmin = r1IsMin ? l1 : l2; - lmin = dot2 < 0 ? -lmin : lmin; - lmax = dot < 0 ? -lmax : lmax; - // Contact normal direction, relative to o1's axis - dReal normaldir = (dot < 0) != r1IsMin ? REAL(1.0) : -REAL(1.0); - - if (rmin + radiiDiff <= rmax) { - // Case 1: The smaller disc is fully contained within the larger one - // Simply generate N points on the rim of the smaller disc - dReal maxContactsRecip = (dReal)(0 < maxContacts ? (2.0 * M_PI / maxContacts) : (2.0 * M_PI)); // The 'else' value does not matter. Just try helping the optimizer. - for (int i = 0; i < maxContacts; i++) { - dReal depth; - dReal a = maxContactsRecip * i; - if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { - contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); - if ((flags & CONTACTS_UNIMPORTANT) != 0) { - dIASSERT(contactCount != 0); - break; - } - } - } - return contactCount; - - } else { - // Case 2: Discs intersect - // Firstly, find intersections assuming the larger cylinder is placed at (0,0,0) - // http://math.stackexchange.com/questions/256100/how-can-i-find-the-points-at-which-two-circles-intersect - ccd_vec3_t proj2; - ccdVec3Copy(&proj2, &proj); - ccdQuatRotVec(&proj, &maxCyl->o.rot_inv); - dReal d = dSqrt(ccdVec3X(&proj) * ccdVec3X(&proj) + ccdVec3Y(&proj) * ccdVec3Y(&proj)); - dIASSERT(d != REAL(0.0)); - - dReal dRecip = REAL(1.0) / d; - dReal rmaxSquare = rmax * rmax, rminSquare = rmin * rmin, dSquare = d * d; - - dReal minA, diffA, minB, diffB; - - { - dReal l = (rmaxSquare - rminSquare + dSquare) * (REAL(0.5) * dRecip); - dReal h = dSqrt(rmaxSquare - l * l); - dReal divLbyD = l * dRecip, divHbyD = h * dRecip; - dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); - dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); - dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); - dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); - // Map the intersection points to angles - dReal ap1 = dAtan2(y1, x1); - dReal ap2 = dAtan2(y2, x2); - minA = dMin(ap1, ap2); - dReal maxA = dMax(ap1, ap2); - // If the segment connecting cylinders' centers does not intersect the arc, change the angles - dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); - if (a < minA || a > maxA) { - a = maxA; - maxA = (dReal)(minA + M_PI * 2.0); - minA = a; - } - diffA = maxA - minA; - } - - // Do the same for the smaller cylinder assuming it is placed at (0,0,0) now - ccdVec3Copy(&proj, &proj2); - ccdVec3Scale(&proj, -1); - ccdQuatRotVec(&proj, &minCyl->o.rot_inv); - - { - dReal l = (rminSquare - rmaxSquare + dSquare) * (REAL(0.5) * dRecip); - dReal h = dSqrt(rminSquare - l * l); - dReal divLbyD = l * dRecip, divHbyD = h * dRecip; - dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); - dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); - dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); - dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); - dReal ap1 = dAtan2(y1, x1); - dReal ap2 = dAtan2(y2, x2); - minB = dMin(ap1, ap2); - dReal maxB = dMax(ap1, ap2); - dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); - if (a < minB || a > maxB) { - a = maxB; - maxB = (dReal)(minB + M_PI * 2.0); - minB = a; - } - diffB = maxB - minB; - } - - // Find contact point distribution ratio based on arcs lengths - dReal ratio = diffA * rmax / (diffA * rmax + diffB * rmin); - dIASSERT(ratio <= REAL(1.0)); - dIASSERT(ratio >= REAL(0.0)); - - int nMax = (int)dFloor(ratio * maxContacts + REAL(0.5)); - int nMin = maxContacts - nMax; - dIASSERT(nMax <= maxContacts); - - // Make sure there is at least one point on the smaller radius rim - if (nMin < 1) { - nMin = 1; nMax -= 1; - } - // Otherwise transfer one point to the larger radius rim as it is going to fill the rim intersection points - else if (nMin > 1) { - nMin -= 1; nMax += 1; - } - - // Smaller disc first, skipping the overlapping points - dReal nMinRecip = 0 < nMin ? diffB / (nMin + 1) : diffB; // The 'else' value does not matter. Just try helping the optimizer. - for (int i = 1; i <= nMin; i++) { - dReal depth; - dReal a = minB + nMinRecip * i; - if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { - contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); - if ((flags & CONTACTS_UNIMPORTANT) != 0) { - dIASSERT(contactCount != 0); - break; - } - } - } - - if (contactCount == 0 || (flags & CONTACTS_UNIMPORTANT) == 0) { - // Then the larger disc, + additional point as the start/end points of arcs overlap - // (or a single contact at the arc middle point if just one is required) - dReal nMaxRecip = nMax > 1 ? diffA / (nMax - 1) : diffA; // The 'else' value does not matter. Just try helping the optimizer. - dReal adjustedMinA = nMax == 1 ? minA + REAL(0.5) * diffA : minA; - - for (int i = 0; i < nMax; i++) { - dReal depth; - dReal a = adjustedMinA + nMaxRecip * i; - if (testAndPrepareDiscContactForAngle(a, rmax, lmax, lSum, maxCyl, minCyl, p, depth)) { - contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); - if ((flags & CONTACTS_UNIMPORTANT) != 0) { - dIASSERT(contactCount != 0); - break; - } - } - } - } - - return contactCount; - } - } - } - return -1; -} - -static -bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth) -{ - bool ret = false; - - ccd_vec3_t p2; - ccdVec3Set(&p, dCos(angle) * radius, dSin(angle) * radius, 0); - ccdQuatRotVec(&p, &priCyl->o.rot); - ccdVec3Add(&p, &priCyl->o.pos); - ccdVec3Copy(&p2, &p); - ccdVec3Sub(&p2, &secCyl->o.pos); - dReal depth = lSum - dFabs(ccdVec3Dot(&p2, &secCyl->axis)); - - if (depth >= 0) { - ccdVec3Copy(&p2, &priCyl->axis); - ccdVec3Scale(&p2, length); - ccdVec3Add(&p, &p2); - - out_depth = depth; - ret = true; - } - - return ret; -} - -static -int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, - ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip) -{ - dIASSERT(depth >= 0); - - dContactGeom* contact = SAFECONTACT(flags, contacts, j, skip); - contact->g1 = o1; - contact->g2 = o2; - contact->side1 = -1; - contact->side2 = -1; - contact->normal[0] = normaldir * ccdVec3X(axis); - contact->normal[1] = normaldir * ccdVec3Y(axis); - contact->normal[2] = normaldir * ccdVec3Z(axis); - contact->depth = depth; - contact->pos[0] = ccdVec3X(p); - contact->pos[1] = ccdVec3Y(p); - contact->pos[2] = ccdVec3Z(p); - - return j + 1; -} - - -#if dTRIMESH_ENABLED - -const static float CONTACT_DEPTH_EPSILON = 0.0001f; -const static float CONTACT_POS_EPSILON = 0.0001f; -const static float CONTACT_PERTURBATION_ANGLE = 0.001f; -const static float NORMAL_PROJ_EPSILON = 0.0001f; - - -/*extern */ -unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip) -{ - ccd_convex_t c1; - ccd_triangle_t c2; - dVector3 triangle[dMTV__MAX]; - unsigned maxContacts = (flags & NUMC_MASK); - unsigned contactCount = 0; - ccdGeomToConvex(o1, &c1); - ccdGeomToObj(o2, (ccd_obj_t *)&c2); - - IFaceAngleStorageView *meshFaceAngleView = dxGeomTriMeshGetFaceAngleView(o2); - dUASSERT(meshFaceAngleView != NULL, "Please preprocess the trimesh data with dTRIDATAPREPROCESS_BUILD_FACE_ANGLES"); - - for (unsigned i = 0; i != numIndices; ++i) { - dContactGeom tempContact; - dGeomTriMeshGetTriangle(o2, indices[i], &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); - - for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { - ccdVec3Set(&c2.vertices[j], (ccd_real_t)triangle[j][dV3E_X], (ccd_real_t)triangle[j][dV3E_Y], (ccd_real_t)triangle[j][dV3E_Z]); - } - - setObjPosToTriangleCenter(&c2); - - if (ccdCollide(o1, o2, flags, &tempContact, skip, &c1, &ccdSupportConvex, &ccdCenter, &c2, &ccdSupportTriangle, &ccdCenter) == 1) { - tempContact.side2 = i; - - if (meshFaceAngleView == NULL || correctTriangleContactNormal(&c2, &tempContact, meshFaceAngleView, indices, numIndices)) { - contactCount = addUniqueContact(contacts, &tempContact, contactCount, maxContacts, flags, skip); - - if ((flags & CONTACTS_UNIMPORTANT) != 0) { - break; - } - } - } - } - - if ((flags & CONTACTS_UNIMPORTANT) == 0 && contactCount == 1) { - dContactGeom *contact = SAFECONTACT(flags, contacts, 0, skip); - dGeomTriMeshGetTriangle(o2, contact->side2, &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); - contactCount = addTrianglePerturbedContacts(o1, o2, meshFaceAngleView, indices, numIndices, flags, contacts, skip, &c1, &c2, triangle, contact, contactCount); - } - - // Normalize accumulated normals, if necessary - for (unsigned k = 0; k != contactCount; ) { - dContactGeom *contact = SAFECONTACT(flags, contacts, k, skip); - bool stayWithinThisIndex = false; - - // Only the merged contact normals need to be normalized - if (*_const_type_cast_union(&contact->normal[dV3E_PAD])) { - - if (!dxSafeNormalize3(contact->normal)) { - // If the contact normals have added up to zero, erase the contact - // Normally the time step is to be shorter so that the objects do not get into each other that deep - --contactCount; - - if (k != contactCount) { - dContactGeom *lastContact = SAFECONTACT(flags, contacts, contactCount, skip); - *contact = *lastContact; - } - - stayWithinThisIndex = true; - } - } - - if (!stayWithinThisIndex) { - ++k; - } - } - - return contactCount; -} - -static -unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, - const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, - ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount) -{ - unsigned maxContacts = (flags & NUMC_MASK); - - dVector3 pos; - dCopyVector3(pos, contact->pos); - - dQuaternion q1[2], q2[2]; - dReal perturbationAngle = CONTACT_PERTURBATION_ANGLE; - - dVector3 upAxis; - bool upAvailable = false; - if (fabs(contact->normal[dV3E_Y]) > 0.7) { - dAssignVector3(upAxis, 0, 0, 1); - } - else { - dAssignVector3(upAxis, 0, 1, 0); - } - - dVector3 cross; - dCalcVectorCross3(cross, contact->normal, upAxis); - - if (dSafeNormalize3(cross)) { - dCalcVectorCross3(upAxis, cross, contact->normal); - - if (dSafeNormalize3(upAxis)) { - upAvailable = true; - } - } - - for (unsigned j = upAvailable ? 0 : 2; j != 2; ++j) { - dQFromAxisAndAngle(q1[j], upAxis[dV3E_X], upAxis[dV3E_Y], upAxis[dV3E_Z], perturbationAngle); - dQFromAxisAndAngle(q2[j], cross[dV3E_X], cross[dV3E_Y], cross[dV3E_Z], perturbationAngle); - perturbationAngle = -perturbationAngle; - } - - for (unsigned k = upAvailable ? 0 : 4; k != 4; ++k) { - dQuaternion qr; - dQMultiply0(qr, q1[k % 2], q2[k / 2]); - - for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { - dVector3 p, perturbed; - dSubtractVectors3(p, triangle[j], pos); - dQuatTransform(qr, p, perturbed); - dAddVectors3(perturbed, perturbed, pos); - - ccdVec3Set(&c2->vertices[j], (ccd_real_t)perturbed[dV3E_X], (ccd_real_t)perturbed[dV3E_Y], (ccd_real_t)perturbed[dV3E_Z]); - } - - dContactGeom perturbedContact; - setObjPosToTriangleCenter(c2); - - if (ccdCollide(o1, o2, flags, &perturbedContact, skip, c1, &ccdSupportConvex, &ccdCenter, c2, &ccdSupportTriangle, &ccdCenter) == 1) { - perturbedContact.side2 = contact->side2; - - if (meshFaceAngleView == NULL || correctTriangleContactNormal(c2, &perturbedContact, meshFaceAngleView, indices, numIndices)) { - contacCount = addUniqueContact(contacts, &perturbedContact, contacCount, maxContacts, flags, skip); - } - } - } - - return contacCount; -} - -static -bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, - IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices) -{ - dIASSERT(meshFaceAngleView != NULL); - - bool anyFault = false; - - ccd_vec3_t cntOrigNormal, cntNormal; - ccdVec3Set(&cntNormal, contact->normal[0], contact->normal[1], contact->normal[2]); - ccdVec3Copy(&cntOrigNormal, &cntNormal); - - // Check if the contact point is located close to any edge - move it back and forth - // and check the resulting segment for intersection with the edge plane - ccd_vec3_t cntScaledNormal; - ccdVec3CopyScaled(&cntScaledNormal, &cntNormal, contact->depth); - - ccd_vec3_t edges[dMTV__MAX]; - ccdVec3Sub2(&edges[dMTV_THIRD], &t->vertices[0], &t->vertices[2]); - ccdVec3Sub2(&edges[dMTV_SECOND], &t->vertices[2], &t->vertices[1]); - ccdVec3Sub2(&edges[dMTV_FIRST], &t->vertices[1], &t->vertices[0]); - dSASSERT(dMTV__MAX == 3); - - bool contactGenerated = false, contactPreserved = false; - // Triangle face normal - ccd_vec3_t triNormal; - ccdVec3Cross(&triNormal, &edges[dMTV_FIRST], &edges[dMTV_SECOND]); - if (ccdVec3SafeNormalize(&triNormal) != 0) { - anyFault = true; - } - - // Check the edges to see if one of them is involved - for (unsigned testEdgeIndex = !anyFault ? dMTV__MIN : dMTV__MAX; testEdgeIndex != dMTV__MAX; ++testEdgeIndex) { - ccd_vec3_t edgeNormal, vertexToPos, v; - ccd_vec3_t &edgeAxis = edges[testEdgeIndex]; - - // Edge axis - if (ccdVec3SafeNormalize(&edgeAxis) != 0) { - // This should not happen normally as in the case on of edges is degenerated - // the triangle normal calculation would have to fail above. If for some - // reason the above calculation succeeds and this one would not, it is - // OK to break as this point as well. - anyFault = true; - break; - } - - // Edge Normal - ccdVec3Cross(&edgeNormal, &edgeAxis, &triNormal); - // ccdVec3Normalize(&edgeNormal); -- the two vectors above were already normalized and perpendicular - - // Check if the contact point is located close to any edge - move it back and forth - // and check the resulting segment for intersection with the edge plane - ccdVec3Set(&vertexToPos, contact->pos[0], contact->pos[1], contact->pos[2]); - ccdVec3Sub(&vertexToPos, &t->vertices[testEdgeIndex]); - ccdVec3Sub2(&v, &vertexToPos, &cntScaledNormal); - - if (ccdVec3Dot(&edgeNormal, &v) < 0) { - ccdVec3Add2(&v, &vertexToPos, &cntScaledNormal); - - if (ccdVec3Dot(&edgeNormal, &v) > 0) { - // This is an edge contact - - ccd_real_t x = ccdVec3Dot(&triNormal, &cntNormal); - ccd_real_t y = ccdVec3Dot(&edgeNormal, &cntNormal); - ccd_real_t contactNormalToTriangleNormalAngle = CCD_ATAN2(y, x); - - dReal angleValueAsDRead; - FaceAngleDomain angleDomain = meshFaceAngleView->retrieveFacesAngleFromStorage(angleValueAsDRead, contact->side2, (dMeshTriangleVertex)testEdgeIndex); - ccd_real_t angleValue = (ccd_real_t)angleValueAsDRead; - - ccd_real_t targetAngle; - contactGenerated = false, contactPreserved = false; // re-assign to make optimizer's task easier - - if (angleDomain != FAD_CONCAVE) { - // Convex or flat - ensure the contact normal is within the allowed range - // formed by the two triangles' normals. - if (contactNormalToTriangleNormalAngle < CCD_ZERO) { - targetAngle = CCD_ZERO; - } - else if (contactNormalToTriangleNormalAngle > angleValue) { - targetAngle = angleValue; - } - else { - contactPreserved = true; - } - } - else { - // Concave - rotate the contact normal to the face angle bisect plane - // (or to triangle normal-edge plane if negative angles are not stored) - targetAngle = angleValue != 0 ? CCD_REAL(0.5) * angleValue : CCD_ZERO; - // There is little chance the normal will initially match the correct plane, but still, a small check could save lots of calculations - if (contactNormalToTriangleNormalAngle == targetAngle) { - contactPreserved = true; - } - } - - if (!contactPreserved) { - ccd_quat_t q; - ccdQuatSetAngleAxis(&q, targetAngle - contactNormalToTriangleNormalAngle, &edgeAxis); - ccdQuatRotVec2(&cntNormal, &cntNormal, &q); - contactGenerated = true; - } - - // Calculated successfully - break; - } - } - } - - if (!anyFault && !contactPreserved) { - // No edge contact detected, set contact normal to triangle normal - const ccd_vec3_t &cntNormalToUse = !contactGenerated ? triNormal : cntNormal; - - contact->normal[dV3E_X] = ccdVec3X(&cntNormalToUse); - contact->normal[dV3E_Y] = ccdVec3Y(&cntNormalToUse); - contact->normal[dV3E_Z] = ccdVec3Z(&cntNormalToUse); - contact->depth *= CCD_FMAX(0.0, ccdVec3Dot(&cntOrigNormal, &cntNormalToUse)); - } - - bool result = !anyFault; - return result; -} - - -static -unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip) -{ - dReal minDepth = c->depth; - unsigned index = contactcount; - bool isDuplicate = false; - - dReal c_posX = c->pos[dV3E_X], c_posY = c->pos[dV3E_Y], c_posZ = c->pos[dV3E_Z]; - for (unsigned k = 0; k != contactcount; k++) { - dContactGeom* pc = SAFECONTACT(flags, contacts, k, skip); - - if (fabs(c_posX - pc->pos[dV3E_X]) < CONTACT_POS_EPSILON - && fabs(c_posY - pc->pos[dV3E_Y]) < CONTACT_POS_EPSILON - && fabs(c_posZ - pc->pos[dV3E_Z]) < CONTACT_POS_EPSILON) { - dSASSERT(dV3E__AXES_MAX - dV3E__AXES_MIN == 3); - - // Accumulate similar contacts - dAddVectors3(pc->normal, pc->normal, c->normal); - pc->depth = dMax(pc->depth, c->depth); - *_type_cast_union(&pc->normal[dV3E_PAD]) = true; // Mark the contact as a merged one - - isDuplicate = true; - break; - } - - if (contactcount == maxcontacts && pc->depth < minDepth) { - minDepth = pc->depth; - index = k; - } - } - - if (!isDuplicate && index < maxcontacts) { - dContactGeom* contact = SAFECONTACT(flags, contacts, index, skip); - contact->g1 = c->g1; - contact->g2 = c->g2; - contact->depth = c->depth; - contact->side1 = c->side1; - contact->side2 = c->side2; - dCopyVector3(contact->pos, c->pos); - dCopyVector3(contact->normal, c->normal); - *_type_cast_union(&contact->normal[dV3E_PAD]) = false; // Indicates whether the contact is merged or not - contactcount = index == contactcount ? contactcount + 1 : contactcount; - } - - return contactcount; -} - -static -void setObjPosToTriangleCenter(ccd_triangle_t *t) -{ - ccdVec3Set(&t->o.pos, 0, 0, 0); - for (int j = 0; j < 3; j++) { - ccdVec3Add(&t->o.pos, &t->vertices[j]); - } - ccdVec3Scale(&t->o.pos, 1.0f / 3.0f); -} - -static -void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) -{ - const ccd_triangle_t* o = (ccd_triangle_t *) obj; - ccd_real_t maxdot, dot; - maxdot = -CCD_REAL_MAX; - for (unsigned i = 0; i != 3; i++) { - dot = ccdVec3Dot(_dir, &o->vertices[i]); - if (dot > maxdot) { - ccdVec3Copy(v, &o->vertices[i]); - maxdot = dot; - } - } -} - - -#endif // dTRIMESH_ENABLED +/************************************************************************* + * * + * 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 +#include "ccdcustom/vec3.h" +#include "ccdcustom/quat.h" +#include "config.h" +#include "odemath.h" +#include "collision_libccd.h" +#include "collision_trimesh_internal.h" +#include "collision_std.h" +#include "collision_util.h" +#include "error.h" + + +struct _ccd_obj_t { + ccd_vec3_t pos; + ccd_quat_t rot, rot_inv; +}; +typedef struct _ccd_obj_t ccd_obj_t; + +struct _ccd_box_t { + ccd_obj_t o; + ccd_real_t dim[3]; +}; +typedef struct _ccd_box_t ccd_box_t; + +struct _ccd_cap_t { + ccd_obj_t o; + ccd_real_t radius; + ccd_vec3_t axis; + ccd_vec3_t p1; + ccd_vec3_t p2; +}; +typedef struct _ccd_cap_t ccd_cap_t; + +struct _ccd_cyl_t { + ccd_obj_t o; + ccd_real_t radius; + ccd_vec3_t axis; + ccd_vec3_t p1; + ccd_vec3_t p2; +}; +typedef struct _ccd_cyl_t ccd_cyl_t; + +struct _ccd_sphere_t { + ccd_obj_t o; + ccd_real_t radius; +}; +typedef struct _ccd_sphere_t ccd_sphere_t; + +struct _ccd_convex_t { + ccd_obj_t o; + dxConvex *convex; +}; +typedef struct _ccd_convex_t ccd_convex_t; + +struct _ccd_triangle_t { + ccd_obj_t o; + ccd_vec3_t vertices[3]; +}; +typedef struct _ccd_triangle_t ccd_triangle_t; + +/** Transforms geom to ccd struct */ +static void ccdGeomToObj(const dGeomID g, ccd_obj_t *); +static void ccdGeomToBox(const dGeomID g, ccd_box_t *); +static void ccdGeomToCap(const dGeomID g, ccd_cap_t *); +static void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *); +static void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *); +static void ccdGeomToConvex(const dGeomID g, ccd_convex_t *); + +/** Support functions */ +static void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); +static void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); + +/** Center function */ +static void ccdCenter(const void *obj, ccd_vec3_t *c); + +/** General collide function */ +static int ccdCollide(dGeomID o1, dGeomID o2, int flags, + dContactGeom *contact, int skip, + void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, + void *obj2, ccd_support_fn supp2, ccd_center_fn cen2); + +static int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip); +static bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth); +// Adds a contact between 2 cylinders +static int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip); + +static unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, + const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, + ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount); +static bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices); +static unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip); +static void setObjPosToTriangleCenter(ccd_triangle_t *t); +static void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v); + + +static +void ccdGeomToObj(const dGeomID g, ccd_obj_t *o) +{ + const dReal *ode_pos; + dQuaternion ode_rot; + + ode_pos = dGeomGetPosition(g); + dGeomGetQuaternion(g, ode_rot); + + ccdVec3Set(&o->pos, ode_pos[0], ode_pos[1], ode_pos[2]); + ccdQuatSet(&o->rot, ode_rot[1], ode_rot[2], ode_rot[3], ode_rot[0]); + + ccdQuatInvert2(&o->rot_inv, &o->rot); +} + +static +void ccdGeomToBox(const dGeomID g, ccd_box_t *box) +{ + dVector3 dim; + + ccdGeomToObj(g, (ccd_obj_t *)box); + + dGeomBoxGetLengths(g, dim); + box->dim[0] = (ccd_real_t)(dim[0] * 0.5); + box->dim[1] = (ccd_real_t)(dim[1] * 0.5); + box->dim[2] = (ccd_real_t)(dim[2] * 0.5); +} + +static +void ccdGeomToCap(const dGeomID g, ccd_cap_t *cap) +{ + dReal r, h; + ccdGeomToObj(g, (ccd_obj_t *)cap); + + dGeomCapsuleGetParams(g, &r, &h); + cap->radius = r; + ccdVec3Set(&cap->axis, 0.0, 0.0, h / 2); + ccdQuatRotVec(&cap->axis, &cap->o.rot); + ccdVec3Copy(&cap->p1, &cap->axis); + ccdVec3Copy(&cap->p2, &cap->axis); + ccdVec3Scale(&cap->p2, -1.0); + ccdVec3Add(&cap->p1, &cap->o.pos); + ccdVec3Add(&cap->p2, &cap->o.pos); +} + +static +void ccdGeomToCyl(const dGeomID g, ccd_cyl_t *cyl) +{ + dReal r, h; + ccdGeomToObj(g, (ccd_obj_t *)cyl); + + dGeomCylinderGetParams(g, &r, &h); + cyl->radius = r; + ccdVec3Set(&cyl->axis, 0.0, 0.0, h / 2); + ccdQuatRotVec(&cyl->axis, &cyl->o.rot); + ccdVec3Copy(&cyl->p1, &cyl->axis); + ccdVec3Copy(&cyl->p2, &cyl->axis); + int cylAxisNormalizationResult = ccdVec3SafeNormalize(&cyl->axis); + dUVERIFY(cylAxisNormalizationResult == 0, "Invalid cylinder has been passed"); + ccdVec3Scale(&cyl->p2, -1.0); + ccdVec3Add(&cyl->p1, &cyl->o.pos); + ccdVec3Add(&cyl->p2, &cyl->o.pos); +} + +static +void ccdGeomToSphere(const dGeomID g, ccd_sphere_t *s) +{ + ccdGeomToObj(g, (ccd_obj_t *)s); + s->radius = dGeomSphereGetRadius(g); +} + +static +void ccdGeomToConvex(const dGeomID g, ccd_convex_t *c) +{ + ccdGeomToObj(g, (ccd_obj_t *)c); + c->convex = (dxConvex *)g; +} + + +static +void ccdSupportBox(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_box_t *o = (const ccd_box_t *)obj; + ccd_vec3_t dir; + + ccdVec3Copy(&dir, _dir); + ccdQuatRotVec(&dir, &o->o.rot_inv); + + ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0], + ccdSign(ccdVec3Y(&dir)) * o->dim[1], + ccdSign(ccdVec3Z(&dir)) * o->dim[2]); + + // transform support vertex + ccdQuatRotVec(v, &o->o.rot); + ccdVec3Add(v, &o->o.pos); +} + +static +void ccdSupportCap(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_cap_t *o = (const ccd_cap_t *)obj; + + ccdVec3Copy(v, _dir); + ccdVec3Scale(v, o->radius); + + if (ccdVec3Dot(_dir, &o->axis) > 0.0){ + ccdVec3Add(v, &o->p1); + }else{ + ccdVec3Add(v, &o->p2); + } + +} + +static +void ccdSupportCyl(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_cyl_t *cyl = (const ccd_cyl_t *)obj; + ccd_vec3_t dir; + ccd_real_t len; + + ccd_real_t dot = ccdVec3Dot(_dir, &cyl->axis); + if (dot > 0.0){ + ccdVec3Copy(v, &cyl->p1); + } else{ + ccdVec3Copy(v, &cyl->p2); + } + // project dir onto cylinder's 'top'/'bottom' plane + ccdVec3Copy(&dir, &cyl->axis); + ccdVec3Scale(&dir, -dot); + ccdVec3Add(&dir, _dir); + len = CCD_SQRT(ccdVec3Len2(&dir)); + if (!ccdIsZero(len)) { + ccdVec3Scale(&dir, cyl->radius / len); + ccdVec3Add(v, &dir); + } +} + +static +void ccdSupportSphere(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_sphere_t *s = (const ccd_sphere_t *)obj; + + ccdVec3Copy(v, _dir); + ccdVec3Scale(v, s->radius); + dIASSERT(dFabs(CCD_SQRT(ccdVec3Len2(_dir)) - REAL(1.0)) < 1e-6); // ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(_dir))); + + ccdVec3Add(v, &s->o.pos); +} + +static +void ccdSupportConvex(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_convex_t *c = (const ccd_convex_t *)obj; + ccd_vec3_t dir, p; + ccd_real_t maxdot, dot; + sizeint i; + const dReal *curp; + + ccdVec3Copy(&dir, _dir); + ccdQuatRotVec(&dir, &c->o.rot_inv); + + maxdot = -CCD_REAL_MAX; + curp = c->convex->points; + for (i = 0; i < c->convex->pointcount; i++, curp += 3){ + ccdVec3Set(&p, curp[0], curp[1], curp[2]); + dot = ccdVec3Dot(&dir, &p); + if (dot > maxdot){ + ccdVec3Copy(v, &p); + maxdot = dot; + } + } + + + // transform support vertex + ccdQuatRotVec(v, &c->o.rot); + ccdVec3Add(v, &c->o.pos); +} + +static +void ccdCenter(const void *obj, ccd_vec3_t *c) +{ + const ccd_obj_t *o = (const ccd_obj_t *)obj; + ccdVec3Copy(c, &o->pos); +} + +static +int ccdCollide( + dGeomID o1, dGeomID o2, int flags, dContactGeom *contact, int skip, + void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, + void *obj2, ccd_support_fn supp2, ccd_center_fn cen2) +{ + ccd_t ccd; + int res; + ccd_real_t depth; + ccd_vec3_t dir, pos; + int max_contacts = (flags & NUMC_MASK); + + if (max_contacts < 1) + return 0; + + CCD_INIT(&ccd); + ccd.support1 = supp1; + ccd.support2 = supp2; + ccd.center1 = cen1; + ccd.center2 = cen2; + ccd.max_iterations = 500; + ccd.mpr_tolerance = (ccd_real_t)1E-6; + + + if (flags & CONTACTS_UNIMPORTANT){ + if (ccdMPRIntersect(obj1, obj2, &ccd)){ + return 1; + }else{ + return 0; + } + } + + res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos); + if (res == 0){ + contact->g1 = o1; + contact->g2 = o2; + + contact->side1 = contact->side2 = -1; + + contact->depth = depth; + + contact->pos[0] = ccdVec3X(&pos); + contact->pos[1] = ccdVec3Y(&pos); + contact->pos[2] = ccdVec3Z(&pos); + + ccdVec3Scale(&dir, -1.); + contact->normal[0] = ccdVec3X(&dir); + contact->normal[1] = ccdVec3Y(&dir); + contact->normal[2] = ccdVec3Z(&dir); + + return 1; + } + + return 0; +} + +/*extern */ +int dCollideBoxCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl; + ccd_box_t box; + + ccdGeomToBox(o1, &box); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &box, ccdSupportBox, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideCapsuleCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cap_t cap; + ccd_cyl_t cyl; + + ccdGeomToCap(o1, &cap); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &cap, ccdSupportCap, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideConvexBoxCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_box_t box; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToBox(o2, &box); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &box, ccdSupportBox, ccdCenter); +} + +/*extern */ +int dCollideConvexCapsuleCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cap_t cap; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToCap(o2, &cap); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &cap, ccdSupportCap, ccdCenter); +} + +/*extern */ +int dCollideConvexSphereCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_sphere_t sphere; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToSphere(o2, &sphere); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &sphere, ccdSupportSphere, ccdCenter); +} + +/*extern */ +int dCollideConvexCylinderCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl; + ccd_convex_t conv; + + ccdGeomToConvex(o1, &conv); + ccdGeomToCyl(o2, &cyl); + + return ccdCollide(o1, o2, flags, contact, skip, + &conv, ccdSupportConvex, ccdCenter, + &cyl, ccdSupportCyl, ccdCenter); +} + +/*extern */ +int dCollideConvexConvexCCD(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_convex_t c1, c2; + + ccdGeomToConvex(o1, &c1); + ccdGeomToConvex(o2, &c2); + + return ccdCollide(o1, o2, flags, contact, skip, + &c1, ccdSupportConvex, ccdCenter, + &c2, ccdSupportConvex, ccdCenter); +} + + +/*extern */ +int dCollideCylinderCylinder(dxGeom *o1, dxGeom *o2, int flags, dContactGeom *contact, int skip) +{ + ccd_cyl_t cyl1, cyl2; + + ccdGeomToCyl(o1, &cyl1); + ccdGeomToCyl(o2, &cyl2); + + int numContacts = collideCylCyl(o1, o2, &cyl1, &cyl2, flags, contact, skip); + if (numContacts < 0) { + numContacts = ccdCollide(o1, o2, flags, contact, skip, + &cyl1, ccdSupportCyl, ccdCenter, + &cyl2, ccdSupportCyl, ccdCenter); + } + return numContacts; +} + +static +int collideCylCyl(dxGeom *o1, dxGeom *o2, ccd_cyl_t* cyl1, ccd_cyl_t* cyl2, int flags, dContactGeom *contacts, int skip) +{ + int maxContacts = (flags & NUMC_MASK); + dAASSERT(maxContacts != 0); + + maxContacts = maxContacts > 8 ? 8 : maxContacts; + + dReal axesProd = dFabs(ccdVec3Dot(&cyl1->axis, &cyl2->axis)); + // Check if cylinders' axes are in line + if (REAL(1.0) - axesProd < 1e-3f) { + ccd_vec3_t p, proj; + dReal r1, l1; + dReal r2, l2; + dGeomCylinderGetParams(o1, &r1, &l1); + dGeomCylinderGetParams(o2, &r2, &l2); + l1 *= 0.5f; + l2 *= 0.5f; + + // Determine the cylinder with smaller radius (minCyl) and bigger radius (maxCyl) and their respective properties: radius, length + bool r1IsMin; + dReal rmin, rmax; + ccd_cyl_t *minCyl, *maxCyl; + if (r1 <= r2) { + rmin = r1; rmax = r2; + minCyl = cyl1; maxCyl = cyl2; + r1IsMin = true; + } + else { + rmin = r2; rmax = r1; + minCyl = cyl2; maxCyl = cyl1; + r1IsMin = false; + } + + dReal lSum = l1 + l2; + + ccdVec3Copy(&p, &minCyl->o.pos); + ccdVec3Sub(&p, &maxCyl->o.pos); + dReal dot = ccdVec3Dot(&p, &maxCyl->axis); + + // Maximum possible contact depth + dReal depth_v = lSum - dFabs(dot) + dSqrt(dMax(0, REAL(1.0) - axesProd * axesProd)) * rmin; + if (depth_v < 0) { + return 0; + } + + // Project the smaller cylinder's center onto the larger cylinder's plane + ccdVec3Copy(&proj, &maxCyl->axis); + ccdVec3Scale(&proj, -dot); + ccdVec3Add(&proj, &p); + dReal radiiDiff = (dReal)sqrt(ccdVec3Len2(&proj)); + dReal depth_h = r1 + r2 - radiiDiff; + + // Check the distance between cylinders' centers + if (depth_h < 0) { + return 0; + } + + // Check if "vertical" contact depth is less than "horizontal" contact depth + if (depth_v < depth_h) { + int contactCount = 0; + dReal dot2 = -ccdVec3Dot(&p, &minCyl->axis); + // lmin, lmax - distances from cylinders' centers to potential contact points relative to cylinders' axes + dReal lmax = r1IsMin ? l2 : l1; + dReal lmin = r1IsMin ? l1 : l2; + lmin = dot2 < 0 ? -lmin : lmin; + lmax = dot < 0 ? -lmax : lmax; + // Contact normal direction, relative to o1's axis + dReal normaldir = (dot < 0) != r1IsMin ? REAL(1.0) : -REAL(1.0); + + if (rmin + radiiDiff <= rmax) { + // Case 1: The smaller disc is fully contained within the larger one + // Simply generate N points on the rim of the smaller disc + dReal maxContactsRecip = (dReal)(0 < maxContacts ? (2.0 * M_PI / maxContacts) : (2.0 * M_PI)); // The 'else' value does not matter. Just try helping the optimizer. + for (int i = 0; i < maxContacts; i++) { + dReal depth; + dReal a = maxContactsRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + return contactCount; + + } else { + // Case 2: Discs intersect + // Firstly, find intersections assuming the larger cylinder is placed at (0,0,0) + // http://math.stackexchange.com/questions/256100/how-can-i-find-the-points-at-which-two-circles-intersect + ccd_vec3_t proj2; + ccdVec3Copy(&proj2, &proj); + ccdQuatRotVec(&proj, &maxCyl->o.rot_inv); + dReal d = dSqrt(ccdVec3X(&proj) * ccdVec3X(&proj) + ccdVec3Y(&proj) * ccdVec3Y(&proj)); + dIASSERT(d != REAL(0.0)); + + dReal dRecip = REAL(1.0) / d; + dReal rmaxSquare = rmax * rmax, rminSquare = rmin * rmin, dSquare = d * d; + + dReal minA, diffA, minB, diffB; + + { + dReal l = (rmaxSquare - rminSquare + dSquare) * (REAL(0.5) * dRecip); + dReal h = dSqrt(rmaxSquare - l * l); + dReal divLbyD = l * dRecip, divHbyD = h * dRecip; + dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); + dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); + dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); + dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); + // Map the intersection points to angles + dReal ap1 = dAtan2(y1, x1); + dReal ap2 = dAtan2(y2, x2); + minA = dMin(ap1, ap2); + dReal maxA = dMax(ap1, ap2); + // If the segment connecting cylinders' centers does not intersect the arc, change the angles + dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); + if (a < minA || a > maxA) { + a = maxA; + maxA = (dReal)(minA + M_PI * 2.0); + minA = a; + } + diffA = maxA - minA; + } + + // Do the same for the smaller cylinder assuming it is placed at (0,0,0) now + ccdVec3Copy(&proj, &proj2); + ccdVec3Scale(&proj, -1); + ccdQuatRotVec(&proj, &minCyl->o.rot_inv); + + { + dReal l = (rminSquare - rmaxSquare + dSquare) * (REAL(0.5) * dRecip); + dReal h = dSqrt(rminSquare - l * l); + dReal divLbyD = l * dRecip, divHbyD = h * dRecip; + dReal x1 = divLbyD * ccdVec3X(&proj) + divHbyD * ccdVec3Y(&proj); + dReal y1 = divLbyD * ccdVec3Y(&proj) - divHbyD * ccdVec3X(&proj); + dReal x2 = divLbyD * ccdVec3X(&proj) - divHbyD * ccdVec3Y(&proj); + dReal y2 = divLbyD * ccdVec3Y(&proj) + divHbyD * ccdVec3X(&proj); + dReal ap1 = dAtan2(y1, x1); + dReal ap2 = dAtan2(y2, x2); + minB = dMin(ap1, ap2); + dReal maxB = dMax(ap1, ap2); + dReal a = dAtan2(ccdVec3Y(&proj), ccdVec3X(&proj)); + if (a < minB || a > maxB) { + a = maxB; + maxB = (dReal)(minB + M_PI * 2.0); + minB = a; + } + diffB = maxB - minB; + } + + // Find contact point distribution ratio based on arcs lengths + dReal ratio = diffA * rmax / (diffA * rmax + diffB * rmin); + dIASSERT(ratio <= REAL(1.0)); + dIASSERT(ratio >= REAL(0.0)); + + int nMax = (int)dFloor(ratio * maxContacts + REAL(0.5)); + int nMin = maxContacts - nMax; + dIASSERT(nMax <= maxContacts); + + // Make sure there is at least one point on the smaller radius rim + if (nMin < 1) { + nMin = 1; nMax -= 1; + } + // Otherwise transfer one point to the larger radius rim as it is going to fill the rim intersection points + else if (nMin > 1) { + nMin -= 1; nMax += 1; + } + + // Smaller disc first, skipping the overlapping points + dReal nMinRecip = 0 < nMin ? diffB / (nMin + 1) : diffB; // The 'else' value does not matter. Just try helping the optimizer. + for (int i = 1; i <= nMin; i++) { + dReal depth; + dReal a = minB + nMinRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmin, lmin, lSum, minCyl, maxCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + + if (contactCount == 0 || (flags & CONTACTS_UNIMPORTANT) == 0) { + // Then the larger disc, + additional point as the start/end points of arcs overlap + // (or a single contact at the arc middle point if just one is required) + dReal nMaxRecip = nMax > 1 ? diffA / (nMax - 1) : diffA; // The 'else' value does not matter. Just try helping the optimizer. + dReal adjustedMinA = nMax == 1 ? minA + REAL(0.5) * diffA : minA; + + for (int i = 0; i < nMax; i++) { + dReal depth; + dReal a = adjustedMinA + nMaxRecip * i; + if (testAndPrepareDiscContactForAngle(a, rmax, lmax, lSum, maxCyl, minCyl, p, depth)) { + contactCount = addCylCylContact(o1, o2, &maxCyl->axis, contacts, &p, normaldir, depth, contactCount, flags, skip); + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + dIASSERT(contactCount != 0); + break; + } + } + } + } + + return contactCount; + } + } + } + return -1; +} + +static +bool testAndPrepareDiscContactForAngle(dReal angle, dReal radius, dReal length, dReal lSum, ccd_cyl_t *priCyl, ccd_cyl_t *secCyl, ccd_vec3_t &p, dReal &out_depth) +{ + bool ret = false; + + ccd_vec3_t p2; + ccdVec3Set(&p, dCos(angle) * radius, dSin(angle) * radius, 0); + ccdQuatRotVec(&p, &priCyl->o.rot); + ccdVec3Add(&p, &priCyl->o.pos); + ccdVec3Copy(&p2, &p); + ccdVec3Sub(&p2, &secCyl->o.pos); + dReal depth = lSum - dFabs(ccdVec3Dot(&p2, &secCyl->axis)); + + if (depth >= 0) { + ccdVec3Copy(&p2, &priCyl->axis); + ccdVec3Scale(&p2, length); + ccdVec3Add(&p, &p2); + + out_depth = depth; + ret = true; + } + + return ret; +} + +static +int addCylCylContact(dxGeom *o1, dxGeom *o2, ccd_vec3_t* axis, dContactGeom *contacts, + ccd_vec3_t* p, dReal normaldir, dReal depth, int j, int flags, int skip) +{ + dIASSERT(depth >= 0); + + dContactGeom* contact = SAFECONTACT(flags, contacts, j, skip); + contact->g1 = o1; + contact->g2 = o2; + contact->side1 = -1; + contact->side2 = -1; + contact->normal[0] = normaldir * ccdVec3X(axis); + contact->normal[1] = normaldir * ccdVec3Y(axis); + contact->normal[2] = normaldir * ccdVec3Z(axis); + contact->depth = depth; + contact->pos[0] = ccdVec3X(p); + contact->pos[1] = ccdVec3Y(p); + contact->pos[2] = ccdVec3Z(p); + + return j + 1; +} + + +#if dTRIMESH_ENABLED + +const static float CONTACT_DEPTH_EPSILON = 0.0001f; +const static float CONTACT_POS_EPSILON = 0.0001f; +const static float CONTACT_PERTURBATION_ANGLE = 0.001f; +const static float NORMAL_PROJ_EPSILON = 0.0001f; + + +/*extern */ +unsigned dCollideConvexTrimeshTrianglesCCD(dxGeom *o1, dxGeom *o2, const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip) +{ + ccd_convex_t c1; + ccd_triangle_t c2; + dVector3 triangle[dMTV__MAX]; + unsigned maxContacts = (flags & NUMC_MASK); + unsigned contactCount = 0; + ccdGeomToConvex(o1, &c1); + ccdGeomToObj(o2, (ccd_obj_t *)&c2); + + IFaceAngleStorageView *meshFaceAngleView = dxGeomTriMeshGetFaceAngleView(o2); + dUASSERT(meshFaceAngleView != NULL, "Please preprocess the trimesh data with dTRIDATAPREPROCESS_BUILD_FACE_ANGLES"); + + for (unsigned i = 0; i != numIndices; ++i) { + dContactGeom tempContact; + dGeomTriMeshGetTriangle(o2, indices[i], &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); + + for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { + ccdVec3Set(&c2.vertices[j], (ccd_real_t)triangle[j][dV3E_X], (ccd_real_t)triangle[j][dV3E_Y], (ccd_real_t)triangle[j][dV3E_Z]); + } + + setObjPosToTriangleCenter(&c2); + + if (ccdCollide(o1, o2, flags, &tempContact, skip, &c1, &ccdSupportConvex, &ccdCenter, &c2, &ccdSupportTriangle, &ccdCenter) == 1) { + tempContact.side2 = indices[i]; + + if (meshFaceAngleView == NULL || correctTriangleContactNormal(&c2, &tempContact, meshFaceAngleView, indices, numIndices)) { + contactCount = addUniqueContact(contacts, &tempContact, contactCount, maxContacts, flags, skip); + + if ((flags & CONTACTS_UNIMPORTANT) != 0) { + break; + } + } + } + } + + if ((flags & CONTACTS_UNIMPORTANT) == 0 && contactCount == 1) { + dContactGeom *contact = SAFECONTACT(flags, contacts, 0, skip); + dGeomTriMeshGetTriangle(o2, contact->side2, &triangle[dMTV_FIRST], &triangle[dMTV_SECOND], &triangle[dMTV_THIRD]); + contactCount = addTrianglePerturbedContacts(o1, o2, meshFaceAngleView, indices, numIndices, flags, contacts, skip, &c1, &c2, triangle, contact, contactCount); + } + + // Normalize accumulated normals, if necessary + for (unsigned k = 0; k != contactCount; ) { + dContactGeom *contact = SAFECONTACT(flags, contacts, k, skip); + bool stayWithinThisIndex = false; + + // Only the merged contact normals need to be normalized + if (*_const_type_cast_union(&contact->normal[dV3E_PAD])) { + + if (!dxSafeNormalize3(contact->normal)) { + // If the contact normals have added up to zero, erase the contact + // Normally the time step is to be shorter so that the objects do not get into each other that deep + --contactCount; + + if (k != contactCount) { + dContactGeom *lastContact = SAFECONTACT(flags, contacts, contactCount, skip); + *contact = *lastContact; + } + + stayWithinThisIndex = true; + } + } + + if (!stayWithinThisIndex) { + ++k; + } + } + + return contactCount; +} + +static +unsigned addTrianglePerturbedContacts(dxGeom *o1, dxGeom *o2, IFaceAngleStorageView *meshFaceAngleView, + const int *indices, unsigned numIndices, int flags, dContactGeom *contacts, int skip, + ccd_convex_t *c1, ccd_triangle_t *c2, dVector3 *triangle, dContactGeom *contact, unsigned contacCount) +{ + unsigned maxContacts = (flags & NUMC_MASK); + + dVector3 pos; + dCopyVector3(pos, contact->pos); + + dQuaternion q1[2], q2[2]; + dReal perturbationAngle = CONTACT_PERTURBATION_ANGLE; + + dVector3 upAxis; + bool upAvailable = false; + if (fabs(contact->normal[dV3E_Y]) > 0.7) { + dAssignVector3(upAxis, 0, 0, 1); + } + else { + dAssignVector3(upAxis, 0, 1, 0); + } + + dVector3 cross; + dCalcVectorCross3(cross, contact->normal, upAxis); + + if (dSafeNormalize3(cross)) { + dCalcVectorCross3(upAxis, cross, contact->normal); + + if (dSafeNormalize3(upAxis)) { + upAvailable = true; + } + } + + for (unsigned j = upAvailable ? 0 : 2; j != 2; ++j) { + dQFromAxisAndAngle(q1[j], upAxis[dV3E_X], upAxis[dV3E_Y], upAxis[dV3E_Z], perturbationAngle); + dQFromAxisAndAngle(q2[j], cross[dV3E_X], cross[dV3E_Y], cross[dV3E_Z], perturbationAngle); + perturbationAngle = -perturbationAngle; + } + + for (unsigned k = upAvailable ? 0 : 4; k != 4; ++k) { + dQuaternion qr; + dQMultiply0(qr, q1[k % 2], q2[k / 2]); + + for (unsigned j = dMTV__MIN; j != dMTV__MAX; ++j) { + dVector3 p, perturbed; + dSubtractVectors3(p, triangle[j], pos); + dQuatTransform(qr, p, perturbed); + dAddVectors3(perturbed, perturbed, pos); + + ccdVec3Set(&c2->vertices[j], (ccd_real_t)perturbed[dV3E_X], (ccd_real_t)perturbed[dV3E_Y], (ccd_real_t)perturbed[dV3E_Z]); + } + + dContactGeom perturbedContact; + setObjPosToTriangleCenter(c2); + + if (ccdCollide(o1, o2, flags, &perturbedContact, skip, c1, &ccdSupportConvex, &ccdCenter, c2, &ccdSupportTriangle, &ccdCenter) == 1) { + perturbedContact.side2 = contact->side2; + + if (meshFaceAngleView == NULL || correctTriangleContactNormal(c2, &perturbedContact, meshFaceAngleView, indices, numIndices)) { + contacCount = addUniqueContact(contacts, &perturbedContact, contacCount, maxContacts, flags, skip); + } + } + } + + return contacCount; +} + +static +bool correctTriangleContactNormal(ccd_triangle_t *t, dContactGeom *contact, + IFaceAngleStorageView *meshFaceAngleView, const int *indices, unsigned numIndices) +{ + dIASSERT(meshFaceAngleView != NULL); + + bool anyFault = false; + + ccd_vec3_t cntOrigNormal, cntNormal; + ccdVec3Set(&cntNormal, contact->normal[0], contact->normal[1], contact->normal[2]); + ccdVec3Copy(&cntOrigNormal, &cntNormal); + + // Check if the contact point is located close to any edge - move it back and forth + // and check the resulting segment for intersection with the edge plane + ccd_vec3_t cntScaledNormal; + ccdVec3CopyScaled(&cntScaledNormal, &cntNormal, contact->depth); + + ccd_vec3_t edges[dMTV__MAX]; + ccdVec3Sub2(&edges[dMTV_THIRD], &t->vertices[0], &t->vertices[2]); + ccdVec3Sub2(&edges[dMTV_SECOND], &t->vertices[2], &t->vertices[1]); + ccdVec3Sub2(&edges[dMTV_FIRST], &t->vertices[1], &t->vertices[0]); + dSASSERT(dMTV__MAX == 3); + + bool contactGenerated = false, contactPreserved = false; + // Triangle face normal + ccd_vec3_t triNormal; + ccdVec3Cross(&triNormal, &edges[dMTV_FIRST], &edges[dMTV_SECOND]); + if (ccdVec3SafeNormalize(&triNormal) != 0) { + anyFault = true; + } + + // Check the edges to see if one of them is involved + for (unsigned testEdgeIndex = !anyFault ? dMTV__MIN : dMTV__MAX; testEdgeIndex != dMTV__MAX; ++testEdgeIndex) { + ccd_vec3_t edgeNormal, vertexToPos, v; + ccd_vec3_t &edgeAxis = edges[testEdgeIndex]; + + // Edge axis + if (ccdVec3SafeNormalize(&edgeAxis) != 0) { + // This should not happen normally as in the case on of edges is degenerated + // the triangle normal calculation would have to fail above. If for some + // reason the above calculation succeeds and this one would not, it is + // OK to break as this point as well. + anyFault = true; + break; + } + + // Edge Normal + ccdVec3Cross(&edgeNormal, &edgeAxis, &triNormal); + // ccdVec3Normalize(&edgeNormal); -- the two vectors above were already normalized and perpendicular + + // Check if the contact point is located close to any edge - move it back and forth + // and check the resulting segment for intersection with the edge plane + ccdVec3Set(&vertexToPos, contact->pos[0], contact->pos[1], contact->pos[2]); + ccdVec3Sub(&vertexToPos, &t->vertices[testEdgeIndex]); + ccdVec3Sub2(&v, &vertexToPos, &cntScaledNormal); + + if (ccdVec3Dot(&edgeNormal, &v) < 0) { + ccdVec3Add2(&v, &vertexToPos, &cntScaledNormal); + + if (ccdVec3Dot(&edgeNormal, &v) > 0) { + // This is an edge contact + + ccd_real_t x = ccdVec3Dot(&triNormal, &cntNormal); + ccd_real_t y = ccdVec3Dot(&edgeNormal, &cntNormal); + ccd_real_t contactNormalToTriangleNormalAngle = CCD_ATAN2(y, x); + + dReal angleValueAsDRead; + FaceAngleDomain angleDomain = meshFaceAngleView->retrieveFacesAngleFromStorage(angleValueAsDRead, contact->side2, (dMeshTriangleVertex)testEdgeIndex); + ccd_real_t angleValue = (ccd_real_t)angleValueAsDRead; + + ccd_real_t targetAngle; + contactGenerated = false, contactPreserved = false; // re-assign to make optimizer's task easier + + if (angleDomain != FAD_CONCAVE) { + // Convex or flat - ensure the contact normal is within the allowed range + // formed by the two triangles' normals. + if (contactNormalToTriangleNormalAngle < CCD_ZERO) { + targetAngle = CCD_ZERO; + } + else if (contactNormalToTriangleNormalAngle > angleValue) { + targetAngle = angleValue; + } + else { + contactPreserved = true; + } + } + else { + // Concave - rotate the contact normal to the face angle bisect plane + // (or to triangle normal-edge plane if negative angles are not stored) + targetAngle = angleValue != 0 ? CCD_REAL(0.5) * angleValue : CCD_ZERO; + // There is little chance the normal will initially match the correct plane, but still, a small check could save lots of calculations + if (contactNormalToTriangleNormalAngle == targetAngle) { + contactPreserved = true; + } + } + + if (!contactPreserved) { + ccd_quat_t q; + ccdQuatSetAngleAxis(&q, targetAngle - contactNormalToTriangleNormalAngle, &edgeAxis); + ccdQuatRotVec2(&cntNormal, &cntNormal, &q); + contactGenerated = true; + } + + // Calculated successfully + break; + } + } + } + + if (!anyFault && !contactPreserved) { + // No edge contact detected, set contact normal to triangle normal + const ccd_vec3_t &cntNormalToUse = !contactGenerated ? triNormal : cntNormal; + + contact->normal[dV3E_X] = ccdVec3X(&cntNormalToUse); + contact->normal[dV3E_Y] = ccdVec3Y(&cntNormalToUse); + contact->normal[dV3E_Z] = ccdVec3Z(&cntNormalToUse); + contact->depth *= CCD_FMAX(0.0, ccdVec3Dot(&cntOrigNormal, &cntNormalToUse)); + } + + bool result = !anyFault; + return result; +} + + +static +unsigned addUniqueContact(dContactGeom *contacts, dContactGeom *c, unsigned contactcount, unsigned maxcontacts, int flags, int skip) +{ + dReal minDepth = c->depth; + unsigned index = contactcount; + bool isDuplicate = false; + + dReal c_posX = c->pos[dV3E_X], c_posY = c->pos[dV3E_Y], c_posZ = c->pos[dV3E_Z]; + for (unsigned k = 0; k != contactcount; k++) { + dContactGeom* pc = SAFECONTACT(flags, contacts, k, skip); + + if (fabs(c_posX - pc->pos[dV3E_X]) < CONTACT_POS_EPSILON + && fabs(c_posY - pc->pos[dV3E_Y]) < CONTACT_POS_EPSILON + && fabs(c_posZ - pc->pos[dV3E_Z]) < CONTACT_POS_EPSILON) { + dSASSERT(dV3E__AXES_MAX - dV3E__AXES_MIN == 3); + + // Accumulate similar contacts + dAddVectors3(pc->normal, pc->normal, c->normal); + pc->depth = dMax(pc->depth, c->depth); + *_type_cast_union(&pc->normal[dV3E_PAD]) = true; // Mark the contact as a merged one + + isDuplicate = true; + break; + } + + if (contactcount == maxcontacts && pc->depth < minDepth) { + minDepth = pc->depth; + index = k; + } + } + + if (!isDuplicate && index < maxcontacts) { + dContactGeom* contact = SAFECONTACT(flags, contacts, index, skip); + contact->g1 = c->g1; + contact->g2 = c->g2; + contact->depth = c->depth; + contact->side1 = c->side1; + contact->side2 = c->side2; + dCopyVector3(contact->pos, c->pos); + dCopyVector3(contact->normal, c->normal); + *_type_cast_union(&contact->normal[dV3E_PAD]) = false; // Indicates whether the contact is merged or not + contactcount = index == contactcount ? contactcount + 1 : contactcount; + } + + return contactcount; +} + +static +void setObjPosToTriangleCenter(ccd_triangle_t *t) +{ + ccdVec3Set(&t->o.pos, 0, 0, 0); + for (int j = 0; j < 3; j++) { + ccdVec3Add(&t->o.pos, &t->vertices[j]); + } + ccdVec3Scale(&t->o.pos, 1.0f / 3.0f); +} + +static +void ccdSupportTriangle(const void *obj, const ccd_vec3_t *_dir, ccd_vec3_t *v) +{ + const ccd_triangle_t* o = (ccd_triangle_t *) obj; + ccd_real_t maxdot, dot; + maxdot = -CCD_REAL_MAX; + for (unsigned i = 0; i != 3; i++) { + dot = ccdVec3Dot(_dir, &o->vertices[i]); + if (dot > maxdot) { + ccdVec3Copy(v, &o->vertices[i]); + maxdot = dot; + } + } +} + + +#endif // dTRIMESH_ENABLED diff --git a/ode/src/joints/pu.cpp b/ode/src/joints/pu.cpp index 9c0d1a3a..72b22b68 100644 --- a/ode/src/joints/pu.cpp +++ b/ode/src/joints/pu.cpp @@ -1,761 +1,759 @@ -/************************************************************************* - * * - * 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 "pu.h" -#include "joint_internal.h" - - -//**************************************************************************** -// Prismatic and Universal - -dxJointPU::dxJointPU( dxWorld *w ) : - dxJointUniversal( w ) -{ - // Default Position - // Y ^ Axis2 - // ^ | - // / | ^ Axis1 - // Z^ / | / - // | / Body 2 | / Body 1 - // | / +---------+ | / +-----------+ - // | / / /| | / / /| - // | / / / + _/ - / / + - // | / / /-/--------(_)----|--- /-----------/-------> AxisP - // | / +---------+ / - +-----------+ / - // | / | |/ | |/ - // | / +---------+ +-----------+ - // |/ - // .-----------------------------------------> X - // |-----------------> - // Anchor2 <--------------| - // Anchor1 - // - - // Setting member variables which are w.r.t body2 - dSetZero( axis1, 4 ); - axis1[1] = 1; - - // Setting member variables which are w.r.t body2 - dSetZero( anchor2, 4 ); - dSetZero( axis2, 4 ); - axis2[2] = 1; - - dSetZero( axisP1, 4 ); - axisP1[0] = 1; - - dSetZero( qrel1, 4 ); - dSetZero( qrel2, 4 ); - - - limotP.init( world ); - limot1.init( world ); - limot2.init( world ); -} - - -dReal dJointGetPUPosition( dJointID j ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - dVector3 q; - // get the offset in global coordinates - dMultiply0_331( q, joint->node[0].body->posr.R, joint->anchor1 ); - - if ( joint->node[1].body ) - { - dVector3 anchor2; - - // get the anchor2 in global coordinates - dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 ); - - q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - - ( joint->node[1].body->posr.pos[0] + anchor2[0] ) ); - q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - - ( joint->node[1].body->posr.pos[1] + anchor2[1] ) ); - q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - - ( joint->node[1].body->posr.pos[2] + anchor2[2] ) ); - } - else - { - //N.B. When there is no body 2 the joint->anchor2 is already in - // global coordinates - - q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - - ( joint->anchor2[0] ) ); - q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - - ( joint->anchor2[1] ) ); - q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - - ( joint->anchor2[2] ) ); - - if ( joint->flags & dJOINT_REVERSE ) - { - q[0] = -q[0]; - q[1] = -q[1]; - q[2] = -q[2]; - } - } - - dVector3 axP; - // get prismatic axis in global coordinates - dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 ); - - return dCalcVectorDot3( axP, q ); -} - - -dReal dJointGetPUPositionRate( dJointID j ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - if ( joint->node[0].body ) - { - // We want to find the rate of change of the prismatic part of the joint - // We can find it by looking at the speed difference between body1 and the - // anchor point. - - // r will be used to find the distance between body1 and the anchor point - dVector3 r, anchor2; - - if ( joint->node[1].body ) - { - // Find joint->anchor2 in global coordinates - - // NOTE! anchor2 needs a volatile assignment on the multiplication to discard computation errors. - // Otherwise, tests fail for single type on x86. - dxTruncToType::dMultiply0_331(anchor2, joint->node[1].body->posr.R, joint->anchor2); - - r[0] = ( joint->node[0].body->posr.pos[0] - - ( anchor2[0] + joint->node[1].body->posr.pos[0] ) ); - r[1] = ( joint->node[0].body->posr.pos[1] - - ( anchor2[1] + joint->node[1].body->posr.pos[1] ) ); - r[2] = ( joint->node[0].body->posr.pos[2] - - ( anchor2[2] + joint->node[1].body->posr.pos[2] ) ); - } - else - { - dZeroVector3(anchor2); - - //N.B. When there is no body 2 the joint->anchor2 is already in - // global coordinates - // r = joint->node[0].body->posr.pos - joint->anchor2; - dSubtractVectors3( r, joint->node[0].body->posr.pos, joint->anchor2 ); - } - - // The body1 can have velocity coming from the rotation of - // the rotoide axis. We need to remove this. - - // N.B. We do vel = r X w instead of vel = w x r to have vel negative - // since we want to remove it from the linear velocity of the body - dVector3 lvel1; - dCalcVectorCross3( lvel1, r, joint->node[0].body->avel ); - - // lvel1 += joint->node[0].body->lvel; - dAddVectors3( lvel1, lvel1, joint->node[0].body->lvel ); - - // Since we want rate of change along the prismatic axis - // get axisP1 in global coordinates and get the component - // along this axis only - dVector3 axP1; - dMultiply0_331( axP1, joint->node[0].body->posr.R, joint->axisP1 ); - - if ( joint->node[1].body ) - { - // Find the contribution of the angular rotation to the linear speed - // N.B. We do vel = r X w instead of vel = w x r to have vel negative - // since we want to remove it from the linear velocity of the body - dVector3 lvel2; - dCalcVectorCross3( lvel2, anchor2, joint->node[1].body->avel ); - - // lvel1 -= lvel2 + joint->node[1].body->lvel; - dVector3 tmp; - dAddVectors3( tmp, lvel2, joint->node[1].body->lvel ); - dSubtractVectors3( lvel1, lvel1, tmp ); - - return dCalcVectorDot3( axP1, lvel1 ); - } - else - { - dReal rate = dCalcVectorDot3( axP1, lvel1 ); - return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate); - } - } - - return 0.0; -} - - - -void -dxJointPU::getSureMaxInfo( SureMaxInfo* info ) -{ - info->max_m = 6; -} - - - -void -dxJointPU::getInfo1( dxJoint::Info1 *info ) -{ - info->m = 3; - info->nub = 3; - - // powered needs an extra constraint row - - // see if we're at a joint limit. - limotP.limit = 0; - if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) && - limotP.lostop <= limotP.histop ) - { - // measure joint position - dReal pos = dJointGetPUPosition( this ); - limotP.testRotationalLimit( pos ); // N.B. The function is ill named - } - - if ( limotP.limit || limotP.fmax > 0 ) info->m++; - - - bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) && - limot1.lostop <= limot1.histop; - bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) && - limot2.lostop <= limot2.histop; - - // We need to call testRotationLimit() even if we're motored, since it - // records the result. - limot1.limit = 0; - limot2.limit = 0; - if ( limiting1 || limiting2 ) - { - dReal angle1, angle2; - getAngles( &angle1, &angle2 ); - if ( limiting1 ) - limot1.testRotationalLimit( angle1 ); - if ( limiting2 ) - limot2.testRotationalLimit( angle2 ); - } - - if ( limot1.limit || limot1.fmax > 0 ) info->m++; - if ( limot2.limit || limot2.fmax > 0 ) info->m++; -} - - - -void -dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, - int rowskip, dReal *J1, dReal *J2, - int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, - int *findex ) -{ - const dReal k = worldFPS * worldERP; - - // ====================================================================== - // The angular constraint - // - dVector3 ax1, ax2; // Global axes of rotation - getAxis(this, ax1, axis1); - getAxis2(this,ax2, axis2); - - dVector3 uniPerp; // Axis perpendicular to axes of rotation - dCalcVectorCross3(uniPerp,ax1,ax2); - dNormalize3( uniPerp ); - - dCopyVector3( J1 + GI2__JA_MIN, uniPerp ); - - dxBody *body1 = node[1].body; - - if ( body1 ) { - dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp ); - } - // Corrective velocity attempting to keep uni axes perpendicular - dReal val = dCalcVectorDot3( ax1, ax2 ); - // Small angle approximation : - // theta = asin(val) - // theta is approximately val when val is near zero. - pairRhsCfm[GI2_RHS] = -k * val; - - // ========================================================================== - // Handle axes orthogonal to the prismatic - dVector3 an1, an2; // Global anchor positions - dVector3 axP, sep; // Prismatic axis and separation vector - getAnchor(this, an1, anchor1); - getAnchor2(this, an2, anchor2); - - if (flags & dJOINT_REVERSE) { - getAxis2(this, axP, axisP1); - } else { - getAxis(this, axP, axisP1); - } - dSubtractVectors3(sep, an2, an1); - - dVector3 p, q; - dPlaneSpace(axP, p, q); - - dCopyVector3( J1 + rowskip + GI2__JL_MIN, p ); - dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q ); - // Make the anchors be body local - // Aliasing isn't a problem here. - dSubtractVectors3(an1, an1, node[0].body->posr.pos); - dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p ); - dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q ); - - if (body1) { - dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p ); - dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q ); - dSubtractVectors3(an2, an2, body1->posr.pos); - dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 ); - dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 ); - } - - pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep ); - pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep ); - - // ========================================================================== - // Handle the limits/motors - int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip; - - if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) { - currRowSkip += rowskip; currPairSkip += pairskip; - } - - if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) { - currRowSkip += rowskip; currPairSkip += pairskip; - } - - if ( body1 || (flags & dJOINT_REVERSE) == 0 ) { - limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); - } else { - dNegateVector3(axP); - limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); - } -} - -void dJointSetPUAnchor( dJointID j, dReal x, dReal y, dReal z ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); - joint->computeInitialRelativeRotations(); -} - -/** - * This function initialize the anchor and the relative position of each body - * as if body2 was at its current position + [dx,dy,dy]. - * Ex: - * 
- * dReal offset = 1;
- * dVector3 dir;
- * dJointGetPUAxis3(jId, dir);
- * dJointSetPUAnchor(jId, 0, 0, 0);
- * // If you request the position you will have: dJointGetPUPosition(jId) == 0
- * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
- * // If you request the position you will have: dJointGetPUPosition(jId) == -offset
- *
- - * @param j The PU joint for which the anchor point will be set - * @param x The X position of the anchor point in world frame - * @param y The Y position of the anchor point in world frame - * @param z The Z position of the anchor point in world frame - * @param dx A delta to be added to the X position as if the anchor was set - * when body1 was at current_position[X] + dx - * @param dx A delta to be added to the Y position as if the anchor was set - * when body1 was at current_position[Y] + dy - * @param dx A delta to be added to the Z position as if the anchor was set - * when body1 was at current_position[Z] + dz - * @note Should have the same meaning as dJointSetSliderAxisDelta - */ -void dJointSetPUAnchorDelta( dJointID j, dReal x, dReal y, dReal z, - dReal dx, dReal dy, dReal dz ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - if ( joint->node[0].body ) - { - joint->node[0].body->posr.pos[0] += dx; - joint->node[0].body->posr.pos[1] += dy; - joint->node[0].body->posr.pos[2] += dz; - } - - setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); - - if ( joint->node[0].body ) - { - joint->node[0].body->posr.pos[0] -= dx; - joint->node[0].body->posr.pos[1] -= dy; - joint->node[0].body->posr.pos[2] -= dz; - } - - joint->computeInitialRelativeRotations(); -} - -/** - * \brief This function initialize the anchor and the relative position of each body - * such that dJointGetPUPosition will return the dot product of axis and [dx,dy,dy]. - * - * The body 1 is moved to [-dx, -dy, -dx] then the anchor is set. This will be the - * position 0 for the prismatic part of the joint. Then the body 1 is moved to its - * original position. - * - * Ex: - * 
- * dReal offset = 1;
- * dVector3 dir;
- * dJointGetPUAxis3(jId, dir);
- * dJointSetPUAnchor(jId, 0, 0, 0);
- * // If you request the position you will have: dJointGetPUPosition(jId) == 0
- * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
- * // If you request the position you will have: dJointGetPUPosition(jId) == offset
- *
- - * @param j The PU joint for which the anchor point will be set - * @param x The X position of the anchor point in world frame - * @param y The Y position of the anchor point in world frame - * @param z The Z position of the anchor point in world frame - * @param dx A delta to be added to the X position as if the anchor was set - * when body1 was at current_position[X] + dx - * @param dx A delta to be added to the Y position as if the anchor was set - * when body1 was at current_position[Y] + dy - * @param dx A delta to be added to the Z position as if the anchor was set - * when body1 was at current_position[Z] + dz - * @note Should have the same meaning as dJointSetSliderAxisDelta - */ -void dJointSetPUAnchorOffset( dJointID j, dReal x, dReal y, dReal z, - dReal dx, dReal dy, dReal dz ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - if (joint->flags & dJOINT_REVERSE) - { - dx = -dx; - dy = -dy; - dz = -dz; - } - - if ( joint->node[0].body ) - { - joint->node[0].body->posr.pos[0] -= dx; - joint->node[0].body->posr.pos[1] -= dy; - joint->node[0].body->posr.pos[2] -= dz; - } - - setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); - - if ( joint->node[0].body ) - { - joint->node[0].body->posr.pos[0] += dx; - joint->node[0].body->posr.pos[1] += dy; - joint->node[0].body->posr.pos[2] += dz; - } - - joint->computeInitialRelativeRotations(); -} - - - - - -void dJointSetPUAxis1( dJointID j, dReal x, dReal y, dReal z ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - setAxes( joint, x, y, z, NULL, joint->axis2 ); - else - setAxes( joint, x, y, z, joint->axis1, NULL ); - joint->computeInitialRelativeRotations(); -} - -void dJointSetPUAxis2( dJointID j, dReal x, dReal y, dReal z ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - setAxes( joint, x, y, z, joint->axis1, NULL ); - else - setAxes( joint, x, y, z, NULL, joint->axis2 ); - joint->computeInitialRelativeRotations(); -} - - -void dJointSetPUAxisP( dJointID id, dReal x, dReal y, dReal z ) -{ - dJointSetPUAxis3( id, x, y, z ); -} - - - -void dJointSetPUAxis3( dJointID j, dReal x, dReal y, dReal z ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - setAxes( joint, x, y, z, joint->axisP1, 0 ); - - joint->computeInitialRelativeRotations(); -} - - - - -void dJointGetPUAngles( dJointID j, dReal *angle1, dReal *angle2 ) -{ - dxJointUniversal* joint = ( dxJointUniversal* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - joint->getAngles( angle2, angle1 ); - else - joint->getAngles( angle1, angle2 ); -} - - -dReal dJointGetPUAngle1( dJointID j ) -{ - dxJointUniversal* joint = ( dxJointUniversal* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - return joint->getAngle2(); - else - return joint->getAngle1(); -} - - -dReal dJointGetPUAngle2( dJointID j ) -{ - dxJointUniversal* joint = ( dxJointUniversal* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - return joint->getAngle1(); - else - return joint->getAngle2(); -} - - -dReal dJointGetPUAngle1Rate( dJointID j ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - 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 = dCalcVectorDot3( axis, joint->node[0].body->avel ); - if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); - return rate; - } - return 0; -} - - -dReal dJointGetPUAngle2Rate( dJointID j ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - 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 = dCalcVectorDot3( axis, joint->node[0].body->avel ); - if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); - return rate; - } - return 0; -} - - -void dJointSetPUParam( dJointID j, int parameter, dReal value ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - switch ( parameter & 0xff00 ) - { - case dParamGroup1: - joint->limot1.set( parameter, value ); - break; - case dParamGroup2: - joint->limot2.set( parameter & 0xff, value ); - break; - case dParamGroup3: - joint->limotP.set( parameter & 0xff, value ); - break; - } -} - -void dJointGetPUAnchor( dJointID j, dVector3 result ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - dUASSERT( result, "bad result argument" ); - checktype( joint, PU ); - - if ( joint->node[1].body ) - getAnchor2( joint, result, joint->anchor2 ); - else - { - // result[i] = joint->anchor2[i]; - dCopyVector3( result, joint->anchor2 ); - } -} - -void dJointGetPUAxis1( dJointID j, dVector3 result ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - dUASSERT( result, "bad result argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - getAxis2( joint, result, joint->axis2 ); - else - getAxis( joint, result, joint->axis1 ); -} - -void dJointGetPUAxis2( dJointID j, dVector3 result ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - dUASSERT( result, "bad result argument" ); - checktype( joint, PU ); - if ( joint->flags & dJOINT_REVERSE ) - getAxis( joint, result, joint->axis1 ); - else - getAxis2( joint, result, joint->axis2 ); -} - -/** - * @brief Get the prismatic axis - * @ingroup joints - * - * @note This function was added for convenience it is the same as - * dJointGetPUAxis3 - */ -void dJointGetPUAxisP( dJointID id, dVector3 result ) -{ - dJointGetPUAxis3( id, result ); -} - - -void dJointGetPUAxis3( dJointID j, dVector3 result ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - dUASSERT( result, "bad result argument" ); - checktype( joint, PU ); - getAxis( joint, result, joint->axisP1 ); -} - -dReal dJointGetPUParam( dJointID j, int parameter ) -{ - dxJointPU* joint = ( dxJointPU* ) j; - dUASSERT( joint, "bad joint argument" ); - checktype( joint, PU ); - - switch ( parameter & 0xff00 ) - { - case dParamGroup1: - return joint->limot1.get( parameter ); - break; - case dParamGroup2: - return joint->limot2.get( parameter & 0xff ); - break; - case dParamGroup3: - return joint->limotP.get( parameter & 0xff ); - break; - } - - return 0; -} - - -dJointType -dxJointPU::type() const -{ - return dJointTypePU; -} - - -sizeint -dxJointPU::size() const -{ - return sizeof( *this ); -} - - -void -dxJointPU::setRelativeValues() -{ - dVector3 anchor; - dJointGetPUAnchor(this, anchor); - setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 ); - - dVector3 ax1, ax2, ax3; - dJointGetPUAxis1(this, ax1); - dJointGetPUAxis2(this, ax2); - dJointGetPUAxis3(this, ax3); - - if ( flags & dJOINT_REVERSE ) - { - setAxes( this, ax1[0], ax1[1], ax1[2], NULL, axis2 ); - setAxes( this, ax2[0], ax2[1], ax2[2], axis1, NULL ); - } - else - { - setAxes( this, ax1[0], ax1[1], ax1[2], axis1, NULL ); - setAxes( this, ax2[0], ax2[1], ax2[2], NULL, axis2 ); - } - - - setAxes( this, ax3[0], ax3[1], ax3[2], axisP1, NULL ); - - computeInitialRelativeRotations(); -} - +/************************************************************************* + * * + * 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 "pu.h" +#include "joint_internal.h" + + +//**************************************************************************** +// Prismatic and Universal + +dxJointPU::dxJointPU( dxWorld *w ) : + dxJointUniversal( w ) +{ + // Default Position + // Y ^ Axis2 + // ^ | + // / | ^ Axis1 + // Z^ / | / + // | / Body 2 | / Body 1 + // | / +---------+ | / +-----------+ + // | / / /| | / / /| + // | / / / + _/ - / / + + // | / / /-/--------(_)----|--- /-----------/-------> AxisP + // | / +---------+ / - +-----------+ / + // | / | |/ | |/ + // | / +---------+ +-----------+ + // |/ + // .-----------------------------------------> X + // |-----------------> + // Anchor2 <--------------| + // Anchor1 + // + + // Setting member variables which are w.r.t body2 + dSetZero( axis1, 4 ); + axis1[1] = 1; + + // Setting member variables which are w.r.t body2 + dSetZero( anchor2, 4 ); + dSetZero( axis2, 4 ); + axis2[2] = 1; + + dSetZero( axisP1, 4 ); + axisP1[0] = 1; + + dSetZero( qrel1, 4 ); + dSetZero( qrel2, 4 ); + + + limotP.init( world ); + limot1.init( world ); + limot2.init( world ); +} + + +dReal dJointGetPUPosition( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + dVector3 q; + // get the offset in global coordinates + dMultiply0_331( q, joint->node[0].body->posr.R, joint->anchor1 ); + + if ( joint->node[1].body ) + { + dVector3 anchor2; + + // get the anchor2 in global coordinates + dMultiply0_331( anchor2, joint->node[1].body->posr.R, joint->anchor2 ); + + q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - + ( joint->node[1].body->posr.pos[0] + anchor2[0] ) ); + q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - + ( joint->node[1].body->posr.pos[1] + anchor2[1] ) ); + q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - + ( joint->node[1].body->posr.pos[2] + anchor2[2] ) ); + } + else + { + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + + q[0] = (( joint->node[0].body->posr.pos[0] + q[0] ) - + ( joint->anchor2[0] ) ); + q[1] = (( joint->node[0].body->posr.pos[1] + q[1] ) - + ( joint->anchor2[1] ) ); + q[2] = (( joint->node[0].body->posr.pos[2] + q[2] ) - + ( joint->anchor2[2] ) ); + + if ( joint->flags & dJOINT_REVERSE ) + { + q[0] = -q[0]; + q[1] = -q[1]; + q[2] = -q[2]; + } + } + + dVector3 axP; + // get prismatic axis in global coordinates + dMultiply0_331( axP, joint->node[0].body->posr.R, joint->axisP1 ); + + return dCalcVectorDot3( axP, q ); +} + + +dReal dJointGetPUPositionRate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + // We want to find the rate of change of the prismatic part of the joint + // We can find it by looking at the speed difference between body1 and the + // anchor point. + + // r will be used to find the distance between body1 and the anchor point + dVector3 r, anchor2; + + if ( joint->node[1].body ) + { + // Find joint->anchor2 in global coordinates + + // NOTE! anchor2 needs a volatile assignment on the multiplication to discard computation errors. + // Otherwise, tests fail for single type on x86. + dxTruncToType::dMultiply0_331(anchor2, joint->node[1].body->posr.R, joint->anchor2); + + r[0] = ( joint->node[0].body->posr.pos[0] - + ( anchor2[0] + joint->node[1].body->posr.pos[0] ) ); + r[1] = ( joint->node[0].body->posr.pos[1] - + ( anchor2[1] + joint->node[1].body->posr.pos[1] ) ); + r[2] = ( joint->node[0].body->posr.pos[2] - + ( anchor2[2] + joint->node[1].body->posr.pos[2] ) ); + } + else + { + dZeroVector3(anchor2); + + //N.B. When there is no body 2 the joint->anchor2 is already in + // global coordinates + // r = joint->node[0].body->posr.pos - joint->anchor2; + dSubtractVectors3( r, joint->node[0].body->posr.pos, joint->anchor2 ); + } + + // The body1 can have velocity coming from the rotation of + // the rotoide axis. We need to remove this. + + // N.B. We do vel = r X w instead of vel = w x r to have vel negative + // since we want to remove it from the linear velocity of the body + dVector3 lvel1; + dCalcVectorCross3( lvel1, r, joint->node[0].body->avel ); + + // lvel1 += joint->node[0].body->lvel; + dAddVectors3( lvel1, lvel1, joint->node[0].body->lvel ); + + // Since we want rate of change along the prismatic axis + // get axisP1 in global coordinates and get the component + // along this axis only + dVector3 axP1; + dMultiply0_331( axP1, joint->node[0].body->posr.R, joint->axisP1 ); + + if ( joint->node[1].body ) + { + // Find the contribution of the angular rotation to the linear speed + // N.B. We do vel = r X w instead of vel = w x r to have vel negative + // since we want to remove it from the linear velocity of the body + dVector3 lvel2; + dCalcVectorCross3( lvel2, anchor2, joint->node[1].body->avel ); + + // lvel1 -= lvel2 + joint->node[1].body->lvel; + dVector3 tmp; + dAddVectors3( tmp, lvel2, joint->node[1].body->lvel ); + dSubtractVectors3( lvel1, lvel1, tmp ); + + return dCalcVectorDot3( axP1, lvel1 ); + } + else + { + dReal rate = dCalcVectorDot3( axP1, lvel1 ); + return ( (joint->flags & dJOINT_REVERSE) ? -rate : rate); + } + } + + return 0.0; +} + + + +void +dxJointPU::getSureMaxInfo( SureMaxInfo* info ) +{ + info->max_m = 6; +} + + + +void +dxJointPU::getInfo1( dxJoint::Info1 *info ) +{ + info->m = 3; + info->nub = 3; + + // powered needs an extra constraint row + + // see if we're at a joint limit. + limotP.limit = 0; + if (( limotP.lostop > -dInfinity || limotP.histop < dInfinity ) && + limotP.lostop <= limotP.histop ) + { + // measure joint position + dReal pos = dJointGetPUPosition( this ); + limotP.testRotationalLimit( pos ); // N.B. The function is ill named + } + + if ( limotP.limit || limotP.fmax > 0 ) info->m++; + + + bool limiting1 = ( limot1.lostop >= -M_PI || limot1.histop <= M_PI ) && + limot1.lostop <= limot1.histop; + bool limiting2 = ( limot2.lostop >= -M_PI || limot2.histop <= M_PI ) && + limot2.lostop <= limot2.histop; + + // We need to call testRotationLimit() even if we're motored, since it + // records the result. + limot1.limit = 0; + limot2.limit = 0; + if ( limiting1 || limiting2 ) + { + dReal angle1, angle2; + getAngles( &angle1, &angle2 ); + if ( limiting1 ) + limot1.testRotationalLimit( angle1 ); + if ( limiting2 ) + limot2.testRotationalLimit( angle2 ); + } + + if ( limot1.limit || limot1.fmax > 0 ) info->m++; + if ( limot2.limit || limot2.fmax > 0 ) info->m++; +} + + + +void +dxJointPU::getInfo2( dReal worldFPS, dReal worldERP, + int rowskip, dReal *J1, dReal *J2, + int pairskip, dReal *pairRhsCfm, dReal *pairLoHi, + int *findex ) +{ + const dReal k = worldFPS * worldERP; + + // ====================================================================== + // The angular constraint + // + dVector3 ax1, ax2; // Global axes of rotation + getAxis(this, ax1, axis1); + getAxis2(this,ax2, axis2); + + dVector3 uniPerp; // Axis perpendicular to axes of rotation + dCalcVectorCross3(uniPerp,ax1,ax2); + dNormalize3( uniPerp ); + + dCopyVector3( J1 + GI2__JA_MIN, uniPerp ); + + dxBody *body1 = node[1].body; + + if ( body1 ) { + dCopyNegatedVector3( J2 + GI2__JA_MIN , uniPerp ); + } + // Corrective velocity attempting to keep uni axes perpendicular + dReal val = dCalcVectorDot3( ax1, ax2 ); + // Small angle approximation : + // theta = asin(val) + // theta is approximately val when val is near zero. + pairRhsCfm[GI2_RHS] = -k * val; + + // ========================================================================== + // Handle axes orthogonal to the prismatic + dVector3 an1, an2; // Global anchor positions + dVector3 axP, sep; // Prismatic axis and separation vector + getAnchor(this, an1, anchor1); + getAnchor2(this, an2, anchor2); + + if (flags & dJOINT_REVERSE) { + getAxis2(this, axP, axisP1); + } else { + getAxis(this, axP, axisP1); + } + dSubtractVectors3(sep, an2, an1); + + dVector3 p, q; + dPlaneSpace(axP, p, q); + + dCopyVector3( J1 + rowskip + GI2__JL_MIN, p ); + dCopyVector3( J1 + 2 * rowskip + GI2__JL_MIN, q ); + // Make the anchors be body local + // Aliasing isn't a problem here. + dSubtractVectors3(an1, an1, node[0].body->posr.pos); + dCalcVectorCross3( J1 + rowskip + GI2__JA_MIN, an1, p ); + dCalcVectorCross3( J1 + 2 * rowskip + GI2__JA_MIN, an1, q ); + + if (body1) { + dCopyNegatedVector3( J2 + rowskip + GI2__JL_MIN, p ); + dCopyNegatedVector3( J2 + 2 * rowskip + GI2__JL_MIN, q ); + dSubtractVectors3(an2, an2, body1->posr.pos); + dCalcVectorCross3( J2 + rowskip + GI2__JA_MIN, p, an2 ); + dCalcVectorCross3( J2 + 2 * rowskip + GI2__JA_MIN, q, an2 ); + } + + pairRhsCfm[pairskip + GI2_RHS] = k * dCalcVectorDot3( p, sep ); + pairRhsCfm[2 * pairskip + GI2_RHS] = k * dCalcVectorDot3( q, sep ); + + // ========================================================================== + // Handle the limits/motors + int currRowSkip = 3 * rowskip, currPairSkip = 3 * pairskip; + + if (limot1.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax1, 1 )) { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + if (limot2.addLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, ax2, 1 )) { + currRowSkip += rowskip; currPairSkip += pairskip; + } + + if ( body1 || (flags & dJOINT_REVERSE) == 0 ) { + limotP.addTwoPointLimot( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); + } else { + dNegateVector3(axP); + limotP.addTwoPointLimot ( this, worldFPS, J1 + currRowSkip, J2 + currRowSkip, pairRhsCfm + currPairSkip, pairLoHi + currPairSkip, axP, an1, an2 ); + } +} + +void dJointSetPUAnchor( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + joint->computeInitialRelativeRotations(); +} + +/** + * This function initialize the anchor and the relative position of each body + * as if body2 was at its current position + [dx,dy,dy]. + * Ex: + * 
+ * dReal offset = 1;
+ * dVector3 dir;
+ * dJointGetPUAxis3(jId, dir);
+ * dJointSetPUAnchor(jId, 0, 0, 0);
+ * // If you request the position you will have: dJointGetPUPosition(jId) == 0
+ * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
+ * // If you request the position you will have: dJointGetPUPosition(jId) == -offset
+ *
+ + * @param j The PU joint for which the anchor point will be set + * @param x The X position of the anchor point in world frame + * @param y The Y position of the anchor point in world frame + * @param z The Z position of the anchor point in world frame + * @param dx A delta to be added to the X position as if the anchor was set + * when body1 was at current_position[X] + dx + * @param dx A delta to be added to the Y position as if the anchor was set + * when body1 was at current_position[Y] + dy + * @param dx A delta to be added to the Z position as if the anchor was set + * when body1 was at current_position[Z] + dz + * @note Should have the same meaning as dJointSetSliderAxisDelta + */ +void dJointSetPUAnchorDelta( dJointID j, dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] += dx; + joint->node[0].body->posr.pos[1] += dy; + joint->node[0].body->posr.pos[2] += dz; + } + + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] -= dx; + joint->node[0].body->posr.pos[1] -= dy; + joint->node[0].body->posr.pos[2] -= dz; + } + + joint->computeInitialRelativeRotations(); +} + +/** + * \brief This function initialize the anchor and the relative position of each body + * such that dJointGetPUPosition will return the dot product of axis and [dx,dy,dy]. + * + * The body 1 is moved to [-dx, -dy, -dx] then the anchor is set. This will be the + * position 0 for the prismatic part of the joint. Then the body 1 is moved to its + * original position. + * + * Ex: + * 
+ * dReal offset = 1;
+ * dVector3 dir;
+ * dJointGetPUAxis3(jId, dir);
+ * dJointSetPUAnchor(jId, 0, 0, 0);
+ * // If you request the position you will have: dJointGetPUPosition(jId) == 0
+ * dJointSetPUAnchorDelta(jId, 0, 0, 0, dir[X]*offset, dir[Y]*offset, dir[Z]*offset);
+ * // If you request the position you will have: dJointGetPUPosition(jId) == offset
+ *
+ + * @param j The PU joint for which the anchor point will be set + * @param x The X position of the anchor point in world frame + * @param y The Y position of the anchor point in world frame + * @param z The Z position of the anchor point in world frame + * @param dx A delta to be added to the X position as if the anchor was set + * when body1 was at current_position[X] + dx + * @param dx A delta to be added to the Y position as if the anchor was set + * when body1 was at current_position[Y] + dy + * @param dx A delta to be added to the Z position as if the anchor was set + * when body1 was at current_position[Z] + dz + * @note Should have the same meaning as dJointSetSliderAxisDelta + */ +void dJointSetPUAnchorOffset( dJointID j, dReal x, dReal y, dReal z, + dReal dx, dReal dy, dReal dz ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + if (joint->flags & dJOINT_REVERSE) + { + dx = -dx; + dy = -dy; + dz = -dz; + } + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] -= dx; + joint->node[0].body->posr.pos[1] -= dy; + joint->node[0].body->posr.pos[2] -= dz; + } + + setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 ); + + if ( joint->node[0].body ) + { + joint->node[0].body->posr.pos[0] += dx; + joint->node[0].body->posr.pos[1] += dy; + joint->node[0].body->posr.pos[2] += dz; + } + + joint->computeInitialRelativeRotations(); +} + + + + + +void dJointSetPUAxis1( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, NULL, joint->axis2 ); + else + setAxes( joint, x, y, z, joint->axis1, NULL ); + joint->computeInitialRelativeRotations(); +} + +void dJointSetPUAxis2( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + setAxes( joint, x, y, z, joint->axis1, NULL ); + else + setAxes( joint, x, y, z, NULL, joint->axis2 ); + joint->computeInitialRelativeRotations(); +} + + +void dJointSetPUAxis3( dJointID j, dReal x, dReal y, dReal z ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + setAxes( joint, x, y, z, joint->axisP1, 0 ); + + joint->computeInitialRelativeRotations(); +} + + +void dJointSetPUAxisP(dJointID id, dReal x, dReal y, dReal z) +{ + dJointSetPUAxis3(id, x, y, z); +} + + +void dJointSetPUParam(dJointID j, int parameter, dReal value) +{ + dxJointPU *joint = (dxJointPU *)j; + dUASSERT(joint, "bad joint argument"); + checktype(joint, PU); + + switch (parameter & 0xff00) + { + case dParamGroup1: + joint->limot1.set(parameter, value); + break; + case dParamGroup2: + joint->limot2.set(parameter & 0xff, value); + break; + case dParamGroup3: + joint->limotP.set(parameter & 0xff, value); + break; + } +} + + +void dJointGetPUAngles( dJointID j, dReal *angle1, dReal *angle2 ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + joint->getAngles( angle2, angle1 ); + else + joint->getAngles( angle1, angle2 ); +} + + +dReal dJointGetPUAngle1( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + return joint->getAngle2(); + else + return joint->getAngle1(); +} + + +dReal dJointGetPUAngle2( dJointID j ) +{ + dxJointUniversal* joint = ( dxJointUniversal* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + return joint->getAngle1(); + else + return joint->getAngle2(); +} + + +dReal dJointGetPUAngle1Rate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + 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 = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +dReal dJointGetPUAngle2Rate( dJointID j ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + 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 = dCalcVectorDot3( axis, joint->node[0].body->avel ); + if ( joint->node[1].body ) rate -= dCalcVectorDot3( axis, joint->node[1].body->avel ); + return rate; + } + return 0; +} + + +void dJointGetPUAnchor( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + + if ( joint->node[1].body ) + getAnchor2( joint, result, joint->anchor2 ); + else + { + // result[i] = joint->anchor2[i]; + dCopyVector3( result, joint->anchor2 ); + } +} + +void dJointGetPUAxis1( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis2( joint, result, joint->axis2 ); + else + getAxis( joint, result, joint->axis1 ); +} + +void dJointGetPUAxis2( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + if ( joint->flags & dJOINT_REVERSE ) + getAxis( joint, result, joint->axis1 ); + else + getAxis2( joint, result, joint->axis2 ); +} + +/** + * @brief Get the prismatic axis + * @ingroup joints + * + * @note This function was added for convenience it is the same as + * dJointGetPUAxis3 + */ +void dJointGetPUAxisP( dJointID id, dVector3 result ) +{ + dJointGetPUAxis3( id, result ); +} + + +void dJointGetPUAxis3( dJointID j, dVector3 result ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + dUASSERT( result, "bad result argument" ); + checktype( joint, PU ); + getAxis( joint, result, joint->axisP1 ); +} + +dReal dJointGetPUParam( dJointID j, int parameter ) +{ + dxJointPU* joint = ( dxJointPU* ) j; + dUASSERT( joint, "bad joint argument" ); + checktype( joint, PU ); + + switch ( parameter & 0xff00 ) + { + case dParamGroup1: + return joint->limot1.get( parameter ); + break; + case dParamGroup2: + return joint->limot2.get( parameter & 0xff ); + break; + case dParamGroup3: + return joint->limotP.get( parameter & 0xff ); + break; + } + + return 0; +} + + +dJointType +dxJointPU::type() const +{ + return dJointTypePU; +} + + +sizeint +dxJointPU::size() const +{ + return sizeof( *this ); +} + + +void +dxJointPU::setRelativeValues() +{ + dVector3 anchor; + dJointGetPUAnchor(this, anchor); + setAnchors( this, anchor[0], anchor[1], anchor[2], anchor1, anchor2 ); + + dVector3 ax1, ax2, ax3; + dJointGetPUAxis1(this, ax1); + dJointGetPUAxis2(this, ax2); + dJointGetPUAxis3(this, ax3); + + if ( flags & dJOINT_REVERSE ) + { + setAxes( this, ax1[0], ax1[1], ax1[2], NULL, axis2 ); + setAxes( this, ax2[0], ax2[1], ax2[2], axis1, NULL ); + } + else + { + setAxes( this, ax1[0], ax1[1], ax1[2], axis1, NULL ); + setAxes( this, ax2[0], ax2[1], ax2[2], NULL, axis2 ); + } + + + setAxes( this, ax3[0], ax3[1], ax3[2], axisP1, NULL ); + + computeInitialRelativeRotations(); +} + -- 2.11.4.GIT