List <float[]> getVelocityObstacle(MotionModelSwedish buddy)
{
List <float[]> vertices = new List <float[]>();
//Skip if buddy to far away??1
float theta = Mathf.Acos((buddy.getRadius() + getRadius()) / ((buddy.getPosition() - getPosition()).magnitude));
Vector3 D = getPosition() - buddy.getPosition();
float baseAngle = Vector3.Angle(Vector3.right, D) * Mathf.Deg2Rad;
if (D.z < 0)
{
baseAngle = -baseAngle;
}
float x1 = buddy.getPosition().x + (buddy.getRadius() + getRadius()) * Mathf.Cos(baseAngle + theta);
float z1 = buddy.getPosition().z + (buddy.getRadius() + getRadius()) * Mathf.Sin(baseAngle + theta);
float x2 = buddy.getPosition().x + (buddy.getRadius() + getRadius()) * Mathf.Cos(baseAngle - theta);
float z2 = buddy.getPosition().z + (buddy.getRadius() + getRadius()) * Mathf.Sin(baseAngle - theta);
Vector3 dir1 = new Vector3(x1 - getPosition().x, 0, z1 - getPosition().z).normalized;
Vector3 dir2 = new Vector3(x2 - getPosition().x, 0, z2 - getPosition().z).normalized;
Vector3 point1 = getPosition() + dir1 * vMax * dt;
Vector3 point2 = getPosition() + dir2 * vMax * dt;
point1 = point1 + buddy.getVelocity();
point2 = point2 + buddy.getVelocity();
Vector3 point3 = getPosition() + buddy.getVelocity();
vertices.Add(new float[] { point1.x, point1.z });
vertices.Add(new float[] { point2.x, point2.z });
vertices.Add(new float[] { point3.x, point3.z });
return(vertices);
}
OrcaLine getOrcaLine(MotionModelSwedish buddy, float time, float dt)
{
Vector3 relPos = buddy.getPosition() - getPosition();
Vector3 relVel = getVelocity() - buddy.getVelocity();
float combRadius = buddy.getRadius() + getRadius();
OrcaLine orcaline = new OrcaLine();
Vector3 u;
Vector3 w = relVel - (relPos / time);
float decBoundary = Vector3.Dot(w, relPos);
if (relPos.sqrMagnitude > Mathf.Pow(combRadius, 2))
{
if (!(decBoundary < 0.0f && Mathf.Pow(decBoundary, 2) > Mathf.Pow(combRadius, 2) * w.sqrMagnitude))
{
// Legs
float distance = Mathf.Sqrt(relPos.sqrMagnitude - Mathf.Pow(combRadius, 2));
if (Vector3.Cross(relPos, w).y < 0.0f)
{
orcaline.direction = new Vector3(relPos.x * distance - relPos.z * combRadius, 0.0f, relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
}
else
{
orcaline.direction = -new Vector3(relPos.x * distance + relPos.z * combRadius, 0.0f, -relPos.x * combRadius + relPos.z * distance) / relPos.sqrMagnitude;
}
float uLen = Vector3.Dot(relVel, orcaline.direction);
u = uLen * orcaline.direction - relVel;
}
else
{
Vector3 wNorm = w.normalized;
orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
u = ((combRadius / time) - w.magnitude) * wNorm;
}
}
else
{
w = relVel - (relPos / dt);
Vector3 wNorm = w.normalized;
orcaline.direction = new Vector3(wNorm.z, 0.0f, -wNorm.x);
u = ((combRadius / dt) - w.magnitude) * wNorm;
}
orcaline.point = getVelocity() + 0.5f * u;
//Debug.DrawLine(getPosition() + orcaline.point - (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, getPosition() + orcaline.point + (Vector3.Cross(Vector3.up, orcaline.direction)).normalized * vMax, Color.blue, Time.deltaTime);
//Debug.DrawLine(getPosition() + orcaline.point, getPosition() + orcaline.point + orcaline.direction, Color.blue, Time.deltaTime);
return(orcaline);
}
MotionModelSwedish spawnActor(float[] origin, string name, float orientation, int index, float[] goalPoint)
{
Vector3 newOrientation = new Vector3(Mathf.Cos(orientation), 0, Mathf.Sin(orientation));
Vector3 position = new Vector3(origin[0], actorHeight, origin[1]);
Vector3 goal = new Vector3(goalPoint[0], actorHeight, goalPoint[1]);
GameObject actor = Instantiate(actorObject, actorsBoundingObject.transform, true);
actor.name = name;
actor.transform.position = position;
actor.transform.forward = newOrientation;
MotionModelSwedish mm = actor.AddComponent <MotionModelSwedish>();
mm.setParams(problem.vehicle_v_max, actorRadius, 3f, index, goal);
return(mm);
}