Esempio n. 1
0
 public GameLogics(IKinematics kinematics, IImageProcessing imageProcessing) : this()
 {
     // setting up the communication between the modules:
     PostMessageShowRequest                 += PostMessageHandler;         // game logics handles its own post messages
     kinematics.PostMessageShowRequest      += PostMessageHandler;         // game logics handles the post messages of the kinematics
     imageProcessing.PostMessageShowRequest += PostMessageHandler;         // game logics handles the post messages of the image processing
     kinematics.RobotStatusChanged          += RobotStatusChangedHandler;  // game logics handles the changes of the robot arm status (source: kinematics)
     imageProcessing.CameraStatusChanged    += CameraStatusChangedHandler; // game logics handles the changes of the camera status (source: image processing)
     imageProcessing.TableStateChanged      += TableSetupChangedHandler;   // game logics handles the changes of the table set-up (source: image processing)
     kinematics.SignUpToRobotMovementRequestEvent(this);                   // kinematics handles the robot movement requests (source: game logics)
 }
Esempio n. 2
0
        public Vector3 Collide(Contact c)
        {
            /// IMPLEMENTATION EXPLANATION
            /// point p is the collision point
            /// vector n is the collision normal and it defines the 'collision plane'
            /// the relative velocity vector and n are in another plane called the 'parallel plane" and it is defined by a vector called nPerp (vRel % n = nPerp)
            /// point a is the cm of body a
            /// point b is the cm of body b
            /// imagine the collision occuring in the frame where body b is fixed

            IBody        a            = c.BodyA;
            IBody        b            = c.BodyB;
            Intersection intersection = c.Intersection;

            IKinematics kinematicsA = a.Dynamics.Kinematics;
            IKinematics kinematicsB = b.Dynamics.Kinematics;

            Vector3 aToP = intersection.Point - kinematicsA.Transform.Pos;
            Vector3 bToP = intersection.Point - kinematicsB.Transform.Pos;
            // the velocity of p relative to both bodies
            Vector3 aVelP = kinematicsA.SurfaceVelocity(aToP);
            Vector3 bVelP = kinematicsB.SurfaceVelocity(bToP);

            Vector3 vRel     = aVelP - bVelP;                      // the relative velocity of the colliding points
            double  vRelPerp = vRel * intersection.Normal.Inverse; // negative means A is moving towards the plane (colliding)

            if (vRelPerp > CollidingThresholdSpeed)
            {
                return(Vector3.Zero);                                    // objects are receding
            }
            double  scalarNormalImpulse = NormalImpulse(vRelPerp, a, b, aToP, bToP, intersection.Normal);
            Vector3 normalImpulse       = scalarNormalImpulse * intersection.Normal.Inverse;
            Vector3 frictionImpulse     = Vector3.Zero;

            Vector3 nPerp = (vRel % intersection.Normal).UnitDirection; // will be zero if vRel is completely normal to collision plane and therefore no friction component

            if (nPerp.MagSquared != 0)                                  // there is motion in collision plane -> friction exists
            {
                // the normalized projectio0n of vRel onto the collision plane
                // it lies in the collision plane and defines the paralell plane
                Vector3 nPar    = (intersection.Normal % nPerp).UnitDirection;
                double  vRelPar = vRel * nPar;
                double  fric    = FrictionImpulse(scalarNormalImpulse, vRelPar, a.Material, b.Material);
                double  maxFric = MaxFrictionImpulse(a, b, vRelPar, nPar, nPerp, scalarNormalImpulse, aToP, bToP);
                if (Math.Abs(fric) > Math.Abs(maxFric))
                {
                    fric = maxFric;
                }
                frictionImpulse = fric * nPar.Inverse;
            }
            Validate(frictionImpulse);
            Validate(normalImpulse);
            return(frictionImpulse + normalImpulse);
        }
Esempio n. 3
0
        public RigidBody6DOF(IKinematics initialState, double mass, Matrix3 inertiaBody)
        {
            if (initialState == null) throw new ArgumentNullException(nameof(initialState));
            if (mass <= 0) throw new ArgumentException(nameof(mass)  + " must be larger than zero");
            InvMass = 1.0 / mass;
            InvIBody = inertiaBody.InverseMatrix();

            kinematicBody = new EuclideanKinematicBody(initialState.Transform);
            kinematicBody.V = initialState.V;
            kinematicBody.Omega = initialState.Omega;

            P = Mass * kinematicBody.V;
            L = I * kinematicBody.Omega;
        }
        public PickPlaceMovements(IKinematics ik, IEnumerable <RecordedMovement> movements)
        {
            //set fields
            _ik       = ik;
            Movements = movements.Where(i => i != null).ToArray();

            //rebuild if entities aren't right
            if (Movements.Length != 5)
            {
                Rebuild();
                WasGenerated = true;
            }
            else
            {
                ShoulderOffset = GetShoulderOffset();
            }
        }
Esempio n. 5
0
        public RigidBody6DOF(IKinematics initialState, double mass, Matrix3 inertiaBody)
        {
            if (initialState == null)
            {
                throw new ArgumentNullException(nameof(initialState));
            }
            if (mass <= 0)
            {
                throw new ArgumentException(nameof(mass) + " must be larger than zero");
            }
            InvMass  = 1.0 / mass;
            InvIBody = inertiaBody.InverseMatrix();

            kinematicBody       = new EuclideanKinematicBody(initialState.Transform);
            kinematicBody.V     = initialState.V;
            kinematicBody.Omega = initialState.Omega;

            P = Mass * kinematicBody.V;
            L = I * kinematicBody.Omega;
        }
 public PlaceMovements(IKinematics ik, RecordedMovement target, RecordedMovement entry = null) : base(ik, target, entry)
 {
 }
        // 0 = [secondary target] should have closed gripper - ease out
        // 1 = same as target but without gripper - sync - include shoulder offset - half speed - ease out
        // 2 = [target] should have closed gripper - ease in - sync
        // 3 = same as target but without gripper - sync
        // 4 = [secondary target]

        public PlaceMovements(IKinematics ik, IEnumerable <RecordedMovement> movements) : base(ik, movements)
        {
        }
 public PickPlaceMovements(IKinematics ik, RecordedMovement target, RecordedMovement entry = null) : this(ik, new RecordedMovement[] { target, entry })
 {
 }