void AddBackWheel(Vector3 wheelOffset, Entity body)
{
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 revoluteAngularJoint = new RevoluteAngularJoint(body, wheel, Vector3.Right);
//Add the wheel and connection to the space.
Space.Add(wheel);
Space.Add(pointOnLineJoint);
Space.Add(suspensionLimit);
Space.Add(suspensionSpring);
Space.Add(revoluteAngularJoint);
}
public TreadSegment(Vector3 segmentPosition, Entity body, TreadSegmentDescription treadSegmentDescription)
{
Entity = new Cylinder(segmentPosition, treadSegmentDescription.Width, treadSegmentDescription.Radius, treadSegmentDescription.Mass);
Entity.Material.KineticFriction = treadSegmentDescription.Friction;
Entity.Material.StaticFriction = treadSegmentDescription.Friction;
Entity.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
//Preventing the occasional pointless collision pair can speed things up.
CollisionRules.AddRule(Entity, body, CollisionRule.NoBroadPhase);
//Connect the wheel to the body.
SuspensionAxisJoint = new PointOnLineJoint(body, Entity, Entity.Position, Vector3.Down, Entity.Position);
SuspensionLengthLimit = new LinearAxisLimit(body, Entity, Entity.Position, Entity.Position, Vector3.Down, -treadSegmentDescription.SuspensionLength, 0);
//This linear axis motor will give the suspension its springiness by pushing the wheels outward.
SuspensionSpring = new LinearAxisMotor(body, Entity, Entity.Position, Entity.Position, Vector3.Down);
SuspensionSpring.Settings.Mode = MotorMode.Servomechanism;
SuspensionSpring.Settings.Servo.Goal = 0;
SuspensionSpring.Settings.Servo.SpringSettings.Stiffness = treadSegmentDescription.SuspensionStiffness;
SuspensionSpring.Settings.Servo.SpringSettings.Damping = treadSegmentDescription.SuspensionDamping;
SuspensionAngularJoint = new RevoluteAngularJoint(body, Entity, Vector3.Right);
//Make the joint extremely rigid. There are going to be extreme conditions when the wheels get up to speed;
//we don't want the forces involved to torque the wheel off the frame!
SuspensionAngularJoint.SpringSettings.Damping *= Entity.Mass * 50;
SuspensionAngularJoint.SpringSettings.Stiffness *= Entity.Mass * 50;
//Motorize the wheel.
Motor = new RevoluteMotor(body, Entity, Vector3.Left);
Motor.Settings.VelocityMotor.Softness = treadSegmentDescription.MotorSoftness;
Motor.Settings.MaximumForce = treadSegmentDescription.MotorMaximumForce;
}
public override SolverUpdateable GetBaseJoint()
{
PointOnLineJoint polj = new PointOnLineJoint(Ent1.Body, Ent2.Body, Ent2.GetPosition().ToBVector(), Direction.Normalize().ToBVector(), Ent2.GetPosition().ToBVector());
//polj.SpringSettings.Stiffness = DEF_STIFF * 400;
//polj.SpringSettings.Damping = DEF_DAMP * 400;
return(polj);
}
/// <summary>
/// Constructs a new constraint which restricts two 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 LineSliderJoint()
{
IsActive = false;
PointOnLineJoint = new PointOnLineJoint();
AngularJoint = new RevoluteAngularJoint();
Limit = new LinearAxisLimit();
Motor = new LinearAxisMotor();
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom.
/// 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 PrismaticJoint()
{
IsActive = false;
PointOnLineJoint = new PointOnLineJoint();
AngularJoint = new NoRotationJoint();
Limit = new LinearAxisLimit();
Motor = new LinearAxisMotor();
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
void jointDemo()
{
BepuEntity e1;
BepuEntity e2;
Joint joint;
// Ball & socket joint
e1 = createBox(new Vector3(20, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(20, 5, -10), 1, 1, 5);
joint = new BallSocketJoint(e1.body, e2.body, new Vector3(20, 5, -15));
space.Add(joint);
// Hinge
e1 = createBox(new Vector3(30, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(30, 5, -10), 1, 1, 5);
RevoluteJoint hinge = new RevoluteJoint(e1.body, e2.body, new Vector3(20, 5, -15), new Vector3(1, 0, 0));
space.Add(hinge);
// Universal
e1 = createBox(new Vector3(40, 5, -20), 1, 1, 5);
e2 = createBox(new Vector3(40, 5, -10), 1, 1, 5);
UniversalJoint uni = new UniversalJoint(e1.body, e2.body, new Vector3(40, 5, -15));
space.Add(uni);
// Weld Joint
e1 = createBox(new Vector3(50, 5, -20), 1, 1, 5);
e2 = createBox(new Vector3(50, 5, -10), 1, 1, 5);
WeldJoint weld = new WeldJoint(e1.body, e2.body);
space.Add(weld);
// PointOnLine Joint
// create the line
e1 = createBox(new Vector3(60, 5, -20), 1, 1, 5);
e1.body.BecomeKinematic();
e2 = createBox(new Vector3(60, 10, -10), 1, 1, 1);
PointOnLineJoint pol = new PointOnLineJoint(e1.body, e2.body, new Vector3(60, 5, -20), new Vector3(0, 0, -1), new Vector3(60, 5, -10));
space.Add(pol);
}
public void ConnectWheelBody(Entity backWheel, BepuEntity roverBody)
{
//code taken from bepu physics demo suspensioncardemo.cs and adapted for this project
PointOnLineJoint pointOnLineJoint = new PointOnLineJoint(roverBody.body, backWheel, backWheel.Position, Vector3.Down, backWheel.Position);
LinearAxisLimit suspensionLimit = new LinearAxisLimit(roverBody.body, backWheel, backWheel.Position, backWheel.Position, Vector3.Down, -1, 0);
CreateSuspensionString(roverBody, backWheel);
RevoluteAngularJoint revoluteAngularJoint = new RevoluteAngularJoint(roverBody.body, backWheel, Vector3.Right);
Game1.Instance.Space.Add(pointOnLineJoint);
Game1.Instance.Space.Add(suspensionLimit);
Game1.Instance.Space.Add(revoluteAngularJoint);
//my own code ,add joints to lists of joints for explosionn
joints.Add(pointOnLineJoint);
joints.Add(suspensionLimit);
joints.Add(revoluteAngularJoint);
}
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);
}
/// <summary>
/// Constructs a new constraint which restricts two 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="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
/// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move and rotate.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public LineSliderJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointAnchor)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, lineDirection);
Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
Limit.IsActive = false;
Motor.IsActive = false;
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
/// <summary>
/// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom.
/// </summary>
/// <param name="connectionA">First entity of the constraint pair.</param>
/// <param name="connectionB">Second entity of the constraint pair.</param>
/// <param name="lineAnchor">Location of the anchor for the line to be attached to connectionA in world space.</param>
/// <param name="lineDirection">Axis in world space to be attached to connectionA along which connectionB can move.</param>
/// <param name="pointAnchor">Location of the anchor for the point to be attached to connectionB in world space.</param>
public PrismaticJoint(Entity connectionA, Entity connectionB, System.Numerics.Vector3 lineAnchor, System.Numerics.Vector3 lineDirection, System.Numerics.Vector3 pointAnchor)
{
if (connectionA == null)
{
connectionA = TwoEntityConstraint.WorldEntity;
}
if (connectionB == null)
{
connectionB = TwoEntityConstraint.WorldEntity;
}
PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor);
AngularJoint = new NoRotationJoint(connectionA, connectionB);
Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0);
Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection);
Limit.IsActive = false;
Motor.IsActive = false;
Add(PointOnLineJoint);
Add(AngularJoint);
Add(Limit);
Add(Motor);
}
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);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public DogbotDemo(DemosGame game)
: base(game)
{
Entity body = new Box(new Vector3(0, 0, 0), 4, 2, 2, 20);
Space.Add(body);
Entity head = new Cone(body.Position + new Vector3(3.2f, .3f, 0), 1.5f, .7f, 4);
head.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2);
Space.Add(head);
//Attach the head to the body
var universalJoint = new UniversalJoint(body, head, head.Position + new Vector3(-.8f, 0, 0));
Space.Add(universalJoint);
//Keep the head from swinging around too much.
var angleLimit = new SwingLimit(body, head, Vector3.Right, Vector3.Right, MathHelper.PiOver4);
Space.Add(angleLimit);
var tail = new Box(body.Position + new Vector3(-3f, 1f, 0), 1.6f, .1f, .1f, 4);
Space.Add(tail);
//Keep the tail from twisting itself off.
universalJoint = new UniversalJoint(body, tail, tail.Position + new Vector3(.8f, 0, 0));
Space.Add(universalJoint);
//Give 'em some floppy ears.
var ear = new Box(head.Position + new Vector3(-.2f, 0, -.65f), .01f, .7f, .2f, 1);
Space.Add(ear);
var ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, -.65f));
Space.Add(ballSocketJoint);
ear = new Box(head.Position + new Vector3(-.2f, 0, .65f), .01f, .7f, .3f, 1);
Space.Add(ear);
ballSocketJoint = new BallSocketJoint(head, ear, head.Position + new Vector3(-.2f, .35f, .65f));
Space.Add(ballSocketJoint);
Box arm;
Cylinder shoulder;
PointOnLineJoint pointOnLineJoint;
//************* First Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
var axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Second Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, 1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, 1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//************* Third Arm *************//
arm = new Box(body.Position + new Vector3(-1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(-1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
shoulder.OrientationMatrix *= Matrix3x3.CreateFromAxisAngle(Vector3.Forward, MathHelper.Pi); //Force the walker's legs out of phase.
//************* Fourth Arm *************//
arm = new Box(body.Position + new Vector3(1.8f, -.5f, -1.5f), .5f, 3, .2f, 20);
Space.Add(arm);
shoulder = new Cylinder(body.Position + new Vector3(1.8f, .3f, -1.25f), .1f, .7f, 10);
shoulder.OrientationMatrix = Matrix3x3.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2);
Space.Add(shoulder);
//Connect the shoulder to the body.
axisJoint = new RevoluteJoint(body, shoulder, shoulder.Position, Vector3.Forward);
//Motorize the connection.
axisJoint.Motor.IsActive = true;
axisJoint.Motor.Settings.VelocityMotor.GoalVelocity = 1;
Space.Add(axisJoint);
//Connect the arm to the shoulder.
axisJoint = new RevoluteJoint(shoulder, arm, shoulder.Position + new Vector3(0, .6f, 0), Vector3.Forward);
Space.Add(axisJoint);
//Connect the arm to the body.
pointOnLineJoint = new PointOnLineJoint(arm, body, arm.Position, Vector3.Up, arm.Position + new Vector3(0, -.4f, 0));
Space.Add(pointOnLineJoint);
//Add some ground.
Space.Add(new Box(new Vector3(0, -3.5f, 0), 20f, 1, 20f));
game.Camera.Position = new Vector3(0, 2, 20);
}
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);
}
