Rectf RigidBody2D::GetAABB() const
{
if (m_shapes.empty())
return Rectf::Zero();
auto it = m_shapes.begin();
cpBB bb = cpShapeGetBB(*it++);
for (; it != m_shapes.end(); ++it)
bb = cpBBMerge(bb, cpShapeGetBB(*it));
return Rectf(Rect<cpFloat>(bb.l, bb.b, bb.r - bb.l, bb.t - bb.b));
}
static void
BallIterator(cpBody *body, cpArbiter *arb, int *count)
{
// body is the body we are iterating the arbiters for.
// CP_ARBITER_GET_*() in an arbiter iterator always returns the body/shape for the iterated body first.
CP_ARBITER_GET_SHAPES(arb, ball, other);
ChipmunkDebugDrawBB(cpShapeGetBB(other), RGBAColor(1, 0, 0, 1));
(*count)++;
}
void CDynamics2DSingleBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Move the body to the desired position */
m_ptBody->p = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpBodySetAngle(m_ptBody, cZAngle.GetValue());
/* Update shape index */
if(cpBodyIsStatic(m_ptBody)) {
cpBB tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
cpSpaceReindexStatic(GetDynamics2DEngine().GetPhysicsSpace());
tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
}
else {
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody);
}
/* Update ARGoS entity state */
CDynamics2DModel::UpdateEntityStatus();
}
void CDynamics2DSingleBodyObjectModel::CalculateBoundingBox() {
cpBB tBoundingBox = cpShapeGetBB(m_ptBody->shapeList);
for(cpShape* pt_shape = m_ptBody->shapeList->next;
pt_shape != NULL;
pt_shape = pt_shape->next) {
cpBB* ptBB = &pt_shape->bb;
if(ptBB->l < tBoundingBox.l) tBoundingBox.l = ptBB->l;
if(ptBB->b < tBoundingBox.b) tBoundingBox.b = ptBB->b;
if(ptBB->r > tBoundingBox.r) tBoundingBox.r = ptBB->r;
if(ptBB->t > tBoundingBox.t) tBoundingBox.t = ptBB->t;
}
GetBoundingBox().MinCorner.SetX(tBoundingBox.l);
GetBoundingBox().MinCorner.SetY(tBoundingBox.b);
GetBoundingBox().MaxCorner.SetX(tBoundingBox.r);
GetBoundingBox().MaxCorner.SetY(tBoundingBox.t);
}
static void
BallIterator(cpBody *body, cpArbiter *arb, int *count)
{
// body is the body we are iterating the arbiters for.
// CP_ARBITER_GET_*() in an arbiter iterator always returns the body/shape for the iterated body first.
CP_ARBITER_GET_SHAPES(arb, ball, other);
// Grab the bounding box, expand it slightly (for visibility) and draw it.
cpBB bb = cpShapeGetBB(other);
bb.l -= 5.0;
bb.b -= 5.0;
bb.r += 5.0;
bb.t += 5.0;
ChipmunkDebugDrawBB(bb, RGBAColor(1, 0, 0, 1));
(*count)++;
}
static void
draw(void)
{
ChipmunkDemoDefaultDrawImpl();
cpVect start = QUERY_START;
cpVect end = ChipmunkDemoMouse;
ChipmunkDebugDrawSegment(start, end, RGBAColor(0,1,0,1));
ChipmunkDemoPrintString("Query: Dist(%f) Point%s, ", cpvdist(start, end), cpvstr(end));
cpSegmentQueryInfo segInfo = {};
if(cpSpaceSegmentQueryFirst(space, start, end, CP_ALL_LAYERS, CP_NO_GROUP, &segInfo)){
cpVect point = cpSegmentQueryHitPoint(start, end, segInfo);
// Draw red over the occluded part of the query
ChipmunkDebugDrawSegment(point, end, RGBAColor(1,0,0,1));
// Draw a little blue surface normal
ChipmunkDebugDrawSegment(point, cpvadd(point, cpvmult(segInfo.n, 16)), RGBAColor(0,0,1,1));
// Draw a little red dot on the hit point.
ChipmunkDebugDrawPoints(3, 1, &point, RGBAColor(1,0,0,1));
ChipmunkDemoPrintString("Segment Query: Dist(%f) Normal%s", cpSegmentQueryHitDist(start, end, segInfo), cpvstr(segInfo.n));
} else {
ChipmunkDemoPrintString("Segment Query (None)");
}
cpNearestPointQueryInfo nearestInfo = {};
cpSpaceNearestPointQueryNearest(space, ChipmunkDemoMouse, 100.0, CP_ALL_LAYERS, CP_NO_GROUP, &nearestInfo);
if(nearestInfo.shape){
// Draw a grey line to the closest shape.
ChipmunkDebugDrawPoints(3, 1, &ChipmunkDemoMouse, RGBAColor(0.5, 0.5, 0.5, 1.0));
ChipmunkDebugDrawSegment(ChipmunkDemoMouse, nearestInfo.p, RGBAColor(0.5, 0.5, 0.5, 1.0));
// Draw a red bounding box around the shape under the mouse.
if(nearestInfo.d < 0) ChipmunkDebugDrawBB(cpShapeGetBB(nearestInfo.shape), RGBAColor(1,0,0,1));
}
}
void CDynamics2DMultiBodyObjectModel::CalculateBoundingBox() {
if(m_vecBodies.empty()) return;
cpBB tBoundingBox;
Real fMaxHeight;
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
tBoundingBox = cpShapeGetBB(m_vecBodies[i].Body->shapeList);
for(cpShape* pt_shape = m_vecBodies[i].Body->shapeList->next;
pt_shape != NULL;
pt_shape = pt_shape->next) {
cpBB* ptBB = &pt_shape->bb;
if(ptBB->l < tBoundingBox.l) tBoundingBox.l = ptBB->l;
if(ptBB->b < tBoundingBox.b) tBoundingBox.b = ptBB->b;
if(ptBB->r > tBoundingBox.r) tBoundingBox.r = ptBB->r;
if(ptBB->t > tBoundingBox.t) tBoundingBox.t = ptBB->t;
}
fMaxHeight = Max(fMaxHeight, m_vecBodies[i].Height);
}
GetBoundingBox().MinCorner.SetX(tBoundingBox.l);
GetBoundingBox().MinCorner.SetY(tBoundingBox.b);
GetBoundingBox().MinCorner.SetZ(GetDynamics2DEngine().GetElevation());
GetBoundingBox().MaxCorner.SetX(tBoundingBox.r);
GetBoundingBox().MaxCorner.SetY(tBoundingBox.t);
GetBoundingBox().MaxCorner.SetZ(GetDynamics2DEngine().GetElevation() + fMaxHeight);
}
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr)
{
CP_ARBITER_GET_SHAPES(arb, water, poly);
cpBody *body = cpShapeGetBody(poly);
// Get the top of the water sensor bounding box to use as the water level.
cpFloat level = cpShapeGetBB(water).t;
// Clip the polygon against the water level
int count = cpPolyShapeGetCount(poly);
int clippedCount = 0;
#ifdef _MSC_VER
// MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays.
cpVect clipped[10];
#else
cpVect clipped[count + 1];
#endif
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j));
cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i));
if(a.y < level){
clipped[clippedCount] = a;
clippedCount++;
}
cpFloat a_level = a.y - level;
cpFloat b_level = b.y - level;
if(a_level*b_level < 0.0f){
cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
// Calculate buoyancy from the clipped polygon area
cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f);
cpFloat displacedMass = clippedArea*FLUID_DENSITY;
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpDataPointer data = ptr;
DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data);
DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data);
cpFloat dt = cpSpaceGetCurrentTimeStep(space);
cpVect g = cpSpaceGetGravity(space);
// Apply the buoyancy force as an impulse.
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid);
// Apply linear damping for the fluid drag.
cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid);
cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid));
cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY;
cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag
// cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid);
// Apply angular damping for the fluid drag.
cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body));
cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f);
cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body)));
return cpTrue;
}
