private static void DrawPathAndObstacles()
{
// draw a line along each segment of path
PolylinePathway path = Globals.GetTestPath();
for (int i = 0; i < path.PointCount; i++)
{
if (i > 0)
{
Drawing.DrawLine(path.Points[i], path.Points[i - 1], Color.Red);
}
}
// draw obstacles
Drawing.DrawXZCircle(Globals.Obstacle1.Radius, Globals.Obstacle1.Center, Color.White, 40);
Drawing.DrawXZCircle(Globals.Obstacle2.Radius, Globals.Obstacle2.Center, Color.White, 40);
}
private Vector3 ComputeArrivalVelocity(Vector3 seekPos, Vector3 target, Vector3 position, Vector3 currentVelocity)
{
Vector3 targetOffset = target - position;
float distance = targetOffset.magnitude;
float rampedSpeed = m_maxSpeed * (distance / m_slowingDistance);
float minSpeed = m_maxSpeed / 4.0f;
float clippedSpeed = Mathf.Clamp(rampedSpeed, minSpeed, m_maxSpeed);
Vector3 accelerationDir = seekPos - position;
accelerationDir.y = 0.0f;
accelerationDir.Normalize();
Vector3 gravitationalForce = -Vector3.up * m_gravitationalAccelerationRate * rigidbody.mass;
Vector3 acceleration = m_accelerationRate * accelerationDir + gravitationalForce;
Vector3 newVelocity = currentVelocity + acceleration * Time.deltaTime;
newVelocity = PolylinePathway.TruncateLength(newVelocity, clippedSpeed);
return(newVelocity);
}
// create path for PlugIn
//
//
// | gap |
//
// f b
// |\ /\ -
// | \ / \ ^
// | \/ \ |
// | /\ \ |
// | / \ c top
// |/ \g / |
// / / |
// /| / V z y=0
// / |______/ - ^
// / e d |
// a/ |
// |<---out-->| o----> x
//
public static PolylinePathway GetTestPath()
{
if (TestPath == null)
{
const float pathRadius = 2;
const int pathPointCount = 7;
const float size = 30;
float top = 2 * size;
float gap = 1.2f * size;
float outter = 2 * size;
float h = 0.5f;
Vector3[] pathPoints = new Vector3[pathPointCount]
{
new Vector3(h + gap - outter, 0, h + top - outter), // 0 a
new Vector3(h + gap, 0, h + top), // 1 b
new Vector3(h + gap + (top / 2), 0, h + top / 2), // 2 c
new Vector3(h + gap, 0, h), // 3 d
new Vector3(h, 0, h), // 4 e
new Vector3(h, 0, h + top), // 5 f
new Vector3(h + gap, 0, h + top / 2) // 6 g
};
Obstacle1.Center = Utilities.Interpolate(0.2f, pathPoints[0], pathPoints[1]);
Obstacle2.Center = Utilities.Interpolate(0.5f, pathPoints[2], pathPoints[3]);
Obstacle1.Radius = 3;
Obstacle2.Radius = 5;
Obstacles.Add(Obstacle1);
Obstacles.Add(Obstacle2);
Endpoint0 = pathPoints[0];
Endpoint1 = pathPoints[pathPointCount - 1];
TestPath = new PolylinePathway(pathPointCount,
pathPoints,
pathRadius,
false);
}
return(TestPath);
}
// create path for PlugIn
//
//
// | gap |
//
// f b
// |\ /\ -
// | \ / \ ^
// | \/ \ |
// | /\ \ |
// | / \ c top
// |/ \g / |
// / / |
// /| / V z y=0
// / |______/ - ^
// / e d |
// a/ |
// |<---out-->| o----> x
//
public static PolylinePathway GetTestPath()
{
if (_testPath == null)
{
const float PATH_RADIUS = 2;
const int PATH_POINT_COUNT = 7;
const float SIZE = 30;
const float TOP = 2 * SIZE;
const float GAP = 1.2f * SIZE;
const float OUTTER = 2 * SIZE;
const float H = 0.5f;
Vector3[] pathPoints = new Vector3[PATH_POINT_COUNT]
{
new Vector3(H + GAP - OUTTER, 0, H + TOP - OUTTER), // 0 a
new Vector3(H + GAP, 0, H + TOP), // 1 b
new Vector3(H + GAP + (TOP / 2), 0, H + TOP / 2), // 2 c
new Vector3(H + GAP, 0, H), // 3 d
new Vector3(H, 0, H), // 4 e
new Vector3(H, 0, H + TOP), // 5 f
new Vector3(H + GAP, 0, H + TOP / 2) // 6 g
};
Obstacle1.Center = Vector3.Lerp(pathPoints[0], pathPoints[1], 0.2f);
Obstacle2.Center = Vector3.Lerp(pathPoints[2], pathPoints[3], 0.5f);
Obstacle1.Radius = 3;
Obstacle2.Radius = 5;
Obstacles.Add(Obstacle1);
Obstacles.Add(Obstacle2);
Endpoint0 = pathPoints[0];
Endpoint1 = pathPoints[PATH_POINT_COUNT - 1];
_testPath = new PolylinePathway(pathPoints,
PATH_RADIUS,
false);
}
return(_testPath);
}
// create path for PlugIn
//
//
// | gap |
//
// f b
// |\ /\ -
// | \ / \ ^
// | \/ \ |
// | /\ \ |
// | / \ c top
// |/ \g / |
// / / |
// /| / V z y=0
// / |______/ - ^
// / e d |
// a/ |
// |<---out-->| o----> x
//
public static PolylinePathway GetTestPath()
{
if (_testPath == null)
{
const float PATH_RADIUS = 2;
const int PATH_POINT_COUNT = 7;
const float SIZE = 30;
const float TOP = 2 * SIZE;
const float GAP = 1.2f * SIZE;
const float OUTTER = 2 * SIZE;
const float H = 0.5f;
Vector3[] pathPoints = new Vector3[PATH_POINT_COUNT]
{
new Vector3 (H+GAP-OUTTER, 0, H+TOP-OUTTER), // 0 a
new Vector3 (H+GAP, 0, H+TOP), // 1 b
new Vector3 (H+GAP+(TOP/2), 0, H+TOP/2), // 2 c
new Vector3 (H+GAP, 0, H), // 3 d
new Vector3 (H, 0, H), // 4 e
new Vector3 (H, 0, H+TOP), // 5 f
new Vector3 (H+GAP, 0, H+TOP/2) // 6 g
};
Obstacle1.Center = Vector3.Lerp(pathPoints[0], pathPoints[1], 0.2f);
Obstacle2.Center = Vector3.Lerp(pathPoints[2], pathPoints[3], 0.5f);
Obstacle1.Radius = 3;
Obstacle2.Radius = 5;
Obstacles.Add(Obstacle1);
Obstacles.Add(Obstacle2);
Endpoint0 = pathPoints[0];
Endpoint1 = pathPoints[PATH_POINT_COUNT - 1];
_testPath = new PolylinePathway(pathPoints,
PATH_RADIUS,
false);
}
return _testPath;
}
// reset all instance state
public override void Reset()
{
// reset the vehicle
base.Reset();
// max speed and max steering force (maneuverability)
MaxSpeed = 2.0f;
MaxForce = 8.0f;
// initially stopped
Speed = 0;
// size of bounding sphere, for obstacle avoidance, etc.
Radius = 0.5f; // width = 0.7, add 0.3 margin, take half
// set the path for this Pedestrian to follow
path = Globals.GetTestPath();
// set initial position
// (random point on path + random horizontal offset)
float d = path.TotalPathLength * Utilities.Random();
float r = path.radius;
Vector3 randomOffset = Vector3Helpers.RandomVectorOnUnitRadiusXZDisk() * r;
Position = (path.MapPathDistanceToPoint(d) + randomOffset);
// randomize 2D heading
RandomizeHeadingOnXZPlane();
// pick a random direction for path following (upstream or downstream)
pathDirection = (Utilities.Random() > 0.5) ? -1 : +1;
// trail parameters: 3 seconds with 60 points along the trail
trail = new Trail(3, 60);
// notify proximity database that our position has changed
if (proximityToken != null) proximityToken.UpdateForNewPosition(Position);
}
// create path for PlugIn
//
//
// | gap |
//
// f b
// |\ /\ -
// | \ / \ ^
// | \/ \ |
// | /\ \ |
// | / \ c top
// |/ \g / |
// / / |
// /| / V z y=0
// / |______/ - ^
// / e d |
// a/ |
// |<---out-->| o----> x
//
public static PolylinePathway GetTestPath()
{
if (TestPath == null)
{
const float pathRadius = 2;
const int pathPointCount = 7;
const float size = 30;
float top = 2 * size;
float gap = 1.2f * size;
float outter = 2 * size;
float h = 0.5f;
Vector3[] pathPoints = new Vector3[pathPointCount]
{
new Vector3 (h+gap-outter, 0, h+top-outter), // 0 a
new Vector3 (h+gap, 0, h+top), // 1 b
new Vector3 (h+gap+(top/2), 0, h+top/2), // 2 c
new Vector3 (h+gap, 0, h), // 3 d
new Vector3 (h, 0, h), // 4 e
new Vector3 (h, 0, h+top), // 5 f
new Vector3 (h+gap, 0, h+top/2) // 6 g
};
Obstacle1.Center = Utilities.Interpolate(0.2f, pathPoints[0], pathPoints[1]);
Obstacle2.Center = Utilities.Interpolate(0.5f, pathPoints[2], pathPoints[3]);
Obstacle1.Radius = 3;
Obstacle2.Radius = 5;
Obstacles.Add(Obstacle1);
Obstacles.Add(Obstacle2);
Endpoint0 = pathPoints[0];
Endpoint1 = pathPoints[pathPointCount - 1];
TestPath = new PolylinePathway(pathPointCount,
pathPoints,
pathRadius,
false);
}
return TestPath;
}
