internal MechanismBase[] GetMechanismsForBody(RigidBodyBase body)
{
ArrayList list = new ArrayList();
foreach (MechanismBase mech in Mechanisms)
{
BindingMechanismBase bmech = mech as BindingMechanismBase;
ForceMechanismBase fmech = mech as ForceMechanismBase;
if (bmech != null)
{
if (bmech.EndpointA.strokeref == body.strokeid ||
bmech.EndpointB.strokeref == body.strokeid)
{
list.Add(bmech);
}
}
else if (fmech != null)
{
if (fmech.Body.strokeref == body.strokeid)
{
list.Add(fmech);
}
}
}
return(list.ToArray(typeof(MechanismBase)) as MechanismBase[]);
}
private void hover_EditStraightenClicked(object sender, EventArgs e)
{
Strokes selected = inkoverlay.Selection;
if (selected == null || selected.Count != 1)
{
return;
}
// Get objects for the targeted stroke(s). Ensure only one selected.
RigidBodyBase[] bodies = doc.GetBodiesFor(selected);
if (bodies.Length != 1)
{
return;
}
RigidBodyBase body = bodies[0];
// Repaint the area around the original body.
Rectangle dirtybbox = body.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
// Straighten it.
body.Straighten();
// Repaint the area around the newbody.
dirtybbox = body.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
}
private void inkoverlay_SelectionChanged(object sender, EventArgs e)
{
dbg.WriteLine("----- inkoverlay_SelectionChanged -----");
// Ensure we're on the UI thread.
dbg.Assert(!this.InvokeRequired);
try // To prevent exceptions from propagating back to ink runtime.
{
// Show smarttag if only single-selection.
RigidBodyBase[] bodies = doc.GetBodiesFor(inkoverlay.Selection);
if (bodies.Length != 1)
{
hover.EnablePerItemEditCommands(false);
if (bodytag.Visible)
{
bodytag.Hide();
}
return;
}
RigidBodyBase body = bodies[0];
hover.EnablePerItemEditCommands(true);
ShowBodyTag(body);
}
catch (Exception ex)
{
// Log the error.
Global.HandleThreadException(this, new System.Threading.ThreadExceptionEventArgs(ex));
}
}
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 override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
// Gravity always pulls on the bodies' CG.
p = Point.Empty;
// Return vector times force factor.
v = vector * ForceMechanismBase.forceFactor;
}
public BodyPropertiesForm(RigidBodyBase body)
{
// Required for Windows Form Designer support
InitializeComponent();
// Further initialization.
this.body = body;
InitTrackBarValues();
}
public BodyPropertiesForm(RigidBodyBase body)
{
// Required for Windows Form Designer support
InitializeComponent();
// Further initialization.
this.body = body;
InitTrackBarValues();
}
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)
{
System.Diagnostics.Debug.Assert(Object.ReferenceEquals(body, Body.Object));
// Rotate the attachpoint to match the body's current position.
p = Geometry.TransformPointAsVector(body.displacement, Body.attachloc);
// Rotate the vector as well.
v = Vector.Transform(vector, body.displacement) * ForceMechanismBase.forceFactor;
}
// Note: this method is somewhat expensive, and called fairly often. A better
// approach to region/region hit testing could be implemented, to improve
// performance and return more useful info (such as a collision-normal vector).
internal static Region GetOverlap(RigidBodyBase body1, RigidBodyBase body2)
{
body1.UpdateGP(); body1.UpdateRgn();
body2.UpdateGP(); body2.UpdateRgn();
Region overlap = body1.rgncache.Clone();
overlap.Intersect(body2.rgncache);
return(overlap);
}
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));
}
// Marks initialAtRest = true if overlapping this body.
private void MarkAtRestIfOverlap(RigidBodyBase restingBody)
{
foreach (RigidBodyBase body in doc.Bodies)
{
if (!body.anchored && !body.initiallyAtRest)
{
Point contactPoint;
PointF normal;
if (FindIntersection(restingBody, body, out contactPoint, out normal))
{
body.initiallyAtRest = true;
MarkAtRestIfOverlap(body);
}
}
}
}
// Returns true if the objects are connected by a joint.
// (Assumes body1 != body2)
private bool ConnectedByJoint(RigidBodyBase body1, RigidBodyBase body2)
{
foreach (MechanismBase mech in doc.GetMechanismsForBody(body1))
{
if (mech is JointMech)
{
JointMech joint = (JointMech)mech;
if (Object.ReferenceEquals(joint.EndpointA.Object, body2) ||
Object.ReferenceEquals(joint.EndpointB.Object, body2))
{
return(true);
}
}
}
return(false);
}
private RigidBodyBase MakeBodyFromClosedStroke(Stroke stroke, Point[] vertices)
{
// Form a new body -- ellipse or polygon?
RigidBodyBase newbody = null;
Point[] points = stroke.GetPoints();
Ellipse elli = Ellipse.FromRegression(points);
if (!elli.IsEmpty && elli.IsFit(points))
{
dbg.WriteLine("new EllipticalBody");
newbody = new EllipticalBody();
EllipticalBody body = newbody as EllipticalBody;
body.CenterPoint = elli.Center;
body.MajorAxis = elli.MajorAxis;
body.MinorAxis = elli.MinorAxis;
body.Orientation = elli.Orientation;
// Close to circle? Snap to it.
if ((float)elli.MajorAxis / (float)elli.MinorAxis < 1.25)
{
int r = (elli.MajorAxis + elli.MinorAxis) / 2;
body.MajorAxis = body.MinorAxis = r;
body.Orientation = 0;
}
}
else
{
dbg.WriteLine("new PolygonalBody");
newbody = new PolygonalBody();
PolygonalBody body = newbody as PolygonalBody;
body.Vertices = vertices;
}
dbg.WriteLine(String.Format("Mass={0}, I={1}", newbody.Mass, newbody.I));
newbody.strokeid = stroke.Id;
doc.Bodies.Add(newbody);
return(newbody);
}
private void hover_EditCloneClicked(object sender, EventArgs e)
{
Strokes selected = inkoverlay.Selection;
if (selected == null || selected.Count != 1)
{
return;
}
// Get objects for the targeted stroke(s). Ensure that only one is selected.
RigidBodyBase[] bodies = doc.GetBodiesFor(selected);
if (bodies.Length != 1)
{
return;
}
RigidBodyBase body = bodies[0];
// First, clone the ink stroke. Move it down and to the right a bit.
Rectangle newrect = selected.GetBoundingBox();
newrect.Offset(1000, 1000);
doc.Ink.AddStrokesAtRectangle(selected, newrect);
// Next, clone the body, binding it to the new stroke id.
// Note: we got the new Strokes' ids by listening to the InkAdded event
// AddStrokesAtRectangle doesn't return the strokes' ids.
RigidBodyBase newbody = body.Clone(neweststrokeids[0]);
doc.Bodies.Add(newbody);
// Repaint the area around the newbody.
Rectangle dirtybbox = newbody.BoundingBox;
InvalidateInkSpaceRectangle(dirtybbox);
// Select it, to show the smart tag.
inkoverlay.Selection = doc.Ink.CreateStrokes(neweststrokeids);
}
private void ShowBodyTag(RigidBodyBase body)
{
// Establish point on lower-right of selected body's bbox.
Point p = new Point(body.BoundingBox.Right, body.BoundingBox.Bottom);
using (Graphics g = this.CreateGraphics())
{
// Convert to pixel-space.
inkoverlay.Renderer.InkSpaceToPixel(g, ref p);
}
// Make sure we're not off the edge of the screen.
if (p.X > this.Right - bodytag.Width)
{
p.X = this.Right - bodytag.Width;
}
if (p.Y > this.Bottom - bodytag.Height)
{
p.Y = this.Bottom - bodytag.Height;
}
bodytag.Show(p);
}
private void hover_EditPropertiesClicked(object sender, EventArgs e)
{
Strokes selected = inkoverlay.Selection;
if (selected == null || selected.Count != 1)
{
return;
}
// Get objects for the targeted stroke(s). Ensure that only one is selected.
RigidBodyBase[] bodies = doc.GetBodiesFor(selected);
if (bodies.Length != 1)
{
return;
}
RigidBodyBase body = bodies[0];
using (BodyPropertiesForm bpf = new BodyPropertiesForm(body))
{
bpf.ShowDialog(this);
Invalidate();
}
}
internal override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
System.Diagnostics.Debug.Assert(Object.ReferenceEquals(body,Body.Object));
// Rotate the attachpoint to match the body's current position.
p = Geometry.TransformPointAsVector(body.displacement,Body.attachloc);
// Rotate the vector as well.
v = Vector.Transform(vector,body.displacement) * ForceMechanismBase.forceFactor;
}
// Note: this method is somewhat expensive, and called fairly often. A better
// approach to region/region hit testing could be implemented, to improve
// performance and return more useful info (such as a collision-normal vector).
internal static Region GetOverlap(RigidBodyBase body1, RigidBodyBase body2)
{
body1.UpdateGP(); body1.UpdateRgn();
body2.UpdateGP(); body2.UpdateRgn();
Region overlap = body1.rgncache.Clone();
overlap.Intersect(body2.rgncache);
return overlap;
}
private void Tick(int milliseconds)
{
double dt = milliseconds / 1000.0; // Convert to fractional seconds.
// Integrate forces for each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// Don't move background-anchored bodies.
if (body.anchored)
{
continue;
}
// First, apply gravitational force (if defined).
body.totalForce = new Vector(0, 0);
body.totalAngularForce = 0.0;
if (doc.Gravity != null)
{
Point fp; // force point, body-local coordinates (ink-space)
Vector fv; // force vector, g·isu/s²
doc.Gravity.GetForceForBody(body, out fp, out fv);
// Gravity acts proportional to bodies' mass.
// The fudgefactor variable makes forces match expected perceptions.
double fudgefactor = 3.0;
fv = (fv * body.Mass) / fudgefactor;
body.totalForce += fv;
}
// Integrate the forces and moments from mechanisms.
MechanismBase[] mechs = doc.GetMechanismsForBody(body);
foreach (MechanismBase mech in mechs)
{
// Rods require the delta time to calculate force properly.
if (mech is RodMech)
{
((RodMech)mech).dt = dt;
}
Point fp; // force point, body-local coords (ink-space)
Vector fv; // force vector, g·isu/s²
mech.GetForceForBody(body, out fp, out fv);
body.totalForce += fv;
body.totalAngularForce +=
Vector.Cross(Vector.FromPoints(Point.Empty, fp), fv) / 1e6;
}
// Integrate forces and moments from any pending collisions.
foreach (CollisionResponse cr in this.collisions)
{
if (!Object.ReferenceEquals(cr.body, body))
{
continue;
}
body.totalForce += cr.impulseforce;
body.totalAngularForce += Vector.Cross(Vector.FromPoints(
cr.body.CG, cr.contactpoint), cr.impulseforce) / 1e6;
// If collision is between body not at rest and body at rest,
// then the body at rest is no longer at rest.
if (cr.body.initiallyAtRest &&
!cr.collidingBody.anchored && !cr.collidingBody.initiallyAtRest)
{
cr.body.initiallyAtRest = false;
}
}
}
// Apply forces, to move each body.
foreach (RigidBodyBase body in doc.Bodies)
{
// If body still at rest, don't move it.
if (body.initiallyAtRest)
{
continue;
}
// Add it up, and move the bodies. Note, we're just doing simple
// Euler integration, for now. If integration error proves to be a
// problem, Runge-Kutta or Improved Euler could be implemented here,
// for better results (at the expense of a little performance).
body.Vx += dt * body.totalForce.DX / body.Mass; // isu/sec
body.Vy += dt * body.totalForce.DY / body.Mass; // isu/sec
body.Va += dt * body.totalAngularForce / body.I; // radians/sec
int dx = 0, dy = 0; float da = 0f;
dx = MathEx.Round(dt * body.Vx);
dy = MathEx.Round(dt * body.Vy);
da = (float)Geometry.Rad2Deg(dt * body.Va);
body.Move(dx, dy, da);
// Move force mechanisms
foreach (MechanismBase mech in doc.GetMechanismsForBody(body))
{
if (mech is ForceMechanismBase)
{
mech.Move(dx, dy, da);
}
}
}
// Apply a method to stabilize rods, pin joints, and ropes.
StabilizeBindingMechanisms();
// Everything works better when there are no overlaps, so physically
// separate any objects that are intersecting.
SeparateIntersectingObjects();
// Look for collisions, apply impulses.
collisions.Clear();
foreach (RigidBodyBase body1 in doc.Bodies)
{
foreach (RigidBodyBase body2 in doc.Bodies)
{
if (Object.ReferenceEquals(body1, body2))
{
continue;
}
if (body1.anchored && body2.anchored)
{
continue;
}
Point contactPoint;
PointF normal;
if (!FindIntersection(body1, body2, out contactPoint, out normal))
{
continue;
}
using (Graphics g = wnd.CreateGraphics())
using (Region contactrgn = RigidBodyBase.GetOverlap(body1, body2))
{
if (contactrgn.IsEmpty(g))
{
continue;
}
// We've got a hit; but make sure it's waxing not waning.
int dx1 = 0, dy1 = 0, dx2 = 0, dy2 = 0; float da1 = 0f, da2 = 0f;
dx1 = MathEx.Round(dt * body1.Vx);
dy1 = MathEx.Round(dt * body1.Vy);
da1 = (float)Geometry.Rad2Deg(dt * body1.Va);
dx2 = MathEx.Round(dt * body2.Vx);
dy2 = MathEx.Round(dt * body2.Vy);
da2 = (float)Geometry.Rad2Deg(dt * body2.Va);
using (Matrix m1 = new Matrix())
using (Matrix m2 = new Matrix())
using (Region rgn = new Region())
{
m1.Translate(dx1, dy1);
m1.RotateAt(da1, body1.CG);
m2.Translate(dx2, dy2);
m2.RotateAt(da2, body2.CG);
Region contactrgn1 = body1.rgncache.Clone();
Region contactrgn2 = body2.rgncache.Clone();
contactrgn1.Transform(m1);
contactrgn2.Transform(m2);
rgn.Intersect(contactrgn1);
rgn.Intersect(contactrgn2);
float newarea = Geometry.CalculateArea(rgn);
float oldarea = Geometry.CalculateArea(contactrgn);
if (newarea < oldarea)
{
continue;
}
}
// Calculate contact point, and relative velocities.
// Make a 1000 unit normal so that we can use ints.
Vector collNormal = new Vector(MathEx.Round(normal.X * 1000),
MathEx.Round(normal.Y * 1000));
double collNormalLength = collNormal.Length;
if (collNormalLength == 0)
{
continue;
}
// Find the relative velocity at the collision point.
Vector rvBodies = new Vector(MathEx.Round(body1.Vx - body2.Vx),
MathEx.Round(body1.Vy - body2.Vy));
// Add in the velocity due to rotation of the bodies.
Vector cgToContact1 = Vector.FromPoints(body1.CG, contactPoint);
Vector orthoToCG1 = new Vector(-cgToContact1.DY, cgToContact1.DX);
Vector cgToContact2 = Vector.FromPoints(body2.CG, contactPoint);
Vector orthoToCG2 = new Vector(-cgToContact2.DY, cgToContact2.DX);
Vector rv = rvBodies + orthoToCG1 * body1.Va - orthoToCG2 * body2.Va;
CollisionResponse cr = new CollisionResponse();
cr.body = body2;
cr.collidingBody = body1;
cr.contactpoint = contactPoint;
// Take the smaller of the two elasticities for the collision.
double elasticity = Math.Min(body1.elasticity, body2.elasticity);
// Calculate the change in velocity times mass.
// These formulas come from _Physics for Game Developers_ by Bourg, p 98.
double impulseTimesMass = 0;
if (body1.anchored && collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1 / body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
else if (collNormalLength > 0)
{
impulseTimesMass = (1 + elasticity) * rv.Dot(collNormal) /
(1 / body1.Mass + 1 / body2.Mass +
Math.Abs(collNormal.Dot(orthoToCG1) * Vector.Cross(cgToContact1, collNormal)) /
body1.I / 1e6 / collNormal.LengthSq +
Math.Abs(collNormal.Dot(orthoToCG2) * Vector.Cross(cgToContact2, collNormal)) /
body2.I / 1e6 / collNormal.LengthSq) /
collNormalLength;
}
// Force that will result in that change in velocity.
cr.impulseforce = collNormal * (impulseTimesMass / dt / collNormalLength);
// Add sliding friction for ellipses colliding with polygons.
if (!body2.anchored && body2 is EllipticalBody && body1 is PolygonalBody)
{
// Figure out the velocity parallel to the normal.
double velocityNormal = rvBodies.Dot(collNormal) / collNormalLength;
// The frictional force is proportional to that.
// Note: For some reason, a coefficient of friction of 1.0
// sometimes creates a singularity.
double cfriction = Math.Min(body2.cfriction, .99);
double frictionForceMagnitude =
Math.Abs(velocityNormal) * cfriction * body2.Mass / dt;
// Figure out the velocity orthogonal to the normal.
Vector orthoNormal = new Vector(collNormal.DY, -collNormal.DX);
double velocityOrtho = -rv.Dot(orthoNormal) / collNormalLength;
// You can't have a frictional force that actually reverses the velocity.
double maximumForce = Math.Abs(velocityOrtho * body2.Mass / dt);
try
{
// The frictional force will be along the orthogonal to the normal,
// in the opposite direction of the velocity.
Vector frictionForce = orthoNormal * (-Math.Sign(velocityOrtho) *
Math.Min(frictionForceMagnitude, maximumForce)) / collNormalLength;
// Add friction.
cr.impulseforce += frictionForce;
}
catch (ArithmeticException ex)
{
Console.WriteLine("Silding friction exception: " + ex.ToString());
}
}
collisions.Add(cr);
}
}
}
}
// For now, just return whether collision has occurred.
private bool FindIntersection(RigidBodyBase body1, RigidBodyBase body2,
out Point contactPoint, out PointF normal)
{
// Initialize out variables.
contactPoint = new Point(0, 0);
normal = new PointF(0, 0);
if (!body1.BoundingBox.IntersectsWith(body2.BoundingBox))
{
return(false);
}
if (ConnectedByJoint(body1, body2))
{
return(false);
}
Point[] vertices1, vertices2;
if (body1 is PolygonalBody)
{
vertices1 = (Point[])((PolygonalBody)body1).Vertices.Clone();
body1.displacement.TransformPoints(vertices1);
}
else
{
// Approximate vertices for ellipse.
vertices1 = ((EllipticalBody)body1).GetPoints();
}
if (body2 is PolygonalBody)
{
vertices2 = (Point[])((PolygonalBody)body2).Vertices.Clone();
body2.displacement.TransformPoints(vertices2);
}
else
{
// Approximate vertices for ellipse.
vertices2 = ((EllipticalBody)body2).GetPoints();
}
// Loop through each segment and look for intersections.
ArrayList intersections = new ArrayList();
for (int i = 0; i < vertices1.Length; i++)
{
for (int j = 0; j < vertices2.Length; j++)
{
double tAB, tPQ;
Point v1 = vertices1[i];
Point v1Next = vertices1[(i + 1) % vertices1.Length];
Point v2 = vertices2[j];
Point v2Next = vertices2[(j + 1) % vertices2.Length];
bool hit = SegmentCollision.HitTest(v1, v1Next, v2, v2Next,
out tAB, out tPQ);
if (hit)
{
// Find intersections from here.
Point intersection = new Point((int)(v1.X + (v1Next.X - v1.X) * tAB),
(int)(v1.Y + (v1Next.Y - v1.Y) * tAB));
intersections.Add(intersection);
}
}
}
// If no intersections, then no collisions.
if (intersections.Count == 0)
{
return(false);
}
// Get average intersection.
int sumX = 0;
int sumY = 0;
foreach (Point intersection in intersections)
{
sumX += intersection.X;
sumY += intersection.Y;
}
contactPoint = new Point(sumX / intersections.Count,
sumY / intersections.Count);
// Find normal by constructing orthogonal vector from first/last intersections.
// Note: this can be improved by doing a linear fit through the intersections.
Point i0 = (Point)intersections[0];
Point i1 = (Point)intersections[intersections.Count - 1];
// Create a normal vector.
Vector normalVec = new Vector(i1.Y - i0.Y, i0.X - i1.X);
// Compare to vector between contact point and body1.
Vector collToBody1 =
new Vector(contactPoint.X - body1.CG.X, contactPoint.Y - body1.CG.Y);
// If in the opposite direction, reverse normal.
if (collToBody1.Dot(normalVec) < 0)
{
normalVec = normalVec * -1.0;
}
// Normalize.
double normalLength = normalVec.Length;
if (normalLength > 0)
{
normal = new PointF((float)(normalVec.DX / normalLength),
(float)(normalVec.DY / normalLength));
}
else
{
normal = new PointF(0, 0);
}
return(true);
}
private void TryTapToSelect(Stroke stroke)
{
// We interpret strokes with fewer than 25 packets,
// starting and ending on the same body, as tap gestures.
int np = stroke.PacketCount;
Point head = stroke.GetPoint(0), tail = stroke.GetPoint(np-1);
RigidBodyBase[] headhits = doc.HitTestBodies(head);
RigidBodyBase[] tailhits = doc.HitTestBodies(tail);
if (np <= 25 &&
headhits.Length > 0 &&
tailhits.Length > 0 &&
Object.ReferenceEquals(headhits[0],tailhits[0]))
{
tappedbody = headhits[0];
// Ensure we didn't just tap what's already selected.
ArrayList selbodies = new ArrayList(
doc.GetBodiesFor(inkoverlay.Selection));
if (selbodies.Contains(tappedbody))
return;
// We must delay the call to set_Selection, until after InkOverlay is
// finished looking at this stroke. BeginInvoke seems to work nicely
// for this purpose.
dbg.WriteLine("-------TTS--------");
base.BeginInvoke(new MethodInvoker(DelayTapToSelect));
}
}
// Mechanisms apply forces to their respective bodies through this method. internal abstract void GetForceForBody(RigidBodyBase body, out Point p, out Vector v);
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);
}
}
private void ShowBodyTag(RigidBodyBase body)
{
// Establish point on lower-right of selected body's bbox.
Point p = new Point(body.BoundingBox.Right,body.BoundingBox.Bottom);
using (Graphics g = this.CreateGraphics())
{
// Convert to pixel-space.
inkoverlay.Renderer.InkSpaceToPixel(g, ref p);
}
// Make sure we're not off the edge of the screen.
if (p.X > this.Right-bodytag.Width) p.X = this.Right-bodytag.Width;
if (p.Y > this.Bottom-bodytag.Height) p.Y = this.Bottom-bodytag.Height;
bodytag.Show(p);
}
// Mechanisms apply forces to their respective bodies through this method. internal abstract void GetForceForBody(RigidBodyBase body, out Point p, out Vector v);
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 MechanismBase[] GetMechanismsForBody(RigidBodyBase body)
{
ArrayList list = new ArrayList();
foreach (MechanismBase mech in Mechanisms)
{
BindingMechanismBase bmech = mech as BindingMechanismBase;
ForceMechanismBase fmech = mech as ForceMechanismBase;
if (bmech != null)
{
if (bmech.EndpointA.strokeref == body.strokeid ||
bmech.EndpointB.strokeref == body.strokeid)
list.Add(bmech);
}
else if (fmech != null)
{
if (fmech.Body.strokeref == body.strokeid)
list.Add(fmech);
}
}
return list.ToArray(typeof(MechanismBase)) as MechanismBase[];
}
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;
}
internal override void GetForceForBody(RigidBodyBase body, out Point p, out Vector v)
{
// Gravity always pulls on the bodies' CG.
p = Point.Empty;
// Return vector times force factor.
v = vector * ForceMechanismBase.forceFactor;
}
// Returns true if the objects are connected by a joint.
// (Assumes body1 != body2)
private bool ConnectedByJoint(RigidBodyBase body1, RigidBodyBase body2)
{
foreach (MechanismBase mech in doc.GetMechanismsForBody(body1))
{
if (mech is JointMech)
{
JointMech joint = (JointMech)mech;
if (Object.ReferenceEquals(joint.EndpointA.Object, body2) ||
Object.ReferenceEquals(joint.EndpointB.Object, body2))
return true;
}
}
return false;
}
// For now, just return whether collision has occurred.
private bool FindIntersection(RigidBodyBase body1, RigidBodyBase body2,
out Point contactPoint, out PointF normal)
{
// Initialize out variables.
contactPoint = new Point(0, 0);
normal = new PointF(0, 0);
if (!body1.BoundingBox.IntersectsWith(body2.BoundingBox))
return false;
if (ConnectedByJoint(body1, body2))
return false;
Point[] vertices1, vertices2;
if (body1 is PolygonalBody)
{
vertices1 = (Point[])((PolygonalBody)body1).Vertices.Clone();
body1.displacement.TransformPoints(vertices1);
}
else
{
// Approximate vertices for ellipse.
vertices1 = ((EllipticalBody)body1).GetPoints();
}
if (body2 is PolygonalBody)
{
vertices2 = (Point[])((PolygonalBody)body2).Vertices.Clone();
body2.displacement.TransformPoints(vertices2);
}
else
{
// Approximate vertices for ellipse.
vertices2 = ((EllipticalBody)body2).GetPoints();
}
// Loop through each segment and look for intersections.
ArrayList intersections = new ArrayList();
for (int i = 0; i < vertices1.Length; i++)
{
for (int j = 0; j < vertices2.Length; j++)
{
double tAB, tPQ;
Point v1 = vertices1[i];
Point v1Next = vertices1[(i + 1) % vertices1.Length];
Point v2 = vertices2[j];
Point v2Next = vertices2[(j + 1) % vertices2.Length];
bool hit = SegmentCollision.HitTest(v1, v1Next, v2, v2Next,
out tAB, out tPQ);
if (hit)
{
// Find intersections from here.
Point intersection = new Point((int)(v1.X + (v1Next.X - v1.X) * tAB),
(int)(v1.Y + (v1Next.Y - v1.Y) * tAB));
intersections.Add(intersection);
}
}
}
// If no intersections, then no collisions.
if (intersections.Count == 0)
return false;
// Get average intersection.
int sumX = 0;
int sumY = 0;
foreach (Point intersection in intersections)
{
sumX += intersection.X;
sumY += intersection.Y;
}
contactPoint = new Point(sumX / intersections.Count,
sumY / intersections.Count);
// Find normal by constructing orthogonal vector from first/last intersections.
// Note: this can be improved by doing a linear fit through the intersections.
Point i0 = (Point)intersections[0];
Point i1 = (Point)intersections[intersections.Count - 1];
// Create a normal vector.
Vector normalVec = new Vector(i1.Y - i0.Y, i0.X - i1.X);
// Compare to vector between contact point and body1.
Vector collToBody1 =
new Vector(contactPoint.X - body1.CG.X, contactPoint.Y - body1.CG.Y);
// If in the opposite direction, reverse normal.
if (collToBody1.Dot(normalVec) < 0)
normalVec = normalVec * -1.0;
// Normalize.
double normalLength = normalVec.Length;
if (normalLength > 0)
normal = new PointF((float)(normalVec.DX / normalLength),
(float)(normalVec.DY / normalLength));
else
normal = new PointF(0, 0);
return true;
}
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);
}
}
// Marks initialAtRest = true if overlapping this body.
private void MarkAtRestIfOverlap(RigidBodyBase restingBody)
{
foreach (RigidBodyBase body in doc.Bodies)
{
if (!body.anchored && !body.initiallyAtRest)
{
Point contactPoint;
PointF normal;
if (FindIntersection(restingBody, body, out contactPoint, out normal))
{
body.initiallyAtRest = true;
MarkAtRestIfOverlap(body);
}
}
}
}
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));
}
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;
}
}
static bool CompareDocuments(MagicDocument a, MagicDocument b)
{
if (a.version != b.version)
{
return(false);
}
if (a.Bodies.Count != b.Bodies.Count)
{
return(false);
}
if (a.Mechanisms.Count != b.Mechanisms.Count)
{
return(false);
}
if (a.xml_Ink != b.xml_Ink)
{
return(false);
}
for (int i = 0; i < a.Bodies.Count; ++i)
{
RigidBodyBase a1 = ((RigidBodyBase)a.Bodies[i]);
RigidBodyBase b1 = ((RigidBodyBase)b.Bodies[i]);
if (a1.GetType() != b1.GetType())
{
return(false);
}
if (a1.strokeid != b1.strokeid)
{
return(false);
}
if (a1.anchored != b1.anchored)
{
return(false);
}
if (a1.cfriction != b1.cfriction)
{
return(false);
}
if (a1.density != b1.density)
{
return(false);
}
}
for (int i = 0; i < a.Mechanisms.Count; ++i)
{
MechanismBase a1 = ((MechanismBase)a.Mechanisms[i]);
MechanismBase b1 = ((MechanismBase)b.Mechanisms[i]);
if (a1.GetType() != b1.GetType())
{
return(false);
}
if (a1 is ForceMechanismBase)
{
ForceMechanismBase a2 = (ForceMechanismBase)a1;
ForceMechanismBase b2 = (ForceMechanismBase)b1;
if (a2.Body.strokeref != b2.Body.strokeref)
{
return(false);
}
if (a2.Body.attachloc != b2.Body.attachloc)
{
return(false);
}
if (a2.vector != b2.vector)
{
return(false);
}
}
}
return(true);
}
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;
}