internal override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
// Rotate the attachpoint to match the body's current position.
BodyRef bodyref = null, farside = null;
if (Object.ReferenceEquals(body, EndpointA.Object))
{
bodyref = EndpointA; farside = EndpointB;
}
else if (Object.ReferenceEquals(body, EndpointB.Object))
{
bodyref = EndpointB; farside = EndpointA;
}
else
{
throw new ApplicationException("Bogus bodyref!");
}
p = Geometry.TransformPointAsVector(body.displacement, bodyref.attachloc);
// Form the force vector from head to tail.
Point head = bodyref.Object.CG +
Vector.Transform(Vector.FromPoint(bodyref.attachloc), bodyref.Object.displacement);
Point tail = farside.Object.CG +
Vector.Transform(Vector.FromPoint(farside.attachloc), farside.Object.displacement);
double distance = Geometry.DistanceBetween(head, tail);
// Scale based on over/under distance.
v = Vector.FromPoints(head, tail);
v *= stiffness * (distance - extension) / distance;
}
private MechanismBase MakeRodRopeOrSpring(Stroke stroke, RigidBodyBase headbody, RigidBodyBase tailbody)
{
// Rod, Rope, or Spring: we decide based on straightness, curvyness, and loopiness.
int np = stroke.PacketCount;
Point head = stroke.GetPoint(0), tail = stroke.GetPoint(np - 1);
double distance = Geometry.DistanceBetween(head, tail);
StrokeGeometry sg = new StrokeGeometry(stroke);
double length = sg.IntegrateLength();
// Consider spring: analyze total net curvature of the stroke, and call it a
// spring if it makes at least 540 degrees (1.5 loops) in the same direction.
double tt = StrokeAnalyzer.AnalyzeTotalCurvature(stroke, 100.0);
if (Math.Abs(Geometry.Rad2Deg(tt)) > 540.0)
{
dbg.WriteLine("new SpringMech");
SpringMech newmech = new SpringMech();
newmech.EndpointA = BodyRef.For(headbody, head);
newmech.EndpointB = BodyRef.For(tailbody, tail);
newmech.extension = distance;
newmech.stiffness = MathEx.Square(tt) / 100; //Heuristic: th²/100 feels about right.
newmech.strokeid = stroke.Id;
doc.Mechanisms.Add(newmech);
return(newmech);
}
// Straight and narrow?
double rodropethreshold = 1.1; //heuristic
if (length / distance < rodropethreshold)
{
dbg.WriteLine("new RodMech");
RodMech newmech = new RodMech();
newmech.EndpointA = BodyRef.For(headbody, head);
newmech.EndpointB = BodyRef.For(tailbody, tail);
newmech.length = distance;
newmech.strokeid = stroke.Id;
doc.Mechanisms.Add(newmech);
return(newmech);
}
else
{
dbg.WriteLine("new RopeMech");
RopeMech newmech = new RopeMech();
newmech.EndpointA = BodyRef.For(headbody, head);
newmech.EndpointB = BodyRef.For(tailbody, tail);
newmech.length = length;
newmech.strokeid = stroke.Id;
doc.Mechanisms.Add(newmech);
return(newmech);
}
}
internal static BodyRef For(RigidBodyBase body, Point location)
{
BodyRef @this = new BodyRef();
@this.body = body;
@this.strokeref = body.strokeid;
@this.attachloc = location - new Size(body.CG);
return(@this);
}
internal override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
// Rotate the attachpoint to match the body's current position.
BodyRef bodyref = null, farside = null;
if (Object.ReferenceEquals(body, EndpointA.Object))
{
bodyref = EndpointA; farside = EndpointB;
}
else if (Object.ReferenceEquals(body, EndpointB.Object))
{
bodyref = EndpointB; farside = EndpointA;
}
else
{
throw new ApplicationException("Bogus bodyref!");
}
p = Geometry.TransformPointAsVector(body.displacement, bodyref.attachloc);
// Form the force vector from head to tail.
Point head = bodyref.Object.CG +
Vector.Transform(Vector.FromPoint(bodyref.attachloc), bodyref.Object.displacement);
Point tail = farside.Object.CG +
Vector.Transform(Vector.FromPoint(farside.attachloc), farside.Object.displacement);
double distance = Geometry.DistanceBetween(head, tail);
if (distance == 0.0)
{
// No force required
v = new Vector(0, 0);
return;
}
// We want to get the distance to be the length as quickly as possible with as
// little overshooting. To do this, let's apply a force that will bring it part
// way towards the goal.
Vector alongRod = Vector.FromPoints(head, tail);
// Calculate force to get us a fraction of the way there. Note: we exclude
// pin joints (by testing length > 0) from this calculation, because it adversely
// affects the rotation of wheels.
double fraction = 0.5;
if (!farside.Object.anchored && bodyref.Object.Mass > 0 && farside.Object.Mass > 0 &&
length > 0)
{
// The fraction should be inversely proportional to the relative mass
// of the object so that lighter objects end up moving more.
fraction *= 1 / ((1 / bodyref.Object.Mass + 1 / farside.Object.Mass) * bodyref.Object.Mass);
}
v = alongRod * (body.Mass * fraction * (distance - length) / distance / (dt * dt));
}
static MagicDocument CreateDocumentFixture()
{
MagicDocument doc = new MagicDocument();
RigidBodyBase body;
body = new EllipticalBody();
body.strokeid = 1;
doc.Bodies.Add(body);
body = new EllipticalBody();
body.strokeid = 2;
doc.Bodies.Add(body);
body = new PolygonalBody();
body.strokeid = 3;
doc.Bodies.Add(body);
((PolygonalBody)body).Vertices = new Point[] {
new Point(120, 340),
new Point(560, 340),
new Point(560, 780),
new Point(120, 780)
};
BindingMechanismBase mech;
mech = new RodMech();
mech.EndpointA = BodyRef.For((RigidBodyBase)doc.Bodies[0], Point.Empty);
mech.EndpointB = BodyRef.For((RigidBodyBase)doc.Bodies[1], Point.Empty);
doc.Mechanisms.Add(mech);
mech = new RopeMech();
mech.EndpointA = BodyRef.For((RigidBodyBase)doc.Bodies[1], Point.Empty);
mech.EndpointB = BodyRef.For((RigidBodyBase)doc.Bodies[2], Point.Empty);
doc.Mechanisms.Add(mech);
mech = new SpringMech();
mech.EndpointA = BodyRef.For((RigidBodyBase)doc.Bodies[2], Point.Empty);
mech.EndpointB = BodyRef.For((RigidBodyBase)doc.Bodies[0], Point.Empty);
doc.Mechanisms.Add(mech);
return(doc);
}
private void inkoverlay_Stroke(object sender, InkCollectorStrokeEventArgs e)
{
dbg.WriteLine("----- inkoverlay_Stroke -----");
// Ensure we're on the UI thread.
dbg.Assert(!this.InvokeRequired);
// Check to make sure we're not erasing.
if (inkoverlay.EditingMode == InkOverlayEditingMode.Delete)
{
return;
}
try // To prevent exceptions from propagating back to ink runtime.
{
// Hook for tap-to-select feature, in lasso-mode.
if (inkoverlay.EditingMode == InkOverlayEditingMode.Select)
{
TryTapToSelect(e.Stroke);
return;
}
// Analyze stroke geometry.
bool closed;
Point[] vertices;
double tolerance = 500.0; //Heuristic: 500 seems about right.
StrokeAnalyzer.AnalyzeClosedness(e.Stroke, tolerance, out closed, out vertices);
// Interpret stroke in document-context: first, consider closed strokes.
if (closed)
{
// Check for a small elliptical-gesture over two or more bodies.
// If so, it's a pin joint!
Rectangle bbox = e.Stroke.GetBoundingBox();
Point midp = Geometry.Midpoint(bbox);
RigidBodyBase[] hits = doc.HitTestBodies(midp);
if (hits.Length >= 2 && bbox.Width < 1000 && bbox.Height < 1000)
{
RigidBodyBase top = hits[0];
RigidBodyBase bottom = hits[1];
// Snap to CG if close to either top's or bottom's.
if (Geometry.DistanceBetween(midp, top.CG) < 500.0)
{
midp = top.CG;
}
else if (Geometry.DistanceBetween(midp, bottom.CG) < 500.0)
{
midp = bottom.CG;
}
dbg.WriteLine("new JointMech");
JointMech newmech = new JointMech();
newmech.EndpointA = BodyRef.For(bottom, midp);
newmech.EndpointB = BodyRef.For(top, midp);
newmech.strokeid = e.Stroke.Id;
doc.Mechanisms.Add(newmech);
// Repaint area around the newmech (unions with stroke bbox, below).
Rectangle dirtybbox = newmech.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
return;
}
else
{
// Larger stroke, and/or no centerpoint hits -- form a new solid body.
RigidBodyBase newbody = MakeBodyFromClosedStroke(e.Stroke, vertices);
// Repaint area around the newbody (unions with stroke bbox, below).
Rectangle dirtybbox = newbody.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
// Select it, to show the smart tag.
inkoverlay.Selection = this.MakeSingleStrokeCollection(e.Stroke);
return;
}
}
// An unclosed stroke --
// Check if head and/or tail is hit on existing bodies.
int np = e.Stroke.PacketCount;
Point head = e.Stroke.GetPoint(0), tail = e.Stroke.GetPoint(np - 1);
RigidBodyBase[] headhits = doc.HitTestBodies(head);
RigidBodyBase[] tailhits = doc.HitTestBodies(tail);
if (headhits.Length == 0 && tailhits.Length == 0)
{
// Neither head or tail hit, so let's try harder to make a body
// out of this stroke.
Point[] dummy;
tolerance = 2000.0; //Heuristic: vastly relax closure tolerance.
StrokeAnalyzer.AnalyzeClosedness(e.Stroke, tolerance, out closed, out dummy);
if (closed)
{
RigidBodyBase newbody = MakeBodyFromClosedStroke(e.Stroke, vertices);
// Repaint area around the newbody (unions with stroke bbox, below).
Rectangle dirtybbox = newbody.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
// Select it, to show the smart tag.
inkoverlay.Selection = this.MakeSingleStrokeCollection(e.Stroke);
return;
}
else if (Geometry.DistanceBetween(head, tail) > 500.0)
{
// Interpret this stroke as a gravity-vector.
GravitationalForceMech newgrav = new GravitationalForceMech();
newgrav.Body = null; // Applies to all bodies!
newgrav.origin = head;
newgrav.vector = Vector.FromPoints(head, tail);
// Repaint area around the gravity vector (unions with stroke bbox, below).
Rectangle dirtybbox = newgrav.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
// Throw out the current gravity-vector stroke, if it exists.
if (doc.Gravity != null)
{
dirtybbox = doc.Gravity.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
Stroke old = GetStrokeById(doc.Gravity.strokeid);
doc.Ink.DeleteStroke(old);
}
newgrav.strokeid = e.Stroke.Id;
doc.Gravity = newgrav;
return;
}
else
{
// This stroke is probably an accidental tap -- discard it.
e.Cancel = true;
return;
}
}
if (headhits.Length > 0 && tailhits.Length == 0)
{
// If only the head is hit, it must be an 'attractive force'.
RigidBodyBase body = headhits[0];
dbg.WriteLine("new ExternalForceMech");
ExternalForceMech newmech = new ExternalForceMech();
newmech.Body = BodyRef.For(body, head);
newmech.vector = Vector.FromPoints(head, tail);
newmech.strokeid = e.Stroke.Id;
doc.Mechanisms.Add(newmech);
// Repaint area around the newmech (unions with stroke bbox, below).
Rectangle dirtybbox = newmech.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
return;
}
if (headhits.Length == 0 && tailhits.Length > 0)
{
// If only the tail is hit, it must be a 'propulsive force'.
RigidBodyBase body = tailhits[0];
dbg.WriteLine("new PropulsiveForceMech");
PropulsiveForceMech newmech = new PropulsiveForceMech();
newmech.Body = BodyRef.For(body, tail);
newmech.vector = Vector.FromPoints(head, tail);
newmech.strokeid = e.Stroke.Id;
doc.Mechanisms.Add(newmech);
// Repaint area around the newmech (unions with stroke bbox, below).
Rectangle dirtybbox = newmech.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
return;
}
if (true) // scope
{
// Create a binding mechanism between two bodies.
RigidBodyBase headbody = headhits[0], tailbody = tailhits[0];
// If both the head and the tail hit same object,
// attach the head to the one behind.
if (Object.ReferenceEquals(headbody, tailbody))
{
if (headhits.Length > 1)
{
headbody = headhits[1];
}
else if (tailhits.Length > 1)
{
tailbody = tailhits[1];
}
else
{
// Don't self-connect. We will perhaps interpret the stroke as an
// anchor-gesture or a selection-gesture,
// but if we cannot, we will cancel.
int nc = e.Stroke.PolylineCusps.Length;
if (np <= 25)
{
inkoverlay.Selection = MakeSingleStrokeCollection(headbody.strokeid);
SetEditingMode(InkOverlayEditingMode.Select, false);
}
else if (np <= 150 && (nc >= 3 && nc <= 5))
{
headbody.anchored = !headbody.anchored; //toggle
}
e.Cancel = true;
// Repaint area around the headbody (unions with stroke bbox, below).
Rectangle dirtybbox = headbody.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
return;
}
}
// Create a rope, rod, or spring out of the stroke.
MechanismBase newmech = MakeRodRopeOrSpring(e.Stroke, headbody, tailbody);
if (newmech != null)
{
// Repaint area around the newmech (unions with stroke bbox, below).
Rectangle dirtybbox = newmech.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
return;
}
else
{
// Throw out the stroke, and return.
e.Cancel = true;
return;
}
}
}
catch (Exception ex)
{
// Cancel the stroke, and log the error.
e.Cancel = true;
Global.HandleThreadException(this, new System.Threading.ThreadExceptionEventArgs(ex));
}
finally
{
// Repaint the area around the stroke (unions with newbody/mech region, above).
Rectangle dirtybbox = e.Stroke.GetBoundingBox();
InvalidateInkSpaceRectangle(dirtybbox);
}
}
internal override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
// Rotate the attachpoint to match the body's current position.
BodyRef bodyref = null, farside = null;
if (Object.ReferenceEquals(body, EndpointA.Object))
{
bodyref = EndpointA; farside = EndpointB;
}
else if (Object.ReferenceEquals(body, EndpointB.Object))
{
bodyref = EndpointB; farside = EndpointA;
}
else
{
throw new ApplicationException("Bogus bodyref!");
}
p = Geometry.TransformPointAsVector(body.displacement, bodyref.attachloc);
// Form the force vector from head to tail.
Point head = bodyref.Object.CG +
Vector.Transform(Vector.FromPoint(bodyref.attachloc), bodyref.Object.displacement);
Point tail = farside.Object.CG +
Vector.Transform(Vector.FromPoint(farside.attachloc), farside.Object.displacement);
double distance = Geometry.DistanceBetween(head, tail);
// Model rope like a rod, but only when the rope is taught.
v = new Vector(0, 0);
if (distance > length)
{
// We want to get the distance to be the length as quickly as possible with as
// little overshooting. To do this, let's apply a force that will bring it part
// way towards the goal.
Vector alongRope = Vector.FromPoints(head, tail);
// This dt doesn't have to be as critical as the rod, so we can just approximate
// with something reasonable.
double dt = 50.0 / 1000;
// Calculate the force to get us a fraction of the way there.
double fraction = 0.25;
if (!farside.Object.anchored && bodyref.Object.Mass > 0 && farside.Object.Mass > 0)
{
// The fraction should be inversely proportional to the relative mass
// of the object so that lighter objects end up moving more.
fraction *= 1 / ((1 / bodyref.Object.Mass + 1 / farside.Object.Mass) * bodyref.Object.Mass);
}
v = alongRope * (body.Mass * fraction * (distance - length) / distance / (dt * dt));
// Calculate the damping force that cancels out velocity along the rope.
Vector velocityAlongRope = new Vector((int)(bodyref.Object.Vx - farside.Object.Vx),
(int)(bodyref.Object.Vy - farside.Object.Vy));
velocityAlongRope.ProjectOnto(alongRope);
Vector damping = velocityAlongRope * (-body.Mass / dt);
// Check to make sure we are not bouncing back and forth.
if (!(Math.Sign(damping.DX) != Math.Sign(previousDamping.DX) &&
Math.Sign(damping.DY) != Math.Sign(previousDamping.DY)))
{
v += damping;
}
previousDamping = damping;
}
else
{
previousDamping = new Vector(0, 0);
}
}
internal static BodyRef For(RigidBodyBase body, Point location)
{
BodyRef @this = new BodyRef();
@this.body = body;
@this.strokeref = body.strokeid;
@this.attachloc = location - new Size(body.CG);
return @this;
}
