/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees 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="freeAxis">Axis around which the hinge can rotate.</param>
public RevoluteJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 freeAxis)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis);
Limit = new RevoluteLimit(connectionA, connectionB);
Motor = new RevoluteMotor(connectionA, connectionB, freeAxis);
Limit.IsActive = false;
Motor.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, freeAxis) * freeAxis;
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(freeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(freeAxis, Vector3.Right);
}
}
Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix);
Motor.Basis.SetWorldAxes(freeAxis, baseAxis, Vector3.Cross(freeAxis, baseAxis), connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3.Dot(baseAxis, freeAxis) * freeAxis;
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(freeAxis, Vector3.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = Vector3.Cross(freeAxis, Vector3.Right);
}
}
Limit.TestAxis = baseAxis;
Motor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts three degrees of linear freedom and two degrees 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 RevoluteJoint()
{
IsActive = false;
BallSocketJoint = new BallSocketJoint();
AngularJoint = new RevoluteAngularJoint();
Limit = new RevoluteLimit();
Motor = new RevoluteMotor();
Add(BallSocketJoint);
Add(AngularJoint);
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);
}
public void SetUpFrontMotorWheels(BepuEntity roverBody, Entity wheel1, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor)
{
//code taken from bepu physics demo suspensioncardemo.cs and adapted for this project
PointOnLineJoint pointOnLineJoint = new PointOnLineJoint(roverBody.body, wheel1, wheel1.Position, Vector3.Down, wheel1.Position);
LinearAxisLimit suspensionLimit = new LinearAxisLimit(roverBody.body, wheel1, wheel1.Position, wheel1.Position, Vector3.Down, -1, 0);
CreateSuspensionString(roverBody, wheel1);
SwivelHingeAngularJoint swivelHingeAngularJoint = new SwivelHingeAngularJoint(roverBody.body, wheel1, Vector3.Up, Vector3.Right);
swivelHingeAngularJoint.SpringSettings.DampingConstant *= 1000;
swivelHingeAngularJoint.SpringSettings.StiffnessConstant *= 1000;
drivingMotor = new RevoluteMotor(roverBody.body, wheel1, -Vector3.Left);
drivingMotor.Settings.VelocityMotor.Softness = .3f;
drivingMotor.Settings.MaximumForce = 100;
drivingMotor.IsActive = false;
steeringMotor = new RevoluteMotor(roverBody.body, wheel1, Vector3.Up);
steeringMotor.Settings.Mode = MotorMode.Servomechanism;
steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
steeringMotor.TestAxis = Vector3.Right;
steeringMotor.Settings.Servo.SpringSettings.Advanced.UseAdvancedSettings = true;
steeringMotor.Settings.Servo.SpringSettings.Advanced.Softness = 0;
steeringMotor.Settings.Servo.SpringSettings.Advanced.ErrorReductionFactor = 0f;
var steeringConstraint = new RevoluteLimit(roverBody.body, wheel1, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);
Game1.Instance.Space.Add(pointOnLineJoint);
Game1.Instance.Space.Add(suspensionLimit);
Game1.Instance.Space.Add(swivelHingeAngularJoint);
Game1.Instance.Space.Add(drivingMotor);
Game1.Instance.Space.Add(steeringMotor);
Game1.Instance.Space.Add(steeringConstraint);
//my own code ,add joints to lists of joints for explosion
joints.Add(pointOnLineJoint);
joints.Add(suspensionLimit);
joints.Add(swivelHingeAngularJoint);
motors.Add(drivingMotor);
motors.Add(steeringMotor);
}
void AddDriveWheel(Vector3 suspensionOffset, Entity body, bool leftSide, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor, out Box suspensionLeg)
{
suspensionLeg = new Box(body.Position + suspensionOffset, 0.25f, 0.8f, 0.25f, 10);
const float horizontalWheelOffset = 0.2f;
var wheel = new Cylinder(suspensionLeg.Position + new Vector3(leftSide ? -horizontalWheelOffset : horizontalWheelOffset, -suspensionLeg.HalfHeight, 0), .2f, .3f, 5f);
wheel.Material.KineticFriction = 2.5f;
wheel.Material.StaticFriction = 3.5f;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(wheel, suspensionLeg, CollisionRule.NoBroadPhase);
CollisionRules.AddRule(suspensionLeg, body, CollisionRule.NoBroadPhase);
//Connect the suspension to the body.
var bodyToSuspension = new LineSliderJoint(body, suspensionLeg, suspensionLeg.Position, Vector3.Down, suspensionLeg.Position);
bodyToSuspension.Limit.IsActive = true;
bodyToSuspension.Limit.Minimum = -0.5f;
bodyToSuspension.Limit.Maximum = 0;
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
bodyToSuspension.Motor.IsActive = true;
bodyToSuspension.Motor.Settings.Mode = MotorMode.Servomechanism;
bodyToSuspension.Motor.Settings.Servo.Goal = 0;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Stiffness = 300;
bodyToSuspension.Motor.Settings.Servo.SpringSettings.Damping = 70;
steeringMotor = new RevoluteMotor(body, suspensionLeg, Vector3.Up);
steeringMotor.Settings.Mode = MotorMode.Servomechanism;
//The constructor makes a guess about how to set up the constraint.
//It can't always be right since it doesn't have all the information;
//in this case, it chooses the basis and test axis incorrectly.
//This leads to a 'flipping' behavior when the wheel is rolling
//(the test axis is 'rolling' with the wheel, and passes over
//a singularity which causes a flip).
//To fix this, we configure the constraint directly.
//The basis is aligned with how the wheel is set up; we choose 'up' as
//the motorized axis, and right/forward to define the angle measurement plane.
//The test axis is set to be perpendicular to the wheel's rotation so that
//it only measures the steering angle.
//If you're curious, the angle measurement is just a Math.Atan2.
//The current world test axis is dotted against the two plane axes (Right and Forward here).
//This gives an x and y value. These can be plugged into Atan2 just like when
//you compute an angle on a normal 2d graph.
steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
steeringMotor.TestAxis = Vector3.Right;
//To make the steering a little more responsive, set a base speed at which error gets corrected.
//This works on top of the default error reduction implied by the constraint's spring constants.
steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 1;
//The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
//Laying a revolute limit on top of it can help mitigate the problem.
var steeringConstraint = new RevoluteLimit(body, suspensionLeg, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);
//Connect the wheel to the suspension.
var suspensionToWheel = new RevoluteJoint(suspensionLeg, wheel, wheel.Position, Vector3.Right);
drivingMotor = suspensionToWheel.Motor;
//The driving motor's default, created by the RevoluteJoint constructor above, chose the axis of rotation such that negatives values made the car go forward and vice versa.
//Swap it around so that the positive values make the car roll forward instead!
drivingMotor.Basis.SetWorldAxes(Vector3.Left, Vector3.Forward);
drivingMotor.TestAxis = Vector3.Forward;
drivingMotor.Settings.VelocityMotor.Softness = .3f;
drivingMotor.Settings.MaximumForce = 100;
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(suspensionLeg);
Space.Add(bodyToSuspension);
Space.Add(drivingMotor);
Space.Add(steeringMotor);
Space.Add(steeringConstraint);
Space.Add(suspensionToWheel);
}
public JointLimitTestDemo(DemosGame game)
: base(game)
{
float bounciness = 1;
float baseMass = 100;
float armMass = 10;
//DistanceLimit
Box boxA = new Box(new Vector3(-21, 4, 0), 3, 3, 3, baseMass);
Box boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var distanceLimit = new DistanceLimit(boxA, boxB, boxA.Position, boxB.Position - new Vector3(0, 2, 0), 1, 6);
distanceLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(distanceLimit);
//EllipseSwingLimit
boxA = new Box(new Vector3(-14, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var ellipseSwingLimit = new EllipseSwingLimit(boxA, boxB, Vector3.Up, MathHelper.Pi / 1.5f, MathHelper.Pi / 3);
ellipseSwingLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(ellipseSwingLimit);
//LinearAxisLimit
boxA = new Box(new Vector3(-7, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
var pointOnLineJoint = new PointOnLineJoint(boxA, boxB, boxA.Position, Vector3.Up, boxB.Position + new Vector3(0, -2, 0));
var linearAxisLimit = new LinearAxisLimit(boxA, boxB, boxA.Position, boxB.Position + new Vector3(0, -2, 0), Vector3.Up, 0, 4);
linearAxisLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(pointOnLineJoint);
Space.Add(linearAxisLimit);
//RevoluteLimit
boxA = new Box(new Vector3(0, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var revoluteAngularJoint = new RevoluteAngularJoint(boxA, boxB, Vector3.Forward);
var revoluteLimit = new RevoluteLimit(boxA, boxB, Vector3.Forward, Vector3.Up, -MathHelper.PiOver4, MathHelper.PiOver4);
revoluteLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(revoluteAngularJoint);
Space.Add(revoluteLimit);
//SwingLimit
boxA = new Box(new Vector3(7, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
var swingLimit = new SwingLimit(boxA, boxB, Vector3.Up, Vector3.Up, MathHelper.PiOver4);
swingLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(swingLimit);
//TwistLimit
boxA = new Box(new Vector3(14, 4, 0), 3, 3, 3, baseMass);
boxB = new Box(boxA.Position + new Vector3(0, 5, 0), 1, 4, 1, armMass);
CollisionRules.AddRule(boxA, boxB, CollisionRule.NoBroadPhase);
boxB.ActivityInformation.IsAlwaysActive = true;
ballSocketJoint = new BallSocketJoint(boxA, boxB, boxB.Position + new Vector3(0, -2, 0));
revoluteAngularJoint = new RevoluteAngularJoint(boxA, boxB, Vector3.Up);
var twistLimit = new TwistLimit(boxA, boxB, Vector3.Up, Vector3.Up, -MathHelper.PiOver4, MathHelper.PiOver4);
twistLimit.Bounciness = bounciness;
Space.Add(boxA);
Space.Add(boxB);
Space.Add(ballSocketJoint);
Space.Add(revoluteAngularJoint);
Space.Add(twistLimit);
Space.Add(new Box(new Vector3(0, 0, 0), 60, 1, 60));
game.Camera.Position = new Vector3(0, 6, 15);
}
Entity AddDriveWheel(Vector3 wheelOffset, Entity body, out RevoluteMotor drivingMotor, out RevoluteMotor steeringMotor)
{
var wheel = new Cylinder(body.Position + wheelOffset, .4m, .5m, 5);
wheel.Material.KineticFriction = 2.5m;
wheel.Material.StaticFriction = 3.5m;
wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(wheel, body, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
var pointOnLineJoint = new PointOnLineJoint(body, wheel, wheel.Position, Vector3.Down, wheel.Position);
var suspensionLimit = new LinearAxisLimit(body, wheel, wheel.Position, wheel.Position, Vector3.Down, -1, 0);
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
var suspensionSpring = new LinearAxisMotor(body, wheel, wheel.Position, wheel.Position, Vector3.Down);
suspensionSpring.Settings.Mode = MotorMode.Servomechanism;
suspensionSpring.Settings.Servo.Goal = 0;
suspensionSpring.Settings.Servo.SpringSettings.Stiffness = 300;
suspensionSpring.Settings.Servo.SpringSettings.Damping = 70;
var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right);
//Motorize the wheel.
drivingMotor = new RevoluteMotor(body, wheel, Vector3.Left);
drivingMotor.Settings.VelocityMotor.Softness = .3m;
drivingMotor.Settings.MaximumForce = 100;
//Let it roll when the user isn't giving specific commands.
drivingMotor.IsActive = false;
steeringMotor = new RevoluteMotor(body, wheel, Vector3.Up);
steeringMotor.Settings.Mode = MotorMode.Servomechanism;
//The constructor makes a guess about how to set up the constraint.
//It can't always be right since it doesn't have all the information;
//in this case, it chooses the basis and test axis incorrectly.
//This leads to a 'flipping' behavior when the wheel is rolling
//(the test axis is 'rolling' with the wheel, and passes over
//a singularity which causes a flip).
//To fix this, we configure the constraint directly.
//The basis is aligned with how the wheel is set up; we choose 'up' as
//the motorized axis, and right/forward to define the angle measurement plane.
//The test axis is set to be perpendicular to the wheel's rotation so that
//it only measures the steering angle.
//If you're curious, the angle measurement is just a Math.Atan2.
//The current world test axis is dotted against the two plane axes (Right and Forward here).
//This gives an x and y value. These can be plugged into Atan2 just like when
//you compute an angle on a normal 2d graph.
steeringMotor.Basis.SetWorldAxes(Vector3.Up, Vector3.Right);
steeringMotor.TestAxis = Vector3.Right;
steeringMotor.Settings.Servo.BaseCorrectiveSpeed = 5;
//The revolute motor is weaker than some other types of constraints and maintaining a goal in the presence of extremely fast rotation and integration issues.
//Laying a revolute limit on top of it can help mitigate the problem.
var steeringConstraint = new RevoluteLimit(body, wheel, Vector3.Up, Vector3.Right, -maximumTurnAngle, maximumTurnAngle);
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(pointOnLineJoint);
Space.Add(suspensionLimit);
Space.Add(suspensionSpring);
Space.Add(swivelHingeAngularJoint);
Space.Add(drivingMotor);
Space.Add(steeringMotor);
Space.Add(steeringConstraint);
return(wheel);
}
/// <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, System.Numerics.Vector3 anchor, System.Numerics.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.
System.Numerics.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 -= Vector3Ex.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 = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Right);
}
}
HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
baseAxis = connectionB.position - anchor;
baseAxis -= Vector3Ex.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 = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Up);
if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
{
baseAxis = System.Numerics.Vector3.Cross(hingeAxis, Vector3Ex.Right);
}
}
HingeLimit.TestAxis = baseAxis;
HingeMotor.TestAxis = baseAxis;
Add(BallSocketJoint);
Add(AngularJoint);
Add(HingeLimit);
Add(HingeMotor);
Add(TwistLimit);
Add(TwistMotor);
}
