/*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(); }