public ObstacleAvoidanceParams(ObstacleAvoidanceParams param)
{
velBias = param.velBias;
weightDesVel = param.weightDesVel;
weightCurVel = param.weightCurVel;
weightSide = param.weightSide;
weightToi = param.weightToi;
horizTime = param.horizTime;
gridSize = param.gridSize;
adaptiveDivs = param.adaptiveDivs;
adaptiveRings = param.adaptiveRings;
adaptiveDepth = param.adaptiveDepth;
}
public int SampleVelocityGrid(Vector3 pos, float rad, float vmax, Vector3 vel, Vector3 desiredVel,
ref Vector3 nvel, ObstacleAvoidanceParams parameters)
{
Prepare(pos, desiredVel);
this.parameters = parameters;
this.invHorizTime = 1.0f / this.parameters.HorizTime;
this.vmax = vmax;
this.invVmax = 1.0f / vmax;
nvel = new Vector3(0, 0, 0);
float cvx = desiredVel.X * this.parameters.VelBias;
float cvz = desiredVel.Z * this.parameters.VelBias;
float cs = vmax * 2 * (1 - this.parameters.VelBias) / (float)(this.parameters.GridSize - 1);
float half = (this.parameters.GridSize - 1) * cs * 0.5f;
float minPenalty = float.MaxValue;
int numSamples = 0;
for (int y = 0; y < this.parameters.GridSize; y++)
{
for (int x = 0; x < this.parameters.GridSize; x++)
{
Vector3 vcand = new Vector3(0, 0, 0);
vcand.X = cvx + x * cs - half;
vcand.Y = 0;
vcand.Z = cvz + y * cs - half;
if (vcand.X * vcand.X + vcand.Z * vcand.Z > (vmax + cs / 2) * (vmax + cs / 2))
{
continue;
}
float penalty = ProcessSample(vcand, cs, pos, rad, vel, desiredVel);
numSamples++;
if (penalty < minPenalty)
{
minPenalty = penalty;
nvel = vcand;
}
}
}
return(numSamples);
}
public int SampleVelocityAdaptive(Vector3 position, float radius, float vmax, Vector3 vel,
Vector3 desiredVel, ref Vector3 nvel, ObstacleAvoidanceParams parameters)
{
Prepare(position, desiredVel);
this.parameters = parameters;
this.invHorizTime = 1.0f / parameters.HorizTime;
this.vmax = vmax;
this.invVmax = 1.0f / vmax;
nvel = new Vector3(0, 0, 0);
//build sampling pattern aligned to desired velocity
float[] pattern = new float[(MaxPatternDivs * MaxPatternRings + 1) * 2];
int numPatterns = 0;
int numDivs = parameters.AdaptiveDivs;
int numRings = parameters.AdaptiveRings;
int depth = parameters.AdaptiveDepth;
int newNumDivs = MathHelper.Clamp(numDivs, 1, MaxPatternDivs);
int newNumRings = MathHelper.Clamp(numRings, 1, MaxPatternRings);
float da = (1.0f / newNumDivs) * (float)Math.PI * 2;
float dang = (float)Math.Atan2(desiredVel.Z, desiredVel.X);
//always add sample at zero
pattern[numPatterns * 2 + 0] = 0;
pattern[numPatterns * 2 + 1] = 0;
numPatterns++;
for (int j = 0; j < newNumRings; j++)
{
float r = (float)(newNumRings - j) / (float)newNumRings;
float a = dang + (j & 1) * 0.5f * da;
for (int i = 0; i < newNumDivs; i++)
{
pattern[numPatterns * 2 + 0] = (float)Math.Cos(a) * r;
pattern[numPatterns * 2 + 1] = (float)Math.Sin(a) * r;
numPatterns++;
a += da;
}
}
//start sampling
float cr = vmax * (1.0f - parameters.VelBias);
Vector3 res = new Vector3(desiredVel.X * parameters.VelBias, 0, desiredVel.Z * parameters.VelBias);
int ns = 0;
for (int k = 0; k < depth; k++)
{
float minPenalty = float.MaxValue;
Vector3 bvel = new Vector3(0, 0, 0);
for (int i = 0; i < numPatterns; i++)
{
Vector3 vcand = new Vector3();
vcand.X = res.X + pattern[i * 2 + 0] * cr;
vcand.Y = 0;
vcand.Z = res.Z + pattern[i * 2 + 1] * cr;
if (vcand.X * vcand.X + vcand.Z * vcand.Z > (vmax + 0.001f) * (vmax + 0.001f))
{
continue;
}
float penalty = ProcessSample(vcand, cr / 10, position, radius, vel, desiredVel);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
bvel = vcand;
}
}
res = bvel;
cr *= 0.5f;
}
nvel = res;
return(ns);
}
public int SampleVelocityGrid(Vector3 pos, float rad, float vmax, Vector3 vel, Vector3 desiredVel, ref Vector3 nvel, ObstacleAvoidanceParams parameters)
{
Prepare(pos, desiredVel);
this.parameters = parameters;
this.invHorizTime = 1.0f / this.parameters.HorizTime;
this.vmax = vmax;
this.invVmax = 1.0f / vmax;
nvel = new Vector3(0, 0, 0);
float cvx = desiredVel.X * this.parameters.VelBias;
float cvz = desiredVel.Z * this.parameters.VelBias;
float cs = vmax * 2 * (1 - this.parameters.VelBias) / (float)(this.parameters.GridSize - 1);
float half = (this.parameters.GridSize - 1) * cs * 0.5f;
float minPenalty = float.MaxValue;
int numSamples = 0;
for (int y = 0; y < this.parameters.GridSize; y++)
{
for (int x = 0; x < this.parameters.GridSize; x++)
{
Vector3 vcand = new Vector3(0, 0, 0);
vcand.X = cvx + x * cs - half;
vcand.Y = 0;
vcand.Z = cvz + y * cs - half;
if (vcand.X * vcand.X + vcand.Z * vcand.Z > (vmax + cs / 2) * (vmax + cs / 2))
continue;
float penalty = ProcessSample(vcand, cs, pos, rad, vel, desiredVel);
numSamples++;
if (penalty < minPenalty)
{
minPenalty = penalty;
nvel = vcand;
}
}
}
return numSamples;
}
public int SampleVelocityAdaptive(Vector3 position, float radius, float vmax, Vector3 vel, Vector3 desiredVel, ref Vector3 nvel, ObstacleAvoidanceParams parameters)
{
Prepare(position, desiredVel);
this.parameters = parameters;
this.invHorizTime = 1.0f / parameters.HorizTime;
this.vmax = vmax;
this.invVmax = 1.0f / vmax;
nvel = new Vector3(0, 0, 0);
//build sampling pattern aligned to desired velocity
float[] pattern = new float[(MaxPatternDivs * MaxPatternRings + 1) * 2];
int numPatterns = 0;
int numDivs = parameters.AdaptiveDivs;
int numRings = parameters.AdaptiveRings;
int depth = parameters.AdaptiveDepth;
int newNumDivs = MathHelper.Clamp(numDivs, 1, MaxPatternDivs);
int newNumRings = MathHelper.Clamp(numRings, 1, MaxPatternRings);
float da = (1.0f / newNumDivs) * (float)Math.PI * 2;
float dang = (float)Math.Atan2(desiredVel.Z, desiredVel.X);
//always add sample at zero
pattern[numPatterns * 2 + 0] = 0;
pattern[numPatterns * 2 + 1] = 0;
numPatterns++;
for (int j = 0; j < newNumRings; j++)
{
float r = (float)(newNumRings - j) / (float)newNumRings;
float a = dang + (j & 1) * 0.5f * da;
for (int i = 0; i < newNumDivs; i++)
{
pattern[numPatterns * 2 + 0] = (float)Math.Cos(a) * r;
pattern[numPatterns * 2 + 1] = (float)Math.Sin(a) * r;
numPatterns++;
a += da;
}
}
//start sampling
float cr = vmax * (1.0f - parameters.VelBias);
Vector3 res = new Vector3(desiredVel.X * parameters.VelBias, 0, desiredVel.Z * parameters.VelBias);
int ns = 0;
for (int k = 0; k < depth; k++)
{
float minPenalty = float.MaxValue;
Vector3 bvel = new Vector3(0, 0, 0);
for (int i = 0; i < numPatterns; i++)
{
Vector3 vcand = new Vector3();
vcand.X = res.X + pattern[i * 2 + 0] * cr;
vcand.Y = 0;
vcand.Z = res.Z + pattern[i * 2 + 1] * cr;
if (vcand.X * vcand.X + vcand.Z * vcand.Z > (vmax + 0.001f) * (vmax + 0.001f))
continue;
float penalty = ProcessSample(vcand, cr / 10, position, radius, vel, desiredVel);
ns++;
if (penalty < minPenalty)
{
minPenalty = penalty;
bvel = vcand;
}
}
res = bvel;
cr *= 0.5f;
}
nvel = res;
return ns;
}
