// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // // // Project: Talina Gaming System (TgS) (∂) // File: TgS Collision - Sphere-Linear.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". // // =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // // ============================================================================================================================== // // // Sphere Definition: |S(v) - C0| < R0 // Line Definition: L1(α) = P1 + α•D1 | α ε [ 0, 1] // // Derivation: Let C0 be the centre of sphere S0 with radius R0 // Let L1 = P1 + α•D1 | α ε [ 0, 1] // Let Q1 = P1 + γ•D1 by the closest point of contact // Let V = C0 - Q1, the vector connecting the closest points of contact ( the minimal distance vector ) // // Minimal distance vector, by definition must be perpendicular to the line. // Therefore, D1•v=0, DS=C0-P1, v=DS-γ•D1 // // 0 = D1_(DS-γ•D1,DIM) // 0 = DS•D1-γ•D1•D1 // γ = DS•D1 / D1•D1 // // However, we know that γ ε [ 0, 1], generating three cases: // // [1] γ ε (-∞, 0) || γ = 0 // Distance: The distance between P1 and C0 // = (C0-P1)T_(C0-P1,DIM) // = DS•DS // // [2] γ ε [ 0, 1] || γ = (DS•D1) / (D1•D1) // Distance: The distance value would be || v ||. // = || v || = v•v = (DS-γ•D1)T_(DS-γ•D1,DIM) // = DS•DS + γ•γ•D1•D1 - 2•γ•DS•D1 // = DS•DS + γ_(γ•D1•D1 - 2•DS•D1,DIM) // = DS•DS + γ_((DS•D1 / D1•D1,DIM)•D1•D1 - 2•DS•D1) // = DS•DS + γ_(DS•D1 - 2•DS•D1,DIM) // = DS•DS - γ_(DS•D1,DIM) // // [3] γ ε ( 1, ∞) || γ = 1 // Distance: The distance between P1+D1 and C0 // = (C0-P1-D1)T_(C0-P1-D1,DIM) // = (DS-D1)T_(DS-D1,DIM) // = DS•DS - 2•DS•D1 + D1•D1 // // Form a right-sided triangle. // (a) The hypotenuse is the sphere radius. // (b) The known edge is the segment from the centre of the sphere to the closest point on the line. // (c) The length of the remaining side (along L1) is the distance from the point of closest proximity and the // points of contact between the sphere and the line. // // .(C0) .(C0) // /|\ /|\ // / | \ / | \ // / | \ / | \ // / | \ / | \ // / | \ / | \ // /(R0) | \(R0) (R0) / | \ (R0) // | .----.------/------.------\------.----> (D1) // | (P1) (Q2) / (Q1) \ (Q3) // .------------.-----.--------------> (D1) / | \ // (P1) (Q2) (Q1) /(R0) | \(R0) // // Contact can not occur under the following conditions: // Given that the point on the line closest to the sphere centre is at γ = DS•D1 / D1•D1 // The squared distance of Q1-C0 is φ = DS•DS - γ•γ•D1•D1 = DS•DS - γ_(DS•D1,DIM) // Thus, if DS•DS - γ_(DS•D1,DIM) > R0•R0, intersection can not occur. // Otherwise: Intersection has definitely occurred. // By definition (sphere) it is known that // R0•R0 = ||C0 - P1 - β•D1||, for β ε (-∞, ∞) // = ||DS - β•D1|| // 0 = β•β•D1•D1 - β•2•DS•D1 + DS•DS - R0•R0 // Solving using the quadratic formula // β = (2_(DS•D1,DIM) ± √(4_(DS•D1,DIM)T_(DS•D1,DIM) - 4_(D1•D1,DIM)(DS•DS - R0•R0))) / (2_(D1•D1,DIM)) // β = ((DS•D1) ± √((DS•D1)T_(DS•D1,DIM) - (D1•D1)(DS•DS - R0•R0))) / (D1•D1) // If the discriminant ((DS•D1)T_(DS•D1,DIM) - (D1•D1)(DS•DS - R0•R0)) is zero, there is only // one point of intersection // // // // Derivation: Let C0 be the centre of sphere S0 with radius R0 // Let L1 = P1 + α•D1 | α ε [ 0, 1] // // R0² = |(P1 + α•D1) - C0|² // = ((P1-C0) + α•D1) • ((P1-C0) + α•D1) // = (DS + α•D1)T_(DS + α•D1,DIM) // = DS•DS + 2•α•D1•DS + α•α•D1•D1 // // Solve for α // = (-2•D1•DS ± √(4_(D1•DS,DIM)T_(D1•DS,DIM) - 4•D1•D1•DS•DS)) / 2•D1•D1 // = (-D1•DS ± √((D1•DS)T_(D1•DS,DIM) - D1•D1•DS•DS)) / D1•D1 // // // ============================================================================================================================== // namespace TGS { // START TGS /////////////////////////////////////////////////////////////////////////////////////////////////////// namespace COL { // START COL /////////////////////////////////////////////////////////////////////////////////////////////////////// // ============================================================================================================================== // // ---- TTgINL_SPLN ------------------------------------------------------------------------------------------------------------- // // -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1} // // Input: tgSP0: Sphere primitive // Input: tvS0,tvD0: Origin and Direction for Linear // Output: tyLN0,tyLN1: Parametric parameter to generate the two points of the linear in contact with the extended tube surface // Output: tvN0, tvN1: Tube surface normal at the points of contact between the two primitives // Return: Result Code // // The internal functions do not clip the linear. All passed in linears are treated as lines - the boolean markers are used to // generate possible quick-out logic to avoid further processing. // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM, bool bC0, bool bC1> TgRESULT TTgINL_SPLN<TYPE,DIM,bC0,bC1>::DO( TYPE *ptyLN0, TYPE *ptyLN1, PC_(VECTOR,DIM) ptvN0, PC_(VECTOR,DIM) ptvN1, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0 ) { TgASSERT(tgSP0.Is_Valid() && MATH::F_Is_Point_Valid( tvS0 ) && MATH::F_Is_Vector_Valid( tvD0 )); C_(VECTOR,DIM) tvDS = MATH::F_SUB( tgSP0.Query_Origin(), tvS0 ); const TYPE tyD0_D0 = MATH::F_LSQ( tvD0 ); const TYPE tyDS_DS = MATH::F_LSQ( tvDS ); if (tyD0_D0 > LIMITS<TYPE>::EPSILON) { // [1] [DS•DS - γ_(DS•D0,DIM)] const TYPE tyDS_D0 = MATH::F_DOT(tvDS,tvD0); if (bC0 && tyDS_DS > tgSP0.Query_RadiusSq() && tyDS_D0 < TYPE(0.0)) { return (TgE_NOINTERSECT); }; if (bC1 && tyDS_DS > tgSP0.Query_RadiusSq() + tyD0_D0) { return (TgE_NOINTERSECT); }; const TYPE tyInvD0_D0 = TYPE(1.0) / tyD0_D0; const TYPE tyT0 = tyDS_D0*tyInvD0_D0; const TYPE tyDSC = tyDS_D0*tyDS_D0 - tyD0_D0*(tyDS_DS-tgSP0.Query_RadiusSq()); if (tyDSC > LIMITS<TYPE>::EPSILON) { const TYPE tyRoot = P::SQRT(tyDSC)*tyInvD0_D0; C_(VECTOR,DIM) tvK0 = MATH::F_SUB( tvS0, tgSP0.Query_Origin() ); C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT0 + tyRoot, tvD0 ); C_(VECTOR,DIM) tvK2 = MATH::F_MUL( tyT0 - tyRoot, tvD0 ); *ptvN0 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) ); *ptyLN0 = tyT0 + tyRoot; *ptvN1 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK2 ) ); *ptyLN1 = tyT0 - tyRoot; return (TgS_OK); } else if (tyDSC > -LIMITS<TYPE>::EPSILON) { C_(VECTOR,DIM) tvK0 = MATH::F_SUB( tvS0, tgSP0.Query_Origin() ); C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT0, tvD0 ); C_(VECTOR,DIM) tvK2 = MATH::F_NORM( MATH::F_ADD( tvK0, tvK1 ) ); *ptvN0 = tvK2; *ptvN1 = tvK2; *ptyLN0 = *ptyLN1 = tyT0; return (TgS_OK); }; return (TgE_NOINTERSECT); } else { if (tyDS_DS > LIMITS<TYPE>::EPSILON) { const TYPE tyVal = tgSP0.Query_RadiusSq()/ tyDS_DS; if (Near_One( tyVal )) { C_(VECTOR,DIM) tvK0 = MATH::F_NORM( MATH::F_SUB( tvS0, tgSP0.Query_Origin() ) ); *ptvN0 = tvK0; *ptvN1 = tvK0; *ptyLN0 = *ptyLN1 = TYPE(0.0); return (TgS_OK); }; }; }; return (TgE_NOINTERSECT); }; // ============================================================================================================================== // // ---- TTgINT_SPLN ------------------------------------------------------------------------------------------------------------- // // -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1} // // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tgSP0: Sphere primitive // Input: tvS0,tvD0: Origin and Direction for Linear // Output: tgPacket: Points of intersection between the two primitives are added to it // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM, bool bC0, bool bC1> TgRESULT TTgINT_SPLN<TYPE,DIM,bC0,bC1>::DO( PC_(CONTACT_PACKET,DIM) ptgPacket, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0 ) { // Check to make sure that a valid contact, and contact packet exist. if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact) { return (TgE_FAIL); }; TYPE tyLN0, tyLN1; T_(VECTOR,DIM) tvN0, tvN1; C_TgRESULT tgResult = TTgINL_SPLN<TYPE,DIM,bC0,bC1>::DO( &tyLN0,&tyLN1, &tvN0,&tvN1, tgSP0, tvS0,tvD0 ); if (TgFAILED( tgResult )) { return (tgResult); }; // Limit the variable to the cap regions P_(CONTACT,DIM) ptgContact; if (bC0 && tyLN0 < TYPE(0.0)) { if(tyLN1 <= TYPE(0.0)) { return (TgE_NOINTERSECT); }; if (bC1 && tyLN1 > TYPE(1.0)) { return (TgE_NOINTERSECT); }; } else if (!bC1 || tyLN0 <= TYPE(1.0)) { ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_ADD( tvS0, MATH::F_MUL( tyLN0, tvD0 ) ); ptgContact->m_tvNormal = tvN0; ptgContact->m_tyT0 = tyLN0; ptgContact->m_tyDepth = TYPE(0.0); ++ptgPacket->m_niContact; }; if (bC1 && tyLN1 > TYPE(1.0)) { if(tyLN0 >= TYPE(1.0)) { return (TgE_NOINTERSECT); }; } else { 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_ADD( tvS0, MATH::F_MUL( tyLN1, tvD0 ) ); ptgContact->m_tvNormal = tvN1; ptgContact->m_tyT0 = tyLN1; ptgContact->m_tyDepth = TYPE(0.0); ++ptgPacket->m_niContact; }; return (TgS_OK); }; // ============================================================================================================================== // // ---- F_Internal_Penetrate ---------------------------------------------------------------------------------------------------- // // -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1} // // Input: tgPacket: The current series of contact points for this query-series, and contact generation parameters. // Input: tvD0: Direction of the Linear // Input: tgSP0: Sphere primitive // Input: tvP1: Point of closest proximity to the sphere origin on the linear // Input: tyDistSq: Minimal distance squared between the linear and sphere origin // Output: tgPacket: Points of penetration between the two primitives are added to it // Return: Result Code // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgRESULT F_Internal_Penetrate( PC_(CONTACT_PACKET,DIM) ptgPacket, M_(VECTOR,DIM) tvD0, CR_(SPHERE,DIM) tgSP0, M_(VECTOR,DIM) tvP1, const TYPE tyDistSq ) { TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_PENETRATE, (((TgUINT)ETgSEGMENT<<16)|(TgUINT)ETgSPHERE)) T_(VECTOR,DIM) tvNormal; TYPE tyNM; if (tyDistSq <= LIMITS<TYPE>::EPSILON) { if (Near_Zero( tvD0(2) )) { tvNormal = MATH::F_SETV<TYPE,DIM>( -tvD0(1),tvD0(0),TYPE(0.0) ); } else { tvNormal = MATH::F_SETV<TYPE,DIM>( TYPE(0.0),tvD0(2),-tvD0(1) ); }; MATH::F_NORM( tvNormal ); tyNM = TYPE(0.0); } else { tvNormal = MATH::F_NORM( &tyNM, MATH::F_SUB( tgSP0.Query_Origin(), tvP1 ) ); }; P_(CONTACT,DIM) ptgContact; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); ptgContact->m_tvPos = MATH::F_SUB( tgSP0.Query_Origin(), MATH::F_MUL( tgSP0.Query_Radius(), tvNormal ) ); ptgContact->m_tvNormal = tvNormal; ptgContact->m_tyT0 = TYPE(0.0); ptgContact->m_tyDepth = tgSP0.Query_Radius() - tyNM; ++ptgPacket->m_niContact; return (TgS_OK); }; template TgRESULT F_Internal_Penetrate( PC_TgF4CONTACT_PACKET, M_TgF4VECTOR, CR_TgF4SPHERE, M_TgF4VECTOR, C_TgFLOAT32 ); // ============================================================================================================================== // // ---- TTgSWP_SPLN ------------------------------------------------------------------------------------------------------------- // // -- Internal Function -- bC0, bC1 : Indicate the termination condition of the linear {bc0,bC1} // // 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: bP: If the swept primitives are in penetration, if true the function will return points of penetration. // Input: tvS0,tvD0: Origin and Direction for Linear // Input: tgSP0: Sphere 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, bool bC0, bool bC1> TgRESULT TTgSWP_SPLN<TYPE,DIM,bC0,bC1>::DO( PC_(CONTACT_PACKET,DIM) ptgPacket, TYPE *ptyPM, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0, CR_(SPHERE,DIM) tgSP0, CR_(DELTA,DIM) tgDT ) { TgBLOCK_FCN_NOOBJ(ETgFAC_COLLISION, 0, ETgTEST_SWEEP, (((TgUINT)ETgLINEAR<<16)|(TgUINT)ETgSPHERE)) TgASSERT( Is_Valid( tgDT ) && tgSP0.Is_Valid() && MATH::F_Is_Point_Valid( tvS0 ) && MATH::F_Is_Vector_Valid( tvD0 ) ) TgASSERT((TgSIZE)ptgPacket->m_iStride >= sizeof( P_(CONTACT,DIM) )) if (0 == ptgPacket->m_niMaxContact || ptgPacket->m_niContact >= ptgPacket->m_niMaxContact || NULL == ptgPacket->m_ptgContact) { return (TgE_FAIL); }; TYPE tyLN1; const TYPE tyDistSq = TTgCSQ_LNPT<TYPE,DIM,bC0,bC1>::DO( &tyLN1, tvS0,tvD0, tgSP0.Query_Origin() ); if (tyDistSq <= tgSP0.Query_RadiusSq()) { if (*ptyPM > ptgPacket->m_tySweepTol) { ptgPacket->m_niContact = 0; }; C_TgBOOL bPenetrate = TgTRUE == ptgPacket->m_bReport_Penetration; C_(VECTOR,DIM) tvK0 = MATH::F_ADD( tvS0, MATH::F_MUL( tyLN1, tvD0 ) ); *ptyPM = TYPE(0.0); if (bPenetrate && TgS_MAXCONTACTS == F_Internal_Penetrate( ptgPacket, tvD0, tgSP0,tvK0,tyDistSq ) ) { return (TgE_PREPENETRATION_MAXCONTACTS); }; return (TgE_PREPENETRATION); }; TYPE tyExtent; C_(VECTOR,DIM) tvUD0 = MATH::F_NORM( &tyExtent, tvD0 ); T_(VECTOR,DIM) tvB0, tvB1; T_(TUBE,DIM) tgTB0; T_(CONTACT_PACKET,DIM) tgTMP_Packet; T_(CONTACT,DIM) atgTMP_Contact[2]; tyExtent *= TYPE(0.5); MATH::F_INIT_Basis_From_Vector( &tvB0, &tvB1, tvUD0 ); tgTB0.Set( tvB0, tvUD0, tvB1, tvS0, tyExtent, tgSP0.Query_Radius() ); tgTMP_Packet.m_ptgContact = atgTMP_Contact; tgTMP_Packet.m_niContact = 0; tgTMP_Packet.m_niMaxContact = 2; tgTMP_Packet.m_iStride = sizeof( T_(CONTACT,DIM) ); C_TgRESULT tgResult = TTgINT_TULN<TYPE,DIM,bC0,bC1,1,1>::DO( &tgTMP_Packet, TYPE(0.0),tgTB0, tgSP0.Query_Origin(),tgDT.m_tvDT ); if (TgFAILED(tgResult)) { return (tgResult); }; TYPE tyMin = LIMITS<TYPE>::MAX; TgINT iMin = -1; for (TgINT iIdx = 0; iIdx < tgTMP_Packet.m_niContact; ++iIdx) { if (atgTMP_Contact[iIdx].m_tyT0 < tyMin) { tyMin = atgTMP_Contact[iMin].m_tyT0; iMin = iIdx; }; }; if (tyMin > *ptyPM + ptgPacket->m_tySweepTol) { return (TgE_NOINTERSECT); }; if (tyMin < *ptyPM - ptgPacket->m_tySweepTol) { ptgPacket->m_niContact = 0; *ptyPM = tyMin; }; P_(CONTACT,DIM) ptgContact; ptgContact = (P_(CONTACT,DIM))((PC_TgUINT08)ptgPacket->m_ptgContact + ptgPacket->m_niContact*ptgPacket->m_iStride); C_(VECTOR,DIM) tvK0 = MATH::F_MUL( tgSP0.Query_Radius(), atgTMP_Contact[iMin].m_tvNormal ); ptgContact->m_tvPos = MATH::F_SUB( atgTMP_Contact[iMin].m_tvPos, tvK0 ); ptgContact->m_tvNormal = atgTMP_Contact[iMin].m_tvNormal; ptgContact->m_tyT0 = atgTMP_Contact[iMin].m_tyT0; ptgContact->m_tyDepth = TYPE(0.0); TgASSERT(atgTMP_Contact[iMin].m_tyDepth == TYPE(0.0)) ++ptgPacket->m_niContact; return (tgResult); }; // ============================================================================================================================== // template struct TTgINT_SPLN<TgFLOAT32,4,0,0>; template struct TTgINT_SPLN<TgFLOAT32,4,1,0>; template struct TTgINT_SPLN<TgFLOAT32,4,1,1>; template struct TTgSWP_SPLN<TgFLOAT32,4,0,0>; template struct TTgSWP_SPLN<TgFLOAT32,4,1,0>; template struct TTgSWP_SPLN<TgFLOAT32,4,1,1>; template struct TTgCLP_SPLN<TgFLOAT32,4,0,0>; template struct TTgCLP_SPLN<TgFLOAT32,4,1,0>; template struct TTgCLP_SPLN<TgFLOAT32,4,1,1>; template struct TTgINL_SPLN<TgFLOAT32,4,0,0>; template struct TTgINL_SPLN<TgFLOAT32,4,1,0>; template struct TTgINL_SPLN<TgFLOAT32,4,1,1>; // ============================================================================================================================== // }; // END COL ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// }; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////