// Collision check for the lower resolutions model
public bool CheckRootCollisions(GridPlanningState state)
{
Vector3 start = state.currentPosition - new Vector3(0, 10 * analyzer.GetHeight() / 2, 0);
Vector3 end = start + new Vector3(0, 10 * analyzer.GetHeight() / 2, 0);
float radius = analyzer.GetRadius();
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.currentPosition,radius,layer))
return(true);
}
if (state.previousState != null)
{
int samples = analyzer.samples;
Vector3 mov = state.actionMov;
for (int i = 1; i < samples - 1; i += 2) // we sample less in this domain the collision check
{
start = (state.previousState as GridPlanningState).currentPosition + i / (samples - 1) * mov;
end = start + new Vector3(0, analyzer.GetHeight() / 2, 0);
if (Physics.CheckCapsule(start, end, radius, layer))
{
//if (Physics.CheckSphere(state.previousState.currentPosition,radius,layer))
return(true);
}
}
}
return(false);
}
public bool CheckObstacleCollision(FootstepPlanningState state, ObstaclesScript script)
{
//if (state == null || script == null)
// return false;
if (script.ShapeChoice.Shape == Enum.Enumeration.Sphere)
{
Vector3 obstaclePos = script.gameObj.transform.position;
Vector3 aux = obstaclePos - state.currentPosition;
float sphereRadius = script.size.x;
return((aux.magnitude - sphereRadius - analyzer.GetRadius()) <= 0);
}
else if (script.ShapeChoice.Shape == Enum.Enumeration.Rectangle)
{
Bounds box = script.gameObj.collider.bounds;
return(box.SqrDistance(state.currentPosition) <= analyzer.GetRadius());
}
return(false);
}
void OnDrawGizmos()
{
if (!debugDraw)
{
return;
}
if (planning == null)
{
return;
}
/*
* //if(firstTime)
* //{
* foreach(ADAstarNode node in ADAstarPlanner.Closed.Values)
* {
* if(node.action.GetType() == typeof(FootstepPlanningAction)){
* Vector3 center = ((node.action as FootstepPlanningAction).state as FootstepPlanningState).currentPosition;
* Gizmos.color = Color.blue;
* Gizmos.DrawSphere(center, .25f);
* }
* }
* // firstTime = false;
* //}
*/
Gizmos.color = Color.red;
Gizmos.DrawWireSphere(lastPos, analyzer.GetRadius());
Gizmos.DrawWireSphere(lastObstacle, 1.0f);
//if (debugOnlyCurrent)
// return;
FootstepPlanningAction[] plan = planning.GetOutputPlan();
numberOfActions = plan.Length;
for (int i = 0; i < numberOfActions; i++)
{
if (plan[i] != null)
{
FootstepPlanningState state = plan[i].state as FootstepPlanningState;
if (state != null)
{
AnotatedAnimation animInfo = analyzer.GetAnotatedAnimation(state.actionName);
//Quaternion rotation = state.currentRotation;
Vector3 position;
if (animInfo.swing == Joint.LeftFoot)
{
position = state.leftFoot;
position[1] = auxHeight;
Gizmos.color = Color.green;
}
else
{
position = state.rightFoot;
position[1] = auxHeight;
Gizmos.color = Color.blue;
}
Gizmos.DrawWireSphere(state.currentPosition, analyzer.GetRadius());
Gizmos.DrawWireSphere(state.obstaclePos, 1.0f);
}
else
{
GridTimeState gtState = plan[i].state as GridTimeState;
if (gtState != null)
{
Vector3 position = gtState.currentPosition;
if (i % 2 == 0)
{
Gizmos.color = Color.green;
}
else
{
Gizmos.color = Color.blue;
}
if (!debugOnlyCurrent || (debugOnlyCurrent && Mathf.Abs(Time.time - gtState.time) < 0.5f))
{
Gizmos.DrawWireSphere(gtState.currentPosition, analyzer.GetRadius());
Gizmos.color = Color.red;
Gizmos.DrawWireSphere(gtState.obstaclePos, 1.0f);
}
}
}
}
}
}