void motor_preSolve(cpConstraint motor, cpSpace space)
{
float dt = space.GetCurrentTimeStep();
float target_x = CCMouse.Instance.Position.x;
paint = new shape
{
point1 = new cpVect(target_x, -1000.0f),
point2 = new cpVect(target_x, 1000.0f),
};
float max_v = 500.0f;
float target_v = cp.cpfclamp(cp.bias_coef(0.5f, dt / 1.2f) * (target_x - balance_body.GetPosition().x) / dt, -max_v, max_v);
float error_v = (target_v - balance_body.GetVelocity().x);
float target_sin = 3.0e-3f * cp.bias_coef(0.1f, dt) * error_v / dt;
float max_sin = cp.cpfsin(0.6f);
balance_sin = cp.cpfclamp(balance_sin - 6.0e-5f * cp.bias_coef(0.2f, dt) * error_v / dt, -max_sin, max_sin);
float target_a = (float)Math.Asin(cp.cpfclamp(-target_sin + balance_sin, -max_sin, max_sin));
float angular_diff = (float)Math.Asin(cpVect.cpvcross(balance_body.GetRotation(), cpVect.cpvforangle(target_a)));
float target_w = cp.bias_coef(0.1f, dt / 0.4f) * (angular_diff) / dt;
float max_rate = 50.0f;
float rate = cp.cpfclamp(wheel_body.GetAngularVelocity() + balance_body.GetAngularVelocity() - target_w, -max_rate, max_rate);
motor.SetRate(cp.cpfclamp(rate, -max_rate, max_rate));
motor.SetMaxForce(8.0e4f);
}
public void ShatterCell(cpPolyShape shape, cpVect cell, int cell_i, int cell_j, ref WorleyContex context)
{
// printf("cell %dx%d: (% 5.2f, % 5.2f)\n", cell_i, cell_j, cell.x, cell.y);
cpBody body = shape.body; // cpShapeGetBody(shape);
cpVect[] ping = new cpVect[MAX_VERTEXES_PER_VORONOI]; // cpVect[ (cpVect*)alloca( * sizeof(cpVect));
cpVect[] pong = new cpVect[MAX_VERTEXES_PER_VORONOI]; //(cpVect*)alloca(MAX_VERTEXES_PER_VORONOI * sizeof(cpVect));
int count = shape.Count; // cpPolyShapeGetCount();
count = (count > MAX_VERTEXES_PER_VORONOI ? MAX_VERTEXES_PER_VORONOI : count);
for (int i = 0; i < count; i++)
{
ping[i] = body.LocalToWorld(shape.GetVert(i));
}
cpPointQueryInfo info = null;
for (int i = 0; i < context.width; i++)
{
for (int j = 0; j < context.height; j++)
{
if (
!(i == cell_i && j == cell_j) &&
shape.PointQuery(cell, ref info) < 0
)
{
count = ClipCell(shape, cell, i, j, context, ping, pong, count);
for (int u = 0; u < pong.Length; u++)
{
if (pong[u] != null)
{
ping[u] = new cpVect(pong[u]);
}
}
}
}
}
cpVect centroid = cp.CentroidForPoly(count, ping);
float mass = cp.AreaForPoly(count, ping, 0) * DENSITY;
float moment = cp.MomentForPoly(mass, count, ping, cpVect.cpvneg(centroid), 0);
cpBody new_body = space.AddBody(new cpBody(mass, moment));
new_body.SetPosition(centroid);
new_body.SetPosition(centroid);
new_body.SetVelocity(body.GetVelocityAtLocalPoint(centroid));
new_body.SetAngularVelocity(body.GetAngularVelocity());
cpTransform transform = cpTransform.Translate(cpVect.cpvneg(centroid));
cpShape new_shape = space.AddShape(new cpPolyShape(new_body, count, ping, transform, 0));
// Copy whatever properties you have set on the original shape that are important
new_shape.SetFriction(shape.GetFriction());
}
public void ClipPoly(cpSpace space, cpShape shp, cpVect n, float dist)
{
cpPolyShape shape = (cpPolyShape)shp;
cpBody body = shape.GetBody();
int count = shape.Count;
int clippedCount = 0;
cpVect[] clipped = new cpVect[count + 1];
for (int i = 0, j = count - 1; i < count; j = i, i++)
{
cpVect a = body.LocalToWorld(shape.GetVert(j));
float a_dist = cpVect.cpvdot(a, n) - dist;
if (a_dist < 0)
{
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = body.LocalToWorld(shape.GetVert(i));
float b_dist = cpVect.cpvdot(b, n) - dist;
if (a_dist * b_dist < 0)
{
float t = cp.cpfabs(a_dist) / (cp.cpfabs(a_dist) + cp.cpfabs(b_dist));
clipped[clippedCount] = cpVect.cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cp.CentroidForPoly(clippedCount, clipped);
float mass = cp.AreaForPoly(clippedCount, clipped, 0) * DENSITY;
float moment = cp.MomentForPoly(mass, clippedCount, clipped, cpVect.cpvneg(centroid), 0);
cpBody new_body = space.AddBody(new cpBody(mass, moment));
new_body.SetPosition(centroid);
new_body.SetVelocity(body.GetVelocityAtWorldPoint(centroid));
new_body.SetAngularVelocity(body.GetAngularVelocity());
cpTransform transform = cpTransform.Translate(cpVect.cpvneg(centroid));
cpShape new_shape = space.AddShape(new cpPolyShape(new_body, clippedCount, clipped, transform, 0));
// Copy whatever properties you have set on the original shape that are important
new_shape.SetFriction(shape.GetFriction());
}
public bool waterPreSolve(cpArbiter arb, cpSpace space, object o)
{
cpShape obj1, obj2;
arb.GetShapes(out obj1, out obj2);
cpPolyShape water = obj1 as cpPolyShape;
cpPolyShape poly = obj2 as cpPolyShape;
cpBody body = poly.GetBody();
float level = water.GetBB().t; // cpShapeGetBB().t;
int count = poly.Count; //cpPolyShapeGetCount(poly.g);
int clippedCount = 0;
cpVect[] clipped = new cpVect[10];
for (int i = 0, j = count - 1; i < count; j = i, i++)
{
cpVect a = body.LocalToWorld(poly.GetVert(j));
cpVect b = body.LocalToWorld(poly.GetVert(i));
if (a.y < level)
{
clipped[clippedCount] = a;
clippedCount++;
}
float a_level = a.y - level;
float b_level = b.y - level;
if (a_level * b_level < 0.0f)
{
float t = cp.cpfabs(a_level) / (cp.cpfabs(a_level) + cp.cpfabs(b_level));
clipped[clippedCount] = cpVect.cpvlerp(a, b, t);
clippedCount++;
}
}
// Calculate buoyancy from the clipped polygon area
float clippedArea = cp.AreaForPoly(clippedCount, clipped, 0.0f);
float displacedMass = clippedArea * FLUID_DENSITY;
cpVect centroid = cp.CentroidForPoly(clippedCount, clipped);
//ChipmunkDebugDrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f));
//ChipmunkDebugDrawDot(5, centroid, RGBAColor(0, 0, 1, 1));
float dt = space.GetCurrentTimeStep();
cpVect g = space.GetGravity();
// Apply the buoyancy force as an impulse.
body.ApplyImpulseAtWorldPoint(cpVect.cpvmult(g, -displacedMass * dt), centroid);
// Apply linear damping for the fluid drag.
cpVect v_centroid = body.GetVelocityAtWorldPoint(centroid);
float k = k_scalar_body(body, centroid, cpVect.cpvnormalize(v_centroid));
float damping = clippedArea * FLUID_DRAG * FLUID_DENSITY;
float v_coef = cp.cpfexp(-damping * dt * k); // linear drag
// cpfloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
body.ApplyImpulseAtWorldPoint(cpVect.cpvmult(cpVect.cpvsub(cpVect.cpvmult(v_centroid, v_coef), v_centroid), 1.0f / k), centroid);
// Apply angular damping for the fluid drag.
cpVect cog = body.LocalToWorld(body.GetCenterOfGravity());
float w_damping = cp.MomentForPoly(FLUID_DRAG * FLUID_DENSITY * clippedArea, clippedCount, clipped, cpVect.cpvneg(cog), 0.0f);
body.SetAngularVelocity(body.GetAngularVelocity() * cp.cpfexp(-w_damping * dt / body.GetMoment()));
return(true);
}