7304164687

Committed 22 Dec 2023 10:01PM UTC coverage: 99.292% (-0.001%) from 99.293%

Build # 7304164687

Build Type

push

github

Committed by

louis-langholtz

Commit Message

Restores use of original macro names

Run Details

11352 of 11433 relevant lines covered (99.29%)

16082123.74 hits per line

Source File
Press 'n' to go to next uncovered line, 'b' for previous

98.04

/Library/source/playrho/d2/Simplex.cpp

/*
 * Original work Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
 * Modified work Copyright (c) 2023 Louis Langholtz https://github.com/louis-langholtz/PlayRho
 *
 * This software is provided 'as-is', without any express or implied
 * warranty. In no event will the authors be held liable for any damages
 * arising from the use of this software.
 *
 * Permission is granted to anyone to use this software for any purpose,
 * including commercial applications, and to alter it and redistribute it
 * freely, subject to the following restrictions:
 *
 * 1. The origin of this software must not be misrepresented; you must not
 *    claim that you wrote the original software. If you use this software
 *    in a product, an acknowledgment in the product documentation would be
 *    appreciated but is not required.
 * 2. Altered source versions must be plainly marked as such, and must not be
 *    misrepresented as being the original software.
 * 3. This notice may not be removed or altered from any source distribution.
 */

#include <cassert> // for assert

#include <playrho/d2/Simplex.hpp>

namespace playrho {
namespace d2 {

IndexPair3 GetIndexPairs(const SimplexEdges& collection) noexcept
{
    auto list = IndexPair3{{InvalidIndexPair, InvalidIndexPair, InvalidIndexPair}};
    switch (size(collection)) {
    case 3:
        list[2] = collection[2].GetIndexPair();
        [[fallthrough]];
    case 2:
        list[1] = collection[1].GetIndexPair();
        [[fallthrough]];
    case 1:
        list[0] = collection[0].GetIndexPair(); // fall through
    }
    return list;
}

Length2 CalcSearchDirection(const SimplexEdges& simplexEdges) noexcept
{
    switch (size(simplexEdges)) {
    case 1:
        return -GetPointDelta(simplexEdges[0]);
    case 2: {
        const auto e12 = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);
        const auto e0 = GetPointDelta(simplexEdges[0]);
        const auto sgn = Cross(e12, -e0);
        // If sgn > 0, then origin is left of e12, else origin is right of e12.
        return (sgn > 0_m2) ? GetRevPerpendicular(e12) : GetFwdPerpendicular(e12);
    }
    default:
        break;
    }
    assert(size(simplexEdges) < 4);
    return Length2{0_m, 0_m};
}

Simplex Simplex::Get(const SimplexEdge& s0) noexcept
{
    return Simplex{{s0}, {1}};
}

Simplex Simplex::Get(const SimplexEdge& s0, const SimplexEdge& s1) noexcept
{
    assert(s0.GetIndexPair() != s1.GetIndexPair() || s0 == s1);

    // Solves the given line segment simplex using barycentric coordinates.
    //
    // p = a1 * w1 + a2 * w2
    // a1 + a2 = 1
    //
    // The vector from the origin to the closest point on the line is
    // perpendicular to the line.
    // e12 = w2 - w1
    // dot(p, e) = 0
    // a1 * dot(w1, e) + a2 * dot(w2, e) = 0
    //
    // 2-by-2 linear system
    // [1      1     ][a1] = [1]
    // [w1.e12 w2.e12][a2] = [0]
    //
    // Define
    // d12_1 =  dot(w2, e12)
    // d12_2 = -dot(w1, e12)
    // d12_sum = d12_1 + d12_2
    //
    // Solution
    // a1 = d12_1 / d12_sum
    // a2 = d12_2 / d12_sum

    const auto w1 = GetPointDelta(s0);
    const auto w2 = GetPointDelta(s1);
    const auto e12 = w2 - w1;

    // w1 region
    const auto d12_2 = -Dot(w1, e12);
    if (d12_2 <= 0_m2) {
        // a2 <= 0, so we clamp it to 0
        return Simplex{{s0}, {1}};
    }

    // w2 region
    const auto d12_1 = Dot(w2, e12);
    if (d12_1 <= 0_m2) {
        // a1 <= 0, so we clamp it to 0
        return Simplex{{s1}, {1}};
    }

    // Must be in e12 region.
    const auto inv_sum = Real{1} / (d12_1 + d12_2);
    return Simplex{{s0, s1}, {d12_1 * inv_sum, d12_2 * inv_sum}};
}

Simplex Simplex::Get(const SimplexEdge& s0, const SimplexEdge& s1, const SimplexEdge& s2) noexcept
{
    // Solves the given 3-edge simplex.
    //
    // Possible regions:
    // - points[2]
    // - edge points[0]-points[2]
    // - edge points[1]-points[2]
    // - inside the triangle

    const auto w1 = GetPointDelta(s0);
    const auto w2 = GetPointDelta(s1);
    const auto w3 = GetPointDelta(s2);

    // Edge12
    // [1      1     ][a1] = [1]
    // [w1.e12 w2.e12][a2] = [0]
    // a3 = 0
    const auto e12 = w2 - w1;
    const auto d12_1 = Dot(w2, e12);
    const auto d12_2 = -Dot(w1, e12);

    // Edge13
    // [1      1     ][a1] = [1]
    // [w1.e13 w3.e13][a3] = [0]
    // a2 = 0
    const auto e13 = w3 - w1;
    const auto d13_1 = Dot(w3, e13);
    const auto d13_2 = -Dot(w1, e13);

    // Edge23
    // [1      1     ][a2] = [1]
    // [w2.e23 w3.e23][a3] = [0]
    // a1 = 0
    const auto e23 = w3 - w2;
    const auto d23_1 = Dot(w3, e23);
    const auto d23_2 = -Dot(w2, e23);

    // w1 region
    if ((d12_2 <= 0_m2) && (d13_2 <= 0_m2)) {
        return Simplex{{s0}, {1}};
    }

    // w2 region
    if ((d12_1 <= 0_m2) && (d23_2 <= 0_m2)) {
        return Simplex{{s1}, {1}};
    }

    // w3 region
    if ((d13_1 <= 0_m2) && (d23_1 <= 0_m2)) {
        return Simplex{{s2}, {1}};
    }

    // Triangle123
    const auto n123 = Cross(e12, e13);

    // e12
    const auto cp_w1_w2 = Cross(w1, w2);
    const auto d123_3 = n123 * cp_w1_w2;
    if ((d12_1 > 0_m2) && (d12_2 > 0_m2) && (d123_3 <= 0 * SquareMeter * SquareMeter)) {
        const auto inv_sum = Real{1} / (d12_1 + d12_2);
        return Simplex{{s0, s1}, {d12_1 * inv_sum, d12_2 * inv_sum}};
    }

    // e13
    const auto cp_w3_w1 = Cross(w3, w1);
    const auto d123_2 = n123 * cp_w3_w1;
    if ((d13_1 > 0_m2) && (d13_2 > 0_m2) && (d123_2 <= 0 * SquareMeter * SquareMeter)) {
        const auto inv_sum = Real{1} / (d13_1 + d13_2);
        return Simplex{{s0, s2}, {d13_1 * inv_sum, d13_2 * inv_sum}};
    }

    // e23
    const auto cp_w2_w3 = Cross(w2, w3);
    const auto d123_1 = n123 * cp_w2_w3;
    if ((d23_1 > 0_m2) && (d23_2 > 0_m2) && (d123_1 <= 0 * SquareMeter * SquareMeter)) {
        const auto inv_sum = Real{1} / (d23_1 + d23_2);
        return Simplex{{s2, s1}, {d23_2 * inv_sum, d23_1 * inv_sum}};
    }

    // Must be in triangle123
    const auto inv_sum = Real{1} / (d123_1 + d123_2 + d123_3);
    return Simplex{{s0, s1, s2}, {d123_1 * inv_sum, d123_2 * inv_sum, d123_3 * inv_sum}};
}

Simplex Simplex::Get(const SimplexEdges& edges) noexcept
{
    const auto count = edges.size();
    switch (count) {
    case 1:
        return Get(edges[0]);
    case 2:
        return Get(edges[0], edges[1]);
    case 3:
        return Get(edges[0], edges[1], edges[2]);
    default:
        break; // should be zero in this case
    }
    return Simplex{};
}

Real Simplex::CalcMetric(const SimplexEdges& simplexEdges)
{
    assert(simplexEdges.size() < 4);
    switch (simplexEdges.size()) {
    case 1:
        return Real{0};
    case 2: {
        const auto delta = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);
        return StripUnit(GetMagnitude(delta)); // Length
    }
    case 3: {
        const auto delta10 = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);
        const auto delta20 = GetPointDelta(simplexEdges[2]) - GetPointDelta(simplexEdges[0]);
        return StripUnit(Cross(delta10, delta20)); // Area
    }
    default:
        break; // should be zero in this case
    }
    return Real{0};
}

} // namespace d2
} // namespace playrho

1	/*
2	* Original work Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
3	* Modified work Copyright (c) 2023 Louis Langholtz https://github.com/louis-langholtz/PlayRho
4	*
5	* This software is provided 'as-is', without any express or implied
6	* warranty. In no event will the authors be held liable for any damages
7	* arising from the use of this software.
8	*
9	* Permission is granted to anyone to use this software for any purpose,
10	* including commercial applications, and to alter it and redistribute it
11	* freely, subject to the following restrictions:
12	*
13	* 1. The origin of this software must not be misrepresented; you must not
14	* claim that you wrote the original software. If you use this software
15	* in a product, an acknowledgment in the product documentation would be
16	* appreciated but is not required.
17	* 2. Altered source versions must be plainly marked as such, and must not be
18	* misrepresented as being the original software.
19	* 3. This notice may not be removed or altered from any source distribution.
20	*/
21
22	#include <cassert> // for assert
23
24	#include <playrho/d2/Simplex.hpp>
25
26	namespace playrho {
27	namespace d2 {
28
29	IndexPair3 GetIndexPairs(const SimplexEdges& collection) noexcept	19,718,083✔
30	{
31	auto list = IndexPair3{{InvalidIndexPair, InvalidIndexPair, InvalidIndexPair}};	19,718,083✔
32	switch (size(collection)) {	19,718,083✔
33	case 3:	10,267,588✔
34	list[2] = collection[2].GetIndexPair();	10,267,588✔
35	[[fallthrough]];
36	case 2:	19,694,435✔
37	list[1] = collection[1].GetIndexPair();	19,694,435✔
38	[[fallthrough]];
39	case 1:	19,718,083✔
40	list[0] = collection[0].GetIndexPair(); // fall through	19,718,083✔
41	}
42	return list;	19,718,083✔
43	}
44
45	Length2 CalcSearchDirection(const SimplexEdges& simplexEdges) noexcept	27,328,063✔
46	{
47	switch (size(simplexEdges)) {	27,328,063✔
48	case 1:	11,399,551✔
49	return -GetPointDelta(simplexEdges[0]);	11,399,551✔
50	case 2: {	15,928,510✔
51	const auto e12 = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);	15,928,510✔
52	const auto e0 = GetPointDelta(simplexEdges[0]);	15,928,510✔
53	const auto sgn = Cross(e12, -e0);	15,928,510✔
54	// If sgn > 0, then origin is left of e12, else origin is right of e12.
55	return (sgn > 0_m2) ? GetRevPerpendicular(e12) : GetFwdPerpendicular(e12);	15,928,510✔
56	}
57	default:	2✔
58	break;	2✔
59	}
60	assert(size(simplexEdges) < 4);	2✔
61	return Length2{0_m, 0_m};	2✔
62	}
63
64	Simplex Simplex::Get(const SimplexEdge& s0) noexcept	11,205,818✔
65	{
66	return Simplex{{s0}, {1}};	11,205,818✔
67	}
68
69	Simplex Simplex::Get(const SimplexEdge& s0, const SimplexEdge& s1) noexcept	7,419,087✔
70	{
71	assert(s0.GetIndexPair() != s1.GetIndexPair() \|\| s0 == s1);	7,419,087✔
72
73	// Solves the given line segment simplex using barycentric coordinates.
74	//
75	// p = a1 * w1 + a2 * w2
76	// a1 + a2 = 1
77	//
78	// The vector from the origin to the closest point on the line is
79	// perpendicular to the line.
80	// e12 = w2 - w1
81	// dot(p, e) = 0
82	// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
83	//
84	// 2-by-2 linear system
85	// [1 1 ][a1] = [1]
86	// [w1.e12 w2.e12][a2] = [0]
87	//
88	// Define
89	// d12_1 = dot(w2, e12)
90	// d12_2 = -dot(w1, e12)
91	// d12_sum = d12_1 + d12_2
92	//
93	// Solution
94	// a1 = d12_1 / d12_sum
95	// a2 = d12_2 / d12_sum
96
97	const auto w1 = GetPointDelta(s0);	7,419,087✔
98	const auto w2 = GetPointDelta(s1);	7,419,087✔
99	const auto e12 = w2 - w1;	7,419,087✔
100
101	// w1 region
102	const auto d12_2 = -Dot(w1, e12);	7,419,087✔
103	if (d12_2 <= 0_m2) {	7,419,087✔
104	// a2 <= 0, so we clamp it to 0
105	return Simplex{{s0}, {1}};	66✔
106	}
107
108	// w2 region
109	const auto d12_1 = Dot(w2, e12);	7,419,021✔
110	if (d12_1 <= 0_m2) {	7,419,021✔
111	// a1 <= 0, so we clamp it to 0
112	return Simplex{{s1}, {1}};	193,623✔
113	}
114
115	// Must be in e12 region.
116	const auto inv_sum = Real{1} / (d12_1 + d12_2);	7,225,398✔
117	return Simplex{{s0, s1}, {d12_1 * inv_sum, d12_2 * inv_sum}};	7,225,398✔
118	}
119
120	Simplex Simplex::Get(const SimplexEdge& s0, const SimplexEdge& s1, const SimplexEdge& s2) noexcept	10,267,584✔
121	{
122	// Solves the given 3-edge simplex.
123	//
124	// Possible regions:
125	// - points[2]
126	// - edge points[0]-points[2]
127	// - edge points[1]-points[2]
128	// - inside the triangle
129
130	const auto w1 = GetPointDelta(s0);	10,267,584✔
131	const auto w2 = GetPointDelta(s1);	10,267,584✔
132	const auto w3 = GetPointDelta(s2);	10,267,584✔
133
134	// Edge12
135	// [1 1 ][a1] = [1]
136	// [w1.e12 w2.e12][a2] = [0]
137	// a3 = 0
138	const auto e12 = w2 - w1;	10,267,584✔
139	const auto d12_1 = Dot(w2, e12);	10,267,584✔
140	const auto d12_2 = -Dot(w1, e12);	10,267,584✔
141
142	// Edge13
143	// [1 1 ][a1] = [1]
144	// [w1.e13 w3.e13][a3] = [0]
145	// a2 = 0
146	const auto e13 = w3 - w1;	10,267,584✔
147	const auto d13_1 = Dot(w3, e13);	10,267,584✔
148	const auto d13_2 = -Dot(w1, e13);	10,267,584✔
149
150	// Edge23
151	// [1 1 ][a2] = [1]
152	// [w2.e23 w3.e23][a3] = [0]
153	// a1 = 0
154	const auto e23 = w3 - w2;	10,267,584✔
155	const auto d23_1 = Dot(w3, e23);	10,267,584✔
156	const auto d23_2 = -Dot(w2, e23);	10,267,584✔
157
158	// w1 region
159	if ((d12_2 <= 0_m2) && (d13_2 <= 0_m2)) {	10,267,584✔
160	return Simplex{{s0}, {1}};	×
161	}
162
163	// w2 region
164	if ((d12_1 <= 0_m2) && (d23_2 <= 0_m2)) {	10,267,584✔
165	return Simplex{{s1}, {1}};	×
166	}
167
168	// w3 region
169	if ((d13_1 <= 0_m2) && (d23_1 <= 0_m2)) {	10,267,584✔
170	return Simplex{{s2}, {1}};	50✔
171	}
172
173	// Triangle123
174	const auto n123 = Cross(e12, e13);	10,267,534✔
175
176	// e12
177	const auto cp_w1_w2 = Cross(w1, w2);	10,267,534✔
178	const auto d123_3 = n123 * cp_w1_w2;	10,267,534✔
179	if ((d12_1 > 0_m2) && (d12_2 > 0_m2) && (d123_3 <= 0 * SquareMeter * SquareMeter)) {	10,267,534✔
180	const auto inv_sum = Real{1} / (d12_1 + d12_2);	260,875✔
181	return Simplex{{s0, s1}, {d12_1 * inv_sum, d12_2 * inv_sum}};	260,875✔
182	}
183
184	// e13
185	const auto cp_w3_w1 = Cross(w3, w1);	10,006,659✔
186	const auto d123_2 = n123 * cp_w3_w1;	10,006,659✔
187	if ((d13_1 > 0_m2) && (d13_2 > 0_m2) && (d123_2 <= 0 * SquareMeter * SquareMeter)) {	10,006,659✔
188	const auto inv_sum = Real{1} / (d13_1 + d13_2);	3,961✔
189	return Simplex{{s0, s2}, {d13_1 * inv_sum, d13_2 * inv_sum}};	3,961✔
190	}
191
192	// e23
193	const auto cp_w2_w3 = Cross(w2, w3);	10,002,698✔
194	const auto d123_1 = n123 * cp_w2_w3;	10,002,698✔
195	if ((d23_1 > 0_m2) && (d23_2 > 0_m2) && (d123_1 <= 0 * SquareMeter * SquareMeter)) {	10,002,698✔
196	const auto inv_sum = Real{1} / (d23_1 + d23_2);	8,438,282✔
197	return Simplex{{s2, s1}, {d23_2 * inv_sum, d23_1 * inv_sum}};	8,438,282✔
198	}
199
200	// Must be in triangle123
201	const auto inv_sum = Real{1} / (d123_1 + d123_2 + d123_3);	1,564,416✔
202	return Simplex{{s0, s1, s2}, {d123_1 * inv_sum, d123_2 * inv_sum, d123_3 * inv_sum}};	1,564,416✔
203	}
204
205	Simplex Simplex::Get(const SimplexEdges& edges) noexcept	28,892,479✔
206	{
207	const auto count = edges.size();	28,892,479✔
208	switch (count) {	28,892,479✔
209	case 1:	11,205,816✔
210	return Get(edges[0]);	11,205,816✔
211	case 2:	7,419,077✔
212	return Get(edges[0], edges[1]);	7,419,077✔
213	case 3:	10,267,584✔
214	return Get(edges[0], edges[1], edges[2]);	10,267,584✔
215	default:	2✔
216	break; // should be zero in this case	2✔
217	}
218	return Simplex{};	2✔
219	}
220
221	Real Simplex::CalcMetric(const SimplexEdges& simplexEdges)	2,181,986✔
222	{
223	assert(simplexEdges.size() < 4);	2,181,986✔
224	switch (simplexEdges.size()) {	2,181,986✔
225	case 1:	23,646✔
226	return Real{0};	23,646✔
227	case 2: {	2,158,326✔
228	const auto delta = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);	2,158,326✔
229	return StripUnit(GetMagnitude(delta)); // Length	2,158,326✔
230	}
231	case 3: {	12✔
232	const auto delta10 = GetPointDelta(simplexEdges[1]) - GetPointDelta(simplexEdges[0]);	12✔
233	const auto delta20 = GetPointDelta(simplexEdges[2]) - GetPointDelta(simplexEdges[0]);	12✔
234	return StripUnit(Cross(delta10, delta20)); // Area	12✔
235	}
236	default:	2✔
237	break; // should be zero in this case	2✔
238	}
239	return Real{0};	2✔
240	}
241
242	} // namespace d2
243	} // namespace playrho

louis-langholtz / PlayRho / 7304164687

Source File Press 'n' to go to next uncovered line, 'b' for previous

Source File
Press 'n' to go to next uncovered line, 'b' for previous