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)
}
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);
}
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();
}
}
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 })
{
}