git.maemo.org Git - blok/blob - Box2D/Source/Collision/Shapes/b2CircleShape.cpp

   1 /*
   2 * Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
   3 *
   4 * This software is provided 'as-is', without any express or implied
   5 * warranty.  In no event will the authors be held liable for any damages
   6 * arising from the use of this software.
   7 * Permission is granted to anyone to use this software for any purpose,
   8 * including commercial applications, and to alter it and redistribute it
   9 * freely, subject to the following restrictions:
  10 * 1. The origin of this software must not be misrepresented; you must not
  11 * claim that you wrote the original software. If you use this software
  12 * in a product, an acknowledgment in the product documentation would be
  13 * appreciated but is not required.
  14 * 2. Altered source versions must be plainly marked as such, and must not be
  15 * misrepresented as being the original software.
  16 * 3. This notice may not be removed or altered from any source distribution.
  17 */
  18
  19 #include "b2CircleShape.h"
  20
  21 b2CircleShape::b2CircleShape(const b2ShapeDef* def)
  22 : b2Shape(def)
  23 {
  24         b2Assert(def->type == e_circleShape);
  25         const b2CircleDef* circleDef = (const b2CircleDef*)def;
  26
  27         m_type = e_circleShape;
  28         m_localPosition = circleDef->localPosition;
  29         m_radius = circleDef->radius;
  30 }
  31
  32 void b2CircleShape::UpdateSweepRadius(const b2Vec2& center)
  33 {
  34         // Update the sweep radius (maximum radius) as measured from
  35         // a local center point.
  36         b2Vec2 d = m_localPosition - center;
  37         m_sweepRadius = d.Length() + m_radius - b2_toiSlop;
  38 }
  39
  40 bool b2CircleShape::TestPoint(const b2XForm& transform, const b2Vec2& p) const
  41 {
  42         b2Vec2 center = transform.position + b2Mul(transform.R, m_localPosition);
  43         b2Vec2 d = p - center;
  44         return b2Dot(d, d) <= m_radius * m_radius;
  45 }
  46
  47 // Collision Detection in Interactive 3D Environments by Gino van den Bergen
  48 // From Section 3.1.2
  49 // x = s + a * r
  50 // norm(x) = radius
  51 bool b2CircleShape::TestSegment(const b2XForm& transform,
  52                                                                 float32* lambda,
  53                                                                 b2Vec2* normal,
  54                                                                 const b2Segment& segment,
  55                                                                 float32 maxLambda) const
  56 {
  57         b2Vec2 position = transform.position + b2Mul(transform.R, m_localPosition);
  58         b2Vec2 s = segment.p1 - position;
  59         float32 b = b2Dot(s, s) - m_radius * m_radius;
  60
  61         // Does the segment start inside the circle?
  62         if (b < 0.0f)
  63         {
  64                 return false;
  65         }
  66
  67         // Solve quadratic equation.
  68         b2Vec2 r = segment.p2 - segment.p1;
  69         float32 c =  b2Dot(s, r);
  70         float32 rr = b2Dot(r, r);
  71         float32 sigma = c * c - rr * b;
  72
  73         // Check for negative discriminant and short segment.
  74         if (sigma < 0.0f || rr < B2_FLT_EPSILON)
  75         {
  76                 return false;
  77         }
  78
  79         // Find the point of intersection of the line with the circle.
  80         float32 a = -(c + b2Sqrt(sigma));
  81
  82         // Is the intersection point on the segment?
  83         if (0.0f <= a && a <= maxLambda * rr)
  84         {
  85                 a /= rr;
  86                 *lambda = a;
  87                 *normal = s + a * r;
  88                 normal->Normalize();
  89                 return true;
  90         }
  91
  92         return false;
  93 }
  94
  95 void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2XForm& transform) const
  96 {
  97         b2Vec2 p = transform.position + b2Mul(transform.R, m_localPosition);
  98         aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
  99         aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
 100 }
 101
 102 void b2CircleShape::ComputeSweptAABB(b2AABB* aabb, const b2XForm& transform1, const b2XForm& transform2) const
 103 {
 104         b2Vec2 p1 = transform1.position + b2Mul(transform1.R, m_localPosition);
 105         b2Vec2 p2 = transform2.position + b2Mul(transform2.R, m_localPosition);
 106         b2Vec2 lower = b2Min(p1, p2);
 107         b2Vec2 upper = b2Max(p1, p2);
 108
 109         aabb->lowerBound.Set(lower.x - m_radius, lower.y - m_radius);
 110         aabb->upperBound.Set(upper.x + m_radius, upper.y + m_radius);
 111 }
 112
 113 void b2CircleShape::ComputeMass(b2MassData* massData) const
 114 {
 115         massData->mass = m_density * b2_pi * m_radius * m_radius;
 116         massData->center = m_localPosition;
 117
 118         // inertia about the local origin
 119         massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_localPosition, m_localPosition));
 120 }