public void Update(float dt) { if (!m_Destination.IsZero()) { m_Navigator.Destination = new destination(m_Destination); } m_LastGotoPos = m_Navigator.GoToPosition.pos; if (m_LastGotoPos.IsZero()) { return; } Vec3 wanted_dir = Vec3.ZERO; float dist = 0; if (!Paused && !SimulateStuck && !m_LastGotoPos.Equals(m_Navigator.CurrentPos)) { wanted_dir = m_LastGotoPos - m_Navigator.CurrentPos; dist = wanted_dir.Length(); wanted_dir.Normalize(); } m_Navigator.IsStandingOnPurpose = m_Navigator.ThreatAhead > 0; //turn m_Direction = Vec3.RotateTowards(m_Direction, wanted_dir, m_AngularSpeed * dt); //move if (m_Navigator.ThreatAhead == 0) { m_Navigator.CurrentPos = m_Navigator.CurrentPos + m_Direction * Math.Min(m_Speed * dt, dist); } }
private void PostProcessPath(ref List <Vec3> path, float merge_distance, float shift_nodes_distance) { if (merge_distance > 0) { for (int i = 0; i < path.Count - 1; ++i) { int start_count = path.Count; Vec3 merge_point = path[i]; while (i < path.Count - 1 && path[i].Distance2D(path[i + 1]) < merge_distance) { merge_point = merge_point + path[i + 1]; path.RemoveAt(i + 1); } if (path.Count != start_count) { path[i] = merge_point / (float)(start_count - path.Count + 1); } } } // shift points to increase movement accuracy if (shift_nodes_distance > 0) { for (int i = path.Count - 2; i > 0; --i) { Vec3 dir_to_next = path[i] - path[i - 1]; dir_to_next.Normalize(); path[i] += dir_to_next * shift_nodes_distance; } } }
public void Update(float dt) { if (m_GotoPosUpdateTimer.ElapsedMilliseconds > GOTO_POS_UPDATE_INTERVAL) { m_LastGotoPos = m_Navigator.GoToPosition.pos; m_GotoPosUpdateTimer.Restart(); } if (!m_Destination.IsZero()) { m_Navigator.Destination = new destination(m_Destination); } if (m_LastGotoPos.IsZero()) { return; } Vec3 dir = Vec3.ZERO; float dist = 0; if (!Paused && !SimulateStuck && !m_LastGotoPos.Equals(m_Navigator.CurrentPos)) { dir = m_LastGotoPos - m_Navigator.CurrentPos; dist = dir.Length(); dir.Normalize(); } m_Navigator.IsStandingOnPurpose = m_Navigator.IsThreatAhead; if (!m_Navigator.IsThreatAhead) { m_Navigator.CurrentPos = m_Navigator.CurrentPos + dir * Math.Min(m_Speed * dt, dist); } }
public Vec3 GetRandomPosInRing(Vec3 ring_center, float min_radius, float max_radius) { var potential_grid_cells = GetCellsOverlappedByCircle(m_GridCells, ring_center, max_radius); List <Cell> cells = new List <Cell>(); foreach (GridCell grid_cell in potential_grid_cells) { cells.AddRange(grid_cell.Cells.FindAll(x => x.Overlaps(ring_center, max_radius) && !x.AABB.Inside(ring_center, min_radius))); } Cell random_cell = cells[Rng.Next(cells.Count)]; Vec3 random_pos = random_cell.AABB.GetRandomPos(); float dist = random_pos.Distance(ring_center); if (dist > min_radius && dist <= max_radius) { return(random_pos); } //align position to ring Vec3 dir_to_random_pos = random_pos - ring_center; dir_to_random_pos.Normalize(); random_pos = ring_center + dir_to_random_pos * (min_radius + (float)Rng.NextDouble() * (max_radius - min_radius)); return(random_cell.AABB.Align(random_pos)); }
public bool RayTrace(Vec3 from, Vec3 to, MovementFlag flags, out Vec3 intersection) { using (new ReadLock(DataLock)) { intersection = Vec3.Empty; Vec3 ray_dir = to - from; ray_dir.Normalize(); foreach (Cell c in m_AllCells) { if (!c.HasFlags(flags)) { continue; } Vec3 new_intersection = null; if (c.AABB.RayTest(from, ray_dir, ref new_intersection) && from.DistanceSqr(new_intersection) < from.DistanceSqr(intersection)) { intersection = new_intersection; } } return(!intersection.IsEmpty); } }
public Plane(Vec3 p1, Vec3 normal) { normal.Normalize(); A = normal.X; B = normal.Y; C = normal.Z; D = -A * p1.X - B * p1.Y - C * p1.Z; }
public Plane(Vec3 p1, Vec3 p2, Vec3 p3) { Vec3 v1 = p2 - p1; Vec3 v2 = p3 - p1; Vec3 plane_norm = v1.Cross(v2); plane_norm.Normalize(); A = plane_norm.X; B = plane_norm.Y; C = plane_norm.Z; D = -A * p1.X - B * p1.Y - C * p1.Z; }
private void SmoothenPath(ref List <path_pos> path, MovementFlag flags, int skip_first_count = 0) { int ray_start_index = skip_first_count; Vec3 intersection = null; while (ray_start_index + 2 < path.Count) { path_pos ray_start_data = path[ray_start_index]; path_pos intermediate_data = path[ray_start_index + 1]; path_pos ray_end_data = path[ray_start_index + 2]; // try remove middle point completely if (m_Navmesh.RayCast2D(ray_start_data.pos, ray_end_data.pos, flags, ref intersection, false)) { path.RemoveAt(ray_start_index + 1); } else { ++ray_start_index; } } // perform second smoothing pass after getting rid of all unnecessary points to get better performance and results // this pass is basically "moving" from point to point and checking at which step (in-between points) I can already see the next point if (PathSmoothingPrecision > 0) { ray_start_index = skip_first_count; while (ray_start_index + 2 < path.Count) { path_pos ray_start_data = path[ray_start_index]; path_pos intermediate_data = path[ray_start_index + 1]; path_pos ray_end_data = path[ray_start_index + 2]; Vec3 dir = intermediate_data.pos - ray_start_data.pos; float length = dir.Normalize(); int steps = (int)(length / PathSmoothingPrecision); for (int i = 1; i < steps; ++i) // checking 0 is unnecessary since this is the current path { if (m_Navmesh.RayCast2D(ray_start_data.pos + dir * (float)i * PathSmoothingPrecision, ray_end_data.pos, flags, ref intersection, false)) { path[ray_start_index + 1] = new path_pos(ray_start_data.pos + dir * (float)i * PathSmoothingPrecision, intermediate_data.cell); break; } } ++ray_start_index; } } }
private bool InternalSegmentTest(Vec3 start, Vec3 end, ref Vec3 result, int num_dim) { Vec3 ray_dir = end - start; float length = num_dim > 2 ? ray_dir.Normalize() : ray_dir.Normalize2D(); float length_2 = length * length; if (InternalRayTest(start, ray_dir, ref result, num_dim)) { if ((num_dim > 2 ? result.DistanceSqr(start) : result.Distance2DSqr(start)) <= length_2) { return(true); } result = end; return(false); } return(false); }
public void Update(float dt) { if (!m_Destination.IsZero()) { m_Navigator.Destination = new destination(m_Destination); } m_LastGotoPos = m_Navigator.GoToPosition.pos; var newPos = m_Navigator.CurrentPos; if (!m_LastGotoPos.IsZero()) { Vec3 wanted_dir = Vec3.ZERO; float dist = 0; wanted_dir = m_LastGotoPos - m_Navigator.CurrentPos; dist = wanted_dir.Length(); wanted_dir.Normalize(); //turn if (m_AngularSpeed > 0) { m_Direction = Vec3.RotateTowards(m_Direction, wanted_dir, m_AngularSpeed * dt); } else { m_Direction = wanted_dir; } //move if (m_Navigator.ThreatAhead.Item2 == 0 && !Paused && !SimulateStuck) { newPos += m_Direction * Math.Min(m_Speed * dt, dist); } } m_Navigator.CurrentPos = newPos; m_Navigator.IsStandingOnPurpose = m_Navigator.ThreatAhead.Item2 > 0; }
// Aquires DataLock (read); returns true when there is no obstacle private bool RayCast(Vec3 from, Cell from_cell, Vec3 to, MovementFlag flags, ref Vec3 intersection, bool test_2d, bool ignore_movement_cost, ref HashSet <Cell> ignored_cells) { using (new ReadLock(DataLock)) { if (to.IsEmpty) { intersection = new Vec3(from); return(false); } if (from_cell == null && !GetCellContaining(from, out from_cell, flags, false, false, -1, test_2d, 2, ignored_cells)) { intersection = new Vec3(from); return(false); } if (test_2d ? from_cell.Contains2D(to) : from_cell.Contains(to, 2)) { intersection = new Vec3(to); return(true); } ignored_cells.Add(from_cell); Vec3 ray_dir = to - from; if (test_2d) { ray_dir.Normalize2D(); } else { ray_dir.Normalize(); } Vec3 ray_origin = new Vec3(from); // check if intersection in foreach (Cell.Neighbour neighbour in from_cell.Neighbours) { if (neighbour.cell.Disabled || (neighbour.connection_flags & flags) != flags || (!ignore_movement_cost && neighbour.cell.MovementCostMult > from_cell.MovementCostMult)) { continue; } Cell neighbour_cell = neighbour.cell; if (ignored_cells.Contains(neighbour_cell)) { continue; } bool ray_test_result = test_2d ? neighbour_cell.AABB.RayTest2D(ray_origin, ray_dir, ref intersection) : neighbour_cell.AABB.RayTest(ray_origin, ray_dir, ref intersection); if (ray_test_result) { AABB shared_aabb = test_2d ? from_cell.AABB.Intersect2D(neighbour_cell.AABB, true) : from_cell.AABB.Intersect(neighbour_cell.AABB, true); // ray intersects on connection plane if (shared_aabb != null) { bool accepted = test_2d ? shared_aabb.Contains2D(intersection) : shared_aabb.Contains(intersection); if (accepted && RayCast(intersection, neighbour_cell, to, flags, ref intersection, test_2d, ignore_movement_cost, ref ignored_cells)) { return(true); } } } } return(false); } }