// Test integration of kinematic model
public void TestKinematicModel(Vector3 startP, ref List <Edge> edges)
{
// Initital values
float max_steering = 10F; // steering
const float V = 10F; // speed
const float K = 1000;
float t = 0;
Vector3 xnew = startP;
Vector3 x = startP;
float dt = Time.deltaTime;
InputVec input = new InputVec(max_steering, V);
OutputVec output = new OutputVec(startP.x, startP.z, max_steering);
for (float k = 0; k < K; k++)
{
x = xnew;
input.steering = max_steering * ((K / 2 - k) / K);
output.theta += runge_kutta_integration(compute_theta, dt, input, ref output);
output.x += runge_kutta_integration(compute_x, dt, input, ref output);
output.z += runge_kutta_integration(compute_z, dt, input, ref output);
xnew.x = output.x;
xnew.z = output.z;
xnew.y = 0.1F;
edges.Add(new Edge(new Vertex(x), new Vertex(xnew), input.steering));
t += dt;
//Debug.Log($"t={t} input.steering={input.steering} theta={output.theta} x={output.x} z={output.z}");
}
}
public float compute_z(InputVec input, ref OutputVec output)
{
float theta = output.theta;
float z = input.v * Mathf.Cos(theta);
return(z);
}
public float compute_x(InputVec input, ref OutputVec output)
{
float theta = output.theta;
float x = input.v * Mathf.Sin(theta);
return(x);
}
public float runge_kutta_integration(MyFunc f, float dt, InputVec input, ref OutputVec output)
{
float result = 0;
float k1 = f(input, ref output);
//Debug.Log($"k1={k1}");
float k2 = f(input.Add(k1 / 2), ref output);
//Debug.Log($"k2={k2}");
float k3 = f(input.Add(k2 / 2), ref output);
//Debug.Log($"k3={k3}");
float k4 = f(input.Add(k3), ref output);
//Debug.Log($"k4={k4}");
result = (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4);
return(result);
}
Vertex new_state2(Vertex xnear, float unew, float dt)
{
// Initital values
float V = Velocity; // speed
const float max_steering = 40;
InputVec input = new InputVec(0, V);
OutputVec output = new OutputVec(xnear.value.x, xnear.value.z, xnear.theta);
//Rnd.Range(-max_steering, max_steering);
//Mathf.Atan((unew * InputVec.L ) / V ) * Mathf.Rad2Deg;
Vertex v_out = new Vertex();
v_out.parent = xnear;
v_out.steering_law = new SteeringLaw(xnear.steering_law);
// change steering law every 100 interations
/*
* if (v_out.steering_law.isTimeOut(Time.deltaTime))
* {
* SteeringLaw new_steering_law = new SteeringLaw(Rnd.Range(-max_steering, max_steering),
* Rnd.Range(-max_steering, max_steering),
* 100 * Time.deltaTime);
* v_out.steering_law = new_steering_law;
*
* //Debug.Log("new_steering_law " + new_steering_law.toString() );
* }
*/
//input.steering = v_out.steering_law.getSample(Time.deltaTime); //(unew - xnear.theta) * Mathf.Rad2Deg; //Rnd.Range(-max_steering, max_steering);
input.steering = unew * 180 / Mathf.PI;
//input.steering = Rnd.Range(-max_steering, max_steering);
//Debug.Log($"k={ v_out.k } steering={input.steering} degrees");
output.theta += model.runge_kutta_integration(model.compute_theta, dt, input, ref output);
output.x += model.runge_kutta_integration(model.compute_x, dt, input, ref output);
output.z += model.runge_kutta_integration(model.compute_z, dt, input, ref output);
v_out.value = new Vector3(output.x, TreeHeight, output.z);
v_out.theta = output.theta;
return(v_out);
}
public float compute_theta(InputVec input, ref OutputVec output)
{
float theta = (input.v / InputVec.L) * Mathf.Tan(input.steering * Mathf.Deg2Rad);
return(theta);
}
