1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
|
/*
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* 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 "b2CircleShape.h"
b2CircleShape::b2CircleShape(const b2ShapeDef* def)
: b2Shape(def)
{
b2Assert(def->type == e_circleShape);
const b2CircleDef* circleDef = (const b2CircleDef*)def;
m_type = e_circleShape;
m_localPosition = circleDef->localPosition;
m_radius = circleDef->radius;
}
void b2CircleShape::UpdateSweepRadius(const b2Vec2& center)
{
// Update the sweep radius (maximum radius) as measured from
// a local center point.
b2Vec2 d = m_localPosition - center;
m_sweepRadius = d.Length() + m_radius - b2_toiSlop;
}
bool b2CircleShape::TestPoint(const b2XForm& transform, const b2Vec2& p) const
{
b2Vec2 center = transform.position + b2Mul(transform.R, m_localPosition);
b2Vec2 d = p - center;
return b2Dot(d, d) <= m_radius * m_radius;
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
bool b2CircleShape::TestSegment(const b2XForm& transform,
float32* lambda,
b2Vec2* normal,
const b2Segment& segment,
float32 maxLambda) const
{
b2Vec2 position = transform.position + b2Mul(transform.R, m_localPosition);
b2Vec2 s = segment.p1 - position;
float32 b = b2Dot(s, s) - m_radius * m_radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
return false;
}
// Solve quadratic equation.
b2Vec2 r = segment.p2 - segment.p1;
float32 c = b2Dot(s, r);
float32 rr = b2Dot(r, r);
float32 sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < B2_FLT_EPSILON)
{
return false;
}
// Find the point of intersection of the line with the circle.
float32 a = -(c + b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
*lambda = a;
*normal = s + a * r;
normal->Normalize();
return true;
}
return false;
}
void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2XForm& transform) const
{
b2Vec2 p = transform.position + b2Mul(transform.R, m_localPosition);
aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
}
void b2CircleShape::ComputeSweptAABB(b2AABB* aabb, const b2XForm& transform1, const b2XForm& transform2) const
{
b2Vec2 p1 = transform1.position + b2Mul(transform1.R, m_localPosition);
b2Vec2 p2 = transform2.position + b2Mul(transform2.R, m_localPosition);
b2Vec2 lower = b2Min(p1, p2);
b2Vec2 upper = b2Max(p1, p2);
aabb->lowerBound.Set(lower.x - m_radius, lower.y - m_radius);
aabb->upperBound.Set(upper.x + m_radius, upper.y + m_radius);
}
void b2CircleShape::ComputeMass(b2MassData* massData) const
{
massData->mass = m_density * b2_pi * m_radius * m_radius;
massData->center = m_localPosition;
// inertia about the local origin
massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_localPosition, m_localPosition));
}
|
