protected void TestProng(Vector3 direction, out float middleSpace, out float leftSpace, out float rightSpace)
{
var cap = _characterCapsuleCollider;
var center = cap.center;
var sideWaysDirection = direction.RotateBy(90f * Mathf.Deg2Rad);
var middleRay = new Ray(_transform.position + center, direction);
var leftRay = new Ray(_transform.position + center + sideWaysDirection*cap.radius, direction);
var rightRay = new Ray(_transform.position + center - sideWaysDirection * cap.radius, direction);
middleSpace = GetSpaceAhead(middleRay);
leftSpace = GetSpaceAhead(leftRay);
rightSpace = GetSpaceAhead(rightRay);
var dt = Time.fixedDeltaTime;
DebugExtension.DebugPoint(_transform.position + center, Color.yellow, 1f, dt, false);
DebugExtension.DebugRay(_transform.position + center, direction * middleSpace, Color.blue, dt, false);
DebugExtension.DebugPoint(_transform.position + center + sideWaysDirection * cap.radius, new Color(1, 0, 0, 0.5f), 1f, dt);
DebugExtension.DebugRay(_transform.position + center + sideWaysDirection * cap.radius, direction * leftSpace, Color.blue, dt, false);
DebugExtension.DebugPoint(_transform.position + center - sideWaysDirection * cap.radius, new Color(1, 0, 0, 0.5f), 1f, dt);
DebugExtension.DebugRay(_transform.position + center - sideWaysDirection * cap.radius, direction * rightSpace, Color.blue, dt, false);
}
protected Vector3 AdjustDirectionForObstacles(Vector3 initialDirection, float maxRotation = 65f)
{
const float rotationSteps = 5f;
float rotationAmount = 0f;
Vector3 adjustedDirection = new Vector3(initialDirection.x, initialDirection.y, initialDirection.z);
int numberOfLoops = 0;
while (Mathf.Abs(rotationAmount) <= maxRotation && numberOfLoops < 100)
{
numberOfLoops++;
float middleSpace, leftSpace, rightSpace;
TestProng(adjustedDirection.normalized, out middleSpace, out leftSpace, out rightSpace);
const float margin = 0.1f;
// rotate left
if (Mathf.Approximately(leftSpace, rightSpace))
{
return adjustedDirection;
}
else if (leftSpace > rightSpace)
{
// try steer right
if (middleSpace < leftSpace - margin || rightSpace < middleSpace - margin)
{
adjustedDirection = adjustedDirection.RotateBy(rotationSteps*Mathf.Deg2Rad);
rotationAmount += rotationSteps;
}
// try steer left
else if (middleSpace < rightSpace - margin || leftSpace < middleSpace - margin)
{
adjustedDirection = adjustedDirection.RotateBy(-rotationSteps*Mathf.Deg2Rad);
rotationAmount -= rotationSteps;
}
else
{
return adjustedDirection;
}
}
else
{
// try steer left
if (middleSpace < rightSpace - margin || leftSpace < middleSpace - margin)
{
adjustedDirection = adjustedDirection.RotateBy(-rotationSteps * Mathf.Deg2Rad);
rotationAmount -= rotationSteps;
}
// try steer right
else if (middleSpace < leftSpace - margin || rightSpace < middleSpace - margin)
{
adjustedDirection = adjustedDirection.RotateBy(rotationSteps * Mathf.Deg2Rad);
rotationAmount += rotationSteps;
}
else
{
return adjustedDirection;
}
}
}
//if (numberOfLoops > 80)
//Debug.Log(numberOfLoops);
return adjustedDirection;
}