// QQQ new utility 2-25-04 -- may replace inline code elsewhere
//
// Given a location in this vehicle's linear local space, convert it into
// the curved space defined by the vehicle's current path curvature. For
// example, forward() gets mapped on a point 1 unit along the circle
// centered on the current center of curvature and passing through the
// vehicle's position().
//
private Vector3 ConvertLinearToCurvedSpaceGlobal(Vector3 linear)
{
Vector3 trimmedLinear = linear.TruncateLength(MaxForce);
// ---------- this block imported from steerToAvoidObstaclesOnMap
float signedRadius = 1 / (NonZeroCurvatureQQQ() /*QQQ*/ * 1);
Vector3 localCenterOfCurvature = Side * signedRadius;
Vector3 center = Position + localCenterOfCurvature;
float sign = signedRadius < 0 ? 1.0f : -1.0f;
float arcLength = Vector3.Dot(trimmedLinear, Forward);
//
float arcRadius = signedRadius * -sign;
const float TWO_PI = 2 * (float)Math.PI;
float circumference = TWO_PI * arcRadius;
float arcAngle = TWO_PI * arcLength / circumference;
// ---------- this block imported from steerToAvoidObstaclesOnMap
// ---------- this block imported from scanObstacleMap
// vector from center of curvature to position of vehicle
Vector3 initialSpoke = Position - center;
// rotate by signed arc angle
Vector3 spoke = initialSpoke.RotateAboutGlobalY(arcAngle * sign);
// ---------- this block imported from scanObstacleMap
Vector3 fromCenter = Vector3.Normalize(-localCenterOfCurvature);
float dRadius = Vector3.Dot(trimmedLinear, fromCenter);
float radiusChangeFactor = (dRadius + arcRadius) / arcRadius;
Vector3 resultLocation = center + (spoke * radiusChangeFactor);
{
Vector3 center2 = Position + localCenterOfCurvature;
AnnotationXZArc(Position, center2, Speed * sign * -3, 20, Color.White);
}
// return the vector from vehicle position to the coimputed location
// of the curved image of the original linear offset
return resultLocation - Position;
}
