Orient BestGrip(Orient orient)
{
float robotPos = 0.7025f;
var left = new Vector3(1, 0, 1);
var right = new Vector3(1, 0, -1);
var bestPos = orient.Center.x < robotPos ? left : right;
var angle = Orient.GetAngle(orient.Rotation * Vector3.right, bestPos);
if (Abs(angle) > 90f)
{
orient.Rotation *= Quaternion.Euler(0, 180f, 0);
}
return(orient);
}
static Orient?FindTile(Vector3 marker, IEnumerable <Vector3> topLayerMarkers)
{
float xLength = 0.175f;
float zLength = 0.02f;
float epsilon = 0.01f;
float epsilonDeg = 5f;
foreach (var otherX in topLayerMarkers)
{
float distanceX = (marker - otherX).magnitude;
if (Abs(distanceX - xLength) > epsilon)
{
continue;
}
foreach (var otherZ in topLayerMarkers)
{
float distanceZ = (marker - otherZ).magnitude;
if (Abs(distanceZ - zLength) > epsilon)
{
continue;
}
var vmx = otherX - marker;
var vmz = otherZ - marker;
var zAngle = Orient.GetAngle(vmx, vmz);
if (Abs(zAngle - 90f) > epsilonDeg)
{
continue;
}
var angle = Orient.GetAngle(Vector3.right, vmx);
var rotation = Quaternion.Euler(0, -angle, 0);
var center = otherX * 0.5f + marker * 0.5f;
return(new Orient(center, rotation));
}
}
return(null);
}
