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; }