public bool QueryCallback(Entity node) { var velocityObstacle = _velocityObstacleLookup[node]; var neighbour = new VelocityObstacle(velocityObstacle); // todo should probably take in to account neighbour radius here, it could be very large var distSq = math.lengthsq(_position - neighbour.Position); if (distSq < _rangeSq) { neighbour.Dist = distSq; if (_neighbours.Length < _maxResults) { _neighbours.Add(neighbour); } var i = _neighbours.Length - 1; while (i != 0 && distSq < _neighbours[i - 1].Dist) { _neighbours[i] = _neighbours[--i]; } _neighbours[i] = neighbour; if (_neighbours.Length == _maxResults) { _rangeSq = _neighbours[_neighbours.Length - 1].Dist; } } return(true); }
//TODO this function will take as input the agent and its //_desired_ velocity (that can be computed by the current goal) //and will return the best _desired_ velocity compatible with the VO obstacles //computed previously public void Update(AIController agent) { for (int i = 0; i < _forbiddenDirections.Count; i++) { _forbiddenDirections[i] = 0; } //for (int i = 0; i < _interestingDirections.Count; i++) //{ // _interestingDirections[i] = 0; //} List <VelocityObstacle> velocityObstacles = agent.GetVelocityObstacles(); Transform agentTransform = agent.transform; for (int i = 0; i < velocityObstacles.Count; i++) { VelocityObstacle VO = velocityObstacles[i]; Vector3 voLeftLocal = agentTransform.InverseTransformVector(VO.VOLeft); Vector3 voRightLocal = agentTransform.InverseTransformVector(VO.VORight); for (int dirIndex = 0; dirIndex < _directionBounds.Count; dirIndex++) { DirectionBounds directionBounds = _directionBounds[i]; int rightIndex = -1; int leftIndex = -1; if (Vector3.Dot(voRightLocal, directionBounds.minDir) > 0 && Vector3.Dot(voRightLocal, directionBounds.maxDir) > 0) { if (Vector3.Cross(voRightLocal, directionBounds.minDir).y *Vector3.Cross(voLeftLocal, directionBounds.maxDir).y < 0) { rightIndex = dirIndex; } } if (Vector3.Dot(voLeftLocal, directionBounds.minDir) > 0 && Vector3.Dot(voLeftLocal, directionBounds.maxDir) > 0) { if (Vector3.Cross(voLeftLocal, directionBounds.minDir).y *Vector3.Cross(voLeftLocal, directionBounds.maxDir).y < 0) { leftIndex = dirIndex; } } bool leftIndexReached = false; for (int safetyCounter = 0; safetyCounter < DIRECTION_COUNT && !leftIndexReached; safetyCounter++) { int index = (rightIndex + safetyCounter) % DIRECTION_COUNT; _forbiddenDirections[index] = -1.0f; leftIndexReached = (index == leftIndex); } } } //TODO now in _forbiddenDirections we have stored what directions are forbidden //at this point the agent has to compute what is the closest allowed direction //to the direction defined by the current goal Vector3 goalLocal = agentTransform.InverseTransformPoint(agent.GetGoal()); }
bool CheckInsideVO(Vector3 intersection, VelocityObstacle vobj) { Line A = LineToClosestPoint(intersection, vobj.sideL, vobj.apex); Line B = LineToClosestPoint(intersection, vobj.sideR, vobj.apex); double angle = angleBetween2Lines(A, B) * (180.0 / Mathf.PI); if (angle > 90 && angle < 180) { return(true); } else { return(false); } }
// Update is called once per frame public void Update() { int idVO = 0; foreach (KinematicData i in DynamicObstacles) { VelocityObstacle VO = new VelocityObstacle(Obstacles, DynamicObstacles) { Character = i, detectRadius = 20f }; Debug.Log("Calculate VO"); List <VelocityObstacle> VOs = VO.calculateVOs(); Debug.Log("Calculated VO"); List <Vector3> intersections = new List <Vector3>(); List <Line> lines = new List <Line>(); float finalDistance = float.MaxValue; Vector3 finalVelocity = new Vector3(); foreach (VelocityObstacle velocityObstacle in VOs) { lines.Add(new Line { id = idVO, origin = velocityObstacle.apex, end = velocityObstacle.apex + velocityObstacle.sideR * 1000 }); lines.Add(new Line { id = idVO, origin = velocityObstacle.apex, end = velocityObstacle.apex + velocityObstacle.sideL * 1000 }); } foreach (Line line1 in lines) { foreach (Line line2 in lines) { if (line1.id != line2.id) { Vector3 intersection = line1.intersectWith(line2); if (intersection != Vector3.zero) { intersections.Add(intersection); } } } } foreach (Vector3 intersection in intersections) { foreach (VelocityObstacle vobj in VOs) { if (CheckInsideVO(intersection, vobj)) { intersections.Remove(intersection); break; } } } foreach (Vector3 candidate in intersections) { float distance = GetDistance(i.prefVelocity.x, i.prefVelocity.z, candidate.x, candidate.z); if (distance < finalDistance) { finalDistance = distance; finalVelocity = candidate; } } if (finalVelocity != Vector3.zero) { i.velocity = finalVelocity; } //Now that we have all the candidates, we need to check which of the candidates //is closer to the preferred velocity, and change the velocity of the Character //to the velocity Debug.Log(i.velocity); } }