MTV CirclevsPolygon(Shape* pa, Shape* pb)
{
Circle* a = dynamic_cast<Circle*>(pa);
Polygon* b = dynamic_cast<Polygon*>(pb);
// Transform circle center to Polygon model space
Vec2 center = a->center;
// Find edge with minimum penetration
float separation = -FLT_MAX;
unsigned faceNormal = 0;
for (unsigned i = 0; i < b->getVertsNum(); ++i)
{
float s = Dot(b->getAxes(i), center - b->vertices[i]);
if (s > a->radius) {
return MTV(false);
}
if (s > separation) {
separation = s;
faceNormal = i;
}
}
// Grab face's vertices
Vec2 v1 = b->vertices[faceNormal];
unsigned i2 = faceNormal + 1 < b->getVertsNum() ? faceNormal + 1 : 0;
Vec2 v2 = b->vertices[i2];
// Determine which voronoi region of the edge center of circle lies within
float dot1 = Dot(center - v1, v2 - v1);
float dot2 = Dot(center - v2, v1 - v2);
if (dot1 <= 0.0f) {
if (DistSqr(center, v1) > a->radius * a->radius)
return MTV(false);
}
else if (dot2 <= 0.0f) {
if (DistSqr(center, v2) > a->radius * a->radius)
return MTV(false);
}
else {
Vec2 n = b->getAxes(faceNormal);
if (Dot(center - v1, n) > a->radius)
return MTV(false);
}
return MTV(b->getAxes(faceNormal), a->radius - separation, true);
}
/// \brief
/// Returns the distance between this vector and the passed vector
inline float Dist(const VVertex3f &v2) const
{
return hkvMath::sqrt (DistSqr(v2));
}
nat32 MeanGridSeg::UpdatePixels(const ds::Array2D<nat32> & oldSeg, ds::Array2D<nat32> & newSeg,
const ds::Array<Mean> & mean)
{
nat32 changes = 0;
for (nat32 y=0;y<oldSeg.Height();y++)
{
for (nat32 x=0;x<oldSeg.Width();x++)
{
nat32 cur = oldSeg.Get(x,y);
nat32 nx = (x!=0)?oldSeg.Get(x-1,y):cur;
nat32 px = (x+1!=oldSeg.Width())?oldSeg.Get(x+1,y):cur;
nat32 ny = (y!=0)?oldSeg.Get(x,y-1):cur;
nat32 py = (y+1!=oldSeg.Height())?oldSeg.Get(x,y+1):cur;
if ((cur!=nx)||(cur!=px)||(cur!=ny)||(cur!=py))
{
real32 best = DistSqr(mean[cur],x,y);
nat32 bestSeg = cur;
if (nx!=cur)
{
real32 cost = DistSqr(mean[nx],x,y);
if (cost<best)
{
best = cost;
bestSeg = nx;
}
}
if (px!=cur)
{
real32 cost = DistSqr(mean[px],x,y);
if (cost<best)
{
best = cost;
bestSeg = px;
}
}
if (ny!=cur)
{
real32 cost = DistSqr(mean[ny],x,y);
if (cost<best)
{
best = cost;
bestSeg = ny;
}
}
if (py!=cur)
{
real32 cost = DistSqr(mean[py],x,y);
if (cost<best)
{
best = cost;
bestSeg = py;
}
}
if (bestSeg!=cur) changes += 1;
newSeg.Get(x,y) = bestSeg;
}
else
{
newSeg.Get(x,y) = oldSeg.Get(x,y);
}
}
}
return changes;
}
void circletoPolygon(Manifold* m, RigidBody* a, RigidBody* b) {
Circle* A = reinterpret_cast<Circle *> (a->shape);
Polygon* B = reinterpret_cast<Polygon *>(b->shape);
m->contactCount = 0;
vec2 center = a->getPosition();
center = B->m_orientation.transpose() * (center - b->getPosition());
float separation = -FLT_MAX;
int faceNormal = 0;
for (int i = 0; i < B->m_vertexCount; ++i) {
float s = dot(B->m_normals[i], center - B->m_vertices[i]);
if (s > A->m_radius)
return;
if (s > separation) {
separation = s;
faceNormal = i;
}
}
vec2 v1 = B->m_vertices[faceNormal];
unsigned int i2 = faceNormal + 1 < B->m_vertexCount ? faceNormal + 1 : 0;
vec2 v2 = B->m_vertices[i2];
if (separation < EPSILON) {
m->contactCount = 1;
m->normal = -(B->m_orientation * B->m_normals[faceNormal]);
m->contacts[0] = m->normal * A->m_radius + a->getPosition();
m->penetration = A->m_radius;
return;
}
float dot1 = dot(center - v1, v2 - v1);
float dot2 = dot(center - v2, v1 - v2);
m->penetration = A->m_radius - separation;
if (dot1 <= 0.0f) {
if (DistSqr(center, v1) > A->m_radius * A->m_radius)
return;
m->contactCount = 1;
vec2 n = v1 - center;
n = B->m_orientation * n;
n.normalise();
m->normal = n;
v1 = B->m_orientation * v1 + b->getPosition();
m->contacts[0] = v1;
}
else if (dot2 <= 0.0f) {
if (DistSqr(center, v2) > A->m_radius * A->m_radius)
return;
m->contactCount = 1;
vec2 n = v2 - center;
v2 = B->m_orientation * v2 + b->getPosition();
m->contacts[0] = v2;
n = B->m_orientation * n;
n.normalise();
m->normal = n;
}
else {
vec2 n = B->m_normals[faceNormal];
if (dot(center - v1, n) > A->m_radius)
return;
n = B->m_orientation * n;
m->normal = -n;
m->contacts[0] = m->normal * A->m_radius + a->getPosition();
m->contactCount = 1;
}
}
