public override void Update(float dt)
{
base.Update(dt);
if (CCMouse.Instance.HasPosition)
{
// turn the control body based on the angle relative to the actual body
cpVect mouseDelta = cpVect.cpvsub(CCMouse.Instance.Position, tankBody.GetPosition());
float turn = cpVect.cpvtoangle(cpVect.cpvunrotate(tankBody.GetRotation(), mouseDelta));
tankControlBody.SetAngle(tankBody.GetAngle() - turn);
// drive the tank towards the mouse
if (cpVect.cpvnear(CCMouse.Instance.Position, tankBody.GetPosition(), 30))
{
tankControlBody.SetVelocity(cpVect.Zero); // stop
}
else
{
float direction = (cpVect.cpvdot(mouseDelta, tankBody.GetRotation()) > 0 ? 1 : -1);
tankControlBody.SetVelocity(cpVect.cpvrotate(tankBody.GetRotation(), new cpVect(30 * direction, 0)));
}
}
space.Step(dt);
}
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 override void Visit()
//{
// base.AdditionalTransform = NodeToBodyTransform();
// base.Visit();
//}
// returns the transform matrix according the Chipmunk Body values
public CCAffineTransform NodeToBodyTransform()
{
var angle = CCPoint.ForAngle(-CCMathHelper.ToRadians(RotationX));
cpVect rot = (IgnoreBodyRotation ? new cpVect(angle.X, angle.Y) : _body.GetRotation()); //TODO: CHECK ROT
double x = _body.GetPosition().x + (double)rot.x * -AnchorPointInPoints.X - (double)rot.y * (-AnchorPointInPoints.Y);
double y = _body.GetPosition().y + (double)rot.y * -AnchorPointInPoints.X + (double)rot.x * (-AnchorPointInPoints.Y);
return(new CCAffineTransform((float)rot.x, (float)rot.y, (float)-rot.y, (float)rot.x, (float)x, (float)y));
}