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