/// adds a point to the queue. /// @param point the point to add public override void AddPoint(ref Vector3 point) { ClearTimes(m_speeds, m_maxTimeToRemember); if (m_points.Count > 0) { TimedPoint pnt = m_points.Peek(); if (pnt.m_time + m_maxTimeToRemember > Time.time) { if (Time.time == pnt.m_time) { pnt.m_point = Vector3.zero; } else { pnt.m_point -= point; pnt.m_point /= (Time.time - pnt.m_time); pnt.m_time = Time.time; m_speeds.Enqueue(pnt); } } } base.AddPoint(ref point); }
/// <summary> /// Calculate Rod Intersection with current rod. /// /// The Main Idea of this method is to set Intersection Property of /// ControlRod Class as Intersection Coordinate with Timestamp. /// In current case both time and coordinates should be defined /// if intersection exists or both undefined because if there is no /// intersection found the timestamp is also not relevant. /// </summary> /// <param name="currentCoordinates">Current ball coordinates to calculate intersection</param> public void CalculateSectorIntersection(BallCoordinates currentCoordinates) { try { //If unable to calculate OR no intersection - set Intersection as undefined and exit if (!BallCoordinates.NotNullAndDefined(currentCoordinates) || currentCoordinates.Vector.X == 0) { Intersection = new TimedPoint(); return; } int xintersection = RodXCoordinate; if (_useDynamicSector) { xintersection = (RodXCoordinate > currentCoordinates.X) ? Convert.ToInt32(RodXCoordinate - DynamicSector / 2.0) : Convert.ToInt32(RodXCoordinate + DynamicSector / 2.0); } //find intersection time using: T = dX/V in seconds double intersectionTimestamp = (xintersection - currentCoordinates.X) / currentCoordinates.Vector.X; //If intersecion will happen in the future but before maximum future time limit if (intersectionTimestamp >= 0 && intersectionTimestamp < PredictIntersectionMaxTimespan) { TimeSpan intersectionTime = TimeSpan.FromSeconds(intersectionTimestamp); int yintersection = Convert.ToInt32(currentCoordinates.Vector.Y * intersectionTime.TotalSeconds + currentCoordinates.Y); if (_surveyor.IsCoordinatesYInRange(yintersection)) { Intersection = new TimedPoint(xintersection, yintersection, currentCoordinates.Timestamp + intersectionTime); // Log.Common.Debug(String.Format("[{0}] Found intersection point with rod [{1}]: [{2}x{3}]", // MethodBase.GetCurrentMethod().Name, _rodType, xintersection, yintersection)); } else { BallCoordinates ricoshetCoordiantes = _vectorUtils.Ricochet(currentCoordinates); CalculateSectorIntersection(ricoshetCoordiantes); } } else { //Intersection found is from the past or not in the near future and is irrelevant Intersection = new TimedPoint(); } } catch(Exception e) { Intersection = new TimedPoint(); Log.Print(String.Format("Unable to calculate rod intersection. Reason: {0}", e.Message), eCategory.Error, LogTag.DECISION); } }