// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // // // Project: Talina Gaming System (TgS) (∂) // File: TgS Collision - Capsule-Triangle.cpp // Author: Andrew Aye (EMail: andrew.aye@gmail.com, Web: http://www.andrewaye.com) // Version: 3.11 // // ------------------------------------------------------------------------------------------------------------------------------ // // // Copyright: © 2002-2008, Andrew Aye. All Rights Reserved. // // This software is free for non-commercial use. Redistribution and use in source and binary forms, with or without modification, // are permitted provided that the following conditions are met: // Redistributions of source code must retain this copyright notice, this list of conditions and the following disclaimers. // Redistributions in binary form must reproduce this copyright notice, this list of conditions and the following // disclaimers in the documentation and other materials provided with the distribution. // // Neither the names of the copyright owner nor the names of its contributors may be used to endorse or promote products derived // from this software without specific prior written permission. // // The intellectual property rights of the algorithms used reside with Andrew Aye. You may not use this software, in whole or // in part, in support of any commercial product without the express written consent of the author. // // There is no warranty or other guarantee of fitness of this software for any purpose. It is provided solely "as is". // // =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // namespace TGS { // START TGS /////////////////////////////////////////////////////////////////////////////////////////////////////// namespace COL { // START COL /////////////////////////////////////////////////////////////////////////////////////////////////////// // ============================================================================================================================== // // ---- F_Internal_SphereCap ---------------------------------------------------------------------------------------------------- // // -- Internal Function -- // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgCP0: Capsule primitive - contact points are generated on this primitive // Input: tgST0: Space Triangle primitive // Input: tyDist: The minimal distance between the capsule axis (segment) and the triangle // Input: tvP0: Point of closest proximity on the capsule axis between it and the triangle // Output: tgPacket: Points of penetration between the two primitives are added to it // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgFORCEINLINE TgRESULT F_Internal_SphereCap( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(CAPSULE,DIM) tgCP0, CR_(STRI,DIM) tgST0, const TYPE tyDist, M_(VECTOR,DIM) tvP0 ) { P_(CONTACT,DIM) ptgContact; // Check the start cap (origin - axis) for contact generation. if (tyDist >= TYPE(0.0)) { if (tyDist >= tgCP0.Query_Radius()) { return (TgE_NOINTERSECT); }; // Capsule cap is penetrating the triangle plane with the origin above the plane. Execute the sphere penetration // code to generate the contact point for the capsule cap. However, since the caps are hemi-spherical in reality, // it is necessary to examine the resultant contact point and make sure that the point was not created on the illegal // space of the sphere. This is done by culling points out based on the their contact normal. // Create contact points for the two end caps. T_(CONTACT_PACKET,DIM) tgCap_Packet; T_(CONTACT,DIM) tgCap_Contact; T_(SPHERE,DIM) tgCap; tgCap_Packet.m_ptgContact = &tgCap_Contact; tgCap_Packet.m_tySweepTol = TYPE(0.0); tgCap_Packet.m_niContact = 0; tgCap_Packet.m_niMaxContact = 1; tgCap_Packet.m_iStride = sizeof( T_(CONTACT,DIM) ); tgCap.Set( tvP0, tgCP0.Query_Radius() ); C_TgRESULT tgResult = F_Internal_Penetrate( &tgCap_Packet, tgST0.Query_CT(),tgCap ); C_(VECTOR,DIM) tvK0 = MATH::F_SUB( tgCap_Contact.m_tvPos, tvP0 ); C_(VECTOR,DIM) tvK1 = MATH::F_SUB( tvP0, tgCP0.Query_Origin() ); if (TgFAILED(tgResult) || MATH::F_DOT( tvK0, tvK1 ) <= TYPE(0.0)) { return (TgE_NOINTERSECT); }; if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = tgCap_Contact.m_tvPos; ptgContact->m_tvNormal = tgCap_Contact.m_tvNormal; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgCap_Contact.m_tyDepth; ++ptgPacket->m_niContact; return (TgS_OK); } else { // Capsule cap lies below the plane. Thus, the sphere penetration code would have ignored it, so for the capsule // it is necessary to deal with this separately. In this case only generate a contact point if the cap position is // contained inside of the normal extruded space of the triangle. For this case, the contact will only have a normal // equal to the triangle's normal. if (!tgST0.Is_Contained( tvP0 )) { return (TgE_NOINTERSECT); }; if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP0, MATH::F_MUL( tgCP0.Query_Radius(), tgST0.Query_Normal() ) ); ptgContact->m_tvNormal = tgST0.Query_Normal(); ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgCP0.Query_Radius() - tyDist; ++ptgPacket->m_niContact; return (TgS_OK); }; return (TgE_NOINTERSECT); }; tiC_TgRESULT F_Internal_SphereCap( PC_TgF4CONTACT_PACKET, CR_TgF4CAPSULE, CR_TgF4STRI, C_TgFLOAT32, M_TgF4VECTOR ); // ============================================================================================================================== // // ---- F_Internal_Parallel ----------------------------------------------------------------------------------------------------- // // -- Internal Function -- // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgST0: Space Triangle primitive // Input: tgCP0: Capsule primitive - contact points are generated on this primitive // Output: tgPacket: Points of penetration between the two primitives are added to it // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgRESULT F_Internal_Parallel( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(STRI,DIM) tgST0, CR_(CAPSULE,DIM) tgCP0 ) { // The capsule is, within tolerance, parallel to the triangle plane. The format of the surface of contact is therefore // definitively a longitudinal surface - stretching along the capsule. It is necessary to trap this specific case since // the non-parallel case will use a closest point algorithm derivative. Using that system will cause a varying and random // selection of one of the capsule extremities - which would then introduce a time varying reaction force resulting in // an unrealistic rocking motion or could potentially feed existing rotations. Keep in mind that since the selection process // would be at the extremities, an unbalanced moment would be added into the simulation which could be very problematic. TYPE tyT0,tyT1; if (TgFAILED(F_Clip( &tyT0,&tyT1, tgST0, tgCP0.Query_Segment() ))) { // The capsule axis does not exist anywhere in the triangle's normal extruded space. A specific contact routine will // create the contacts between the triangle edge and the capsule. F_Internal_Parallel_NoClip( ptgPacket, tgST0,tgCP0 ); }; C_(VECTOR,DIM) tvK0 = MATH::F_MUL( tyT0, tgCP0.Query_Segment().Query_DirN() ); C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT1, tgCP0.Query_Segment().Query_DirN() ); C_(VECTOR,DIM) tvP0 = MATH::F_ADD( tgCP0.Query_Segment().Query_Origin(), tvK0 ); C_(VECTOR,DIM) tvP1 = MATH::F_ADD( tgCP0.Query_Segment().Query_Origin(), tvK1 ); C_(VECTOR,DIM) tvK2 = MATH::F_SUB( tvP0, tgST0.Query_Origin() ); C_(VECTOR,DIM) tvK3 = MATH::F_SUB( tvP1, tgST0.Query_Origin() ); const TYPE tyDist0 = tgCP0.Query_Radius() - MATH::F_DOT( tvK2, tgST0.Query_Normal() ); const TYPE tyDist1 = tgCP0.Query_Radius() - MATH::F_DOT( tvK3, tgST0.Query_Normal() ); P_(CONTACT,DIM) ptgContact; if (tyDist0 >= TYPE(0.0)) { if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP0, MATH::F_MUL( tgCP0.Query_Radius(), tgST0.Query_Normal() ) ); ptgContact->m_tvNormal = tgST0.Query_Normal(); ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tyDist0; ++ptgPacket->m_niContact; }; if (tyDist1 >= TYPE(0.0)) { if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP1, MATH::F_MUL( tgCP0.Query_Radius(), tgST0.Query_Normal() ) ); ptgContact->m_tvNormal = tgST0.Query_Normal(); ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tyDist1; ++ptgPacket->m_niContact; }; return (tyDist0 >= TYPE(0.0) || tyDist1 >= TYPE(0.0) ? TgS_OK : TgE_NOINTERSECT); }; tiC_TgRESULT F_Internal_Parallel( PC_TgF4CONTACT_PACKET, CR_TgF4STRI, CR_TgF4CAPSULE ); // ============================================================================================================================== // // ---- F_Internal_EdgeParallel ------------------------------------------------------------------------------------------------- // // -- Internal Function -- // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgST0: Space Triangle primitive // Input: tgCP0: Capsule primitive - contact points are generated on this primitive // Output: tgPacket: Points of penetration between the two primitives are added to it // Return: Result Code // // Used when the capsule is parallel to the triangle plane, and the axis lies outside of the normal extruded triangle space. // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgRESULT F_Internal_Parallel_NoClip( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(STRI,DIM) tgST0, CR_(CAPSULE,DIM) tgCP0 ) { // It is now known that the capsule axis does not pass through the triangle normal extruded space. However, contact can // still occur because the triangle's edge and/or vertices can penetrate the capsule resulting in a non-planar directed // normal. Special care must be taken into account if the resulting penetrating feature is parallel to the capsule. // Method: Find the nearest triangle feature to the capsule. If the feature is not enabled then its assumed that the // resulting adjoined primitives will produce the required contacts and the feature can be ignored and the procedure // terminated. No matter the type of feature found the resulting edge or connected edges should be tested to see if they // are parallel to the capsule axis so that multiple points of contact can be created. The secondary contact point should // also be tested for feature reduction. C_(VECTOR,DIM) tvS0 = MATH::F_SUB( tgCP0.Query_Origin(), tgCP0.Query_HalfAxis() ); C_(VECTOR,DIM) tvS1 = MATH::F_ADD( tgCP0.Query_Origin(), tgCP0.Query_HalfAxis() ); M_(VECTOR,DIM) tvAX = tgCP0.Query_Segment().Query_DirN(); const TYPE tyAX_AX = MATH::F_LSQ( tvAX ); C_TgINT niContact = ptgPacket->m_niContact; P_(CONTACT,DIM) ptgContact; for (int iEdge = 0; iEdge < 3; ++iEdge) { // Test each vertex individually to prevent the creation of duplicate contacts from the edge routines (shared vertices). if (tgST0.Test_Point( iEdge )) { F_Contact_Penetrate( ptgPacket, tgST0.Query_Point( iEdge ),tgCP0 ); }; if (!tgST0.Test_Edge( iEdge )) //« Only collide with those edges marked as valid { continue; }; // Contacts are created only for those edges where the capsule exists entirely in the positive half-space and at least // minimally exists in the feature space. M_(VECTOR,DIM) tvEN = tgST0.Query_EdgePlane( iEdge ).Query_Normal(); M_(VECTOR,DIM) tvEP = tgST0.Query_Point( iEdge ); M_(VECTOR,DIM) tvET = tgST0.Query_Edge( iEdge ); const TYPE tyDist0 = MATH::F_DOT( MATH::F_SUB( tvS0, tvEP ), tvET ); const TYPE tyDist1 = MATH::F_DOT( MATH::F_SUB( tvS1, tvEP ), tvET ); const TYPE tyTest0 = MATH::F_DOT( MATH::F_SUB( tvS0, tvEP ), tvEN ); const TYPE tyTest1 = MATH::F_DOT( MATH::F_SUB( tvS1, tvEP ), tvEN ); const TYPE tyET_ET = MATH::F_LSQ( tvET ); if ( (tyDist0 < TYPE(0.0) && tyDist1 < TYPE(0.0)) || (tyDist0 > tyET_ET && tyDist1 > tyET_ET ) || (tyTest0 < TYPE(0.0) && tyTest1 < TYPE(0.0)) ) { continue; }; // The capsule is known to be on the positive side of the edge normal half-space and is captured passing through it. if (Near_Zero( MATH::F_DOT(tgCP0.Query_AxisUnit(),tvEN) )) { // The capsule axis is parallel to the edge C_(VECTOR,DIM) tvDS = MATH::F_SUB( tvEP, tvS0 ); // Projection Values const TYPE tyAX_ET = MATH::F_DOT( tvAX, tvET ); const TYPE tyDS_AX = MATH::F_DOT( tvDS, tvAX ); const TYPE tyDS_ET =-MATH::F_DOT( tvDS, tvET ); const TYPE tyDE_AX = tyDS_AX + tyAX_ET; //« tvDE = tvEP+tvET, tyDE_ET = (tvEP+tvET - tvS0)•tvAX const TYPE tyDF_ET = tyAX_ET - tyDS_ET; //« tvDF = tvS0+tvAX, tyDF_ET = (tvS0+tvAX - tvEP)•tvET if ( (tyAX_ET >= TYPE(0.0) && (tyDE_AX < TYPE(0.0) || tyDS_AX > TYPE(1.0))) || (tyAX_ET <= TYPE(0.0) && (tyDS_AX < TYPE(0.0) || tyDE_AX > TYPE(1.0))) ) { continue; }; const TYPE tyTA = tyDS_AX / tyAX_AX; const TYPE tyTC = tyDE_AX / tyAX_AX; // Point 0 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 0 of segment 1, else // point 1 of segment 1. const TYPE tyF0 = P::FSEL( tyDS_ET, P::FSEL( tyET_ET - tyDS_ET, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT0 = P::FSEL( tyF0, TYPE(0.0), P::FSEL( tyAX_ET, tyTA, tyTC ) ); const TYPE tyT1 = P::FSEL( tyF0, (tyDS_ET / tyET_ET), P::FSEL( tyAX_ET, TYPE(0.0), TYPE(1.0) ) ); // Point 1 of segment 0 if contained in segment 1, otherwise if segments are mutually directed point 1 of segment 1, else // point 0 of segment 1. const TYPE tyF1 = P::FSEL( tyDF_ET, P::FSEL( tyET_ET - tyDF_ET, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT2 = P::FSEL( tyF1, TYPE(1.0), P::FSEL( tyAX_ET, tyTC, tyTA ) ); const TYPE tyT3 = P::FSEL( tyF1, (tyDF_ET / tyET_ET), P::FSEL( tyAX_ET, TYPE(1.0), TYPE(0.0) ) ); TgASSERT(tyT0 >= TYPE(0.0) && tyT0 <= TYPE(1.0)); TgASSERT(tyT1 >= TYPE(0.0) && tyT1 <= TYPE(1.0)); TgASSERT(tyT2 >= TYPE(0.0) && tyT2 <= TYPE(1.0)); TgASSERT(tyT3 >= TYPE(0.0) && tyT3 <= TYPE(1.0)); T_(VECTOR,DIM) tvP0,tvP1,tvNM; TYPE tyTM; tvP0 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT0, tvAX ) ); tvP1 = MATH::F_ADD( tvEP, MATH::F_MUL( tyT1, tvET ) ); tvNM = MATH::F_NORM( &tyTM, MATH::F_SUB( tvP0, tvP1 ) ); if (tyTM < tgCP0.Query_Radius() && !Near_Zero( tyT1 ) && !Near_One( tyT1 )) { if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP0, MATH::F_MUL( tgCP0.Query_Radius(), tvNM ) ); ptgContact->m_tvNormal = tvNM; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgCP0.Query_Radius() - tyTM; ++ptgPacket->m_niContact; }; tvP0 = MATH::F_ADD( tvS0, MATH::F_MUL( tyT2, tvAX ) ); tvP1 = MATH::F_ADD( tvEP, MATH::F_MUL( tyT3, tvET ) ); tvNM = MATH::F_NORM( &tyTM, MATH::F_SUB( tvP0, tvP1 ) ); if (tyTM < tgCP0.Query_Radius() && !Near_Zero( tyT3 ) && !Near_One( tyT3 )) { if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP0, MATH::F_MUL( tgCP0.Query_Radius(), tvNM ) ); ptgContact->m_tvNormal = tvNM; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgCP0.Query_Radius() - tyTM; ++ptgPacket->m_niContact; }; } else { TYPE tyT0,tyT1; const TYPE tyDistSq = TTgCSQ_LNLN<TYPE,DIM,1,1,1,1>::DO( &tyT0,&tyT1, tvS0, MATH::F_SUB( tvS1, tvS0 ), tvEP,tvET ); if (tyDistSq < tgCP0.Query_RadiusSq() && !Near_Zero( tyT1 ) && !Near_One( tyT1 )) { // The closest point on the triangle is not a vertex - and the edge is valid. if (ptgPacket->m_niContact >= ptgPacket->m_niMaxContact) { return (TgS_MAXCONTACTS); }; TYPE tyDist; C_(VECTOR,DIM) tvK0 = MATH::F_MUL( TYPE(1.0) - tyT1, tvS0 ); C_(VECTOR,DIM) tvP0 = MATH::F_ADD( tvK0, MATH::F_MUL( tyT1, tvS1 ) ); C_(VECTOR,DIM) tvK1 = MATH::F_SUB( tvP0, tvEP ); C_(VECTOR,DIM) tvK2 = MATH::F_MUL( tyT1, tvET ); C_(VECTOR,DIM) tvNM = MATH::F_NORM( &tyDist, MATH::F_ADD( tvK1, tvK2 ) ); ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tvP0, MATH::F_MUL( tgCP0.Query_Radius(), tvNM ) ); ptgContact->m_tvNormal = tvNM; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgCP0.Query_Radius() - tyDist; ++ptgPacket->m_niContact; }; }; }; return (niContact == ptgPacket->m_niContact ? TgE_NOINTERSECT : TgS_OK); }; tiC_TgRESULT F_Internal_Parallel_NoClip( PC_TgF4CONTACT_PACKET, CR_TgF4STRI, CR_TgF4CAPSULE ); // ============================================================================================================================== // // ---- F_Contact_Penetrate ----------------------------------------------------------------------------------------------------- // // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgST0: Space Triangle primitive // Input: tgCP0: Capsule primitive - contact points are generated on this primitive // Output: tgPacket: Points of penetration between the two primitives are added to it // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgRESULT F_Contact_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(STRI,DIM) tgST0, CR_(CAPSULE,DIM) tgCP0 ) { TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgCAPSULE<<16)|(TgUINT)ETgTRIANGLE)) TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof(T_(CONTACT,DIM)) && tgCP0.Is_Valid() && tgST0.Is_Valid()) if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact) { return (TgE_FAIL); }; TgFEBUG_COLLISION_TRIANGLE_CREATEID( iDBG_TriID, tgST0, etgDEBUG_COLLISION_ENTERFCN ); // Primitive Culling - Set of criteria required for the primitive to be considered penetrating the triangle. C_(VECTOR,DIM) tvK0 = MATH::F_SUB( tgCP0.Query_Origin(), tgST0.Query_Origin() ); const TYPE tyDS_N = MATH::F_DOT( tgST0.Query_Normal(), tvK0 ); const TYPE tyEX_N = MATH::F_DOT( tgST0.Query_Normal(), tgCP0.Query_HalfAxis() ); const TYPE tyS0_N = tyDS_N - tyEX_N; const TYPE tyS1_N = tyDS_N + tyEX_N; const TYPE tyRadius = tgCP0.Query_Radius(); if ((tyS0_N > tyRadius && tyS1_N > tyRadius) || (tyS0_N < TYPE(0.0) && tyS1_N < TYPE(0.0))) { // Either both of the capsule's end points are below the plane or more than radius above the plane. return (TgE_NOINTERSECT); }; TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT ); P_(CONTACT,DIM) ptgContact; TYPE tyDepth; // Check the projection of the capsule primary axis against the triangle normal. If its near zero (ie the two vectors are near // perpendicular), the capsule must be near-parallel to the triangle. In this case the method of closest proximity will not work, // and the axis segment will instead be clipped to the triangle space. if (Near_Zero( tyEX_N )) { // Since the capsule is near-parallel to the triangle itself, in terms of the contact surface, it is no longer different // than a regular tube. Execute that primitive's parallel case to generate the contact points. C_TgRESULT tgResult = F_Internal_Parallel( ptgPacket, tgST0, tgCP0 ); if (TgFAILED(tgResult)) { return (tgResult); }; }; TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_CODE2 ); // Find the minimal distance and points of closest proximity for the capsule's axis and the triangle. CR_(SEGMENT,DIM) tgAX0 = tgCP0.Query_Segment(); TYPE tyCT0, tyCT1, tyCP0; const TYPE tyDistSq = F_ClosestSq( &tyCT0,&tyCT1, &tyCP0, tgST0,tgAX0 ); // Intersection is impossible if the minimal distance is greater than the capsule's radius. if (tyDistSq > tgCP0.Query_RadiusSq()) { return (TgE_NOINTERSECT); }; // If the closest point on the triangle to the capsule's axis is not on a valid edge, then the generated axis is ignored // and the system falls back to creating contacts for the two spherical caps. TgBOOL bResultCreated = TgFALSE; if (!Near_Zero( tyCP0 ) && !Near_One( tyCP0 ) && !Is_Point_Culled( tgST0.Query_CT(), tyCT0,tyCT1 )) { // Create contacts only for the tube portion of the capsule. T_(VECTOR,DIM) tvCP0, tvCT1, tvNormal; // Create the two points of closest proximity and the corresponding vector difference between the two. C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyCT0, tgST0.Query_Edge0() ); C_(VECTOR,DIM) tvK2 = MATH::F_MUL( tyCT1, tgST0.Query_Edge2() ); tvCP0 = MATH::F_ADD( tgAX0.Query_Origin(), MATH::F_MUL( tyCP0, tgAX0.Query_DirN() ) ); tvCT1 = MATH::F_ADD( tgST0.Query_Origin(), MATH::F_SUB( tvK1, tvK2 ) ); C_(VECTOR,DIM) tvK3 = MATH::F_SUB( tvCP0, tgST0.Query_Origin() ); const TYPE tyTest = MATH::F_DOT( tvK3, tgST0.Query_Normal() ); tvNormal = tyTest > TYPE(0.0) ? MATH::F_SUB( tvCP0, tvCT1 ) : MATH::F_SUB( tvCT1, tvCP0 ); // Calculate the resultant penetration depth at this point. const TYPE tyDist = tyTest ? P::SQRT( tyDistSq ) : -P::SQRT( tyDistSq ); tyDepth = tyDist >= tgCP0.Query_Radius() ? TYPE(0.0) : tgCP0.Query_Radius() - tyDist; // Check to see if the normal of intersection should be replaced by the triangle's normal. This is done to reduce // floating point noise in the system where near-normal results are returned. By forcing it to the triangle's normal, // extraneous rotations are minimized. The other possibility is that the capsule's segment intersects the triangle itself, // thus, requiring the selection of the triangle's normal for the intersection. TgBOOL bUseNormal = tyDistSq > LIMITS<TYPE>::EPSILON; if (bUseNormal) { MATH::F_NORM( tvNormal ); // Check to see if the resultant normal is near that of the triangle's. If they are close then use the triangle's // normal to help further reduce floating point noise. bUseNormal = Near_Zero( MATH::F_DOT(tvNormal,tgST0.Query_Normal()) - TYPE(1.0) ); }; // Create contact point. ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); C_(VECTOR,DIM) tvK4 = bUseNormal ? tvNormal : tgST0.Query_Normal(); ptgContact->m_tvPos = MATH::F_SUB( tvCT1, MATH::F_MUL( tgCP0.Query_Radius(), tvK4 ) ); ptgContact->m_tvNormal = tvK4; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tyDepth; ++ptgPacket->m_niContact; bResultCreated = TgTRUE; }; TgRESULT tgResult; tgResult = F_Internal_SphereCap( ptgPacket, tgCP0,tgST0, tyS0_N, MATH::F_SUB( tgCP0.Query_Origin(), tgCP0.Query_HalfAxis() ) ); switch (tgResult) { case TgS_MAXCONTACTS: return (TgS_MAXCONTACTS); default: bResultCreated |= TgSUCCEEDED(tgResult); }; tgResult = F_Internal_SphereCap( ptgPacket, tgCP0,tgST0, tyS1_N, MATH::F_ADD( tgCP0.Query_Origin(), tgCP0.Query_HalfAxis() ) ); switch (tgResult) { case TgS_MAXCONTACTS: return (TgS_MAXCONTACTS); default: bResultCreated |= TgSUCCEEDED(tgResult); }; TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, bResultCreated ? etgDEBUG_COLLISION_FINAL : etgDEBUG_COLLISION_CODE4 ); return (bResultCreated ? TgS_OK : TgE_NOINTERSECT); }; template TgRESULT F_Contact_Penetrate( PC_TgF4CONTACT_PACKET, CR_TgF4STRI, CR_TgF4CAPSULE ); // ============================================================================================================================== // // ---- F_Contact_Sweep --------------------------------------------------------------------------------------------------------- // // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tyPM: Current normalized time of first contact. // Input: bPenetrate: If the swept primitives are in penetration, if true the function will return points of penetration. // Input: tgSP0: Space Triangle primitive // Input: tgcP0: Capsule primitive // Input: tgDT: A structure holding the swept primitive displacement for the entire duration of the test period // Output: tgPacket: Contact points are added or replace the current set depending on the time comparison and given parameters // Output: tyPM: New normalized time of first contact // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // //template <typename TYPE, int DIM> //C_TgRESULT F_Contact_Sweep( // PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, CR_(STRI,DIM) tgST0, CR_(CAPSULE,DIM) tgCP0, CR_(DELTA,DIM) tgDT //) { // TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgTRIANGLE<<16)|(TgUINT)ETgBOX)) // // typedef T_(VECTOR,DIM) TgVECTOR; // TgTYPE_DECLARE( TgVECTOR, TgVECTOR ) // // TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) )) // TgASSERT(tgST0.Is_Valid() && tgCP0.Is_Valid()) // // if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact) // { // return (TgE_FAIL); // }; // // TgFEBUG_COLLISION_TRIANGLE_CREATEID(iDBG_TriID, tgST0, ETgFEBUG_COLLISION_ENTERFCN); // // // Primitive Culling - Set of criteria required for the primitive to be considered penetrating the triangle. // // const TYPE tyCO_N = tgST0.Query_Normal() * ( tgCP0.Query_Origin() - tgST0.Query_Origin() ); // const TYPE tyAX_N = tgST0.Query_Normal() * tgCP0.Query_HalfAxis(); // const TYPE tyDT_N = tgST0.Query_Normal() * tgDT.m_tvDT; // // const TYPE tyET_P0 = tyCO_N - tyAX_N; // const TYPE tyET_P1 = tyCO_N + tyAX_N; // // if ( // (tyET_P0 < TYPE(0.0) && tyET_P1 < TYPE(0.0)) || // (tyET_P0 > tgCP0.Query_Radius() && tyET_P1 > tgCP0.Query_Radius() && tyDT_N > -LIMITS<TYPE>::EPSILON) // ) { // // Both of the capsule's caps are above the plane and its moving away from the triangle. // // return (TgE_NOINTERSECT); // }; // // // Check for pre-penetration // // if ( // (tyET_P0 >= TYPE(0.0) && tyET_P0 <= tgCP0.Query_Radius()) || // (tyET_P1 >= TYPE(0.0) && tyET_P1 <= tgCP0.Query_Radius()) || // (tyET_P0 * tyET_P1 < TYPE(0.0)) // ) { // // If pre-penetration information is not requested or the sphere's origin is behind the triangle report a non-intersection. // // if (!bPenetrate) // { // return (TgE_NOINTERSECT); // }; // // // Record the current number of contacts before potentially clearing the list in case pre-penetration is not found. // // C_TgINT niContact = ptgPacket->m_niContact; // // if (tyPM > ptgPacket->m_tySweepTol) // { // ptgPacket->m_niContact = 0; // }; // // C_TgRESULT tgResult = F_Contact_Penetrate( ptgPacket, tgST0,tgCP0 ); // // // Restore the original number of contacts if pre-penetration was not found. // // if (TgFAILED(tgResult)) // { // ptgPacket->m_niContact = niContact; // return (tgResult); // }; // // // Set the time parameter and return back the appropriate result code. // // tyPM = TYPE(0.0); // // switch (tgResult) // { // case TgS_MAXCONTACTS: // return (TgE_PREPENETRATION_MAXCONTACTS); // case TgS_OK: // return (TgE_PREPENETRATION); // default: // return (tgResult); // }; // }; // // TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, etgDEBUG_COLLISION_PASSED_REJECT ); // // // TgBOOL bResultCreated = TgFALSE; // TgRESULT tgResult; // // // AA-NOTE: Need to optimize the edge selection - sleep on this. Slept on it - think on it some more. // // if (tgST0.Test_Edge0()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point0(),tgST0.Query_Edge0(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // } // else if (tgST0.Test_Point0()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point0(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // }; // // if (tgST0.Test_Edge1()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point1(),tgST0.Query_Edge1(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // } // else if (tgST0.Test_Point1()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point1(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // }; // // if (tgST0.Test_Edge2()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point2(),tgST0.Query_Edge2(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // } // else if (tgST0.Test_Point2()) // { // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0.Query_Point2(), tgCP0,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // }; // // TTgSPHERE<TYPE,DIM> tgCap; // // tgCap.Set( tgCP0.Query_Origin() - tgCP0.Query_HalfAxis(), tgCP0.Query_Radius() ); // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0, tgCap,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // // tgCap.Set( tgCP0.Query_Origin() + tgCP0.Query_HalfAxis(), tgCP0.Query_Radius() ); // tgResult = F_Contact_Sweep( tgPacket,tyPM, bPenetrate, tgST0, tgCap,tgDT ); // bResultCreated |= TgSUCCEEDED( tgResult ); // // // Because of the previous checks, it should not be possible for pre-penetrations to occur at this point. Sanity-Check. // TgASSERT( tgResult != TgE_PREPENETRATION || tgResult != TgE_PREPENETRATION_MAXCONTACTS ) // // TgFEBUG_COLLISION_TRIANGLE( iDBG_TriID, bResultCreated ? etgDEBUG_COLLISION_FINAL : etgDEBUG_COLLISION_CODE1 ); // // return (bResultCreated ? TgS_OK : TgE_NOINTERSECT); //}; // //template TgRESULT F_Contact_Sweep( R_TgF4CONTACT_PACKET,R_TgFLOAT32, C_TgBOOL, CR_TgF4STRI, CR_TgF4CAPSULE,CR_TgF4DELTA ); // ============================================================================================================================== // // ---- F_Contact_Test ---------------------------------------------------------------------------------------------------------- // // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgST0: Space Triangle primitive // Input: tgCP0: Capsule primitive // Input: tvUDT: Normalized direction of displacement for the swept primitive (capsule) // Input: tyDT: Length of displacement for the swept primitive // Return: True if the two primitives are in contact at anytime during the sweep // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgBOOL F_Contact_Test( CR_(STRI,DIM) tgST0, CR_(CAPSULE,DIM) tgCP0, M_(VECTOR,DIM) tvUDT, const TYPE tyDT ) { TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_BOOLEAN, (((TgUINT)ETgCAPSULE<<16)|(TgUINT)ETgTRIANGLE)) TgASSERT(tgCP0.Is_Valid() && tgST0.Is_Valid()) const TYPE tyUDT_CA = MATH::F_DOT(tvUDT,tgCP0.Query_AxisUnit()); C_(VECTOR,DIM) tvC0 = tgCP0.Query_Segment().Query_Origin(); C_(VECTOR,DIM) tvCA = tgCP0.Query_Segment().Query_DirN(); //const TYPE tyLimit = tgCP0.Query_Radius(); C_(VECTOR,DIM) tvDT = MATH::F_MUL( tvUDT, tyDT ); if (Near_Zero( tyUDT_CA - TYPE(1.0) )) //« Delta vector is parallel to the capsule axis. { return (TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, tvC0, MATH::F_ADD( tvCA, tvDT ) ) < tgCP0.Query_RadiusSq()); }; if (Near_Zero( tyUDT_CA + TYPE(1.0) )) //« Delta vector is parallel to the capsule axis. { return (TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, MATH::F_ADD( tvC0, tvDT ), MATH::F_SUB( tvCA, tvDT ) ) < tgCP0.Query_RadiusSq()); }; // Test to see if the swept capsule (represented by the edges of a parallelogram) comes within range of the triangle. if ( TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, tvC0, tvCA ) < tgCP0.Query_RadiusSq() || TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, tvC0, tvDT ) < tgCP0.Query_RadiusSq() || TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, MATH::F_ADD( tvC0, tvDT ), tvCA ) < tgCP0.Query_RadiusSq() || TTgFSQ_STLN<TYPE,DIM,1,1>::DO( tgST0, MATH::F_ADD( tvC0, tvCA ), tvDT ) < tgCP0.Query_RadiusSq() ) { return (TgTRUE); }; // Test to see if the triangle ever comes within range of the swept capsule (represented by a parallelogram) TTgPARALLELOGRAM<TYPE,DIM> tgSweptCapsule; tgSweptCapsule.Set( tvC0, tvCA, tvDT ); if ( TTgTST_PELN<TYPE,DIM,1,1>::DO( tgSweptCapsule, tgST0.Query_Origin(), tgST0.Query_Edge0() ) || TTgTST_PELN<TYPE,DIM,1,1>::DO( tgSweptCapsule, tgST0.Query_Origin(), tgST0.Query_Edge1() ) || TTgTST_PELN<TYPE,DIM,1,1>::DO( tgSweptCapsule, tgST0.Query_Point1(), tgST0.Query_Edge2() ) ) { return (TgTRUE); }; return (TgFALSE); }; template TgBOOL F_Contact_Test( CR_TgF4STRI, CR_TgF4CAPSULE, M_TgF4VECTOR, C_TgFLOAT32 ); // ============================================================================================================================== // }; // END COL ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// }; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////