private void AddDistance(int p1, int p2)
{
PointPointDistance dc = new PointPointDistance(bodies[p1], bodies[p2], bodies[p1].Position, bodies[p2].Position);
dc.Softness = 0.1f; //2//0.0001f
dc.BiasFactor = 0.1f;
//dc.Distance *= 0.01f;
dc.Behavior = PointPointDistance.DistanceBehavior.LimitDistance;
world.AddConstraint(dc);
this.constraints.Add(dc);
}
public PrismaticJoint(World world, RigidBody body1, RigidBody body2, float minimumDistance, float maximumDistance)
: base(world)
{
fixedAngle = new FixedAngle(body1, body2);
pointOnLine = new PointOnLine(body1, body2, body1.position, body2.position);
minDistance = new PointPointDistance(body1, body2, body1.position, body2.position);
minDistance.Behavior = PointPointDistance.DistanceBehavior.LimitMinimumDistance;
minDistance.Distance = minimumDistance;
maxDistance = new PointPointDistance(body1, body2, body1.position, body2.position);
maxDistance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
maxDistance.Distance = maximumDistance;
}
private void CreatePointPointDistanceJoint()
{
JVector anchor1;
Vector3D tempVector = ArrayExtensions.GetWithDefault <PropertyType, Vector3D>(Properties,
PropertyType.Anchor1);
JitterDatatypesMapping.Convert(ref tempVector, out anchor1);
JVector anchor2;
tempVector = ArrayExtensions.GetWithDefault <PropertyType, Vector3D>(Properties,
PropertyType.Anchor2);
JitterDatatypesMapping.Convert(ref tempVector, out anchor2);
Constraint = new PointPointDistance(RigidBodyA, RigidBodyB, anchor1, anchor2);
}
public LimitedHingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis, float hingeFwdAngle, float hingeBckAngle) : base(world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
var pos1 = position;
JVector.Add(pos1, hingeAxis, out pos1);
var pos2 = position;
JVector.Subtract(pos2, hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
hingeAxis = JVector.Normalize(hingeAxis);
var perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
var sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir = JVector.Normalize(perpDir);
float len = 10.0f * 3;
var hingeRelAnchorPos0 = perpDir * len;
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
var hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
var hingePos = body1.Position;
var relPos0c = hingePos + hingeRelAnchorPos0;
var relPos1c = hingePos + hingeRelAnchorPos1;
DistanceConstraint = new PointPointDistance(body1, body2, relPos0c, relPos1c)
{
Distance = allowedDistance,
Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance
};
}
public PrismaticJoint(World world, RigidBody body1, RigidBody body2, float minimumDistance, float maximumDistance)
: base(world)
{
FixedAngleConstraint = new FixedAngle(body1, body2);
PointOnLineConstraint = new PointOnLine(body1, body2, body1.position, body2.position);
MinimumDistanceConstraint = new PointPointDistance(body1, body2, body1.position, body2.position)
{
Behavior = PointPointDistance.DistanceBehavior.LimitMinimumDistance,
Distance = minimumDistance
};
MaximumDistanceConstraint = new PointPointDistance(body1, body2, body1.position, body2.position)
{
Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance,
Distance = maximumDistance
};
}
public LimitHingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis, FP minLimit, FP maxLimit) : base(world, body1, body2, position, hingeAxis)
{
TSVector perpDir = TSVector.up;
if (TSVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = TSVector.right;
}
TSVector sideAxis = TSVector.Cross(hingeAxis, perpDir);
perpDir = TSVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
FP len = 15;
TSVector hingeRelAnchorPos0 = perpDir * len;
FP angleToMiddle = FP.Half * (minLimit - maxLimit);
TSMatrix outMatrix;
TSMatrix.CreateFromAxisAngle(ref hingeAxis, -angleToMiddle * FP.Deg2Rad, out outMatrix);
TSVector hingeRelAnchorPos1 = TSVector.Transform(hingeRelAnchorPos0, outMatrix);
FP hingeHalfAngle = FP.Half * (minLimit + maxLimit);
FP allowedDistance = len * 2 * FP.Sin(hingeHalfAngle * FP.Half * FP.Deg2Rad);
TSVector hingePos = body1.TSPosition;
TSVector relPos0c = hingePos + hingeRelAnchorPos0;
TSVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance((RigidBody)body1, (RigidBody)body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
StateTracker.AddTracking(distance);
}
private void AddDistance(int p1, int p2)
{
//Console.WriteLine("AddDistance");
PointPointDistance dc = new PointPointDistance(bodies[p1], bodies[p2], bodies[p1].Position, bodies[p2].Position);
//Console.WriteLine(bodies[p1].Position + " _ " + bodies[p2].Position);
//DistanceConstraint dc = new DistanceConstraint(bodies[p1], bodies[p2], bodies[p1].position, bodies[p2].position);
dc.Softness = 0.0001f; //2
dc.BiasFactor = 0.75f;
//dc.Distance *= 0.01f;
dc.Behavior = PointPointDistance.DistanceBehavior.LimitDistance;
world.AddConstraint(dc);
this.constraints.Add(dc);
/*PointPointDistance dcer = new PointPointDistance(DirectXComponent._arrayOfClothCubes[p1].transform.Component.rigidbody, DirectXComponent._arrayOfClothCubes[p2].transform.Component.rigidbody, DirectXComponent._arrayOfClothCubes[p1].transform.Component.rigidbody.Position, DirectXComponent._arrayOfClothCubes[p2].transform.Component.rigidbody.Position);
* dcer.Softness = 0.0000001f; //2
* dcer.BiasFactor = 0.000000001f;
* dcer.Distance = 0.01f;
*
* world.AddConstraint(dcer);
* this.constraints.Add(dcer);*/
}
public override void Build()
{
this.Demo.World.Solver = Jitter.World.SolverType.Sequential;
AddGround();
RigidBody boxb = new RigidBody(new BoxShape(7, 1, 2));
boxb.Position = new JVector(3.0f, 12, 0);
this.Demo.World.AddBody(boxb);
boxb.Tag = BodyTag.DontDrawMe;
boxb.IsStatic = true;
this.Demo.World.Solver = Jitter.World.SolverType.Sequential;
//this.Demo.World.SetDampingFactors(1.0f, 1.0f);
SphereShape shape = new SphereShape(0.501f);
for (int i = 0; i < 7; i++)
{
RigidBody body = new RigidBody(shape);
body.Position = new JVector(i, 6, 0);
PointPointDistance dc1 = new PointPointDistance(bodies[p1], bodies[p2], bodies[p1].Position, bodies[p2].Position);
//Console.WriteLine(bodies[p1].Position + " _ " + bodies[p2].Position);
//DistanceConstraint dc = new DistanceConstraint(bodies[p1], bodies[p2], bodies[p1].position, bodies[p2].position);
dc1.Softness = 0.0001f; //2
dc1.BiasFactor = 0.75f;
//dc.Distance *= 0.01f;
dc1.Behavior = PointPointDistance.DistanceBehavior.LimitDistance;
world.AddConstraint(dc1);
this.constraints.Add(dc1);
//DistanceConstraint dc1 = new DistanceConstraint(boxb, body, body.Position + JVector.Up * 6 + JVector.Backward * 5 + JVector.Down * 0.5f, body.Position);
//dc1.Softness = 1.0f;
//DistanceConstraint dc2 = new DistanceConstraint(boxb, body, body.Position + JVector.Up * 6 + JVector.Forward * 5 + JVector.Down * 0.5f, body.Position);
//dc2.Softness = 1.0f;
dc1.BiasFactor = dc2.BiasFactor = 0.8f;
dc1.IsMaxDistance = dc2.IsMaxDistance = false;
this.Demo.World.AddBody(body);
this.Demo.World.AddConstraint(dc1);
this.Demo.World.AddConstraint(dc2);
body.Restitution = 1.0f;
body.StaticFriction = 1.0f;
// this.Demo.World.SetDampingFactors(1.0f, 1.0f);
}
//for (int i = 0; i < 5; i++)
//{
// RigidBody sBody = new RigidBody(new SphereShape(0.5f));
// sBody.Position = new JVector(0, 0.5f, i);
// this.Demo.World.AddBody(sBody);
// sBody.Restitution = 1.0f;
// sBody.Friction = 0.0f;
//}
//for (int i = 0; i < 3; i++)
//{
// RigidBody sBody = new RigidBody(new SphereShape(0.5f));
// sBody.Position = new JVector(0, 0.5f, 10 + i);
// this.Demo.World.AddBody(sBody);
// sBody.LinearVelocity = JVector.Forward * 3;
// sBody.Restitution = 1.0f;
// sBody.Friction = 0.0f;
//}
//this.Demo.World.SetDampingFactors(1, 1);
}
/// <summary>
/// Initializes a new instance of the HingeJoint class.
/// </summary>
/// <param name="world">The world class where the constraints get added to.</param>
/// <param name="body1">The first body connected to the second one.</param>
/// <param name="body2">The second body connected to the first one.</param>
/// <param name="position">The position in world space where both bodies get connected.</param>
/// <param name="hingeAxis">The axis if the hinge.</param>
public LimitedHingeJoint(JitterWorld world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis,
float hingeFwdAngle, float hingeBckAngle)
: base(world)
{
// Create the hinge first, two point constraints
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
JVector pos1 = position; JVector.Add(ref pos1, ref hingeAxis, out pos1);
JVector pos2 = position; JVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
// Now the limit, one max distance constraint
hingeAxis.Normalize();
// choose a direction that is perpendicular to the hinge
JVector perpDir = JVector.Up;
if (JVector.Dot(perpDir, hingeAxis) > 0.1f)
{
perpDir = JVector.Right;
}
// now make it perpendicular to the hinge
JVector sideAxis = JVector.Cross(hingeAxis, perpDir);
perpDir = JVector.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * 3;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
JVector hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
JVector hingeRelAnchorPos1 = JVector.Transform(hingeRelAnchorPos0, JMatrix.CreateFromAxisAngle(hingeAxis, -angleToMiddle / 360.0f * 2.0f * JMath.Pi));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(hingeHalfAngle * 0.5f / 360.0f * 2.0f * JMath.Pi);
JVector hingePos = body1.Position;
JVector relPos0c = hingePos + hingeRelAnchorPos0;
JVector relPos1c = hingePos + hingeRelAnchorPos1;
distance = new PointPointDistance(body1, body2, relPos0c, relPos1c);
distance.Distance = allowedDistance;
distance.Behavior = PointPointDistance.DistanceBehavior.LimitMaximumDistance;
}
public void BuildRagdoll(World world, JVector position)
{
// the torso
var torso = new RigidBody(new BoxShape(1.5f, 3, 0.5f))
{
Position = position
};
// the head
var head = new RigidBody(new SphereShape(0.5f))
{
Position = position + new JVector(0, 2.1f, 0)
};
// connect head and torso
var headTorso = new PointPointDistance(head, torso,
position + new JVector(0, 1.6f, 0), position + new JVector(0, 1.5f, 0));
var arm1 = new RigidBody(new CapsuleShape(0.8f, 0.2f))
{
Position = position + new JVector(1.0f, 0.75f, 0)
};
var arm2 = new RigidBody(new CapsuleShape(0.8f, 0.2f))
{
Position = position + new JVector(-1.0f, 0.75f, 0)
};
var lowerarm1 = new RigidBody(new CapsuleShape(0.6f, 0.2f))
{
Position = position + new JVector(1.0f, -0.45f, 0)
};
var lowerarm2 = new RigidBody(new CapsuleShape(0.6f, 0.2f))
{
Position = position + new JVector(-1.0f, -0.45f, 0)
};
var arm1torso = new PointOnPoint(arm1, torso, position + new JVector(0.9f, 1.4f, 0));
var arm2torso = new PointOnPoint(arm2, torso, position + new JVector(-0.9f, 1.4f, 0));
var arm1Hinge = new HingeJoint(world, arm1, lowerarm1, position + new JVector(1.0f, 0.05f, 0), JVector.Right);
var arm2Hinge = new HingeJoint(world, arm2, lowerarm2, position + new JVector(-1.0f, 0.05f, 0), JVector.Right);
var leg1 = new RigidBody(new CapsuleShape(1.0f, 0.3f))
{
Position = position + new JVector(-0.5f, -2.4f, 0)
};
var leg2 = new RigidBody(new CapsuleShape(1.0f, 0.3f))
{
Position = position + new JVector(0.5f, -2.4f, 0)
};
var leg1torso = new PointOnPoint(leg1, torso, position + new JVector(-0.5f, -1.6f, 0));
var leg2torso = new PointOnPoint(leg2, torso, position + new JVector(+0.5f, -1.6f, 0));
var lowerleg1 = new RigidBody(new CapsuleShape(0.8f, 0.3f))
{
Position = position + new JVector(-0.5f, -4.0f, 0)
};
var lowerleg2 = new RigidBody(new CapsuleShape(0.8f, 0.3f))
{
Position = position + new JVector(+0.5f, -4.0f, 0)
};
var leg1Hinge = new HingeJoint(world, leg1, lowerleg1, position + new JVector(-0.5f, -3.35f, 0), JVector.Right);
var leg2Hinge = new HingeJoint(world, leg2, lowerleg2, position + new JVector(0.5f, -3.35f, 0), JVector.Right);
lowerleg1.IsActive = false;
lowerleg2.IsActive = false;
leg1.IsActive = false;
leg2.IsActive = false;
head.IsActive = false;
torso.IsActive = false;
arm1.IsActive = false;
arm2.IsActive = false;
lowerarm1.IsActive = false;
lowerarm2.IsActive = false;
world.AddBody(head); world.AddBody(torso);
world.AddBody(arm1); world.AddBody(arm2);
world.AddBody(lowerarm1); world.AddBody(lowerarm2);
world.AddBody(leg1); world.AddBody(leg2);
world.AddBody(lowerleg1); world.AddBody(lowerleg2);
arm1Hinge.Activate(); arm2Hinge.Activate();
leg1Hinge.Activate(); leg2Hinge.Activate();
world.AddConstraint(headTorso);
world.AddConstraint(arm1torso);
world.AddConstraint(arm2torso);
world.AddConstraint(leg1torso);
world.AddConstraint(leg2torso);
}
