private void TwoBodyJointConstraint(Func <KinematicsSystem, IConstraintSolver> solver)
{
var a = new PhysicalParticle();
var b = new PhysicalParticle();
var c = new JointConstraint(a, Vector2.UnitX, b, -Vector2.UnitX);
b.Position = 2 * Vector2.UnitX;
b.Velocity.X = 10;
system.AddParticles(a, b);
system.AddConstraints(c);
var constraintSolver = solver(system);
var integrator = new KinematicsIntegrator(system, constraintSolver)
{
MaximumTimeStep = 0.01f,
MaxStepsPerFrame = 500
};
integrator.Integrate(1);
a.Position.X.Should().BeApproximately(5, tolerance);
b.Position.X.Should().BeApproximately(7, tolerance);
a.Velocity.X.Should().BeApproximately(5, tolerance);
b.Velocity.X.Should().BeApproximately(5, tolerance);
}
public void PointTouchConstraintUnsatisfiedValue()
{
particle2.Position = new Vector2(20, 0);
var pointTouchConstraint = new JointConstraint(particle1, point1, particle2, point2);
pointTouchConstraint.Value(new[] { particle1, particle2 }).Should().BeApproximately(32, tolerance);
}
public void PointTouchConstraintSatisfiedDerivative()
{
var pointTouchConstraint = new JointConstraint(particle1, point1, particle2, point2);
var derivative = pointTouchConstraint.Derivative(particle1);
derivative.RespectToX.Should().BeApproximately(0, tolerance);
derivative.RespectToY.Should().BeApproximately(0, tolerance);
derivative.RespectToAngle.Should().BeApproximately(0, tolerance);
derivative = pointTouchConstraint.Derivative(particle2);
derivative.RespectToX.Should().BeApproximately(0, tolerance);
derivative.RespectToY.Should().BeApproximately(0, tolerance);
derivative.RespectToAngle.Should().BeApproximately(0, tolerance);
}
/// <summary>
/// Removes the constraint.
/// </summary>
/// <param name="ctn">The CTN.</param>
public override void RemoveConstraint(IPhysicConstraint ctn)
{
if (ctn.PhysicConstraintType == PhysicConstraintType.JOINT)
{
JointConstraint bctn = ctn as JointConstraint;
space.Remove(bctn.Joint);
ctns.Remove(bctn);
}
else if (ctn.PhysicConstraintType == PhysicConstraintType.SOLVER)
{
MultipleSubConstraints co = ctn as MultipleSubConstraints;
{
space.Remove(co.Constraint);
ctns.Remove(co);;
}
}
}
public void PointTouchConstraintUnsatisfiedDerivative()
{
particle2.Position = new Vector2(20, 0);
var pointTouchConstraint = new JointConstraint(particle1, point1, particle2, point2);
var derivative = pointTouchConstraint.Derivative(particle1);
derivative.RespectToX.Should().BeApproximately(0, tolerance);
derivative.RespectToY.Should().BeApproximately(8, tolerance);
derivative.RespectToAngle.Should().BeApproximately(-80, tolerance);
derivative = pointTouchConstraint.Derivative(particle2);
derivative.RespectToX.Should().BeApproximately(0, tolerance);
derivative.RespectToY.Should().BeApproximately(-8, tolerance);
derivative.RespectToAngle.Should().BeApproximately(-80, tolerance);
}
public KinematicsSystem Initialize(Size area)
{
boxes.Clear();
system = new KinematicsSystem();
for (int i = 0; i < BoxCount; i++)
{
boxes.Add(new PhysicalParticle
{
Polygon = new Rectangle(-boxSize / 2, -boxSize / 2, boxSize, boxSize).ToPolygon(),
Position = new Vector2(
area.Width * 0.5 + (BoxCount / 2 - i) * boxSize,
area.Height * 0.3
),
Velocity = new Vector2(0, (BoxCount / 2 - i) * 100)
});
if (i > 0)
{
var constraint = new JointConstraint(boxes[i - 1], new Vector2(-boxSize * 0.5, 0),
boxes[i], new Vector2(boxSize * 0.5, 0));
system.AddConstraints(constraint);
}
}
sphere = new PhysicalParticle
{
Position = new Vector2(area.Width * 0.5, area.Height * 0.25)
};
var constraintBox = boxes[boxes.Count / 2];
system.AddConstraints(
new CirclePerimeterOffcenterConstraint(constraintBox,
sphere.Position, 40, constraintBox.Polygon[0]));
system.AddParticles(boxes.ToArray());
system.AddForceField(new ConstantGravityField());
return(system);
}
public override void AddConstraint(IPhysicConstraint ctn)
{
if (ctn.PhysicConstraintType == PhysicConstraintType.JOINT)
{
JointConstraint co = (JointConstraint)ctn;
if (co != null)
{
space.Add(co.Joint);
ctns.Add(co);
}
}
else if (ctn.PhysicConstraintType == PhysicConstraintType.SOLVER)
{
MultipleSubConstraints co = (MultipleSubConstraints)ctn;
if (co != null)
{
space.Add(co.Constraint);
ctns.Add(co);
}
}
}
private void ThreeBodyJointConstraint(Func <KinematicsSystem, IConstraintSolver> solver)
{
var a1 = new PhysicalParticle();
var a2 = new PhysicalParticle();
var a3 = new PhysicalParticle();
var c1 = new JointConstraint(a1, Vector2.UnitX, a2, -Vector2.UnitX);
var c2 = new JointConstraint(a2, Vector2.UnitX, a3, -Vector2.UnitX);
a2.Position = 2 * Vector2.UnitX;
a3.Position = 4 * Vector2.UnitX;
a2.Velocity.X = 15;
c1.Value(new List <PhysicalParticle> {
a1, a2
}).Should().Be(0);
c2.Value(new List <PhysicalParticle> {
a2, a3
}).Should().Be(0);
system.AddParticles(a1, a2, a3);
system.AddConstraints(c1, c2);
var constraintSolver = solver(system);
var integrator = new KinematicsIntegrator(system, constraintSolver)
{
MaximumTimeStep = 0.005f,
MaxStepsPerFrame = 1000
};
integrator.Integrate(1);
a1.Position.X.Should().BeApproximately(5, tolerance);
a2.Position.X.Should().BeApproximately(7, tolerance);
a3.Position.X.Should().BeApproximately(9, tolerance);
a1.Velocity.X.Should().BeApproximately(5, tolerance);
a2.Velocity.X.Should().BeApproximately(5, tolerance);
a3.Velocity.X.Should().BeApproximately(5, tolerance);
}
public void Write(TProtocol oprot)
{
oprot.IncrementRecursionDepth();
try
{
TStruct struc = new TStruct("MConstraint");
oprot.WriteStructBegin(struc);
TField field = new TField();
if (ID == null)
{
throw new TProtocolException(TProtocolException.INVALID_DATA, "required field ID not set");
}
field.Name = "ID";
field.Type = TType.String;
field.ID = 1;
oprot.WriteFieldBegin(field);
oprot.WriteString(ID);
oprot.WriteFieldEnd();
if (GeometryConstraint != null && __isset.GeometryConstraint)
{
field.Name = "GeometryConstraint";
field.Type = TType.Struct;
field.ID = 2;
oprot.WriteFieldBegin(field);
GeometryConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (VelocityConstraint != null && __isset.VelocityConstraint)
{
field.Name = "VelocityConstraint";
field.Type = TType.Struct;
field.ID = 3;
oprot.WriteFieldBegin(field);
VelocityConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (AccelerationConstraint != null && __isset.AccelerationConstraint)
{
field.Name = "AccelerationConstraint";
field.Type = TType.Struct;
field.ID = 4;
oprot.WriteFieldBegin(field);
AccelerationConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (PathConstraint != null && __isset.PathConstraint)
{
field.Name = "PathConstraint";
field.Type = TType.Struct;
field.ID = 5;
oprot.WriteFieldBegin(field);
PathConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (JointPathConstraint != null && __isset.JointPathConstraint)
{
field.Name = "JointPathConstraint";
field.Type = TType.Struct;
field.ID = 6;
oprot.WriteFieldBegin(field);
JointPathConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (PostureConstraint != null && __isset.PostureConstraint)
{
field.Name = "PostureConstraint";
field.Type = TType.Struct;
field.ID = 7;
oprot.WriteFieldBegin(field);
PostureConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (JointConstraint != null && __isset.JointConstraint)
{
field.Name = "JointConstraint";
field.Type = TType.Struct;
field.ID = 8;
oprot.WriteFieldBegin(field);
JointConstraint.Write(oprot);
oprot.WriteFieldEnd();
}
if (Properties != null && __isset.Properties)
{
field.Name = "Properties";
field.Type = TType.Map;
field.ID = 9;
oprot.WriteFieldBegin(field);
{
oprot.WriteMapBegin(new TMap(TType.String, TType.String, Properties.Count));
foreach (string _iter4 in Properties.Keys)
{
oprot.WriteString(_iter4);
oprot.WriteString(Properties[_iter4]);
}
oprot.WriteMapEnd();
}
oprot.WriteFieldEnd();
}
oprot.WriteFieldStop();
oprot.WriteStructEnd();
}
finally
{
oprot.DecrementRecursionDepth();
}
}
// joint constraint check
bool check_joint_constraint( JointConstraint constraint )
{
Vector3 ja, jb;
switch (constraint.relation) {
case JointConstraint.Relations.DISTANCE:
switch (constraint.operation) {
case JointConstraint.Operators.GREATER_THAN:
return Vector3.Distance(raw_joint_pos[(int)constraint.joint_a], raw_joint_pos[(int)constraint.joint_b]) > constraint.val.x;
case JointConstraint.Operators.LESS_THAN:
return Vector3.Distance(raw_joint_pos[(int)constraint.joint_a], raw_joint_pos[(int)constraint.joint_b]) < constraint.val.x;
default:
return false;
}
case JointConstraint.Relations.COMPONENT_DISTANCE:
ja = raw_joint_pos[(int)constraint.joint_a];
jb = raw_joint_pos[(int)constraint.joint_b];
switch (constraint.operation) {
case JointConstraint.Operators.GREATER_THAN:
return (ja.x - jb.x) > constraint.val.x && (ja.y - jb.y) > constraint.val.y && (ja.z - jb.z) > constraint.val.z;
case JointConstraint.Operators.LESS_THAN:
return (ja.x - jb.x) < constraint.val.x && (ja.y - jb.y) < constraint.val.y && (ja.z - jb.z) < constraint.val.z;
default:
return false;
}
case JointConstraint.Relations.ABS_COMPONENT_DISTANCE:
ja = raw_joint_pos[(int)constraint.joint_a];
jb = raw_joint_pos[(int)constraint.joint_b];
switch (constraint.operation) {
case JointConstraint.Operators.GREATER_THAN:
return Math.Abs(ja.x - jb.x) > constraint.val.x && Math.Abs(ja.y - jb.y) > constraint.val.y && Math.Abs(ja.z - jb.z) > constraint.val.z;
case JointConstraint.Operators.LESS_THAN:
return Math.Abs(ja.x - jb.x) < constraint.val.x && Math.Abs(ja.y - jb.y) < constraint.val.y && Math.Abs(ja.z - jb.z) < constraint.val.z;
default:
return false;
}
default:
return false;
}
}
public void PointTouchConstraintSatisfiedValue()
{
var pointTouchConstraint = new JointConstraint(particle1, point1, particle2, point2);
pointTouchConstraint.Value(new[] { particle1, particle2 }).Should().BeApproximately(0, tolerance);
}
/// <summary>
/// Load content for the screen.
/// </summary>
/// <param name="GraphicInfo"></param>
/// <param name="factory"></param>
/// <param name="contentManager"></param>
protected override void LoadContent(PloobsEngine.Engine.GraphicInfo GraphicInfo, PloobsEngine.Engine.GraphicFactory factory, IContentManager contentManager)
{
///must be called before all
base.LoadContent(GraphicInfo, factory, contentManager);
this.gf = factory;
///Uncoment to Add an object
/////Create a simple object
/////Geomtric Info and textures (this model automaticaly loads the texture)
//SimpleModel simpleModel = new SimpleModel(factory, "Model FILEPATH GOES HERE", "Diffuse Texture FILEPATH GOES HERE -- Use only if it is not embeded in the Model file");
/////Physic info (position, rotation and scale are set here)
//TriangleMeshObject tmesh = new TriangleMeshObject(simpleModel, Vector3.Zero, Matrix.Identity, Vector3.One, MaterialDescription.DefaultBepuMaterial());
/////Shader info (must be a deferred type)
//DeferredNormalShader shader = new DeferredNormalShader();
/////Material info (must be a deferred type also)
//DeferredMaterial fmaterial = new DeferredMaterial(shader);
/////The object itself
//IObject obj = new IObject(fmaterial, simpleModel, tmesh);
/////Add to the world
//this.World.AddObject(obj);
List <IObject> balls = new List <IObject>();
Vector3 pos1, pos2;
pos1 = Vector3.Zero;
pos2 = new Vector3(0, -3, 0);
IObject ball1 = CreateSphere(pos1, Matrix.Identity);
ball1.PhysicObject.isMotionLess = true; // Setting the Parent object as imovable
World.AddObject(ball1);
IObject ball2 = CreateSphere(pos2, Matrix.Identity);
World.AddObject(ball2);
balls.Add(ball2);
constraint = new PointPointConstraint((pos1 + pos2) / 2, ball1.PhysicObject, ball2.PhysicObject);
World.PhysicWorld.AddConstraint(constraint);
///Add some directional lights to completely iluminate the world
#region Lights
DirectionalLightPE ld1 = new DirectionalLightPE(Vector3.Left, Color.White);
DirectionalLightPE ld2 = new DirectionalLightPE(Vector3.Right, Color.White);
DirectionalLightPE ld3 = new DirectionalLightPE(Vector3.Backward, Color.White);
DirectionalLightPE ld4 = new DirectionalLightPE(Vector3.Forward, Color.White);
DirectionalLightPE ld5 = new DirectionalLightPE(Vector3.Down, Color.White);
float li = 0.4f;
ld1.LightIntensity = li;
ld2.LightIntensity = li;
ld3.LightIntensity = li;
ld4.LightIntensity = li;
ld5.LightIntensity = li;
this.World.AddLight(ld1);
this.World.AddLight(ld2);
this.World.AddLight(ld3);
this.World.AddLight(ld4);
this.World.AddLight(ld5);
#endregion
LightThrowBepu lt = new LightThrowBepu(this.World, factory);
///add a camera
this.World.CameraManager.AddCamera(new CameraFirstPerson(GraphicInfo));
}
