// =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // // // Project: Talina Gaming System (TgS) (∂) // File: TgS Collision - Linear-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". // // =-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-==-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= // // The linear collision tests are grouped into three families. The first returns only the minimal distance, the second and third // additionally respectively return the points of closest contact, or the linear extrapolation values. In each family of // functions its possible to return either the distance or the distance squared. // ============================================================================================================================== // // // Segment Definition: G0(α) = P0 + α•D0 | α ε [ 0, 1] // Segment Definition: G1(β) = P1 + β•D1 | β ε [ 0, 1] // // Derivation: // Let the points of closest contact be Q0 = P0+γ•D0 and Q1 = P1+Φ•D1, and v = Q1-Q0 // Geometrically we know that the vector connecting the two closest points of contact ( or the minimal distance // vector ) must be perpendicular to both lines. Thus, D0•v=0, D1•v=0, DS=P1-P0, v=(P1+Φ•D1)-(P0+γ•D0) // // v = (P1+Φ•D1) - (P0+γ•D0) // = P1 + Φ•D1 - P0 - γ•D0 // = P1 - P0 + Φ•D1 - γ•D0 // = DS + Φ•D1 - γ•D0 // // 0 = D0_(DS + Φ•D1 - γ•D0,DIM) and 0 = D1_(DS + Φ•D1 - γ•D0,DIM) // 0 = (DS•D0) + Φ_(D0•D1,DIM) - γ_(D0•D0,DIM) and 0 = (DS•D1) + Φ_(D1•D1,DIM) - γ_(D0•D1,DIM) // γ = (DS•D0)/(D0•D0) + Φ_(D0•D1,DIM)/(D0•D0) and γ = (DS•D1)/(D0•D1) + Φ_(D1•D1,DIM)/(D0•D1) // Φ = γ_(D0•D0,DIM)/(D0•D1) - (DS•D0)/(D0•D1) and Φ = γ_(D0•D1,DIM)/(D1•D1) - (DS•D1)/(D1•D1) // // Two equations, two unknowns - solving for the gamma and theta yields: // // 0 = γ_(D0•D0,DIM)T_(D1•D1,DIM) - (DS•D0)T_(D1•D1,DIM) - γ_(D0•D1,DIM)T_(D0•D1,DIM) + (DS•D1)T_(D0•D1,DIM) // 0 = γ_((D0•D0,DIM)T_(D1•D1,DIM) - (D0•D1)T_(D0•D1,DIM)) + (DS•D1)T_(D0•D1,DIM) - (DS•D0)T_(D1•D1,DIM) // γ = ((DS•D0)T_(D1•D1,DIM) - (DS•D1)T_(D0•D1,DIM)) / ((D0•D0)T_(D1•D1,DIM) - (D0•D1)T_(D0•D1,DIM)) // // 0 = Φ_(D1•D1,DIM)T_(D0•D0,DIM) + (DS•D1)T_(D0•D0,DIM) - Φ_(D0•D1,DIM)T_(D0•D1,DIM) - (DS•D0)T_(D0•D1,DIM) // 0 = Φ_((D1•D1,DIM)T_(D0•D0,DIM) - (D0•D1)T_(D0•D1,DIM)) + (DS•D1)T_(D0•D0,DIM) - (DS•D0)T_(D0•D1,DIM) // Φ = ((DS•D0)T_(D0•D1,DIM) - (DS•D1)T_(D0•D0,DIM)) / ((D0•D0)T_(D1•D1,DIM) - (D0•D1)T_(D0•D1,DIM)) // // If ((D0•D0)T_(D1•D1,DIM) - (D0•D1)T_(D0•D1,DIM)) < ε, the lines are parallel // // However, we know that γ ε [ 0, 1], Φ ε [ 0, 1], generating nine cases: // // [1] γ ε (-∞, 0) | Φ ε (-∞, 0) // [A] DS•D0 ε ( 0, D0•D0) || γ = DS•D0 / D0•D0 | Φ = 0 // = (P1 - P0 - γ•D0)T_(P1 - P0 - γ•D0,DIM) // = (DS - γ•D0)T_(DS - γ•D0,DIM) // = (DS•DS) - 2•γ_(DS•D0,DIM) + γ•γ_(D0•D0,DIM) // = (DS•DS) + 2_(DS•D0 / D0•D0,DIM)T_(DS•D0,DIM) + (DS•D0 / D0•D0)T_(DS•D0 / D0•D0,DIM)T_(D0•D0,DIM) // = (DS•DS) - (DS•D0)T_(DS•D0,DIM) / (D0•D0) // = (DS•DS) - γ_(DS•D0,DIM) // [B] DS•D0 ε ( D0•D0, ∞) || γ = 1 | Φ = 0 // = (P1 - P0 - D0)T_(P1 - P0 - D0,DIM) // = (DS - D0)T_(DS - D0,DIM) // = (DS•DS) - 2_(DS•D0,DIM) + (D0•D0) // [C] DS•D0 ε (-∞, 0), DS•D1 ε [ 0, ∞) || γ = 0 | Φ = 0 // = (P1 - P0)T_(P1 - P0,DIM) // = (DS•DS) // [D] DS•D0 ε (-∞, 0), DS•D1 ε (-DS•D1, 0) || γ = 0 | Φ = -DS•D1 / D1•D1 // = (P1 + Φ•D1 - P0)T_(P1 + Φ•D1 - P0,DIM) // = (DS + Φ•D1)T_(DS + Φ•D1,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ_(-DS•D1 / D1•D1,DIM)T_(D1•D1,DIM) // = (DS•DS) + Φ_(DS•D1,DIM) // [E] DS•D0 ε (-∞, 0), DS•D1 ε (-∞,-D1•D1] || γ = 0 | Φ = 1 // = (P1 + D1 - P0)T_(P1 + D1 - P0,DIM) // = (DS + D1)T_(DS + D1,DIM) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) // // [2] γ ε (-∞, 0) | Φ ε [ 0, 1] // [A] DS•D1 ε [ 0, ∞) || γ = 0 | Φ = 0 // Same as [1C] // [B] DS•D1 ε (-D1•D1, 0) || γ = 0 | Φ = -DS•D1 / D1•D1 // Same as [1D] // [C] DS•D1 ε (-∞,-D1•D1) || γ = 0 | Φ = 1 // Same as [1E] // // [3] γ ε (-∞, 0) | Φ ε ( 1, ∞) // [A] DS•D0 ε [D0•D0 - D0•D1, ∞) || γ = 1 | Φ = 1 // = (P1 + D1 - P0 - D0)T_(P1 + D1 - P0 - D0,DIM) // = (DS + D1 - D0)T_(DS + D1 - D0,DIM) // = (DS•DS) + (DS•D1) - (DS•D0) + (DS•D1) + (D1•D1) - (D0•D1) - (DS•D0) - (D0•D1) + (D0•D0) // = (DS•DS) + 2_(DS•D1,DIM) - 2_(D0•D1,DIM) - 2_(DS•D0,DIM) + (D0•D0) + (D1•D1) // [B] DS•D0 ε (-D0•D1, D0•D0 - D0•D1) || γ = ((D0•D1) + (DS•D0)) / (D0•D0) | Φ = 1 // = (P1 + D1 - P0 - γ•D0)T_(P1 + D1 - P0 - γ•D0,DIM) // = (DS + D1 - γ•D0)T_(DS + D1 - γ•D0,DIM) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) - 2•γ_(D0•D1,DIM) - 2•γ_(DS•D0,DIM) + γ•γ_(D0•D0,DIM) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) - γ_(2•(D0•D1,DIM) + 2_(DS•D0,DIM) - γ_(D0•D0,DIM)) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) - γ_(2•(D0•D1,DIM) + 2_(DS•D0,DIM) - (D0•D1) - (DS•D0)) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) - γ_((D0•D1,DIM) + (DS•D0)) // = (DS•DS) + 2_(DS•D1,DIM) + (D1•D1) - γ•γ_(D0•D0,DIM) // [C] DS•D0 ε (-∞,-D0•D1) | DS•D1 ε ( 0, ∞) || γ = 0 | Φ = 0 // Same as [1C] // [D] DS•D0 ε (-∞,-D0•D1) | DS•D1 ε (-D1•D1, 0) || γ = 0 | Φ = -DS•D1 / D1•D1 // Same as [1D] // [E] DS•D0 ε (-∞,-D0•D1) | DS•D1 ε (-∞,-D1•D1) || γ = 0 | Φ = 1 // Same as [1E] // // [4] γ ε [ 0, 1] | Φ ε (-∞, 0) // [A] DS•D0 ε (-∞, 0) || γ = 0 | Φ = 0 // Same as [1C] // [B] DS•D0 ε [ 0, D0•D0] || γ = DS•D0 / D0•D0 | Φ = 0 // Same as [1A] // [C] DS•D0 ε ( D0•D0, ∞) || γ = 1 | Φ = 0 // Same as [1B] // // [5] γ ε [ 0, 1] | Φ ε [ 0, 1] // Distance: The distance value would be || v ||. // = || v || = v•v = (P1 + Φ•D1 - P0 - γ•D0)T_(P1 + Φ•D1 - P0 - γ•D0,DIM) // = (DS + Φ•D1 - γ•D0)T_(DS + Φ•D1 - γ•D0,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) - 2•γ_(DS•D0,DIM) + Φ•Φ_(D1•D1,DIM) - 2•γ•Φ_(D0•D1,DIM) + γ•γ_(D0•D0,DIM) // = (DS•DS) + Φ_(Φ•(D1•D1,DIM) + 2_(DS•D1,DIM)) + γ_(γ•(D0•D0,DIM) - 2_(DS•D0,DIM)) - 2•γ•Φ_(D0•D1,DIM) // // [6] γ ε [ 0, 1] | Φ ε ( 1, ∞) // [A] DS•D0 ε [D0•D0 - D0•D1, ∞) || γ = 1 | Φ = 1 // Same as [3A] // [B] DS•D0 ε (-D0•D1, D0•D0 - D0•D1) || γ = ((D0•D1) + (DS•D0)) / (D0•D0) | Φ = 1 // Same as [3B] // [C] DS•D0 ε (-∞,-D0•D1) || γ = 0 | Φ = 1 // Same as [1E] // // [7] γ ε ( 1, ∞) | Φ ε (-∞, 0) // [A] DS•D0 ε (-∞, 0) || γ = 0 | Φ = 0 // Same as [1C] // [B] DS•D0 ε [ 0, D0•D0) || γ = DS•D0 / D0•D0 | Φ = 0 // Same as [1A] // [C] DS•D0 ε ( D0•D0, ∞), DS•D1 ε ( D0•D1, ∞) || γ = 1 | Φ = 0 // Same as [1B] // [D] DS•D0 ε ( D0•D0, ∞), DS•D1 ε ( D0•D1 - D1•D1, D0•D1) || γ = 1 | Φ = ((D0•D1) - (DS•D1)) / (D1•D1) // = (P1 + Φ•D1 - P0 - D0)T_(P1 + Φ•D1 - P0 - D0,DIM) // = (DS + Φ•D1 - D0)T_(DS + Φ•D1 - D0,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) - 2•Φ_(D0•D1,DIM) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) + Φ_(2•(DS•D1,DIM) - 2_(D0•D1,DIM) + Φ_(D1•D1,DIM)) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) + Φ_((DS•D1,DIM) - (D0•D1)) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) - Φ•Φ_(D1•D1,DIM) - 2_(DS•D0,DIM) + (D0•D0) // [E] DS•D0 ε ( D0•D0, ∞), DS•D1 ε (-∞, D0•D1 - D1•D1) || γ = 1 | Φ = 1 // Same as [3A] // // [8] γ ε ( 1, ∞) | Φ ε [ 0, 1] // [A] DS•D1 ε ( D0•D1, ∞) || γ = 1 | Φ = 0 // Same as [1B] // [B] DS•D1 ε ( D0•D1 - D1•D1, D0•D1) || γ = 1 | Φ = ((D0•D1) - (DS•D1)) / (D1•D1) // Same as [7D] // [C] DS•D1 ε (-∞, D0•D1 - D1•D1) || γ = 1 | Φ = 1 // Same as [3A] // // [9] γ ε ( 1, ∞) | Φ ε ( 1, ∞) // [A] DS•D0 ε [-D0•D1, D0•D0-D0•D1] || γ = ((D0•D1) + (DS•D0)) / (D0•D0) | Φ = 1 // Same as [3B] // [B] DS•D0 ε (-∞,-D0•D1) || γ = 0 | Φ = 1 // Same as [1E] // [C] DS•D0 ε ( D0•D0-D0•D1, ∞), DS•D1 ε ( D0•D1, ∞) || γ = 1 | Φ = 0 // Same as [1B] // [D] DS•D0 ε ( D0•D0-D0•D1, ∞), DS•D1 ε ( D0•D1 - D1•D1, D0•D1) || γ = 1 | Φ = ((D0•D1) - (DS•D1)) / (D1•D1) // Same as [7D] // [E] DS•D0 ε ( D0•D0-D0•D1, ∞), DS•D1 ε (-∞, D0•D1 - D1•D1) || γ = 1 | Φ = 1 // Same as [3A] // // -- Parallel Segments -- // // [1] D0•D1 ε (-∞, 0) // [A] DS•D0 ε ( D0•D0-D0•D1, ∞) || γ = 1 | Φ = 1 // Same as [3A] // [B] DS•D0 ε ( D0•D0, D0•D0-D0•D1] || γ = 1 | Φ = ((D0•D0) - (DS•D0)) / (D0•D1) // = (P1 + Φ•D1 - P0 - D0)T_(P1 + Φ•D1 - P0 - D0,DIM) // = (DS + Φ•D1 - D0)T_(DS + Φ•D1 - D0,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) - 2•Φ_(D0•D1,DIM) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) - 2_((D0•D0,DIM) - (DS•D0)) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) - 2_(D0•D0,DIM) + 2_(DS•D0,DIM) - 2_(DS•D0,DIM) + (D0•D0) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) - (D0•D0) // [C] DS•D0 ε ( 0, D0•D0] || γ = DS•D0 / D0•D0 | Φ = 0 // Same as [1A] // [D] DS•D0 ε (-∞, 0) || γ = 0 | Φ = 0 // Same as [1C] // // [2] D0•D1 ε [ 0, ∞) // [A] DS•D0 ε [ D0•D0, ∞) || γ = 1 | Φ = 0 // Same as [1B] // [C] DS•D0 ε (-D0•D1, D0•D0) || γ = 0 | Φ = -DS•D0 / D0•D1 // = (P1 + Φ•D1 - P0)T_(P1 + Φ•D1 - P0,DIM) // = (DS + Φ•D1)T_(DS + Φ•D1,DIM) // = (DS•DS) + 2•Φ_(DS•D1,DIM) + Φ•Φ_(D1•D1,DIM) // [D] DS•D0 ε (-∞,-D0•D1] || γ = 0 | Φ = 1 // Same as [1E] // ============================================================================================================================== // namespace TGS { // START TGS /////////////////////////////////////////////////////////////////////////////////////////////////////// namespace COL { // START COL /////////////////////////////////////////////////////////////////////////////////////////////////////// // ============================================================================================================================== // // ---- TTgCSQ_LNLN ------------------------------------------------------------------------------------------------------------- // // -- Internal Function -- bC0, bC1, bC2, bC3 : Indicate the termination condition of the linear {bc0,bC1}, {bC2,bC3} // // Input: tvS0,tvD0: Origin and Direction for Linear #1 // Input: tvS1,tvD1: Origin and Direction for Linear #2 // Output: _tyT0, _tyT1: Parametric parameter defining points of minimal distance on linear #1, and linear #2 respectively // Return: Minimal distance between the two linears or negative type max if they intersect. // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM, bool bC0, bool bC1, bool bC2, bool bC3> TYPE TTgCSQ_LNLN<TYPE,DIM,bC0,bC1,bC2,bC3>::DO( TYPE *ptyT0, TYPE *ptyT1, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0, M_(VECTOR,DIM) tvS1, M_(VECTOR,DIM) tvD1 ) { TgASSERT(MATH::F_Is_Point_Valid(tvS0) && MATH::F_Is_Vector_Valid(tvD0)) TgASSERT(MATH::F_Is_Point_Valid(tvS1) && MATH::F_Is_Vector_Valid(tvD1)) C_(VECTOR,DIM) tvDS = MATH::F_SUB( tvS1, tvS0 ); const TYPE tyD0_D0 = MATH::F_LSQ( tvD0 ); const TYPE tyD1_D1 = MATH::F_LSQ( tvD1 ); const TYPE tyD0_D1 = MATH::F_DOT( tvD0, tvD1 ); const TYPE tyDS_D0 = MATH::F_DOT( tvDS, tvD0 ); const TYPE tyDS_D1 = -MATH::F_DOT( tvDS, tvD1 ); const TYPE tyDS_DS = MATH::F_LSQ( tvDS ); const TYPE tyDet = tyD0_D0*tyD1_D1 - tyD0_D1*tyD0_D1; TYPE tyT0,tyT1; if (tyDet > LIMITS<TYPE>::EPSILON*tyD0_D0*tyD1_D1) { tyT0 = tyDS_D0*tyD1_D1 + tyDS_D1*tyD0_D1; tyT1 = tyDS_D0*tyD0_D1 + tyDS_D1*tyD0_D0; if (bC0 && tyT0 < TYPE(0.0)) { // == Negative region for Segment 0 ======================================================================= if (bC2 && tyT1 < TYPE(0.0)) { // -- Negative region for Segment 1 ------------------------ tyT0 = INZ_Limit(tyDS_D0, tyD0_D0); tyT1 = tyDS_D0 > TYPE(0.0) ? TYPE(0.0) : INZ_Limit(tyDS_D1, tyD1_D1); } else if (!bC3 || tyT1 <= tyDet) { // -- Interior region for Segment 1 ------------------------ tyT0 = TYPE(0.0); tyT1 = INZ_Limit(tyDS_D1, tyD1_D1); } else { // -- Positive region for Segment 1 ------------------------ const TYPE tyDE_D0 = MATH::F_DOT( tvD0, MATH::F_ADD( MATH::F_SUB( tvS1, tvS0 ), tvD1 ) ); tyT0 = INZ_Limit(tyDE_D0, tyD0_D0); tyT1 = tyDE_D0 > TYPE(0.0) ? TYPE(1.0) : INZ_Limit(tyDS_D1, tyD1_D1); }; } else if (!bC1 || tyT0 <= tyDet) { // == Interior region for Segment 0 ======================================================================= if (bC2 && tyT1 < TYPE(0.0)) { // -- Negative region for Segment 1 ------------------------ tyT0 = INZ_Limit(tyDS_D0, tyD0_D0); tyT1 = TYPE(0.0); } else if (!bC3 || tyT1 <= tyDet) { // -- Interior region for Segment 1 ------------------------ tyT0 = tyT0 / tyDet; tyT1 = tyT1 / tyDet; } else { // -- Positive region for Segment 1 ------------------------ const TYPE tyDE_D0 = MATH::F_DOT( tvD0, MATH::F_ADD( MATH::F_SUB( tvS1, tvS0 ), tvD1 ) ); tyT0 = INZ_Limit(tyDE_D0, tyD0_D0); tyT1 = TYPE(1.0); }; } else { // == Positive region for Segment 0 ======================================================================= const TYPE tyDF_D1 = -MATH::F_DOT( tvD1, MATH::F_SUB( MATH::F_SUB( tvS1, tvS0 ), tvD0 ) ); if (bC2 && tyT1 < TYPE(0.0)) { // -- Negative region for Segment 1 ------------------------ tyT0 = INZ_Limit(tyDS_D0, tyD0_D0); tyT1 = tyDS_D0 > tyD0_D0 ? TYPE(0.0) : INZ_Limit(tyDF_D1, tyD1_D1); } else if (!bC3 || tyT1 <= tyDet) { // -- Interior region for Segment 1 ------------------------ tyT0 = TYPE(1.0); tyT1 = INZ_Limit(tyDF_D1, tyD1_D1); } else { // -- Positive region for Segment 1 ------------------------ const TYPE tyDE_D0 = MATH::F_DOT( tvD0, MATH::F_ADD( MATH::F_SUB( tvS1, tvS0 ), tvD1 ) ); tyT0 = INZ_Limit(tyDE_D0, tyD0_D0); tyT1 = tyDE_D0 <= tyD0_D0 ? TYPE(1.0) : INZ_Limit(tyDF_D1, tyD1_D1); }; }; } else { // Linears are parallel const TYPE tyDE_D0 = tyDS_D0 + tyD0_D1; const TYPE tyDF_D1 = tyD0_D1 - tyDS_D1; TYPE tyTest, tyDSSQ = LIMITS<TYPE>::MAX; tyT0 = LIMITS<TYPE>::MAX; tyT1 = LIMITS<TYPE>::MAX; if (tyD0_D1 >= TYPE(0.0)) { // Both segments have the same direction if (bC0 && bC3 && ((tyDS_D0 < TYPE(0.0)) || (tyDS_D1 > tyD1_D1))) { // First segment origin beyond second segment termination. // Second segment termination lies behind the first segment. tyTest = tyDS_DS - TYPE(2.0)*tyDS_D1 + tyD1_D1; if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = TYPE(0.0); tyT1 = TYPE(1.0); }; }; if (bC1 && bC2 && ((tyDS_D0 > tyD0_D0) || ((-tyDS_D1) > tyD0_D1))) { // First segment termination behind the second segment. // Second segment origin beyond the first segment. tyTest = tyDS_DS - TYPE(2.0)*tyDS_D0 + tyD0_D0; if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = TYPE(1.0); tyT1 = TYPE(0.0); }; }; } else { // The segments have opposing direction. if (bC1 && bC3 && ((tyDF_D1 > tyD1_D1) || (tyDE_D0 > tyD0_D0))) { // First segment termination lies beyond the second segment termination. tyTest = MATH::F_LSQ( MATH::F_ADD( MATH::F_SUB( tvDS, tvD0 ), tvD1 )); if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = TYPE(1.0); tyT1 = TYPE(1.0); }; }; if (bC0 && bC2 && ((tyDS_D0 < TYPE(0.0)) || (tyDS_D1 < TYPE(0.0)))) { // First segment origin lies behind the second segment origin. if (tyDS_DS < tyDSSQ) { tyDSSQ = tyDS_DS; tyT0 = TYPE(0.0); tyT1 = TYPE(0.0); }; }; }; if ((!bC2 || TYPE(0.0) < tyDS_D1) && (!bC3 || tyDS_D1 < tyD1_D1)) { // First segment origin inside second segment. const TYPE tyTMP = tyDS_D1 / tyD1_D1; tyTest = tyDS_DS - tyDS_D1*tyTMP; if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = TYPE(0.0); tyT1 = tyTMP; }; }; if (bC1 && (!bC2 || TYPE(0.0) < tyDF_D1) && (!bC3 || tyDF_D1 < tyD1_D1)) { // First segment termination inside second segment. const TYPE tyTMP = tyDF_D1 / tyD1_D1; tyTest = MATH::F_LSQ( MATH::F_SUB( MATH::F_SUB( tvD0, tvDS ), MATH::F_MUL( tyTMP, tvD1 ) ) ); if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = TYPE(1.0); tyT1 = tyTMP; }; }; if ((!bC0 || TYPE(0.0) <= tyDS_D0) && (!bC1 || tyDS_D0 <= tyD0_D0)) { // Second segment origin inside first segment. const TYPE tyTMP = tyDS_D0 / tyD0_D0; tyTest = tyDS_DS - tyDS_D0*tyTMP; if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = tyTMP; tyT1 = TYPE(0.0); }; }; if (bC3 && (!bC0 || TYPE(0.0) < tyDE_D0) && (!bC1 || tyDE_D0 <= tyD0_D0)) { // Second segment termination inside first segment. const TYPE tyTMP = tyDE_D0 / tyD0_D0; tyTest = MATH::F_LSQ( MATH::F_SUB( MATH::F_ADD( tvDS, tvD1 ), MATH::F_MUL( tyTMP, tvD0 ) ) ); if (tyTest < tyDSSQ) { tyDSSQ = tyTest; tyT0 = tyTMP; tyT1 = TYPE(1.0); }; }; TgASSERT(tyT0 < LIMITS<TYPE>::MAX && tyT1 < LIMITS<TYPE>::MAX) }; *ptyT0 = tyT0; *ptyT1 = tyT1; return (MATH::F_LSQ( MATH::F_ADD( MATH::F_SUB( tvDS, MATH::F_MUL( tyT0, tvD0 ) ), MATH::F_MUL( tyT1, tvD1 ) ) )); }; // ============================================================================================================================== // // ---- F_Internal_Intersect ---------------------------------------------------------------------------------------------------- // // -- Internal Function -- Used in contact generation, for multiple point returns on parallel case. // // Assume: The two segements are in contact. // // Input: tvS0,tvD0: Origin and Direction for Segment #1 // Input: tvS1,tvD1: Origin and Direction for Segment #2 // Output: tvRN0, tvRN1: Points of contact between the two segments. // Return: The number of contacts between the two segments, where a negative value represents an error condition. // // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgINT F_Internal_Intersect( PC_(VECTOR,DIM) ptvRN0, PC_(VECTOR,DIM) ptvRN1, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0, M_(VECTOR,DIM) tvS1, M_(VECTOR,DIM) tvD1 ) { C_(VECTOR,DIM) tvDS = MATH::F_SUB( tvS1, tvS0 ); const TYPE tyD0_D0 = MATH::F_LSQ( tvD0 ); const TYPE tyD1_D1 = MATH::F_LSQ( tvD1 ); const TYPE tyD0_D1 = MATH::F_DOT( tvD0, tvD1 ); const TYPE tyDS_D0 = MATH::F_DOT( tvDS, tvD0 ); const TYPE tyDS_D1 = -MATH::F_DOT( tvDS, tvD1 ); const TYPE tyDet = tyD0_D0*tyD1_D1 - tyD0_D1*tyD0_D1; C_(VECTOR,DIM) tvOrgSum = MATH::F_ADD( tvS0, tvS1 ); if (tyDet > LIMITS<TYPE>::EPSILON*tyD0_D0*tyD1_D1) { const TYPE tyT0 = (tyDS_D0*tyD1_D1 + tyDS_D1*tyD0_D1) / tyDet; const TYPE tyT1 = (tyDS_D0*tyD0_D1 + tyDS_D1*tyD0_D0) / tyDet; if ((tyT0 < TYPE(0.0)) || (tyT0 > TYPE(1.0)) || (tyT1 < TYPE(0.0)) || (tyT1 > TYPE(1.0))) { COUT_NOOBJ( ERROR, TgT("%-16.16s(%-48.48s): Edge-Edge outside of range.\n"), TgT("Collision"), TgT("F_Internal_Intersect") ); }; C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT1, tvD1 ); *ptvRN0 = MATH::F_MUL( TYPE(0.5), MATH::F_ADD( MATH::F_ADD( tvOrgSum, MATH::F_MUL( tyT0, tvD0 ) ), tvK1 ) ); return (1); }; // Linears are parallel const TYPE tyDE_D0 = tyDS_D0 + tyD0_D1; //« tvDE = tvS1+tvD1, tyDE_D1 = (tvS1+tvD1 - tvS0)•tvD0 const TYPE tyDF_D1 = tyD0_D1 - tyDS_D1; //« tvDF = tvS0+tvD0, tyDF_D1 = (tvS0+tvD0 - tvS1)•tvD1 const TYPE tyTA = tyDS_D0 / tyD0_D0; const TYPE tyTC = tyDE_D0 / tyD0_D0; // 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_D1, P::FSEL( tyD1_D1 - tyDS_D1, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT0 = P::FSEL( tyF0, TYPE(0.0), P::FSEL( tyD0_D1, tyTA, tyTC ) ); const TYPE tyT1 = P::FSEL( tyF0, (tyDS_D1 / tyD1_D1), P::FSEL( tyD0_D1, 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_D1, P::FSEL( tyD1_D1 - tyDF_D1, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT2 = P::FSEL( tyF1, TYPE(1.0), P::FSEL( tyD0_D1, tyTC, tyTA ) ); const TYPE tyT3 = P::FSEL( tyF1, (tyDF_D1 / tyD1_D1), P::FSEL( tyD0_D1, TYPE(1.0), TYPE(0.0) ) ); C_(VECTOR,DIM) tvK0 = MATH::F_MUL( tyT0, tvD0 ); C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT1, tvD1 ); C_(VECTOR,DIM) tvK2 = MATH::F_MUL( tyT2, tvD0 ); C_(VECTOR,DIM) tvK3 = MATH::F_MUL( tyT3, tvD1 ); *ptvRN0 = MATH::F_MUL( TYPE(0.5), MATH::F_ADD( MATH::F_ADD( tvOrgSum, tvK0 ), tvK1 ) ); *ptvRN1 = MATH::F_MUL( TYPE(0.5), MATH::F_ADD( MATH::F_ADD( tvOrgSum, tvK2 ), tvK3 ) ); if ( (tyD0_D1 >= TYPE(0.0) && (tyDE_D0 < TYPE(0.0) || tyDS_D0 > TYPE(1.0))) || (tyD0_D1 <= TYPE(0.0) && (tyDS_D0 < TYPE(0.0) || tyDE_D0 > TYPE(1.0))) ) { return (-1); }; return (2); }; template TgINT F_Internal_Intersect( PC_TgF4VECTOR,PC_TgF4VECTOR, M_TgF4VECTOR,M_TgF4VECTOR, M_TgF4VECTOR,M_TgF4VECTOR ); // ---- F_Internal_Parallel ----------------------------------------------------------------------------------------------------- // // -- Internal Function -- Used in contact generation, for multiple point returns on parallel case. // // Assume: The two segments are in contact, and are parallel. // // Input: tvS0,tvD0: Origin and Direction for Segment #1 // Input: tvS1,tvD1: Origin and Direction for Segment #2 // Output: tvRN0, tvRN1: Points of contact between the two segments. // Return: The number of contacts between the two segments, where a negative value represents an error condition. // // ------------------------------------------------------------------------------------------------------------------------------ // template <typename TYPE, int DIM> TgINT F_Internal_Parallel( PC_(VECTOR,DIM) ptvRN0, PC_(VECTOR,DIM) ptvRN1, M_(VECTOR,DIM) tvS0, M_(VECTOR,DIM) tvD0, M_(VECTOR,DIM) tvS1, M_(VECTOR,DIM) tvD1 ) { const TYPE tyD0_D0 = MATH::F_LSQ( tvD0 ), tyD1_D1 = MATH::F_LSQ( tvD1 ); C_(VECTOR,DIM) tvDS = MATH::F_SUB( tvS1, tvS0 ); // Projection Values const TYPE tyD0_D1 = MATH::F_DOT( tvD0, tvD1 ); const TYPE tyDS_D0 = MATH::F_DOT( tvDS, tvD0 ); const TYPE tyDS_D1 =-MATH::F_DOT( tvDS, tvD1 ); const TYPE tyDE_D0 = tyDS_D0 + tyD0_D1; //« tvDE = tvS1+tvD1, tyDE_D1 = (tvS1+tvD1 - tvS0)•tvD0 const TYPE tyDF_D1 = tyD0_D1 - tyDS_D1; //« tvDF = tvS0+tvD0, tyDF_D1 = (tvS0+tvD0 - tvS1)•tvD1 const TYPE tyTA = tyDS_D0 / tyD0_D0; const TYPE tyTC = tyDE_D0 / tyD0_D0; // 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_D1, P::FSEL( tyD1_D1 - tyDS_D1, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT0 = P::FSEL( tyF0, TYPE(0.0), P::FSEL( tyD0_D1, tyTA, tyTC ) ); const TYPE tyT1 = P::FSEL( tyF0, (tyDS_D1 / tyD1_D1), P::FSEL( tyD0_D1, 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_D1, P::FSEL( tyD1_D1 - tyDF_D1, TYPE(1.0), TYPE(-1.0) ), TYPE(-1.0) ); const TYPE tyT2 = P::FSEL( tyF1, TYPE(1.0), P::FSEL( tyD0_D1, tyTC, tyTA ) ); const TYPE tyT3 = P::FSEL( tyF1, (tyDF_D1 / tyD1_D1), P::FSEL( tyD0_D1, TYPE(1.0), TYPE(0.0) ) ); C_(VECTOR,DIM) tvOrgSum = MATH::F_ADD( tvS0, tvS1 ); C_(VECTOR,DIM) tvK0 = MATH::F_MUL( tyT0, tvD0 ); C_(VECTOR,DIM) tvK1 = MATH::F_MUL( tyT1, tvD1 ); C_(VECTOR,DIM) tvK2 = MATH::F_MUL( tyT2, tvD0 ); C_(VECTOR,DIM) tvK3 = MATH::F_MUL( tyT3, tvD1 ); *ptvRN0 = MATH::F_MUL( TYPE(0.5), MATH::F_ADD( MATH::F_ADD( tvOrgSum, tvK0 ), tvK1 ) ); *ptvRN1 = MATH::F_MUL( TYPE(0.5), MATH::F_ADD( MATH::F_ADD( tvOrgSum, tvK2 ), tvK3 ) ); if ( (tyD0_D1 >= TYPE(0.0) && (tyDE_D0 < TYPE(0.0) || tyDS_D0 > TYPE(1.0))) || (tyD0_D1 <= TYPE(0.0) && (tyDS_D0 < TYPE(0.0) || tyDE_D0 > TYPE(1.0))) ) { return (-1); }; return (2); }; template TgINT F_Internal_Parallel( PC_TgF4VECTOR,PC_TgF4VECTOR, M_TgF4VECTOR,M_TgF4VECTOR, M_TgF4VECTOR,M_TgF4VECTOR ); // ============================================================================================================================== // template struct TTgFSQ_LNLN<TgFLOAT32,4,0,0,0,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,0,0,1,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,0,0,1,1>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,0,0,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,0,1,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,0,1,1>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,1,0,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,1,1,0>; template struct TTgFSQ_LNLN<TgFLOAT32,4,1,1,1,1>; template struct TTgCSQ_LNLN<TgFLOAT32,4,0,0,0,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,0,0,1,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,0,0,1,1>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,0,0,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,0,1,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,0,1,1>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,1,0,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,1,1,0>; template struct TTgCSQ_LNLN<TgFLOAT32,4,1,1,1,1>; template struct TTgTST_LNLN<TgFLOAT32,4,0,0,0,0>; template struct TTgTST_LNLN<TgFLOAT32,4,0,0,1,0>; template struct TTgTST_LNLN<TgFLOAT32,4,0,0,1,1>; template struct TTgTST_LNLN<TgFLOAT32,4,1,0,0,0>; template struct TTgTST_LNLN<TgFLOAT32,4,1,0,1,0>; template struct TTgTST_LNLN<TgFLOAT32,4,1,0,1,1>; template struct TTgTST_LNLN<TgFLOAT32,4,1,1,0,0>; template struct TTgTST_LNLN<TgFLOAT32,4,1,1,1,0>; template struct TTgTST_LNLN<TgFLOAT32,4,1,1,1,1>; // ============================================================================================================================== // }; // END COL ////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// }; // END TGS //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////