public PhysicsState()
{
this.Position = ALVector2D.Zero;
this.Velocity = ALVector2D.Zero;
this.Acceleration = ALVector2D.Zero;
this.ForceAccumulator = ALVector2D.Zero;
}
public static IList <BaseModelBody> BuildNodeSlots(IHaveConnectionSlots parent, Guid modelId)
{
var parPos = parent.Position;
var slots = parent.Slots.Where(s => !s.IsOccupied);
var result = new List <BaseModelBody>();
foreach (var slot in slots)
{
var nodeSlot = CreateConnectionSlotBody(slot, modelId);
var slotXAngle = slot.Direction + parPos.Angular;
var slotCenter = Vector2D.Rotate(slotXAngle, new Vector2D(slot.DistanceFromCenter, 0.0f)); // Zero Angle corresponds X Axis
var slotPos = new ALVector2D(slot.Orientation + slotXAngle, slotCenter + parPos.Linear);
nodeSlot.State.Position = slotPos;
nodeSlot.ApplyPosition();
nodeSlot.Parent = parent as BaseModelBody;
result.Add(nodeSlot);
}
return(result);
}
public static BoneBody AddBone(this ConnectionSlotBody slotBody, BoneModel boneModel)
{
var slot = slotBody.Model;
slot.IsOccupied = true;
var slotPos = slotBody.State.Position;
var slotSize = slotBody.Model.Size;
var centerLoc = Vector2D.FromLengthAndAngle((boneModel.Length + slotSize) * 0.5, slotPos.Angular);
var bonePos = new ALVector2D(slotPos.Angular, slotPos.Linear + centerLoc);
var rectBody = CreateRectangle(boneModel.Thickness, boneModel.Length, 0.00001, bonePos);
var newBone = rectBody.CopyAsBone(slotBody.ModelId);
newBone.Model = boneModel;
newBone.Parent = slotBody;
var joints = slotBody.ConnectWith(newBone, (2 * bonePos.Linear + 8 * slotBody.State.Position.Linear) * 0.1f);
Will.Instance.AddBody(newBone);
Will.Instance.AddJoint(joints.Item1);
Will.Instance.AddJoint(joints.Item2);
return(newBone);
}
public static Body AddShape(DemoOpenInfo info, IShape shape, Scalar mass, ALVector2D position)
{
Body body = new Body(new PhysicsState(position), shape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(body));
return(body);
}
/// <summary>
/// Creates new Rectange Body
/// </summary>
/// <param name="height">Height of the Body</param>
/// <param name="width">Width of the Body</param>
/// <param name="mass">Mass of the Body</param>
/// <param name="position">Initial Direction and Linear Position of the Body</param>
/// <returns>Return the new value of the BasePolygonBody</returns>
/// <remarks>The Guid of new Body will be stored in Body.Tags["Guid"]. The raw Colored Drawable of new Body will be stored in Body.Tags["Drawable"].</remarks>
public static Body CreateRectangle(Scalar height, Scalar width, Scalar mass, ALVector2D position)
{
var vertices = VertexHelper.CreateRectangle(width, height);
vertices = VertexHelper.Subdivide(vertices, Math.Min(height, width) / 5);
var boxShape = ShapeFactory.GetOrCreateColoredPolygonShape(vertices, Math.Min(height, width) / 5);
var newBody = new Body(new PhysicsState(position), boxShape, mass, Coefficients.Duplicate(), new Lifespan());
return(newBody);
}
public static void SetSubShipsVelocity(IShip source, ALVector2D velocity)
{
foreach (IShip ship in source.SubShips)
{
if (ship.CurrentControlInput == null)
{
ship.Current.Velocity = velocity;
ship.Current.ForceAccumulator.Linear = source.Current.ForceAccumulator.Linear * (ship.MassInfo.Mass * source.MassInfo.MassInv);
ship.Current.ForceAccumulator.Angular = source.Current.ForceAccumulator.Angular * (ship.MassInfo.MomentofInertia * source.MassInfo.MomentofInertiaInv);
}
}
}
public static Body AddFloor(DemoOpenInfo info, ALVector2D position)
{
Scalar height = 60;
Scalar width = 2000;
Vector2D[] vertexes = VertexHelper.CreateRectangle(width, height);
IShape boxShape = ShapeFactory.CreateColoredPolygon(vertexes, Math.Min(height, width) / 5);
Body body = new Body(new PhysicsState(position), boxShape, Scalar.PositiveInfinity, Coefficients.Duplicate(), new Lifespan());
body.IgnoresGravity = true;
info.Scene.AddGraphic(CreateGraphic(body));
return(body);
}
void Transform(ALVector2D vector, IList<Body> collection)
{
Matrix2x3 matrix;
ALVector2D.ToMatrix2x3(ref vector, out matrix);
Transform(matrix, collection);
}
public static InterconnectionBody TryAddInterconnectionBody(this InterconnectionModel model, int maxTryCount)
{
var allSlots = Will.Instance.Bodies.OfType <ConnectionSlotBody>().Where(s => s.Model.IsOccupied == false).ToList();
while (allSlots.Count > 0 && --maxTryCount > 0)
{
var randSlot = allSlots.RandomOrDefault <ConnectionSlotBody>();
var begPos = randSlot.State.Position;
var alignedSlot = allSlots.Where(s => !s.Parent.Equals(randSlot.Parent)).FirstOrDefault(s =>
{
var angle = MathHelper.WrapClamp(s.State.Position.Angular + MathHelper.Pi, 0.0f, MathHelper.TwoPi);
var diff = Math.Abs(angle - begPos.Angular);
var dist = (begPos.Linear - s.State.Position.Linear).Magnitude;
return(diff <= model.MaxMissAlign && dist <= model.MaxDistance);
});
if (alignedSlot == null)
{
continue;
}
//Aligned slots pair found. Let's build a connection between
randSlot.Model.IsOccupied = true;
alignedSlot.Model.IsOccupied = true;
var endPos = alignedSlot.State.Position;
var centerPos = (begPos.Linear + endPos.Linear) * 0.5;
var begToCenter = centerPos - begPos.Linear;
var connPos = new ALVector2D(begToCenter.Angle, centerPos);
var rectBody = CreateRectangle(1, begToCenter.Magnitude * 1.8, 0.00001, connPos);
var connBody = rectBody.CopyAsInterconnection(randSlot.ModelId);
connBody.IsCollidable = false;
connBody.BegSlot = randSlot;
connBody.EndSlot = alignedSlot;
Will.Instance.RunPauseWilling(false);
Will.Instance.AddBody(connBody);
//randSlot.State.Position = new ALVector2D((begToCenter.Angle+begPos.Angular)*0.5, begPos.Linear);
//alignedSlot.State.Position = new ALVector2D(((-begToCenter).Angle+endPos.Angular)*0.5, endPos.Linear);
randSlot.State.Position = new ALVector2D(begToCenter.Angle, begPos.Linear);
alignedSlot.State.Position = new ALVector2D((-begToCenter).Angle, endPos.Linear);
var begJoint = new HingeJoint(randSlot, connBody, (2 * centerPos + 8 * begPos.Linear) * 0.1f, new Lifespan())
{
DistanceTolerance = model.MaxDistance * 0.5,
Softness = 10
};
var begAngle = new AngleJoint(randSlot, connBody, new Lifespan())
{
Softness = 0.0001, BiasFactor = 0.2f
};
var endJoint = new HingeJoint(connBody, alignedSlot, (2 * centerPos + 8 * endPos.Linear) * 0.1f, new Lifespan())
{
DistanceTolerance = model.MaxDistance * 0.5,
Softness = 10
};
var endAngle = new AngleJoint(connBody, alignedSlot, new Lifespan())
{
Softness = 0.0001, BiasFactor = 0.2f
};
Will.Instance.AddJoint(begJoint);
Will.Instance.AddJoint(begAngle);
Will.Instance.AddJoint(endAngle);
Will.Instance.RunPauseWilling(true);
return(connBody);
}
// Sorry :( Aligned pair not found. Please, try again later.
return(null);
}
public static Vector2D GetRelativeVelocity(
ALVector2D velocity1, ALVector2D velocity2,
Vector2D point1, Vector2D point2)
{
Vector2D result;
GetRelativeVelocity(ref velocity1, ref velocity2, ref point1, ref point2, out result);
return result;
}
protected internal override void Solve(TimeStep step)
{
Detect(step);
Arbiter[] arbs = RemoveEmpty(step);
this.Engine.RunLogic(step);
if (freezing)
{
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].CheckFrozen();
}
}
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
tag.biasVelocity = ALVector2D.Zero;
if (freezing)
{
bool accelSame = tag.body.State.Acceleration == tag.lastAccel;
ALVector2D vel = tag.body.State.Velocity;
ALVector2D force = tag.body.State.ForceAccumulator;
bool isVelZero =
Math.Abs(vel.X) < freezeVelocityTolerance.X &&
Math.Abs(vel.Y) < freezeVelocityTolerance.Y &&
Math.Abs(vel.Angular) < freezeVelocityTolerance.Angular;
bool isForceZero = tag.body.State.ForceAccumulator == ALVector2D.Zero;
if (accelSame && isVelZero && isForceZero)
{
if (tag.body.Joints.Count == 0)
{
tag.body.idleCount++;
}
if (tag.body.idleCount > freezeTimeout)
{
tag.body.idleCount = freezeTimeout;
tag.body.IsFrozen = true;
tag.body.State.Velocity = ALVector2D.Zero;
}
}
else
{
tag.body.IsFrozen = false;
tag.body.idleCount = 0;
}
tag.lastAccel = tag.body.State.Acceleration;
if (tag.body.IsFrozen)
{
tag.body.State.ForceAccumulator = ALVector2D.Zero;
tag.body.State.Acceleration = ALVector2D.Zero;
}
}
tag.body.UpdateVelocity(step);
tag.body.ClearForces();
}
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].PreApply(step.DtInv);
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].PreStep(step);
}
for (int i = 0; i < iterations; ++i)
{
for (int index = 0; index < arbs.Length; ++index)
{
arbs[index].Apply();
}
for (int index = 0; index < siJoints.Count; ++index)
{
siJoints[index].ApplyImpulse();
}
}
for (int index = 0; index < tags.Count; ++index)
{
SequentialImpulsesTag tag = tags[index];
if (splitImpulse)
{
tag.body.UpdatePosition(step, ref tag.biasVelocity);
}
else
{
tag.body.UpdatePosition(step);
}
tag.body.ApplyPosition();
}
}
public static Matrix2D FromALVector2D(ALVector2D source)
{
Matrix2D result;
FromALVector2D(ref source, out result);
return result;
}
public static Body AddFloor(DemoOpenInfo info, ALVector2D position)
{
Scalar height = 60;
Scalar width = 2000;
Vector2D[] vertexes = VertexHelper.CreateRectangle(width, height);
IShape boxShape = ShapeFactory.CreateColoredPolygon(vertexes, Math.Min(height, width) / 5);
Body body = new Body(new PhysicsState(position), boxShape, Scalar.PositiveInfinity, Coefficients.Duplicate(), new Lifespan());
body.IgnoresGravity = true;
info.Scene.AddGraphic(CreateGraphic(body));
return body;
}
/// <summary>
/// Adds new Circle Body into World
/// </summary>
/// <param name="radius">Radius of the Circle Shape</param>
/// <param name="verticesCount">Count of vertices of the Circle Shape</param>
/// <param name="mass">Mass of corresponding Body</param>
/// <param name="position">Position of the Circle Shape</param>
/// <param name="modelId">Id of the parent Model</param>
/// <returns>Newly created and added into world Body object.</returns>
public static BaseModelBody AddCircle(Scalar radius, ushort verticesCount, Scalar mass, ALVector2D position, Guid modelId)
{
var newBody = CreateCircle(radius, verticesCount, mass, modelId);
newBody.State.Position = position;
newBody.ApplyPosition();
Will.Instance.AddBody(newBody);
return(newBody);
}
public static DisposeCallback RegisterBodyMovement(
DemoOpenInfo info, Body body, ALVector2D force,
Key forward, Key back, Key left, Key right)
{
ALVector2D currentForce = ALVector2D.Zero;
EventHandler<KeyboardEventArgs> downHandler = delegate(object sender, KeyboardEventArgs e)
{
if (e.Key == forward)
{
currentForce.Linear += force.Linear;
}
else if (e.Key == back)
{
currentForce.Linear -= force.Linear;
}
else if (e.Key == left)
{
currentForce.Angular -= force.Angular;
}
else if (e.Key == right)
{
currentForce.Angular += force.Angular;
}
};
EventHandler<KeyboardEventArgs> upHandler = delegate(object sender, KeyboardEventArgs e)
{
if (e.Key == forward)
{
currentForce.Linear -= force.Linear;
}
else if (e.Key == back)
{
currentForce.Linear += force.Linear;
}
else if (e.Key == left)
{
currentForce.Angular += force.Angular;
}
else if (e.Key == right)
{
currentForce.Angular -= force.Angular;
}
};
EventHandler<UpdatedEventArgs> update = delegate(object sender, UpdatedEventArgs e)
{
Vector2D force2 = body.Matrices.ToWorldNormal * currentForce.Linear;
body.State.ForceAccumulator.Linear += force2;
body.State.ForceAccumulator.Angular += currentForce.Angular;
};
body.Updated += update;
Events.KeyboardDown += downHandler;
Events.KeyboardUp += upHandler;
return delegate()
{
body.Updated -= update;
Events.KeyboardDown -= downHandler;
Events.KeyboardUp -= upHandler;
};
}
public static Body AddRectangle(DemoOpenInfo info, Scalar height, Scalar width, Scalar mass, ALVector2D position)
{
Vector2D[] vertexes = VertexHelper.CreateRectangle(width, height);
vertexes = VertexHelper.Subdivide(vertexes, Math.Min(height, width) / 5);
IShape boxShape = ShapeFactory.CreateColoredPolygon(vertexes, Math.Min(height, width) / 5);
Body body = new Body(new PhysicsState(position), boxShape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(body));
return body;
}
Body AddShape(IShape shape, Scalar mass, ALVector2D position)
{
Body e =
new Body(
new PhysicsState(position),
shape,
mass,
coefficients.Duplicate(),
new Lifespan());
AddGlObject(e);
engine.AddBody(e);
return e;
}
Body AddCircle(Scalar radius, int vertexCount, Scalar mass, ALVector2D position)
{
IShape circleShape = new CircleShape(radius, vertexCount); ;
Body e =
new Body(
new PhysicsState(position),
circleShape,
mass,
coefficients.Duplicate(),
new Lifespan());
AddGlObject(e);
engine.AddBody(e);
return e;
}
void DemoO()
{
BeginDemoChange();
Reset(false);
BoundingRectangle rect = this.clipper.Rectangle;
rect.Min.X -= 75;
rect.Min.Y -= 75;
rect.Max.X += 75;
rect.Max.Y += 75;
AddShell(rect, 100, Scalar.PositiveInfinity);
rect.Min.X += 100;
rect.Min.Y += 100;
rect.Max.X -= 100;
rect.Max.Y -= 100;
Scalar spacing = 10;
int randSpacing = (int)(spacing/2 );
for (Scalar x = rect.Min.X; x < rect.Max.X; x += spacing )
{
for (Scalar y = rect.Min.Y; y < rect.Max.Y; y += spacing )
{
Scalar radius = rand.Next(2, 5);
ALVector2D position = new ALVector2D(
0,
x + rand.Next(-randSpacing, randSpacing),
y + rand.Next(-randSpacing, randSpacing));
Body circle = AddCircle(radius, 10, radius * 2, position);
circle.State.Velocity.Linear.X = rand.Next(-1000, 1001);
circle.State.Velocity.Linear.Y = rand.Next(-1000, 1001);
}
}
EndDemoChange();
}
public static DisposeCallback RegisterBodyMovement(DemoOpenInfo info, Body body, ALVector2D force)
{
return(RegisterBodyMovement(info, body, force, Key.UpArrow, Key.DownArrow, Key.LeftArrow, Key.RightArrow));
}
public static Body AddRectangle(DemoOpenInfo info, Scalar height, Scalar width, Scalar mass, ALVector2D position)
{
Vector2D[] vertexes = VertexHelper.CreateRectangle(width, height);
vertexes = VertexHelper.Subdivide(vertexes, Math.Min(height, width) / 5);
IShape boxShape = ShapeFactory.CreateColoredPolygon(vertexes, Math.Min(height, width) / 5);
Body body = new Body(new PhysicsState(position), boxShape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(body));
return(body);
}
public static Body AddCircle(DemoOpenInfo info, Scalar radius, int vertexCount, Scalar mass, ALVector2D position)
{
CircleShape shape = ShapeFactory.CreateColoredCircle(radius, vertexCount);
Body result = new Body(new PhysicsState(position), shape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(result));
return(result);
}
public static DisposeCallback RegisterBodyMovement(
DemoOpenInfo info, Body body, ALVector2D force,
Key forward, Key back, Key left, Key right)
{
ALVector2D currentForce = ALVector2D.Zero;
EventHandler <KeyboardEventArgs> downHandler = delegate(object sender, KeyboardEventArgs e)
{
if (e.Key == forward)
{
currentForce.Linear += force.Linear;
}
else if (e.Key == back)
{
currentForce.Linear -= force.Linear;
}
else if (e.Key == left)
{
currentForce.Angular -= force.Angular;
}
else if (e.Key == right)
{
currentForce.Angular += force.Angular;
}
};
EventHandler <KeyboardEventArgs> upHandler = delegate(object sender, KeyboardEventArgs e)
{
if (e.Key == forward)
{
currentForce.Linear -= force.Linear;
}
else if (e.Key == back)
{
currentForce.Linear += force.Linear;
}
else if (e.Key == left)
{
currentForce.Angular += force.Angular;
}
else if (e.Key == right)
{
currentForce.Angular -= force.Angular;
}
};
EventHandler <UpdatedEventArgs> update = delegate(object sender, UpdatedEventArgs e)
{
Vector2D force2 = body.Matrices.ToWorldNormal * currentForce.Linear;
body.State.ForceAccumulator.Linear += force2;
body.State.ForceAccumulator.Angular += currentForce.Angular;
};
body.Updated += update;
Events.KeyboardDown += downHandler;
Events.KeyboardUp += upHandler;
return(delegate()
{
body.Updated -= update;
Events.KeyboardDown -= downHandler;
Events.KeyboardUp -= upHandler;
});
}
Body AddFloor(ALVector2D position)
{
Body line = new Body(
new PhysicsState(position),
new PolygonShape(VertexHelper.CreateRectangle(2000, 60), 40),
new MassInfo(Scalar.PositiveInfinity, Scalar.PositiveInfinity),
coefficients.Duplicate(),
new Lifespan());
line.IgnoresGravity = true;
AddGlObject(line);
engine.AddBody(line);
return line;
}
void AddFloor(ALVector2D position)
{
count++;
Body line = new Body(
new PhysicsState(position),
new Physics2DDotNet.Polygon(Physics2DDotNet.Polygon.CreateRectangle(60, 2000), 40),
new MassInfo(float.PositiveInfinity, float.PositiveInfinity),
new Coefficients(.2f, .2f, friction),
new Lifespan());
line.IgnoresGravity = true;
AddGlObject(line);
engine.AddBody(line);
}
Body AddRectangle(Scalar height, Scalar width, Scalar mass, ALVector2D position)
{
Vector2D[] vertexes = VertexHelper.CreateRectangle(width, height);
vertexes = VertexHelper.Subdivide(vertexes, (height + width) / 9);
IShape boxShape = new PolygonShape(vertexes, Math.Min(height, width) / 2);
Body e =
new Body(
new PhysicsState(position),
boxShape,
mass,
coefficients.Duplicate(),
new Lifespan());
AddGlObject(e);
engine.AddBody(e);
return e;
}
Body AddRectangle(float length, float width, float mass, ALVector2D position)
{
width += rand.Next(-4, 5) * .01f;
length += rand.Next(-4, 5) * .01f;
count++;
Vector2D[] vertices = Physics2DDotNet.Polygon.CreateRectangle(length, width);
vertices = Physics2DDotNet.Polygon.Subdivide(vertices, (length + width) / 4);
Shape boxShape = new Physics2DDotNet.Polygon(vertices, MathHelper.Min(length, width) / 2);
Body e =
new Body(
new PhysicsState(position),
boxShape,
mass,
new Coefficients(.2f, .2f, friction),
new Lifespan());
AddGlObject(e);
engine.AddBody(e);
return e;
}
public static DisposeCallback RegisterBodyMovement(DemoOpenInfo info, Body body, ALVector2D force)
{
return RegisterBodyMovement(info, body, force, Key.UpArrow, Key.DownArrow, Key.LeftArrow, Key.RightArrow);
}
Body AddCircle(float radius, int vertexCount, float mass, ALVector2D position)
{
count++;
Shape circleShape = new Physics2DDotNet.Circle(radius, vertexCount); ;
Body e =
new Body(
new PhysicsState(position),
circleShape,
mass,
new Coefficients(.2f, .2f, friction),
new Lifespan());
AddGlObject(e);
engine.AddBody(e);
return e;
}
public static Body AddCircle(DemoOpenInfo info, Scalar radius, int vertexCount, Scalar mass, ALVector2D position)
{
CircleShape shape = ShapeFactory.CreateColoredCircle(radius, vertexCount);
Body result = new Body(new PhysicsState(position), shape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(result));
return result;
}
static void Main(string[] args)
{
float x = 2;
float y = 2;
float z = +x * +y;
Console.WriteLine(z);
Vector2D[] loc = new Vector2D[] { new Vector2D(0, 0), new Vector2D(10, 0) };
Vector2D[] loc2 = Polygon.Subdivide(loc, 2);
ALVector2D alv = new ALVector2D(MathHelper.PI / 2, new Vector2D(50, 100));
Matrix2D matrix;
Matrix2D.FromALVector2D(ref alv, out matrix);
Vector2D vector = new Vector2D(0,10);
Console.WriteLine(vector);
Vector2D r1 = matrix.NormalMatrix * vector;
Vector2D r2 = matrix.VertexMatrix * vector;
Console.WriteLine(r1);
Console.WriteLine(r2);
Matrix2D matrixInv;
Matrix2D.Invert(ref matrix, out matrixInv);
Console.WriteLine(matrixInv.NormalMatrix * r1);
Console.WriteLine(matrixInv.VertexMatrix * r2);
Polygon polygon = new Polygon(Polygon.CreateRectangle(20, 20), 2);
Circle circle = new Circle(10,8);
IntersectionInfo info;
if (circle.TryGetIntersection(new Vector2D(0,10), out info) || info != null)
{
Console.WriteLine("circle");
Console.WriteLine(info.Normal);
Console.WriteLine(info.Distance);
}
if (polygon.TryGetIntersection(new Vector2D(3, 5), out info) || info != null)
{
Console.WriteLine("polygon");
Console.WriteLine(info.Normal);
Console.WriteLine(info.Distance);
}
Console.ReadLine();
/*
int[,,] arr = new int[,,]
{
{ { 1, 2 }, { 3, 4 } },
{ { 5, 6}, { 7,8 } }
};
int[,,] arr3 = new int[2, 2,3];
Array.Copy(arr, arr3, 4);
//Functions.ArrayCopy(arr, new int[] { 0, 0,0 }, arr3, new int[] { 0, 0,0 }, new int[] { 2, 2,2 });
int[,,] arr2 = (int[,,])Functions.ArrayRemoveRange(arr, 0, 1, 0);
return;*/
/* Form1 gggg = new Form1();
gggg.propertyGrid1.SelectedObject = new TESTObj();
Application.Run(gggg);
return;
TimeTester test = new TimeTester(100000000, TEST1, TEST2);
test.Run();
Console.WriteLine(test);
Polygon firstOld, firstNew, secondOld, secondNew;
Vector2D[] shape = new Vector2D[]{new Vector2D(1,1),new Vector2D(1,-1),new Vector2D(-1,-1),new Vector2D(-1,1)};
//Array.Reverse(shape);
firstOld = new Polygon(shape);
firstNew = (Polygon)firstOld.Clone();
secondOld = new Polygon(shape);
secondNew = (Polygon)secondOld.Clone();
ALVector2D pos = new ALVector2D();
pos.Linear = new Vector2D(20, 0);
firstOld.ApplyMatrix(pos.ToMatrix2D());
// pos.Angular = .01f;
secondNew.ApplyMatrix(pos.ToMatrix2D());
pos.Linear = new Vector2D(-20, 0);
firstNew.ApplyMatrix(pos.ToMatrix2D());
//pos.Angular = .02f;
secondOld.ApplyMatrix(pos.ToMatrix2D());
HorrableNarrowPhase phase = new HorrableNarrowPhase();
Matrix2D g = pos.ToMatrix2D();
Vector2D tt = new Vector2D();
Vector2D.Multiply(ref g.VertexMatrix, ref tt, out tt);
CollisionInfo info = phase.TestCollision(1, firstOld, firstNew, secondOld, secondNew);
nothering(info);
nothering(tt);*/
Console.WriteLine("Finished");
Console.ReadLine();
}
public static Body AddShape(DemoOpenInfo info, IShape shape, Scalar mass, ALVector2D position)
{
Body body = new Body(new PhysicsState(position), shape, mass, Coefficients.Duplicate(), new Lifespan());
info.Scene.AddGraphic(CreateGraphic(body));
return body;
}
private void AddSlot(BaseGeneViewModel gene)
{
Will.Instance.RunPauseWilling(false);
var slotGene = (NodeGeneViewModel)gene;
var boneBody = Will.Instance.Bodies.RandomOrDefault<BoneBody>(b => b.Model.ChildSlots.Any(s => s.IsOccupied == false));
if (boneBody == null)
{
Will.Instance.RunPauseWilling(true);
return;
}
var parPos = boneBody.State.Position;
var randSlot = boneBody.Model.ChildSlots.Where(s => s.IsOccupied == false).RandomOrDefault();
var slot = slotGene.GetModelDuplicate();
slot.Direction = randSlot.Direction;
slot.DistanceFromCenter = randSlot.DistanceFromCenter;
slot.Orientation = randSlot.Orientation;
randSlot.IsOccupied = true;
var slotBody = WillHelper.CreateConnectionSlotBody(slot, boneBody.ModelId);
var slotXAngle = slot.Direction + parPos.Angular;
var slotCenter = Vector2D.Rotate(slotXAngle, new Vector2D(slot.DistanceFromCenter, 0.0f));
var slotPos = new ALVector2D(slot.Orientation + slotXAngle, slotCenter + parPos.Linear);
slotBody.State.Position = slotPos;
slotBody.ApplyPosition();
slotBody.Parent = boneBody;
boneBody.Children.Add(slotBody);
var joints = new List<Joint>();
var nodePos = slotBody.State.Position;
var hinge = new HingeJoint(boneBody, slotBody, (slot.Size * nodePos.Linear + boneBody.Model.Length * parPos.Linear) * (1/(slot.Size + boneBody.Model.Length)), new Lifespan())
{
DistanceTolerance = 50,
Softness = 10.1
};
var angle = new AngleJoint(boneBody, slotBody, new Lifespan()) { Softness = 0.00001 };
joints.Add(hinge);
joints.Add(angle);
Will.Instance.AddBody(slotBody);
Will.Instance.AddJoints(joints);
Will.Instance.RunPauseWilling(true);
}
public void Set(PhysicsState state)
{
if (state == null) { throw new ArgumentNullException("state"); }
this.Position = state.Position;
this.Velocity = state.Velocity;
this.Acceleration = state.Acceleration;
this.ForceAccumulator = state.ForceAccumulator;
}
public static PhysicsState CreateState(ScatterRectangle target, Scalar radius, Scalar width, Scalar height)
{
ALVector2D answer;
answer = new ALVector2D(radius, (float)(target.Position.X + width * .5f), (float)(target.Position.Y + height * .5f));
return new PhysicsState(answer);
}
public PhysicsState(ALVector2D position, ALVector2D velocity)
{
this.Position = position;
this.Velocity = velocity;
this.Acceleration = ALVector2D.Zero;
this.ForceAccumulator = ALVector2D.Zero;
}
public static void GetRelativeVelocity(
ref ALVector2D velocity1, ref Vector2D point1, out Vector2D result)
{
result.X = -(velocity1.Linear.X - velocity1.Angular * point1.Y);
result.Y = -(velocity1.Linear.Y + velocity1.Angular * point1.X);
}
protected internal override void RunLogic(TimeStep step)
{
Scalar area = MathHelper.Pi * radius * radius;
Scalar density = explosionBody.Mass.Mass / area;
BoundingCircle circle = new BoundingCircle(explosionBody.State.Position.Linear, radius);
Matrix2x3 temp;
ALVector2D.ToMatrix2x3(ref explosionBody.State.Position, out temp);
Matrices matrices = new Matrices();
matrices.SetToWorld(ref temp);
Vector2D relativeVelocity = Vector2D.Zero;
Vector2D velocityDirection = Vector2D.Zero;
Vector2D dragDirection = Vector2D.Zero;
for (int index = 0; index < items.Count; ++index)
{
Wrapper wrapper = items[index];
Body body = wrapper.body;
Matrix2x3 matrix;
Matrix2x3.Multiply(ref matrices.ToBody, ref body.Matrices.ToWorld, out matrix);
ContainmentType containmentType;
BoundingRectangle rect = body.Rectangle;
circle.Contains(ref rect, out containmentType);
if (containmentType == ContainmentType.Intersects)
{
return;
GetTangentCallback callback = delegate(Vector2D centroid)
{
centroid = body.Matrices.ToWorld * centroid;
Vector2D p1 = centroid - explosionBody.State.Position.Linear;
Vector2D p2 = centroid - body.State.Position.Linear;
PhysicsHelper.GetRelativeVelocity(
ref explosionBody.State.Velocity,
ref body.State.Velocity,
ref p1,
ref p2,
out relativeVelocity);
relativeVelocity = p1.Normalized * this.pressurePulseSpeed;
relativeVelocity = -relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
return(dragDirection);
};
DragInfo dragInfo = wrapper.affectable.GetExplosionInfo(matrix, radius, callback);
if (dragInfo == null)
{
continue;
}
if (velocityDirection == Vector2D.Zero)
{
continue;
}
if (dragInfo.DragArea < .01f)
{
continue;
}
Scalar speedSq = relativeVelocity.MagnitudeSq;
Scalar dragForceMag = -.5f * density * speedSq * dragInfo.DragArea * dragCoefficient;
Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv;
if (dragForceMag < maxDrag)
{
dragForceMag = maxDrag;
}
Vector2D dragForce = dragForceMag * velocityDirection;
wrapper.body.ApplyForce(dragForce, (body.Matrices.ToBody * matrices.ToWorld) * dragInfo.DragCenter);
}
else if (containmentType == ContainmentType.Contains)
{
Vector2D centroid = body.Matrices.ToWorld * wrapper.affectable.Centroid;
Vector2D p1 = centroid - explosionBody.State.Position.Linear;
Vector2D p2 = centroid - body.State.Position.Linear;
PhysicsHelper.GetRelativeVelocity(
ref explosionBody.State.Velocity,
ref body.State.Velocity,
ref p1,
ref p2,
out relativeVelocity);
relativeVelocity = p1.Normalized * this.pressurePulseSpeed;
relativeVelocity = -relativeVelocity;
velocityDirection = relativeVelocity.Normalized;
dragDirection = matrices.ToBodyNormal * velocityDirection.LeftHandNormal;
DragInfo dragInfo = wrapper.affectable.GetFluidInfo(dragDirection);
if (dragInfo.DragArea < .01f)
{
continue;
}
Scalar speedSq = relativeVelocity.MagnitudeSq;
Scalar dragForceMag = -.5f * density * speedSq * dragInfo.DragArea * dragCoefficient;
Scalar maxDrag = -MathHelper.Sqrt(speedSq) * body.Mass.Mass * step.DtInv;
if (dragForceMag < maxDrag)
{
dragForceMag = maxDrag;
}
Vector2D dragForce = dragForceMag * velocityDirection;
wrapper.body.ApplyForce(dragForce, body.Matrices.ToWorldNormal * dragInfo.DragCenter);
wrapper.body.ApplyTorque(
-body.Mass.MomentOfInertia *
(body.Coefficients.DynamicFriction + density + dragCoefficient) *
body.State.Velocity.Angular);
}
}
}
public static void SubtractImpulse(
ref ALVector2D velocity, ref Vector2D impulse, ref Vector2D point,
ref Scalar massInv, ref Scalar inertiaInv)
{
velocity.Linear.X -= impulse.X * massInv;
velocity.Linear.Y -= impulse.Y * massInv;
velocity.Angular -= (inertiaInv * (point.X * impulse.Y - point.Y * impulse.X));
}
public static void FromALVector2D(ref ALVector2D source,out Matrix2D result)
{
Matrix2x2.FromRotation(ref source.Angular, out result.NormalMatrix);
Matrix3x3.FromTranslate2D(ref source.Linear, out result.VertexMatrix);
Matrix3x3.Multiply(ref result.VertexMatrix, ref result.NormalMatrix, out result.VertexMatrix);
}
private void AddSlot(BaseGeneViewModel gene)
{
Will.Instance.RunPauseWilling(false);
var slotGene = (NodeGeneViewModel)gene;
var boneBody = Will.Instance.Bodies.RandomOrDefault <BoneBody>(b => b.Model.ChildSlots.Any(s => s.IsOccupied == false));
if (boneBody == null)
{
Will.Instance.RunPauseWilling(true);
return;
}
var parPos = boneBody.State.Position;
var randSlot = boneBody.Model.ChildSlots.Where(s => s.IsOccupied == false).RandomOrDefault();
var slot = slotGene.GetModelDuplicate();
slot.Direction = randSlot.Direction;
slot.DistanceFromCenter = randSlot.DistanceFromCenter;
slot.Orientation = randSlot.Orientation;
randSlot.IsOccupied = true;
var slotBody = WillHelper.CreateConnectionSlotBody(slot, boneBody.ModelId);
var slotXAngle = slot.Direction + parPos.Angular;
var slotCenter = Vector2D.Rotate(slotXAngle, new Vector2D(slot.DistanceFromCenter, 0.0f));
var slotPos = new ALVector2D(slot.Orientation + slotXAngle, slotCenter + parPos.Linear);
slotBody.State.Position = slotPos;
slotBody.ApplyPosition();
slotBody.Parent = boneBody;
boneBody.Children.Add(slotBody);
var joints = new List <Joint>();
var nodePos = slotBody.State.Position;
var hinge = new HingeJoint(boneBody, slotBody, (slot.Size * nodePos.Linear + boneBody.Model.Length * parPos.Linear) * (1 / (slot.Size + boneBody.Model.Length)), new Lifespan())
{
DistanceTolerance = 50,
Softness = 10.1
};
var angle = new AngleJoint(boneBody, slotBody, new Lifespan())
{
Softness = 0.00001
};
joints.Add(hinge);
joints.Add(angle);
Will.Instance.AddBody(slotBody);
Will.Instance.AddJoints(joints);
Will.Instance.RunPauseWilling(true);
}