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(); } }