ObstacleSegment[] obstacleArray = new ObstacleSegment[11]; //the obstacle's blocks and positions will be determined in an array before spawning
// Use this for initialization
public void ForcedStart() //controlled by obstacle spawner
// entranceHeight = 5;
{
entranceSize = 3;
EntranceSegments entranceSegs = new EntranceSegments(entranceHeight, entranceSize);
for (int i = 0; i < obstacleArray.Length; i++)
{
obstacleArray[i] = new ObstacleSegment();
for (int j = 0; j < entranceSize; j++)
{
if (i == entranceSegs.GetEntranceHeights()[j])
{
obstacleArray[i].isEmpty = false; //lets add trigger colliders for score count
}
}
}
//draw
for (int i = 0; i < obstacleArray.Length; i++)
{
if (obstacleArray[i].isEmpty == true)
{
GameObject obj = Instantiate(block[0], new Vector3(transform.position.x, i - 5, 4), Quaternion.identity) as GameObject;
obj.transform.parent = this.transform;
}
else
{
GameObject entrance = Instantiate(entranceTrigger, new Vector3(transform.position.x + .9f, i - 5, 4), Quaternion.identity) as GameObject;
entrance.transform.parent = this.transform;
}
Debug.Log(obstacleArray[i].isEmpty + " | ");
}
}
public float ProcessSample(Vector3 vcand, float cs, Vector3 position, float radius, Vector3 vel, Vector3 desiredVel)
{
//find min time of impact and exit amongst all obstacles
float tmin = parameters.HorizTime;
float side = 0;
int numSide = 0;
for (int i = 0; i < numCircles; i++)
{
ObstacleCircle cir = circles[i];
//RVO
Vector3 vab = vcand * 2;
vab = vab - vel;
vab = vab - cir.Vel;
//side
side += MathHelper.Clamp(Math.Min(Vector3Extensions.Dot2D(ref cir.Dp, ref vab) * 0.5f + 0.5f,
Vector3Extensions.Dot2D(ref cir.Np, ref vab) * 2.0f), 0.0f, 1.0f);
numSide++;
float htmin = 0, htmax = 0;
if (!SweepCircleCircle(position, radius, vab, cir.Position, cir.Radius, ref htmin, ref htmax))
{
continue;
}
//handle overlapping obstacles
if (htmin < 0.0f && htmax > 0.0f)
{
//avoid more when overlapped
htmin = -htmin * 0.5f;
}
if (htmin >= 0.0f)
{
//the closest obstacle is sometime ahead of us, keep track of nearest obstacle
if (htmin < tmin)
{
tmin = htmin;
}
}
}
for (int i = 0; i < numSegments; i++)
{
ObstacleSegment seg = segments[i];
float htmin = 0;
if (seg.Touch)
{
//special case when the agent is very close to the segment
Vector3 sdir = seg.Q - seg.P;
Vector3 snorm = new Vector3(0, 0, 0);
snorm.X = -sdir.Z;
snorm.Z = sdir.X;
//if the velocity is pointing towards the segment, no collision
if (Vector3Extensions.Dot2D(ref snorm, ref vcand) < 0.0f)
{
continue;
}
//else immediate collision
htmin = 0.0f;
}
else
{
if (!IntersectRaySegment(position, vcand, seg.P, seg.Q, ref htmin))
{
continue;
}
}
//avoid less when facing walls
htmin *= 2.0f;
//the closest obstacle is somewhere ahead of us, keep track of the nearest obstacle
if (htmin < tmin)
{
tmin = htmin;
}
}
//normalize side bias
if (numSide != 0)
{
side /= numSide;
}
float vpen = parameters.WeightDesVel * (Vector3Extensions.Distance2D(vcand, desiredVel) * invVmax);
float vcpen = parameters.WeightCurVel * (Vector3Extensions.Distance2D(vcand, vel) * invVmax);
float spen = parameters.WeightSide * side;
float tpen = parameters.WeightToi * (1.0f / (0.1f + tmin * invHorizTime));
float penalty = vpen + vcpen + spen + tpen;
return(penalty);
}
