public CharacterJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
RigidBody rigid1 = (RigidBody)body1;
RigidBody rigid2 = (RigidBody)body2;
fixedAngle = new FixedAngle(rigid1, rigid2);
pointOnLine = new PointOnLine(rigid1, rigid2, rigid1.position, rigid2.position);
firstBody = body1;
secondBody = body2;
hingeA = hingeAxis;
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body2, (RigidBody)body1, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
//UnityEngine.CharacterJoint;
Activate();
}
public override void Build()
{
AddGround();
RigidBody last = null;
for (int i = 0; i < 12; i++)
{
var body = new RigidBody(new BoxShape(JVector.One))
{
Position = new JVector((i * 1.5f) - 20, 0.5f, 0)
};
var jpos2 = body.Position;
Demo.World.AddBody(body);
body.Update();
if (last != null)
{
var jpos3 = last.Position;
JVector.Subtract(ref jpos2, ref jpos3, out var dif);
JVector.Multiply(ref dif, 0.5f, out dif);
JVector.Subtract(ref jpos2, ref dif, out dif);
Constraint cons = new PointOnPoint(last, body, dif);
Demo.World.AddConstraint(cons);
}
last = body;
}
}
/// <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 HingeJoint(World world, RigidBody body1, RigidBody body2, Vector3 position, Vector3 hingeAxis) : base(world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= 0.5f;
Vector3 pos1 = position; Vector3.Add(ref pos1, ref hingeAxis, out pos1);
Vector3 pos2 = position; Vector3.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint(body1, body2, pos1);
worldPointConstraint[1] = new PointOnPoint(body1, body2, pos2);
}
public HingeJoint(World world, RigidBody body1, RigidBody body2, JVector position, JVector hingeAxis) : 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);
}
private HingeJoint3D(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector pos1 = position; TSVector.Add(ref pos1, ref hingeAxis, out pos1);
TSVector pos2 = position; TSVector.Subtract(ref pos2, ref hingeAxis, out pos2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos1);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, pos2);
Activate();
}
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 void End()
{
if (constraint == null)
{
return;
}
model.RigidBody.AllowDeactivation = true;
model.Batch.Material = materialStore;
materialStore = null;
sceneManager.World.RemoveConstraint(constraint);
constraint = null;
model = null;
}
private void CreatePointOnPointJoint()
{
JVector localAnchor;
Vector3D tempVector = ArrayExtensions.GetWithDefault <PropertyType, Vector3D>(Properties,
PropertyType.Anchor1);
JitterDatatypesMapping.Convert(ref tempVector, out localAnchor);
if (RigidBodyB != null)
{
Constraint = new PointOnPoint(RigidBodyA, RigidBodyB, localAnchor);
}
else
{
Constraint = new global::Jitter.Dynamics.Constraints.SingleBody.PointOnPoint(RigidBodyA,
localAnchor);
}
}
public MyHingeJoint(IWorld world, IBody3D body1, IBody3D body2, TSVector position, TSVector hingeAxis) : base((World)world)
{
firstBody = body1;
secondBody = body2;
hingeA = hingeAxis;
worldPointConstraint = new PointOnPoint[2];
hingeAxis *= FP.Half;
TSVector anchor = position;
TSVector.Add(ref anchor, ref hingeAxis, out anchor);
TSVector anchor2 = position;
TSVector.Subtract(ref anchor2, ref hingeAxis, out anchor2);
worldPointConstraint[0] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor);
worldPointConstraint[1] = new PointOnPoint((RigidBody)body1, (RigidBody)body2, anchor2);
StateTracker.AddTracking(worldPointConstraint[0]);
StateTracker.AddTracking(worldPointConstraint[1]);
}
public void Begin()
{
if (
(Configuration.physics == false) ||
(selectionManager == null) ||
(selectionManager.HoverModel == null) ||
(selectionManager.HoverModel.RigidBody == null) ||
(selectionManager.HoverModel.Static == true)
)
{
return;
}
model = selectionManager.HoverModel;
materialStore = model.Batch.Material;
var materialManager = Services.Get <MaterialManager>();
model.Batch.Material = materialManager["EdgeLines"];
sceneManager.UnparentModel(model);
Frame frame = model.Frame;
RigidBody body = model.RigidBody;
//sceneManager.ActivateModel(model);
body.IsStatic = false;
body.IsActive = true;
body.AllowDeactivation = false;
frame.Updated = false;
//frame.UpdateHierarchical();
Vector3 snap = selectionManager.HoverPosition;
Vector3 camera = sceneManager.Camera.Frame.LocalToWorld.Matrix.GetColumn3(3);
Vector3 direction = Vector3.Normalize(snap - camera);
lockDistance = camera.Distance(snap);
snapInLocal = model.Frame.LocalToWorld.InverseMatrix.TransformPoint(snap);
//JVector jpos = new JVector(snapInLocal.X, snapInLocal.Y, snapInLocal.Z);
constraint = new PointOnPoint(
body,
snapInLocal
);
constraint.Anchor = snap; //new JVector(snap.X, snap.Y, snap.Z);
sceneManager.World.AddConstraint(constraint);
}
/// <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);
}
