float Vec2D::rotationTo(Vec2D const& other) const
{
float myAngle = angle();
float otherAngle = other.angle();
if(myAngle > otherAngle)
{
return myAngle - (otherAngle + 1.0);
}
else
{
return myAngle - otherAngle;
}
}
WheelsControl::WheelOutput DriveEquation::computeWheel(const WheelConfig &wconfig, const Motion &motion) const {
Vec2D relvel = motion.vel.rotate(motion.curdir);
Vec2D wheelout = relvel + (wconfig.framepos*motion.angvel).rotate(-M_PI/2);
float mag = wheelout.magnitude();
WheelsControl::WheelOutput out;
if (mag < config.minspeed) {
out.angle = 0;
out.effort = 0;
out.enabled = false;
return out;
}
out.angle = (wheelout.angle()+config.wheelangleoffset).getRad();
out.effort = wconfig.effortscale*mag;
if (out.effort > 0)
out.effort += wconfig.effortoffset;
else
out.effort -= wconfig.effortoffset;
return out;
}