inline
Box3d
Box3d::interpolated(Interval x, Interval y, Interval z) const {
IntervalVector3d intervalVector(
x * this->x().width(),
y * this->y().width(),
z * this->z().width()
);
return minVertex() + intervalVector;
}
// Compute shortest path distances from "s".
// Return these distances in "D".
void Dijkstra(Graph* G, int* D, int s) {
int i, v, w;
for (i=0; i<G->n(); i++) { // Process the vertices
v = minVertex(G, D);
if (D[v] == INFINITY) return; // Unreachable vertices
G->setMark(v, VISITED);
for (w=G->first(v); w<G->n(); w = G->next(v,w))
if (D[w] > (D[v] + G->weight(v, w)))
D[w] = D[v] + G->weight(v, w);
}
}
inline
Point3d
Box3d::interpolated(double x, double y, double z) const {
Vector3d vector(x * this->x().width(), y * this->y().width(), z * this->z().width());
return minVertex() + vector;
}
inline
Box2d
Box2d::interpolated(Interval x, Interval y) const {
return minVertex() + IntervalVector2d(x * this->x().width(), y * this->y().width());
}
inline
Point2d
Box2d::interpolated(double x, double y) const {
return minVertex() + Vector2d(x * this->x().width(), y * this->y().width());
}
b2PolyShape::b2PolyShape(const b2ShapeDef* def, b2Body* body,
const b2Vec2& newOrigin)
: b2Shape(def, body)
{
b2Assert(def->type == e_boxShape || def->type == e_polyShape);
m_type = e_polyShape;
b2Mat22 localR(def->localRotation);
// Get the vertices transformed into the body frame.
if (def->type == e_boxShape)
{
m_localCentroid = def->localPosition - newOrigin;
const b2BoxDef* box = (const b2BoxDef*)def;
m_vertexCount = 4;
b2Vec2 h = box->extents;
b2Vec2 hc = h;
hc.x = b2Max(0.0f, h.x - 2.0f * b2_linearSlop);
hc.y = b2Max(0.0f, h.y - 2.0f * b2_linearSlop);
m_vertices[0] = b2Mul(localR, b2Vec2(h.x, h.y));
m_vertices[1] = b2Mul(localR, b2Vec2(-h.x, h.y));
m_vertices[2] = b2Mul(localR, b2Vec2(-h.x, -h.y));
m_vertices[3] = b2Mul(localR, b2Vec2(h.x, -h.y));
m_coreVertices[0] = b2Mul(localR, b2Vec2(hc.x, hc.y));
m_coreVertices[1] = b2Mul(localR, b2Vec2(-hc.x, hc.y));
m_coreVertices[2] = b2Mul(localR, b2Vec2(-hc.x, -hc.y));
m_coreVertices[3] = b2Mul(localR, b2Vec2(hc.x, -hc.y));
}
else
{
const b2PolyDef* poly = (const b2PolyDef*)def;
m_vertexCount = poly->vertexCount;
b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolyVertices);
b2Vec2 centroid = PolyCentroid(poly->vertices, poly->vertexCount);
m_localCentroid = def->localPosition + b2Mul(localR, centroid) - newOrigin;
for (int32 i = 0; i < m_vertexCount; ++i)
{
m_vertices[i] = b2Mul(localR, poly->vertices[i] - centroid);
b2Vec2 u = m_vertices[i];
float32 length = u.Length();
if (length > FLT_EPSILON)
{
u *= 1.0f / length;
}
m_coreVertices[i] = m_vertices[i] - 2.0f * b2_linearSlop * u;
}
}
// Compute bounding box. TODO_ERIN optimize OBB
b2Vec2 minVertex(FLT_MAX, FLT_MAX);
b2Vec2 maxVertex(-FLT_MAX, -FLT_MAX);
m_maxRadius = 0.0f;
for (int32 i = 0; i < m_vertexCount; ++i)
{
b2Vec2 v = m_vertices[i];
minVertex = b2Min(minVertex, v);
maxVertex = b2Max(maxVertex, v);
m_maxRadius = b2Max(m_maxRadius, v.Length());
}
m_localOBB.R.SetIdentity();
m_localOBB.center = 0.5f * (minVertex + maxVertex);
m_localOBB.extents = 0.5f * (maxVertex - minVertex);
// Compute the edge normals and next index map.
for (int32 i = 0; i < m_vertexCount; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
m_normals[i] = b2Cross(edge, 1.0f);
m_normals[i].Normalize();
}
// Ensure the polygon in convex. TODO_ERIN compute convex hull.
for (int32 i = 0; i < m_vertexCount; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
NOT_USED(i1);
NOT_USED(i2);
b2Assert(b2Cross(m_normals[i1], m_normals[i2]) > FLT_EPSILON);
}
m_R = m_body->m_R;
m_position = m_body->m_position + b2Mul(m_body->m_R, m_localCentroid);
b2Mat22 R = b2Mul(m_R, m_localOBB.R);
b2Mat22 absR = b2Abs(R);
b2Vec2 h = b2Mul(absR, m_localOBB.extents);
b2Vec2 position = m_position + b2Mul(m_R, m_localOBB.center);
b2AABB aabb;
aabb.minVertex = position - h;
aabb.maxVertex = position + h;
b2BroadPhase* broadPhase = m_body->m_world->m_broadPhase;
if (broadPhase->InRange(aabb))
{
m_proxyId = broadPhase->CreateProxy(aabb, this);
}
else
{
m_proxyId = b2_nullProxy;
}
if (m_proxyId == b2_nullProxy)
{
m_body->Freeze();
}
}
