/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public TwistTestDemo(DemosGame game)
: base(game)
{
var a = new Box(new Vector3(-2, 2, 0), 1, 2, 2, 5);
var b = new Box(new Vector3(2, 2, 0), 1, 2, 2, 5);
b.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.PiOver4);
Space.Add(a);
Space.Add(b);
var twistJoint = new TwistJoint(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right);
var twistMotor = new TwistMotor(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right);
twistMotor.Settings.Mode = MotorMode.Servomechanism;
//Space.Add(twistJoint);
Space.Add(twistMotor);
var ballSocketJoint = new BallSocketJoint(a, b, (a.Position + b.Position) * 0.5f);
var swingLimit = new SwingLimit(a, b, a.OrientationMatrix.Right, a.OrientationMatrix.Right, MathHelper.PiOver2);
Space.Add(ballSocketJoint);
Space.Add(swingLimit);
Box ground = new Box(new Vector3(0, -.5f, 0), 50, 1, 50);
Space.Add(ground);
game.Camera.Position = new Vector3(0, 6, 15);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param>
public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
Vector3 tmp;
BallSocketJoint.OffsetB.Invert( out tmp );
AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp );
HingeLimit = new RevoluteLimit(connectionA, connectionB);
HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
HingeLimit.IsActive = false;
HingeMotor.IsActive = false;
TwistLimit.IsActive = false;
TwistMotor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
connectionB.position.Sub( ref anchor, out baseAxis );
baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
}
}
HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix);
connectionB.position.Sub( ref anchor, out baseAxis );
baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis );
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate.</param>
/// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param>
public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
HingeLimit = new RevoluteLimit(connectionA, connectionB);
HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
HingeLimit.IsActive = false;
HingeMotor.IsActive = false;
TwistLimit.IsActive = false;
TwistMotor.IsActive = false;
//Ensure that the base and test direction is perpendicular to the free axis.
Vector3 baseAxis = anchor - connectionA.position;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
}
}
HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
//However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public UniversalJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
TwistJoint = new TwistJoint();
Limit = new TwistLimit();
Motor = new TwistMotor();
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate in world space.</param>
public UniversalJoint(Entity connectionA, Entity connectionB, Vector3 anchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
TwistJoint = new TwistJoint(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
Limit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
Motor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
Limit.IsActive = false;
Motor.IsActive = false;
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
/// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
/// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
/// </summary>
public SwivelHingeJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
AngularJoint = new SwivelHingeAngularJoint();
HingeLimit = new RevoluteLimit();
HingeMotor = new RevoluteMotor();
TwistLimit = new TwistLimit();
TwistMotor = new TwistMotor();
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="anchor">Point around which both entities rotate in world space.</param>
public UniversalJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor)
{
if (connectionA == null)
connectionA = TwoEntityConstraint.WorldEntity;
if (connectionB == null)
connectionB = TwoEntityConstraint.WorldEntity;
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor);
Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp );
TwistJoint = new TwistJoint(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp);
Limit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0);
Motor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp );
Limit.IsActive = false;
Motor.IsActive = false;
Add(BallSocketJoint);
Add(TwistJoint);
Add(Limit);
Add(Motor);
}
