/// <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); }
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); }
public Ragdoll() { #region Ragdoll Entities //Create the ragdoll's bones. var pelvis = new Box(Vector3.Zero, .5m, .28m, .33m, 20); var torsoBottom = new Box(pelvis.Position + new Vector3(0, .3m, 0), .42m, .48m, .3m, 15); var torsoTop = new Box(torsoBottom.Position + new Vector3(0, .3m, 0), .5m, .38m, .32m, 20); var neck = new Box(torsoTop.Position + new Vector3(0, .2m, .04m), .19m, .24m, .2m, 5); var head = new Sphere(neck.Position + new Vector3(0, .22m, -.04m), .19m, 7); var leftUpperArm = new Box(torsoTop.Position + new Vector3(-.46m, .1m, 0), .52m, .19m, .19m, 6); var leftForearm = new Box(leftUpperArm.Position + new Vector3(-.5m, 0, 0), .52m, .18m, .18m, 5); var leftHand = new Box(leftForearm.Position + new Vector3(-.35m, 0, 0), .28m, .13m, .22m, 4); var rightUpperArm = new Box(torsoTop.Position + new Vector3(.46m, .1m, 0), .52m, .19m, .19m, 6); var rightForearm = new Box(rightUpperArm.Position + new Vector3(.5m, 0, 0), .52m, .18m, .18m, 5); var rightHand = new Box(rightForearm.Position + new Vector3(.35m, 0, 0), .28m, .13m, .22m, 4); var leftThigh = new Box(pelvis.Position + new Vector3(-.15m, -.4m, 0), .23m, .63m, .23m, 10); var leftShin = new Box(leftThigh.Position + new Vector3(0, -.6m, 0), .21m, .63m, .21m, 7); var leftFoot = new Box(leftShin.Position + new Vector3(0, -.35m, -.1m), .23m, .15m, .43m, 5); var rightThigh = new Box(pelvis.Position + new Vector3(.15m, -.4m, 0), .23m, .63m, .23m, 10); var rightShin = new Box(rightThigh.Position + new Vector3(0, -.6m, 0), .21m, .63m, .21m, 7); var rightFoot = new Box(rightShin.Position + new Vector3(0, -.35m, -.1m), .23m, .15m, .43m, 5); #endregion #region Bone List //Make a convenient list of all of the bones. bones.Add(pelvis); bones.Add(torsoBottom); bones.Add(torsoTop); bones.Add(neck); bones.Add(head); bones.Add(leftUpperArm); bones.Add(leftForearm); bones.Add(leftHand); bones.Add(rightUpperArm); bones.Add(rightForearm); bones.Add(rightHand); bones.Add(leftThigh); bones.Add(leftShin); bones.Add(leftFoot); bones.Add(rightThigh); bones.Add(rightShin); bones.Add(rightFoot); #endregion #region Collision Rules //Prevent adjacent limbs from colliding. CollisionRules.AddRule(pelvis, torsoBottom, CollisionRule.NoBroadPhase); CollisionRules.AddRule(torsoBottom, torsoTop, CollisionRule.NoBroadPhase); CollisionRules.AddRule(torsoTop, neck, CollisionRule.NoBroadPhase); CollisionRules.AddRule(neck, head, CollisionRule.NoBroadPhase); CollisionRules.AddRule(head, torsoTop, CollisionRule.NoBroadPhase); CollisionRules.AddRule(torsoTop, leftUpperArm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(leftUpperArm, leftForearm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(leftForearm, leftHand, CollisionRule.NoBroadPhase); CollisionRules.AddRule(torsoTop, rightUpperArm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(rightUpperArm, rightForearm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(rightForearm, rightHand, CollisionRule.NoBroadPhase); CollisionRules.AddRule(pelvis, leftThigh, CollisionRule.NoBroadPhase); CollisionRules.AddRule(leftThigh, leftShin, CollisionRule.NoBroadPhase); CollisionRules.AddRule(leftThigh, torsoBottom, CollisionRule.NoBroadPhase); CollisionRules.AddRule(leftShin, leftFoot, CollisionRule.NoBroadPhase); CollisionRules.AddRule(pelvis, rightThigh, CollisionRule.NoBroadPhase); CollisionRules.AddRule(rightThigh, rightShin, CollisionRule.NoBroadPhase); CollisionRules.AddRule(rightThigh, torsoBottom, CollisionRule.NoBroadPhase); CollisionRules.AddRule(rightShin, rightFoot, CollisionRule.NoBroadPhase); #endregion //Create the constraints between the bones. #region Pelvis up to Head Constraints var pelvisToTorsoBottomBallSocketJoint = new BallSocketJoint(pelvis, torsoBottom, pelvis.Position + new Vector3(0, .1m, 0)); var pelvisToTorsoBottomTwistLimit = new TwistLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6); var pelvisToTorsoBottomSwingLimit = new SwingLimit(pelvis, torsoBottom, Vector3.Up, Vector3.Up, MathHelper.Pi / 6); var pelvisToTorsoBottomMotor = new AngularMotor(pelvis, torsoBottom); pelvisToTorsoBottomMotor.Settings.VelocityMotor.Softness = .05m; var torsoBottomToTorsoTopBallSocketJoint = new BallSocketJoint(torsoBottom, torsoTop, torsoBottom.Position + new Vector3(0, .25m, 0)); var torsoBottomToTorsoTopSwingLimit = new SwingLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, MathHelper.Pi / 6); var torsoBottomToTorsoTopTwistLimit = new TwistLimit(torsoBottom, torsoTop, Vector3.Up, Vector3.Up, -MathHelper.Pi / 6, MathHelper.Pi / 6); var torsoBottomToTorsoTopMotor = new AngularMotor(torsoBottom, torsoTop); torsoBottomToTorsoTopMotor.Settings.VelocityMotor.Softness = .05m; var torsoTopToNeckBallSocketJoint = new BallSocketJoint(torsoTop, neck, torsoTop.Position + new Vector3(0, .15m, .05m)); var torsoTopToNeckSwingLimit = new SwingLimit(torsoTop, neck, Vector3.Up, Vector3.Up, MathHelper.Pi / 6); var torsoTopToNeckTwistLimit = new TwistLimit(torsoTop, neck, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8); var torsoTopToNeckMotor = new AngularMotor(torsoTop, neck); torsoTopToNeckMotor.Settings.VelocityMotor.Softness = .1m; var neckToHeadBallSocketJoint = new BallSocketJoint(neck, head, neck.Position + new Vector3(0, .1m, .05m)); var neckToHeadTwistLimit = new TwistLimit(neck, head, Vector3.Up, Vector3.Up, -MathHelper.Pi / 8, MathHelper.Pi / 8); var neckToHeadSwingLimit = new SwingLimit(neck, head, Vector3.Up, Vector3.Up, MathHelper.Pi / 6); var neckToHeadMotor = new AngularMotor(neck, head); neckToHeadMotor.Settings.VelocityMotor.Softness = .1m; #endregion #region Left Arm var torsoTopToLeftArmBallSocketJoint = new BallSocketJoint(torsoTop, leftUpperArm, torsoTop.Position + new Vector3(-.3m, .1m, 0)); var torsoTopToLeftArmEllipseLimit = new EllipseSwingLimit(torsoTop, leftUpperArm, Vector3.Left, MathHelper.Pi * .75m, MathHelper.PiOver2); var torsoTopToLeftArmTwistLimit = new TwistLimit(torsoTop, leftUpperArm, Vector3.Left, Vector3.Left, -MathHelper.PiOver2, MathHelper.PiOver2); var torsoTopToLeftArmMotor = new AngularMotor(torsoTop, leftUpperArm); torsoTopToLeftArmMotor.Settings.VelocityMotor.Softness = .2m; var leftUpperArmToLeftForearmSwivelHingeJoint = new SwivelHingeJoint(leftUpperArm, leftForearm, leftUpperArm.Position + new Vector3(-.28m, 0, 0), Vector3.Up); leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.IsActive = true; leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.IsActive = true; leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8; leftUpperArmToLeftForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8; leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MinimumAngle = -MathHelper.Pi * .8m; leftUpperArmToLeftForearmSwivelHingeJoint.HingeLimit.MaximumAngle = 0; //The SwivelHingeJoint has motors, but they are separately defined for twist/bending. //The AngularMotor covers all degrees of freedom. var leftUpperArmToLeftForearmMotor = new AngularMotor(leftUpperArm, leftForearm); leftUpperArmToLeftForearmMotor.Settings.VelocityMotor.Softness = .3m; var leftForearmToLeftHandBallSocketJoint = new BallSocketJoint(leftForearm, leftHand, leftForearm.Position + new Vector3(-.2m, 0, 0)); var leftForearmToLeftHandEllipseSwingLimit = new EllipseSwingLimit(leftForearm, leftHand, Vector3.Left, MathHelper.PiOver2, MathHelper.Pi / 6); var leftForearmToLeftHandTwistLimit = new TwistLimit(leftForearm, leftHand, Vector3.Left, Vector3.Left, -MathHelper.Pi / 6, MathHelper.Pi / 6); var leftForearmToLeftHandMotor = new AngularMotor(leftForearm, leftHand); leftForearmToLeftHandMotor.Settings.VelocityMotor.Softness = .4m; #endregion #region Right Arm var torsoTopToRightArmBallSocketJoint = new BallSocketJoint(torsoTop, rightUpperArm, torsoTop.Position + new Vector3(.3m, .1m, 0)); var torsoTopToRightArmEllipseLimit = new EllipseSwingLimit(torsoTop, rightUpperArm, Vector3.Right, MathHelper.Pi * .75m, MathHelper.PiOver2); var torsoTopToRightArmTwistLimit = new TwistLimit(torsoTop, rightUpperArm, Vector3.Right, Vector3.Right, -MathHelper.PiOver2, MathHelper.PiOver2); var torsoTopToRightArmMotor = new AngularMotor(torsoTop, rightUpperArm); torsoTopToRightArmMotor.Settings.VelocityMotor.Softness = .2m; var rightUpperArmToRightForearmSwivelHingeJoint = new SwivelHingeJoint(rightUpperArm, rightForearm, rightUpperArm.Position + new Vector3(.28m, 0, 0), Vector3.Up); rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.IsActive = true; rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.IsActive = true; rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MinimumAngle = -MathHelper.Pi / 8; rightUpperArmToRightForearmSwivelHingeJoint.TwistLimit.MaximumAngle = MathHelper.Pi / 8; rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MinimumAngle = 0; rightUpperArmToRightForearmSwivelHingeJoint.HingeLimit.MaximumAngle = MathHelper.Pi * .8m; //The SwivelHingeJoint has motors, but they are separately defined for twist/bending. //The AngularMotor covers all degrees of freedom. var rightUpperArmToRightForearmMotor = new AngularMotor(rightUpperArm, rightForearm); rightUpperArmToRightForearmMotor.Settings.VelocityMotor.Softness = .3m; var rightForearmToRightHandBallSocketJoint = new BallSocketJoint(rightForearm, rightHand, rightForearm.Position + new Vector3(.2m, 0, 0)); var rightForearmToRightHandEllipseSwingLimit = new EllipseSwingLimit(rightForearm, rightHand, Vector3.Right, MathHelper.PiOver2, MathHelper.Pi / 6); var rightForearmToRightHandTwistLimit = new TwistLimit(rightForearm, rightHand, Vector3.Right, Vector3.Right, -MathHelper.Pi / 6, MathHelper.Pi / 6); var rightForearmToRightHandMotor = new AngularMotor(rightForearm, rightHand); rightForearmToRightHandMotor.Settings.VelocityMotor.Softness = .4m; #endregion #region Left Leg var pelvisToLeftThighBallSocketJoint = new BallSocketJoint(pelvis, leftThigh, pelvis.Position + new Vector3(-.15m, -.1m, 0)); var pelvisToLeftThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, leftThigh, Vector3.Normalize(new Vector3(-.2m, -1, -.6m)), MathHelper.Pi * .7m, MathHelper.PiOver4); pelvisToLeftThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down; var pelvisToLeftThighTwistLimit = new TwistLimit(pelvis, leftThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6); var pelvisToLeftThighMotor = new AngularMotor(pelvis, leftThigh); pelvisToLeftThighMotor.Settings.VelocityMotor.Softness = .1m; var leftThighToLeftShinRevoluteJoint = new RevoluteJoint(leftThigh, leftShin, leftThigh.Position + new Vector3(0, -.3m, 0), Vector3.Right); leftThighToLeftShinRevoluteJoint.Limit.IsActive = true; leftThighToLeftShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8m; leftThighToLeftShinRevoluteJoint.Limit.MaximumAngle = 0; leftThighToLeftShinRevoluteJoint.Motor.IsActive = true; leftThighToLeftShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2m; var leftShinToLeftFootBallSocketJoint = new BallSocketJoint(leftShin, leftFoot, leftShin.Position + new Vector3(0, -.3m, 0)); var leftShinToLeftFootSwingLimit = new SwingLimit(leftShin, leftFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8); var leftShinToLeftFootTwistLimit = new TwistLimit(leftShin, leftFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8); var leftShinToLeftFootMotor = new AngularMotor(leftShin, leftFoot); leftShinToLeftFootMotor.Settings.VelocityMotor.Softness = .2m; #endregion #region Right Leg var pelvisToRightThighBallSocketJoint = new BallSocketJoint(pelvis, rightThigh, pelvis.Position + new Vector3(.15m, -.1m, 0)); var pelvisToRightThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, rightThigh, Vector3.Normalize(new Vector3(.2m, -1, -.6m)), MathHelper.Pi * .7m, MathHelper.PiOver4); pelvisToRightThighEllipseSwingLimit.LocalTwistAxisB = Vector3.Down; var pelvisToRightThighTwistLimit = new TwistLimit(pelvis, rightThigh, Vector3.Down, Vector3.Down, -MathHelper.Pi / 6, MathHelper.Pi / 6); var pelvisToRightThighMotor = new AngularMotor(pelvis, rightThigh); pelvisToRightThighMotor.Settings.VelocityMotor.Softness = .1m; var rightThighToRightShinRevoluteJoint = new RevoluteJoint(rightThigh, rightShin, rightThigh.Position + new Vector3(0, -.3m, 0), Vector3.Right); rightThighToRightShinRevoluteJoint.Limit.IsActive = true; rightThighToRightShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8m; rightThighToRightShinRevoluteJoint.Limit.MaximumAngle = 0; rightThighToRightShinRevoluteJoint.Motor.IsActive = true; rightThighToRightShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2m; var rightShinToRightFootBallSocketJoint = new BallSocketJoint(rightShin, rightFoot, rightShin.Position + new Vector3(0, -.3m, 0)); var rightShinToRightFootSwingLimit = new SwingLimit(rightShin, rightFoot, Vector3.Forward, Vector3.Forward, MathHelper.Pi / 8); var rightShinToRightFootTwistLimit = new TwistLimit(rightShin, rightFoot, Vector3.Down, Vector3.Forward, -MathHelper.Pi / 8, MathHelper.Pi / 8); var rightShinToRightFootMotor = new AngularMotor(rightShin, rightFoot); rightShinToRightFootMotor.Settings.VelocityMotor.Softness = .2m; #endregion #region Joint List //Collect the joints. joints.Add(pelvisToTorsoBottomBallSocketJoint); joints.Add(pelvisToTorsoBottomTwistLimit); joints.Add(pelvisToTorsoBottomSwingLimit); joints.Add(pelvisToTorsoBottomMotor); joints.Add(torsoBottomToTorsoTopBallSocketJoint); joints.Add(torsoBottomToTorsoTopTwistLimit); joints.Add(torsoBottomToTorsoTopSwingLimit); joints.Add(torsoBottomToTorsoTopMotor); joints.Add(torsoTopToNeckBallSocketJoint); joints.Add(torsoTopToNeckTwistLimit); joints.Add(torsoTopToNeckSwingLimit); joints.Add(torsoTopToNeckMotor); joints.Add(neckToHeadBallSocketJoint); joints.Add(neckToHeadTwistLimit); joints.Add(neckToHeadSwingLimit); joints.Add(neckToHeadMotor); joints.Add(torsoTopToLeftArmBallSocketJoint); joints.Add(torsoTopToLeftArmEllipseLimit); joints.Add(torsoTopToLeftArmTwistLimit); joints.Add(torsoTopToLeftArmMotor); joints.Add(leftUpperArmToLeftForearmSwivelHingeJoint); joints.Add(leftUpperArmToLeftForearmMotor); joints.Add(leftForearmToLeftHandBallSocketJoint); joints.Add(leftForearmToLeftHandEllipseSwingLimit); joints.Add(leftForearmToLeftHandTwistLimit); joints.Add(leftForearmToLeftHandMotor); joints.Add(torsoTopToRightArmBallSocketJoint); joints.Add(torsoTopToRightArmEllipseLimit); joints.Add(torsoTopToRightArmTwistLimit); joints.Add(torsoTopToRightArmMotor); joints.Add(rightUpperArmToRightForearmSwivelHingeJoint); joints.Add(rightUpperArmToRightForearmMotor); joints.Add(rightForearmToRightHandBallSocketJoint); joints.Add(rightForearmToRightHandEllipseSwingLimit); joints.Add(rightForearmToRightHandTwistLimit); joints.Add(rightForearmToRightHandMotor); joints.Add(pelvisToLeftThighBallSocketJoint); joints.Add(pelvisToLeftThighEllipseSwingLimit); joints.Add(pelvisToLeftThighTwistLimit); joints.Add(pelvisToLeftThighMotor); joints.Add(leftThighToLeftShinRevoluteJoint); joints.Add(leftShinToLeftFootBallSocketJoint); joints.Add(leftShinToLeftFootSwingLimit); joints.Add(leftShinToLeftFootTwistLimit); joints.Add(leftShinToLeftFootMotor); joints.Add(pelvisToRightThighBallSocketJoint); joints.Add(pelvisToRightThighEllipseSwingLimit); joints.Add(pelvisToRightThighTwistLimit); joints.Add(pelvisToRightThighMotor); joints.Add(rightThighToRightShinRevoluteJoint); joints.Add(rightShinToRightFootBallSocketJoint); joints.Add(rightShinToRightFootSwingLimit); joints.Add(rightShinToRightFootTwistLimit); joints.Add(rightShinToRightFootMotor); #endregion }
/// <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); }
public unsafe override void Initialize(ContentArchive content, Camera camera) { camera.Position = new Vector3(-20, 10, -20); //camera.Yaw = MathHelper.Pi ; camera.Yaw = MathHelper.Pi * 3f / 4; //camera.Pitch = MathHelper.PiOver2 * 0.999f; Simulation = Simulation.Create(BufferPool, new TestCallbacks()); Simulation.PoseIntegrator.Gravity = new Vector3(0, -10, 0); Simulation.Deterministic = false; var a = AddBody(new Box(3, 1, 1), 1, new RigidPose { Position = new Vector3(0, 10, 0), Orientation = BepuUtilities.Quaternion.Identity }, Simulation); var b = AddBody(new Box(3, 1, 1), 0, new RigidPose { Position = new Vector3(5, 10, 0), Orientation = BepuUtilities.Quaternion.Identity }, Simulation); a.Velocity.Angular = new Vector3(1f, 5f, 1f); //a.Pose.Orientation = BepuUtilities.Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, 1, 1)), MathHelper.PiOver4); var springSettings = new SpringSettings(15, 1); var ballSocket = new BallSocket { LocalOffsetA = new Vector3(2.5f, 0, 0), LocalOffsetB = new Vector3(-2.5f, 0, 0), SpringSettings = springSettings }; Simulation.Solver.Add(a.Handle, b.Handle, ref ballSocket); //springSettings = new BepuPhysics.CollisionDetection.SpringSettings { DampingRatio = 0f, NaturalFrequency = MathHelper.Pi * 1 }; var angularHinge = new AngularHinge { HingeAxisLocalA = Vector3.Normalize(new Vector3(0, 1, 0)), HingeAxisLocalB = Vector3.Normalize(new Vector3(0, 1, 0)), SpringSettings = springSettings }; Simulation.Solver.Add(a.Handle, b.Handle, ref angularHinge); //var swivelHinge = new AngularSwivelHinge //{ // SwivelAxisLocalA = new Vector3(1, 0, 0), // HingeAxisLocalB = new Vector3(0, 1, 0), // SpringSettings = springSettings //}; //Simulation.Solver.Add(a.Handle, b.Handle, ref swivelHinge); var swingLimit = new SwingLimit { AxisLocalA = new Vector3(1, 0, 0), AxisLocalB = new Vector3(1, 0, 0), MinimumDot = -0.5f, SpringSettings = new SpringSettings(15, 1) }; Simulation.Solver.Add(a.Handle, b.Handle, ref swingLimit); var staticShape = new Box(100, 1, 100); var staticShapeIndex = Simulation.Shapes.Add(staticShape); var staticDescription = new StaticDescription { Collidable = new CollidableDescription { Continuity = new ContinuousDetectionSettings { Mode = ContinuousDetectionMode.Discrete }, Shape = staticShapeIndex, SpeculativeMargin = 0.1f }, Pose = new RigidPose { Position = new Vector3(1, -0.5f, 1), Orientation = BepuUtilities.Quaternion.Identity } }; Simulation.Statics.Add(staticDescription); }