planck-js/lib/shape/CollideCirclePolygone.js

2D physics engine for JavaScript/HTML5 game development

/*
 * Copyright (c) 2016      Ali Shakiba http://shakiba.me/planck.js
 * Copyright (c) 2006-2011 Erin Catto  http://www.box2d.org
 *
 * 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.
 */

var Settings = require('../Settings');
var Manifold = require('../Manifold');
var Contact = require('../Contact');
var Shape = require('../Shape');
var Math = require('../common/Math');
var Transform = require('../common/Transform');
var Rot = require('../common/Rot');
var Vec2 = require('../common/Vec2');
var AABB = require('../collision/AABB');

var CircleShape = require('./CircleShape');
var PolygonShape = require('./PolygonShape');

Contact.AddType(PolygonShape.TYPE, CircleShape.TYPE, PolygonCircleContact);

function PolygonCircleContact(manifold, xfA, fixtureA, indexA, xfB, fixtureB,
    indexB) {
  Assert(fixtureA.GetType() == PolygonShape.TYPE);
  Assert(fixtureB.GetType() == CircleShape.TYPE);
  CollidePolygonCircle(manifold, fixtureA.GetShape(), xfA, fixtureB.GetShape(),
      xfB);
}

function CollidePolygonCircle(manifold, polygonA, xfA, circleB, xfB) {
  manifold.pointCount = 0;

  // Compute circle position in the frame of the polygon.
  var c = Transform.Mul(xfB, circleB.m_p);
  var cLocal = Transform.MulT(xfA, c);

  // Find the min separating edge.
  var normalIndex = 0;
  var separation = -Infinity;
  var radius = polygonA.m_radius + circleB.m_radius;
  var vertexCount = polygonA.m_count;
  var vertices = polygonA.m_vertices;
  var normals = polygonA.m_normals;

  for (var i = 0; i < vertexCount; ++i) {
    var s = Vec2.Dot(normals[i], Vec2.Sub(cLocal, vertices[i]));

    if (s > radius) {
      // Early out.
      return;
    }

    if (s > separation) {
      separation = s;
      normalIndex = i;
    }
  }

  // Vertices that subtend the incident face.
  var vertIndex1 = normalIndex;
  var vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
  var v1 = vertices[vertIndex1];
  var v2 = vertices[vertIndex2];

  // If the center is inside the polygon ...
  if (separation < Math.EPSILON) {
    manifold.pointCount = 1;
    manifold.type = Manifold.e_faceA;
    manifold.localNormal.Set(normals[normalIndex]);
    manifold.localPoint.WSet(0.5, v1, 0.5, v2);
    manifold.points[0].localPoint = circleB.m_p;
    manifold.points[0].id.key = 0;
    return;
  }

  // Compute barycentric coordinates
  var u1 = Vec2.Dot(Vec2.Sub(cLocal, v1), Vec2.Sub(v2, v1));
  var u2 = Vec2.Dot(Vec2.Sub(cLocal, v2), Vec2.Sub(v1, v2));
  if (u1 <= 0.0) {
    if (Vec2.DistanceSquared(cLocal, v1) > radius * radius) {
      return;
    }

    manifold.pointCount = 1;
    manifold.type = Manifold.e_faceA;
    manifold.localNormal.WSet(1, cLocal, -1, v1);
    manifold.localNormal.Normalize();
    manifold.localPoint = v1;
    manifold.points[0].localPoint.Set(circleB.m_p);
    manifold.points[0].id.key = 0;
  } else if (u2 <= 0.0) {
    if (Vec2.DistanceSquared(cLocal, v2) > radius * radius) {
      return;
    }

    manifold.pointCount = 1;
    manifold.type = Manifold.e_faceA;
    manifold.localNormal.WSet(1, cLocal, -1, v2);
    manifold.localNormal.Normalize();
    manifold.localPoint.Set(v2);
    manifold.points[0].localPoint.Set(circleB.m_p);
    manifold.points[0].id.key = 0;
  } else {
    var faceCenter = Vec2.Mid(v1, v2);
    var separation = Vec2.Dot(cLocal, normals[vertIndex1])
        - Vec2.Dot(faceCenter, normals[vertIndex1]);
    if (separation > radius) {
      return;
    }

    manifold.pointCount = 1;
    manifold.type = Manifold.e_faceA;
    manifold.localNormal.Set(normals[vertIndex1]);
    manifold.localPoint.Set(faceCenter);
    manifold.points[0].localPoint.Set(circleB.m_p);
    manifold.points[0].id.key = 0;
  }
}