コード例 #1
0
        public void Test_New()
        {
            var ca = new CollisionAnalyzer ();

            Assert.IsNotNull (ca.CollisionWorld);
            Assert.AreEqual (0, ca.CollisionObjectCount);
            Assert.AreEqual (0, ca.CollisionObjects.Count());
            Assert.AreEqual (0, ca.OverlapppingPairCount);
            Assert.AreEqual (0, ca.OverlappingPairs.Count ());
        }
コード例 #2
0
        public void Test_AddCollisionObject()
        {
            var ca = new CollisionAnalyzer ();

            var node = new Node ();
            var col = new CollisionObject ();
            col.Shape = new SphereShape (1);
            node.Attach (col);

            ca.AddGhostObject (node);

            Assert.AreEqual (1, ca.CollisionObjectCount);
            Assert.AreEqual (1, ca.CollisionObjects.Count ());
            Assert.AreEqual (true, ca.IsRegistered (node));
        }
コード例 #3
0
        static void Main(string[] args)
        {
            var AirspaceConfiguration = XMLAirspaceConfiguration.LoadAirspace(@"../../../AirspaceConfiguration.xml");

            Airspace airspace = new Airspace();

            airspace.HighestAltitude      = AirspaceConfiguration.HighestAltitude;
            airspace.LowestAltitude       = AirspaceConfiguration.LowestAltitude;
            airspace.NorthEastXCoordinate = AirspaceConfiguration.NorthEastXCoordinate;
            airspace.NorthEastYCoordinate = AirspaceConfiguration.NorthEastYCoordinate;
            airspace.SouthWestXCoordinate = AirspaceConfiguration.SouthWestXCoordinate;
            airspace.SouthWestYCoordinate = AirspaceConfiguration.SouthWestYCoordinate;

            ITransponderReceiver        transponderReceiver        = TransponderReceiverFactory.CreateTransponderDataReceiver();
            ITrackFactory               trackFactory               = new TrackFactory();
            ITransponderDataReciever    transponderDataReciever    = new TransponderDataReceiver(transponderReceiver, trackFactory);
            IAirspaceTrackChecker       airspaceTrackChecker       = new AirspaceTrackChecker(airspace);
            IAirspaceController         airspaceController         = new AirspaceController.AirspaceController(transponderDataReciever, airspaceTrackChecker);
            ITrackRemover               trackRemover               = new TrackRemover();
            ITrackManagement            trackManagement            = new TrackManagement();
            IFlightManagement           flightManagement           = new FlightManagement.FlightManagement(airspaceController, trackRemover, trackManagement);
            ICompassCalculator          compassCalculator          = new CompassCalculator();
            ICourseAnalyzer             courseAnalyzer             = new CourseAnalyzer(compassCalculator);
            IDistanceCalculator         distanceCalculator         = new DistanceCalculator();
            ITimeSpanCalculator         timeSpanCalculator         = new TimeSpanCalculator();
            IVelocityCalculator         velocityCalculator         = new VelocityCalculator(timeSpanCalculator, distanceCalculator);
            IVelocityAnalyzer           velocityAnalyzer           = new VelocityAnalyzer(velocityCalculator);
            IFlightAnalyzer             flightAnalyzer             = new FlightAnalyzer.FlightAnalyzer(flightManagement, courseAnalyzer, velocityAnalyzer);
            IAltitudeDistanceCalculator altitudeDistanceCalculator = new AltitudeDistanceCalculator();
            ICollisionAnalyzer          collisionAnalyzer          = new CollisionAnalyzer(distanceCalculator, altitudeDistanceCalculator);
            ISeparationStringBuilder    separationStringBuilder    = new SeparationStringBuilder();
            ICollisionController        collisionController        = new CollisionController.CollisionController(flightManagement, collisionAnalyzer, separationStringBuilder);
            IDisplay               display          = new ConsoleDisplay(flightAnalyzer, collisionController);
            IFileWriter            fileWriter       = new FileWriter();
            var                    currentDirectory = Directory.GetCurrentDirectory();
            var                    path             = Path.Combine(currentDirectory, "SeparationLog.txt");
            ISeparationEventLogger logger           = new CollisionController.Logger(collisionController, path, fileWriter);

            Console.ReadLine();
        }
コード例 #4
0
        public void Test_RemoveCollisionObject()
        {
            var ca = new CollisionAnalyzer ();

            var node1 = new Node ();
            var col1 = new CollisionObject ();
            col1.Shape = new SphereShape (1);
            node1.Attach (col1);

            var node2 = new Node ();
            var col2 = new CollisionObject ();
            col2.Shape = new SphereShape (1);
            node2.Attach (col2);

            ca.AddGhostObject (node1);
            ca.AddGhostObject (node2);

            ca.RemoveGhostObject (node1);

            Assert.AreEqual (1, ca.CollisionObjectCount);
            Assert.AreEqual (1, ca.CollisionObjects.Count ());
            Assert.AreEqual (true, ca.IsRegistered (node2));
        }
コード例 #5
0
        /// <summary>
        /// Updates the time-step.
        /// </summary>
        /// <exception cref="System.ArgumentNullException">
        /// Collision Analyzer failed!
        /// or
        /// Collision Analyzer failed!
        /// </exception>
        /// <exception cref="System.ArgumentException">
        /// Collision not found
        /// or
        /// </exception>
        public void TimeStepUpdate()
        {
            try
            {
                #region update destination
                bool decisionPeriodUpdate  = this.WalkTime > this.DecisionMakingPeriod;
                bool desiredStateIsNull    = this.Destination == null;
                bool distanceToDestination = true;
                if (this.Destination != null)
                {
                    distanceToDestination = UV.GetLengthSquared(this.CurrentState.Location, this.Destination) < .01d;
                }
                if (decisionPeriodUpdate || desiredStateIsNull || distanceToDestination || this.EdgeCollisionState.DistanceToBarrier < this.BodySize)
                {
                    this.updateDestination();
                }
                #endregion
                // make a copy of current state
                var newState      = this.CurrentState.Copy();
                var previousState = this.CurrentState.Copy();
                // direction update
                var direction = this.Destination - newState.Location;
                direction.Unitize();
                #region update barrier relation factors
                UV repulsionForce = new UV(0.0, 0.0);
                // this condition is for the first time only when walking begins
                if (this.EdgeCollisionState == null)
                {
                    this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
                }
                if (this.EdgeCollisionState != null)
                {
                    //only if the barrier is visible to the agent the repulstion force will be applied
                    if (this.EdgeCollisionState.NormalizedRepulsion.DotProduct(newState.Direction) >= 0.0d)//this.VisibilityCosineFactor)
                    {
                    }
                    else
                    {
                        //calculate repulsion force
                        repulsionForce = this.EdgeCollisionState.NormalizedRepulsion * this.LoadRepulsionMagnetude();
                    }
                }
                else
                {
                    throw new ArgumentNullException("Collision Analyzer failed!");
                }
                #endregion
                // find Acceleration force
                UV acceleration = this.AccelerationMagnitude * direction + repulsionForce;
                //update velocity
                newState.Velocity += acceleration * this.TimeStep;
                //check velocity magnetude against the cap
                if (newState.Velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
                {
                    newState.Velocity.Unitize();
                    newState.Velocity *= this.VelocityMagnitude;
                }
                //update location
                var location = newState.Location + this.TimeStep * newState.Velocity;
                newState.Location = location;
                //update the direction of the agent in the transformation matrix
                double deltaAngle = this.AngularVelocity * this.TimeStep;
                Axis   axis       = new Axis(this.CurrentState.Direction, direction, deltaAngle);
                newState.Direction = axis.V_Axis;
                this.CurrentState  = newState;
                //checking for collisions
                var newEdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
                if (newEdgeCollisionState == null)
                {
                    throw new ArgumentNullException("Collision Analyzer failed!");
                }
                else
                {
                    if (newEdgeCollisionState.DistanceToBarrier <= this.BodySize / 2)
                    {
                        var collision = CollisionAnalyzer.GetCollision(this.EdgeCollisionState, newEdgeCollisionState, this.BodySize / 2, OSMDocument.AbsoluteTolerance);
                        if (collision == null)
                        {
                            throw new ArgumentException("Collision not found");
                        }
                        else
                        {
                            double newTimeStep = 0.0;
                            if (collision.TimeStepRemainderProportion <= 0)
                            {
                                newTimeStep   = this.TimeStep;
                                this.TimeStep = 0.0;
                            }
                            else
                            {
                                if (collision.TimeStepRemainderProportion > 1.0)
                                {
                                    newTimeStep   = 0;
                                    this.TimeStep = TimeStep;
                                }
                                else
                                {
                                    newTimeStep   = this.TimeStep * (1.0 - collision.TimeStepRemainderProportion);
                                    this.TimeStep = this.TimeStep - newTimeStep;
                                }
                            }
                            // update velocity
                            var velocity = this.CurrentState.Velocity + newTimeStep * acceleration;
                            if (velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
                            {
                                velocity.Unitize();
                                velocity *= this.VelocityMagnitude;
                            }
                            //decompose velocity
                            UV     normal = collision.GetCollisionNormal();
                            UV     velocityVerticalComponent         = normal * (normal.DotProduct(velocity));
                            double velocityVerticalComponentLength   = velocityVerticalComponent.GetLength();
                            UV     velocityHorizontalComponent       = velocity - velocityVerticalComponent;
                            double velocityHorizontalComponentLength = velocityHorizontalComponent.GetLength();
                            //decompose acceleration
                            UV accelerationVerticalComponent = normal * (normal.DotProduct(acceleration));
                            // elasticity
                            UV ve = this.BodyElasticity * velocityVerticalComponent;
                            //friction
                            double f1 = this.BarrierFriction * velocityVerticalComponentLength;
                            double f2 = velocityHorizontalComponentLength;
                            UV     vf = velocityHorizontalComponent - Math.Min(f1, f2) * velocityHorizontalComponent / velocityHorizontalComponentLength;
                            //update velocity
                            var adjustedVelocity = vf - ve - newTimeStep * accelerationVerticalComponent;
                            if (adjustedVelocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
                            {
                                adjustedVelocity.Unitize();
                                adjustedVelocity *= this.VelocityMagnitude;
                            }
                            newState.Velocity = adjustedVelocity;
                            // update location
                            if (newTimeStep == 0)
                            {
                                newState.Location = CurrentState.Location;
                            }
                            else
                            {
                                newState.Location = collision.CollisionPoint;
                            }
                            newState.Location += OSMDocument.PenaltyCollisionReaction * normal;
                            //update direction
                            deltaAngle         = this.AngularVelocity * newTimeStep;
                            axis               = new Axis(this.CurrentState.Direction, direction, deltaAngle);
                            newState.Direction = axis.V_Axis;
                            //update state
                            newEdgeCollisionState.DistanceToBarrier = this.BodySize / 2;
                            newEdgeCollisionState.Location          = newState.Location;

                            this.EdgeCollisionState = newEdgeCollisionState;
                            this.CurrentState       = newState;
                        }
                    }
                    else
                    {
                        //update state for the full timestep length
                        this.EdgeCollisionState = newEdgeCollisionState;
                        this.CurrentState       = newState;
                        this.TimeStep           = 0.0d;
                    }
                }
            }
            catch (Exception error)
            {
                throw new ArgumentException(error.Report());
            }
        }
コード例 #6
0
 public void SetUp()
 {
     _fakeDistanceCalculator         = Substitute.For <IDistanceCalculator>();
     _fakeAltitudeDistanceCalculator = Substitute.For <IAltitudeDistanceCalculator>();
     _uut = new CollisionAnalyzer(_fakeDistanceCalculator, _fakeAltitudeDistanceCalculator);
 }
コード例 #7
0
        /// <summary>
        /// Updates the time-step.
        /// </summary>
        /// <exception cref="System.ArgumentException">
        /// Collision not found
        /// or
        /// The proportion of timestep before collision is out of range: " + collision.TimeStepRemainderProportion.ToString()
        /// </exception>
        /// <exception cref="System.ArgumentNullException">
        /// Collision Analyzer failed!
        /// or
        /// Collision Analyzer failed!
        /// </exception>
        public void TimeStepUpdate()
        {
            // make a copy of current state
            var newState      = this.CurrentState.Copy();
            var previousState = this.CurrentState.Copy();
            // direction update
            UV direction = this.CurrentState.Direction;

            try
            {
                var filed_direction = this.CurrentActivity.Differentiate(this.CurrentState.Location);
                if (filed_direction != null)
                {
                    direction = filed_direction;
                }
            }
            catch (Exception error)
            {
                throw new ArgumentException(error.Report());
            }
            direction.Unitize();
            #region update barrier relation factors
            UV repulsionForce = new UV(0.0, 0.0);
            // this condition is for the first time only when walking begins
            if (this.EdgeCollisionState == null)
            {
                this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
            }
            if (this.EdgeCollisionState != null)
            {
                //only if the barrier is visible to the agent the repulsion force will be applied
                if (this.EdgeCollisionState.NormalizedRepulsion.DotProduct(newState.Direction) >= 0.0d)//this.VisibilityCosineFactor)
                {
                    ////render agent color
                    //this.Fill = VisualAgentMandatoryScenario.OutsideRange;
                    //this.Stroke = VisualAgentMandatoryScenario.OutsideRange;
                }
                else
                {
                    double repulsionMagnitude = this.LoadRepulsionMagnitude();
                    if (repulsionMagnitude != 0.0d)
                    {
                        ////render agent color
                        //this.Fill = VisualAgentMandatoryScenario.InsideRange;
                        //this.Stroke = VisualAgentMandatoryScenario.InsideRange;
                        //calculate repulsion force
                        repulsionForce = this.EdgeCollisionState.NormalizedRepulsion * repulsionMagnitude;
                    }
                    else
                    {
                        ////render agent color
                        //this.Fill = VisualAgentMandatoryScenario.OutsideRange;
                        //this.Stroke = VisualAgentMandatoryScenario.OutsideRange;
                    }
                }
            }
            else
            {
                throw new ArgumentNullException("Collision Analyzer failed!");
            }
            #endregion
            // find Acceleration force and update velocity
            UV acceleration = new UV(0, 0);
            acceleration      += this.AccelerationMagnitude * direction + repulsionForce;
            newState.Velocity += acceleration * this.TimeStep;

            //check velocity magnitude against the cap
            if (newState.Velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
            {
                newState.Velocity.Unitize();
                newState.Velocity *= this.VelocityMagnitude;
            }
            //update location
            var location = newState.Location + this.TimeStep * newState.Velocity;
            newState.Location = location;
            //update the location of the agent in the transformation matrix
            //this._matrix.OffsetX = newState.Location.U;
            //this._matrix.OffsetY = newState.Location.V;

            // update direction
            double deltaAngle = this.AngularVelocity * this.TimeStep;
            Axis   axis       = new Axis(this.CurrentState.Direction, direction, deltaAngle);
            newState.Direction = axis.V_Axis;
            //update state
            newState.Direction.Unitize();
            //checking for collisions
            var newEdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
            if (newEdgeCollisionState == null)
            {
                this.EdgeCollisionState = CollisionAnalyzer.GetCollidingEdge(newState.Location, this._cellularFloor, BarrierType.Field);
                throw new ArgumentNullException("Collision Analyzer failed!");
            }
            else
            {
                if (this.EdgeCollisionState.DistanceToBarrier > this.BodySize / 2 && newEdgeCollisionState.DistanceToBarrier < this.BodySize / 2)
                {
                    var collision = CollisionAnalyzer.GetCollision(this.EdgeCollisionState, newEdgeCollisionState, this.BodySize / 2, OSMDocument.AbsoluteTolerance);
                    if (collision == null)
                    {
                        throw new ArgumentException("Collision not found");
                    }
                    else
                    {
                        if (collision.TimeStepRemainderProportion > 1.0 || collision.TimeStepRemainderProportion < 0.0d)
                        {
                            throw new ArgumentException("The proportion of timestep before collision is out of range: " + collision.TimeStepRemainderProportion.ToString());
                        }
                        else
                        {
                            //update for partial timestep
                            //update timestep
                            double newTimeStep = this.TimeStep * collision.TimeStepRemainderProportion;
                            this.TimeStep -= newTimeStep;
                            // update location
                            newState.Location = collision.CollisionPoint;
                            //this._matrix.OffsetX = newState.Location.U;
                            //this._matrix.OffsetY = newState.Location.V;
                            // update velocity
                            var velocity = this.CurrentState.Velocity + newTimeStep * acceleration;
                            if (velocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
                            {
                                velocity.Unitize();
                                velocity *= this.VelocityMagnitude;
                            }
                            //decompose velocity
                            UV     velocityVerticalComponent         = newEdgeCollisionState.NormalizedRepulsion * (newEdgeCollisionState.NormalizedRepulsion.DotProduct(velocity));
                            double velocityVerticalComponentLength   = velocityVerticalComponent.GetLength();
                            UV     velocityHorizontalComponent       = velocity - velocityVerticalComponent;
                            double velocityHorizontalComponentLength = velocityHorizontalComponent.GetLength();
                            //decompose acceleration
                            UV accelerationVerticalComponent = newEdgeCollisionState.NormalizedRepulsion * (newEdgeCollisionState.NormalizedRepulsion.DotProduct(acceleration));
                            // elasticity
                            UV ve = this.BodyElasticity * velocityVerticalComponent;
                            //friction
                            double f1 = this.BarrierFriction * velocityVerticalComponentLength;
                            double f2 = velocityHorizontalComponentLength;
                            UV     vf = velocityHorizontalComponent - Math.Min(f1, f2) * velocityHorizontalComponent / velocityHorizontalComponentLength;
                            //update velocity
                            var adjustedVelocity = vf - ve - newTimeStep * accelerationVerticalComponent;
                            if (adjustedVelocity.GetLengthSquared() > this.VelocityMagnitude * this.VelocityMagnitude)
                            {
                                adjustedVelocity.Unitize();
                                adjustedVelocity *= this.VelocityMagnitude;
                            }
                            newState.Velocity = adjustedVelocity;
                            //update direction
                            deltaAngle         = this.AngularVelocity * newTimeStep;
                            axis               = new Axis(this.CurrentState.Direction, direction, deltaAngle);
                            newState.Direction = axis.V_Axis;
                            //update state
                            newEdgeCollisionState.DistanceToBarrier = this.BodySize / 2;
                            newEdgeCollisionState.Location          = newState.Location;

                            this.EdgeCollisionState = newEdgeCollisionState;
                            this.CurrentState       = newState;
                        }
                    }
                }
                else
                {
                    //update state for the full timestep length
                    this.EdgeCollisionState = newEdgeCollisionState;
                    this.CurrentState       = newState;
                    this.TimeStep           = 0.0d;
                }
            }
        }