C++ (Cpp) Vec2D::magnitude示例

示例#1

文件： DriveEquation.cpp 项目： matthewbot/IEEEGumstix

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