本文整理汇总了C++中vec2f::length_squared方法的典型用法代码示例。如果您正苦于以下问题:C++ vec2f::length_squared方法的具体用法?C++ vec2f::length_squared怎么用?C++ vec2f::length_squared使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类vec2f
的用法示例。
在下文中一共展示了vec2f::length_squared方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: directionControl
void MoverVehicle::directionControl(const vec2f &dir, float speed)
{
float t = 0.5;
if(speed == 0.f)
t = 1.0;
Pose::vec unitDirection = getMaster()->getDirection();
float forward = 0, angle = 0;
// 1. Interpolate control
Pose::vec control = dir * t + (1.f - t) * lastControl;
lastControl = control;
// 2. Calculate drive control for kinematics mode
if(definition->kinematic)
{
//float errorAngle = dir.length_squared() > 0.f ? vecAngle2d_CW(dir,unitDirection) : 0.f;//,vecAngle2d_CW(sumForce,unit->getDirection())};
float error[] =
{
(speed - currentVelocity[0]) * fSign(unitDirection & control),
dir.length_squared() > 0.f ? vecAngle2d_CW(control,unitDirection) : 0.f,
};
// // 4. calculate prefered direction
if(error[1] > M_PI)
error[1] -= (2 * M_PI);
float turnTime = fabs(error[1] / currentVelocity[1]);
float brakingTime = sqrt(2 * fabs(error[1]) / definition->acceleration[1]);
if( currentVelocity[1] * error[1] >= 0.0)
{
if(turnTime < brakingTime)
angle = -error[1];
else
angle = fSign(error[1]);
//if(fabs(turnTime) < 0.1)
// angle = 0;
}
else
{
angle = (error[1] >= 0) ? 1.f : -1.f;
}
//angle = fSign(errorAngle);
//angle = errorAngle;
//angle = -fSign(currentVelocity[1]);
forward = clamp(deadZone(error[0],-0.1f,0.1f),-1.f,1.f);
//angle = fSign(deadZone(angle,-0.1f,0.1f));
angle = clamp(angle,-1.f,1.f);
}
else
{
// 4. calculate prefered direction
forward = speed;//(dir & unitDirection);
angle = control.length_squared()>0.f?vecAngle2d_CW(control,unitDirection):0.f;//,vecAngle2d_CW(sumForce,unit->getDirection())};
if(angle > M_PI)
angle -= (M_PI+M_PI);
forward = clamp(deadZone(speed,-0.1f,0.1f),-1.f,1.f);
angle = fSign(deadZone(angle,-0.1f,0.1f));
}
// 4. fix values
if(fabs(forward)>1.f)
forward = 0.f;
if(fabs(angle)>1.f)
angle = 0.f;
// 5. apply control
query_Direction(portLinear,dcmdDir_set, forward);
query_Direction(portAngular,dcmdDir_set, angle);
}