/*void SpawnUnit()
{
Ray _ray = Camera.main.ScreenPointToRay(Input.mousePosition);
RaycastHit _hit;
if (Input.GetMouseButtonDown(0) && Physics.Raycast(_ray, out _hit, Mathf.Infinity, layerMask) && (_hit.point - spawnArea.position).sqrMagnitude < spawnArea.localScale.x * spawnArea.localScale.x / 4)
{
GameObject _unit = Instantiate(unit, _hit.point, Quaternion.identity) as GameObject;
AddAgent(_unit, goal.position);
}
}*/
float CalculateError(RVO.Agent agent, float angle)
{
float t = Time.deltaTime;
float V2 = agent.velocity_.x_ * agent.velocity_.x_ + agent.velocity_.y_ * agent.velocity_.y_;
float V = Mathf.Sqrt(V2);
return (Mathf.Sqrt(maxSpeed * maxSpeed * t * t - 2 * maxSpeed * t * Mathf.Sin(angle) / maxAngularSpeed * V + 2 * (1 - Mathf.Cos(angle)) / maxAngularSpeed / maxAngularSpeed * V2)) * maxSpeed;
}
void StartObjects()
{
RVO a1s = a1.GetComponent <RVO>();
if (a1s)
{
a1s.Go();
}
else
{
Debug.Log("Can't find script for agent 1");
}
RVO a2s = a2.GetComponent <RVO>();
if (a2s)
{
a2s.Go();
}
else
{
Debug.Log("Can't find script for agent 2");
}
}
// Update is called once per frame
void Update()
{
// Reset to initial condition
if ((transform.position - target.transform.position).sqrMagnitude < 0.05)
{
running = false;
timeToReset -= Time.deltaTime;
if (timeToReset < 0)
{
RVO other = otherAgent.GetComponent <RVO>();
other.Reset();
Reset();
}
}
if (running)
{
direction = target.transform.position - transform.position;
direction.Normalize();
direction *= speed;
RVO other = otherAgent.GetComponent <RVO>();
// Get our heading after subtracting opposite agent heading
Vector3 adjustedHeading = direction - other.lastDirection;
// Get our speed in that heading
float adjustedSpeed = adjustedHeading.magnitude;
// Get heading towards the other agent
Vector3 headingToAgent = other.transform.position - transform.position;
// Get ratio so we can compute the angle to go around the other agent
float ratio = 2.0f / headingToAgent.magnitude;
if (ratio > 1)
{
Debug.Log("Collision!");
ratio = 1;
}
// Compute angle to go around agent
float angle = Mathf.Asin(ratio) * Mathf.Rad2Deg;
// Get vectors for heading right/left
Vector3 rightHeading = Quaternion.Euler(0, angle, 0) * headingToAgent;
Vector3 leftHeading = Quaternion.Euler(0, -angle, 0) * headingToAgent;
// Normalize vectors
rightHeading = rightHeading.normalized * adjustedSpeed;
leftHeading = leftHeading.normalized * adjustedSpeed;
// Draw debug information
if (showDebug)
{
Debug.DrawLine(transform.position, transform.position + rightHeading, Color.red);
Debug.DrawLine(transform.position, transform.position + headingToAgent, Color.white);
Debug.DrawLine(transform.position, transform.position + leftHeading, Color.yellow);
Debug.DrawLine(transform.position, transform.position + direction * 2, Color.grey);
Debug.DrawLine(transform.position, transform.position + adjustedHeading * 2, Color.blue);
}
// Check if we have a VO collision
if (GetSide(Vector3.zero, leftHeading, adjustedHeading) < 0 &&
GetSide(Vector3.zero, rightHeading, adjustedHeading) > 0)
{
// Debug.Log ("On target to collide");
// Move towards the side that is closest to our current velocity
if (Vector3.Distance(adjustedHeading, rightHeading) < Vector3.Distance(adjustedHeading, leftHeading))
{
// Debug.Log("Going right");
direction = (rightHeading + other.lastDirection);
// direction = (rightHeading + other.lastDirection) * 0.5f + 0.5f * direction;
}
else
{
// Debug.Log("Going left");
direction = (leftHeading + other.lastDirection);
// direction = (leftHeading+other.lastDirection) * 0.5f + 0.5f * direction;
}
if (showDebug)
{
Debug.DrawLine(transform.position, transform.position + direction * 2, Color.magenta);
}
}
}
}
void DebugDraw(int i, Vector3 nullPoint, Vector3 prefVel, RVO.Agent agent)
{
Debug.DrawRay(agents[i].body.transform.position, new Vector3(agent.velocity_.x_, 0, agent.velocity_.y_), Color.red);
Debug.DrawRay(agents[i].body.transform.position, prefVel, Color.blue);
Debug.DrawLine( new Vector3 (simulator.agents_ [agents[i].index].position_.x_ + simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ + simulator.agents_ [agents[i].index].radius_) ,
new Vector3 (simulator.agents_ [agents[i].index].position_.x_ + simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ - simulator.agents_ [agents[i].index].radius_),
Color.red);
Debug.DrawLine( new Vector3 (simulator.agents_ [agents[i].index].position_.x_ + simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ - simulator.agents_ [agents[i].index].radius_) ,
new Vector3 (simulator.agents_ [agents[i].index].position_.x_ - simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ - simulator.agents_ [agents[i].index].radius_),
Color.red);
Debug.DrawLine( new Vector3 (simulator.agents_ [agents[i].index].position_.x_ - simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ - simulator.agents_ [agents[i].index].radius_) ,
new Vector3 (simulator.agents_ [agents[i].index].position_.x_ - simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ + simulator.agents_ [agents[i].index].radius_),
Color.red);
Debug.DrawLine( new Vector3 (simulator.agents_ [agents[i].index].position_.x_ - simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ + simulator.agents_ [agents[i].index].radius_) ,
new Vector3 (simulator.agents_ [agents[i].index].position_.x_ + simulator.agents_ [agents[i].index].radius_, 0, simulator.agents_ [agents[i].index].position_.y_ + simulator.agents_ [agents[i].index].radius_),
Color.red);
foreach (Vector3 point in agents[i].navMeshAgent.path.corners)
{
Debug.DrawLine(nullPoint, point);
nullPoint = point;
}
}
public Vector3 VectorConvert(RVO.Vector2 vector)
{
return new Vector3 (vector.x_, 0, vector.y_);
}
void ExchangeAgents(RVO.Agent agent1, RVO.Agent agent2)
{
RVO.Agent agentTemp = agent1;
agent1 = agent2;
agent2 = agentTemp;
}
public void Drive(GameObject unit, int agentNo, RVO.Vector2 dir)
{
simulator.agents_ [agentNo].prefVelocity_ = dir;
}
protected override void OnUpdate()
{
var invTimeStep = 1 / Time.DeltaTime;
var localToWorldLookup = GetComponentDataFromEntity <LocalToWorld>(true);
Entities
.WithBurst()
.WithReadOnly(localToWorldLookup)
.ForEach((Translation translation, RadiusComponent radius, VelocityComponent velocity, DynamicTreeElementComponent dynamicTree, ref VelocityObstacleComponent obstacle) =>
{
var transform = math.inverse(localToWorldLookup[dynamicTree.Tree].Value);
obstacle.Position = math.transform(transform, translation.Value).xz;
obstacle.Velocity = velocity.Value;
obstacle.Radius = radius;
})
.ScheduleParallel();
var velocityObstacleLookup = GetComponentDataFromEntity <VelocityObstacleComponent>(true);
var obstacleTreeLookup = GetComponentDataFromEntity <ObstacleTreeComponent>(true);
Entities
.WithBurst()
.WithReadOnly(velocityObstacleLookup)
.WithReadOnly(obstacleTreeLookup)
.WithReadOnly(localToWorldLookup)
.ForEach((Translation translation, RadiusComponent radius, DynamicTreeElementComponent agentTree, ObstacleTreeAgentComponent obstacleTree,
RVOSettingsComponent agent, PreferredVelocityComponent preferredVelocity, MaxSpeedComponent maxSpeed, ref VelocityComponent velocity) =>
{
Assert.IsTrue(agentTree.Tree == obstacleTree.Tree);
var ltw = localToWorldLookup[agentTree.Tree].Value;
var inv = math.inverse(ltw);
var pos = math.transform(inv, translation.Value).xz;
var neighbours = ComputeNeighbours(agent, agentTree, pos, velocityObstacleLookup);
var obstacleNeighbours = new NativeList <ObstacleDistance>(16, Allocator.Temp);
var obstacleDist = agent.TimeHorizonObst * maxSpeed.Value + radius;
var ext = obstacleDist / 2;
var aabb = new AABB {
LowerBound = pos - ext, UpperBound = pos + ext
};
obstacleTreeLookup[obstacleTree.Tree].TreeRef.Query(new ObstacleCollector(pos, obstacleDist, obstacleNeighbours), aabb);
RVO.ComputeNewVelocity(agent, pos, radius, neighbours, obstacleNeighbours, invTimeStep, preferredVelocity.Value, velocity.Value, maxSpeed.Value, ref velocity.Value);
velocity.WorldSpace = math.rotate(ltw, velocity.Value.ToXxY());
})
.ScheduleParallel();
Entities
.WithBurst()
.WithNone <ObstacleTreeAgentComponent>()
.WithReadOnly(velocityObstacleLookup)
.WithReadOnly(localToWorldLookup)
.ForEach((Translation translation, RadiusComponent radius, DynamicTreeElementComponent agentTree, RVOSettingsComponent agent,
PreferredVelocityComponent preferredVelocity, MaxSpeedComponent maxSpeed, ref VelocityComponent velocity) =>
{
var ltw = localToWorldLookup[agentTree.Tree].Value;
var inv = math.inverse(ltw);
var pos = math.transform(inv, translation.Value).xz;
var neighbours = ComputeNeighbours(agent, agentTree, pos, velocityObstacleLookup);
var obstacleNeighbours = new NativeList <ObstacleDistance>(0, Allocator.Temp);
RVO.ComputeNewVelocity(agent, pos, radius, neighbours, obstacleNeighbours, invTimeStep, preferredVelocity.Value, velocity.Value, maxSpeed.Value, ref velocity.Value);
velocity.WorldSpace = math.rotate(ltw, velocity.Value.ToXxY());
})
.ScheduleParallel();
}
