/// <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); }