/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public MoreConstraintsTestDemo(DemosGame game) : base(game) { Box boxA = new Box(new Vector3(0, 5, 0), 1, 2, 1, 10); Box boxB = new Box(new Vector3(0, 8, 0), 1, 2, 1, 10); boxA.Orientation = Quaternion.CreateFromYawPitchRoll(0, MathHelper.PiOver4, 0); boxB.Orientation = Quaternion.CreateFromYawPitchRoll(MathHelper.PiOver4, 0, 0); WeldJoint weld = new WeldJoint(boxA, boxB); Space.Add(boxA); Space.Add(boxB); Space.Add(weld); boxA = new Box(new Vector3(3, 5, 0), 1, 2, 1, 10); boxB = new Box(new Vector3(3, 8, 0), 1, 2, 1, 10); boxA.Orientation = Quaternion.CreateFromYawPitchRoll(0, MathHelper.PiOver4, 0); boxB.Orientation = Quaternion.CreateFromYawPitchRoll(MathHelper.PiOver4, 0, 0); BallSocketJoint ballSocket = new BallSocketJoint(boxA, boxB, (boxA.Position + boxB.Position) / 2); AngularMotor angularMotor = new AngularMotor(boxA, boxB); angularMotor.Settings.Mode = MotorMode.Servomechanism; Space.Add(boxA); Space.Add(boxB); Space.Add(ballSocket); Space.Add(angularMotor); Box ground = new Box(new Vector3(0, 0, 0), 10, 1, 10); Space.Add(ground); game.Camera.Position = new Vector3(0, 6, 15); }
/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public TwistTestDemo(DemosGame game) : base(game) { var a = new Box(new Vector3(-2, 2, 0), 1, 2, 2, 5); var b = new Box(new Vector3(2, 2, 0), 1, 2, 2, 5); b.Orientation = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), MathHelper.PiOver4); Space.Add(a); Space.Add(b); var twistJoint = new TwistJoint(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right); var twistMotor = new TwistMotor(a, b, a.OrientationMatrix.Right, b.OrientationMatrix.Right); twistMotor.Settings.Mode = MotorMode.Servomechanism; //Space.Add(twistJoint); Space.Add(twistMotor); var ballSocketJoint = new BallSocketJoint(a, b, (a.Position + b.Position) * 0.5f); var swingLimit = new SwingLimit(a, b, a.OrientationMatrix.Right, a.OrientationMatrix.Right, MathHelper.PiOver2); Space.Add(ballSocketJoint); Space.Add(swingLimit); Box ground = new Box(new Vector3(0, -.5f, 0), 50, 1, 50); Space.Add(ground); game.Camera.Position = new Vector3(0, 6, 15); }
/// <summary> /// Constructs a new constraint which restricts the linear and angular motion 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 WeldJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); NoRotationJoint = new NoRotationJoint(); Add(BallSocketJoint); Add(NoRotationJoint); }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate.</param> /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param> public SwivelHingeJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor, ref Vector3 hingeAxis) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor); Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp ); AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, ref hingeAxis, ref tmp ); HingeLimit = new RevoluteLimit(connectionA, connectionB); HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis); TwistLimit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0); TwistMotor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp ); HingeLimit.IsActive = false; HingeMotor.IsActive = false; TwistLimit.IsActive = false; TwistMotor.IsActive = false; //Ensure that the base and test direction is perpendicular to the free axis. Vector3 baseAxis; anchor.Sub( ref connectionA.position, out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way. connectionB.position.Sub( ref anchor, out baseAxis ); baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot( ref baseAxis, ref hingeAxis) , out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis); } } HingeLimit.Basis.SetWorldAxes(ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix); HingeMotor.Basis.SetWorldAxes( ref hingeAxis, ref baseAxis, ref connectionA.orientationMatrix); connectionB.position.Sub( ref anchor, out baseAxis ); baseAxis.AddScaled( ref hingeAxis, -Vector3.Dot(ref baseAxis, ref hingeAxis), out baseAxis ); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. Vector3.Cross(ref hingeAxis, ref Vector3.Up, out baseAxis); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { Vector3.Cross(ref hingeAxis, ref Vector3.Right, out baseAxis); } } HingeLimit.TestAxis = baseAxis; HingeMotor.TestAxis = baseAxis; Add(BallSocketJoint); Add(AngularJoint); Add(HingeLimit); Add(HingeMotor); Add(TwistLimit); Add(TwistMotor); }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate.</param> /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param> public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor); AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB); HingeLimit = new RevoluteLimit(connectionA, connectionB); HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis); TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0); TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB); HingeLimit.IsActive = false; HingeMotor.IsActive = false; TwistLimit.IsActive = false; TwistMotor.IsActive = false; //Ensure that the base and test direction is perpendicular to the free axis. Vector3 baseAxis = anchor - connectionA.position; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way. baseAxis = connectionB.position - anchor; baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = Vector3.Cross(hingeAxis, Vector3.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = Vector3.Cross(hingeAxis, Vector3.Right); } } HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix); HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix); baseAxis = connectionB.position - anchor; baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = Vector3.Cross(hingeAxis, Vector3.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = Vector3.Cross(hingeAxis, Vector3.Right); } } HingeLimit.TestAxis = baseAxis; HingeMotor.TestAxis = baseAxis; Add(BallSocketJoint); Add(AngularJoint); Add(HingeLimit); Add(HingeMotor); Add(TwistLimit); Add(TwistMotor); }
/// <summary> /// Constructs a new constraint which restricts the linear and angular motion 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">The location of the weld.</param> public WeldJoint(Entity connectionA, Entity connectionB, Vector3 anchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor); NoRotationJoint = new NoRotationJoint(connectionA, connectionB); Add(BallSocketJoint); Add(NoRotationJoint); }
/// <summary> /// Constructs a new constraint which restricts the linear and angular motion between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> public WeldJoint(Entity connectionA, Entity connectionB) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, (connectionA.position + connectionB.position) * .5f); NoRotationJoint = new NoRotationJoint(connectionA, connectionB); Add(BallSocketJoint); Add(NoRotationJoint); }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities. /// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner, /// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false). /// </summary> public UniversalJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); TwistJoint = new TwistJoint(); Limit = new TwistLimit(); Motor = new TwistMotor(); Add(BallSocketJoint); Add(TwistJoint); Add(Limit); Add(Motor); }
/// <summary> /// Constructs a new constraint which restricts the linear and angular motion between two entities. /// Uses the average of the two entity positions for the anchor. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> public WeldJoint(Entity connectionA, Entity connectionB) { if( connectionA == null ) connectionA = TwoEntityConstraint.WorldEntity; if( connectionB == null ) connectionB = TwoEntityConstraint.WorldEntity; Vector3 anchor; GetAnchorGuess( connectionA, connectionB, out anchor ); BallSocketJoint = new BallSocketJoint( connectionA, connectionB, ref anchor ); NoRotationJoint = new NoRotationJoint( connectionA, connectionB ); Add( BallSocketJoint ); Add( NoRotationJoint ); }
/// <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); }
public PointPointConstraint(Vector3 position, IPhysicObject obA, IPhysicObject obB) { if (obA.PhysicObjectTypes != PhysicObjectTypes.TRIANGLEMESHOBJECT && obB.PhysicObjectTypes != PhysicObjectTypes.TRIANGLEMESHOBJECT) { BodyA = (BepuEntityObject)obA; BodyB = (BepuEntityObject)obB; joint = new BallSocketJoint(BodyA.Entity, BodyB.Entity, position); } else { throw new Exception("Cannot apply this Constraint on Triangle Meshes"); } }
/// <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, System.Numerics.Vector3 anchor, System.Numerics.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. 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, 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 = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Right); } } Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); baseAxis = connectionB.position - anchor; baseAxis -= Vector3Ex.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 = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = System.Numerics.Vector3.Cross(freeAxis, Vector3Ex.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 one degree of twisting angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate in world space.</param> public UniversalJoint(Entity connectionA, Entity connectionB, Vector3 anchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor); TwistJoint = new TwistJoint(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB); Limit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0); Motor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB); Limit.IsActive = false; Motor.IsActive = false; Add(BallSocketJoint); Add(TwistJoint); Add(Limit); Add(Motor); }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities. /// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner, /// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false). /// </summary> public SwivelHingeJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); AngularJoint = new SwivelHingeAngularJoint(); HingeLimit = new RevoluteLimit(); HingeMotor = new RevoluteMotor(); TwistLimit = new TwistLimit(); TwistMotor = new TwistMotor(); Add(BallSocketJoint); Add(AngularJoint); Add(HingeLimit); Add(HingeMotor); Add(TwistLimit); Add(TwistMotor); }
/// <summary> /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities. /// </summary> /// <param name="connectionA">First entity of the constraint pair.</param> /// <param name="connectionB">Second entity of the constraint pair.</param> /// <param name="anchor">Point around which both entities rotate in world space.</param> public UniversalJoint(Entity connectionA, Entity connectionB, ref Vector3 anchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, ref anchor); Vector3 tmp; BallSocketJoint.OffsetB.Invert( out tmp ); TwistJoint = new TwistJoint(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp); Limit = new TwistLimit(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp, 0, 0); Motor = new TwistMotor(connectionA, connectionB, ref BallSocketJoint.worldOffsetA, ref tmp ); Limit.IsActive = false; Motor.IsActive = false; Add(BallSocketJoint); Add(TwistJoint); Add(Limit); Add(Motor); }
void BuildCyclicMesh(Vector3 position) { int widthCount = 5; int heightCount = 5; var boneMesh = new BoneRelationship[widthCount, heightCount]; for (int i = 0; i < widthCount; i++) { for (int j = 0; j < heightCount; j++) { var bonePosition = position + new Vector3(i, j, 0); boneMesh[i, j] = new BoneRelationship( new Bone(bonePosition, Quaternion.Identity, .4f, .8f), new Box(bonePosition, .8f, .8f, .8f, 10)); Space.Add(boneMesh[i, j].Entity); bones.Add(boneMesh[i, j]); } } for (int i = 0; i < widthCount; i++) { for (int j = 0; j < heightCount; j++) { if (i > 0) { var boneA = boneMesh[i, j]; var boneB = boneMesh[i - 1, j]; var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2; var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor); CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase); Space.Add(dynamicsJoint); joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor)); } if (j > 0) { var boneA = boneMesh[i, j]; var boneB = boneMesh[i, j - 1]; var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2; var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor); CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase); Space.Add(dynamicsJoint); joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor)); } if (i > 0 && j > 0) { var boneA = boneMesh[i, j]; var boneB = boneMesh[i - 1, j - 1]; var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2; var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor); CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase); Space.Add(dynamicsJoint); joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor)); } if (i < widthCount - 1 && j > 0) { var boneA = boneMesh[i, j]; var boneB = boneMesh[i + 1, j - 1]; var anchor = (boneA.Entity.Position + boneB.Entity.Position) / 2; var dynamicsJoint = new BallSocketJoint(boneA.Entity, boneB.Entity, anchor); CollisionRules.AddRule(boneA.Entity, boneB.Entity, CollisionRule.NoBroadPhase); Space.Add(dynamicsJoint); joints.Add(new IKBallSocketJoint(boneA.Bone, boneB.Bone, anchor)); } } } int limbCount = 4; var previous = boneMesh[boneMesh.GetLength(0) / 2, 0]; for (int i = 0; i < limbCount; i++) { Bone bone = new Bone(previous.Bone.Position + new Vector3(0, -0.8f, 0), Quaternion.Identity, .2f, .4f); Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10); Space.Add(entity); var anchor = (entity.Position + previous.Entity.Position) / 2; Space.Add(new BallSocketJoint(entity, previous.Entity, anchor)); joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor)); CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase); previous = new BoneRelationship(bone, entity); bones.Add(previous); } previous = boneMesh[boneMesh.GetLength(0) / 2, boneMesh.GetLength(1) - 1]; for (int i = 0; i < limbCount; i++) { Bone bone = new Bone(previous.Bone.Position + new Vector3(0, 0.8f, 0), Quaternion.Identity, .2f, .4f); Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10); Space.Add(entity); var anchor = (entity.Position + previous.Entity.Position) / 2; Space.Add(new BallSocketJoint(entity, previous.Entity, anchor)); CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase); joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor)); previous = new BoneRelationship(bone, entity); bones.Add(previous); } previous = boneMesh[0, boneMesh.GetLength(1) / 2]; for (int i = 0; i < limbCount; i++) { Bone bone = new Bone(previous.Bone.Position + new Vector3(-.8f, 0, 0), Quaternion.Identity, .2f, .4f); Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10); Space.Add(entity); var anchor = (entity.Position + previous.Entity.Position) / 2; Space.Add(new BallSocketJoint(entity, previous.Entity, anchor)); CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase); joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor)); previous = new BoneRelationship(bone, entity); bones.Add(previous); } previous = boneMesh[boneMesh.GetLength(0) - 1, boneMesh.GetLength(1) / 2]; for (int i = 0; i < limbCount; i++) { Bone bone = new Bone(previous.Bone.Position + new Vector3(0.8f, 0, 0), Quaternion.Identity, .2f, .4f); Entity entity = new Box(bone.Position, .4f, .4f, .4f, 10); Space.Add(entity); var anchor = (entity.Position + previous.Entity.Position) / 2; Space.Add(new BallSocketJoint(entity, previous.Entity, anchor)); CollisionRules.AddRule(entity, previous.Entity, CollisionRule.NoBroadPhase); joints.Add(new IKBallSocketJoint(bone, previous.Bone, anchor)); previous = new BoneRelationship(bone, entity); bones.Add(previous); } }
public Ragdoll() { #region Ragdoll Entities //Create the ragdoll's bones. var pelvis = new Box(Vector3.Zero, .5f, .28f, .33f, 20); var torsoBottom = new Box(pelvis.Position + new Vector3(0, .3f, 0), .42f, .48f, .3f, 15); var torsoTop = new Box(torsoBottom.Position + new Vector3(0, .3f, 0), .5f, .38f, .32f, 20); var neck = new Box(torsoTop.Position + new Vector3(0, .2f, .04f), .19f, .24f, .2f, 5); var head = new Sphere(neck.Position + new Vector3(0, .22f, -.04f), .19f, 7); var leftUpperArm = new Box(torsoTop.Position + new Vector3(-.46f, .1f, 0), .52f, .19f, .19f, 6); var leftForearm = new Box(leftUpperArm.Position + new Vector3(-.5f, 0, 0), .52f, .18f, .18f, 5); var leftHand = new Box(leftForearm.Position + new Vector3(-.35f, 0, 0), .28f, .13f, .22f, 4); var rightUpperArm = new Box(torsoTop.Position + new Vector3(.46f, .1f, 0), .52f, .19f, .19f, 6); var rightForearm = new Box(rightUpperArm.Position + new Vector3(.5f, 0, 0), .52f, .18f, .18f, 5); var rightHand = new Box(rightForearm.Position + new Vector3(.35f, 0, 0), .28f, .13f, .22f, 4); var leftThigh = new Box(pelvis.Position + new Vector3(-.15f, -.4f, 0), .23f, .63f, .23f, 10); var leftShin = new Box(leftThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7); var leftFoot = new Box(leftShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 5); var rightThigh = new Box(pelvis.Position + new Vector3(.15f, -.4f, 0), .23f, .63f, .23f, 10); var rightShin = new Box(rightThigh.Position + new Vector3(0, -.6f, 0), .21f, .63f, .21f, 7); var rightFoot = new Box(rightShin.Position + new Vector3(0, -.35f, -.1f), .23f, .15f, .43f, 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, .1f, 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 = .05f; var torsoBottomToTorsoTopBallSocketJoint = new BallSocketJoint(torsoBottom, torsoTop, torsoBottom.Position + new Vector3(0, .25f, 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 = .05f; var torsoTopToNeckBallSocketJoint = new BallSocketJoint(torsoTop, neck, torsoTop.Position + new Vector3(0, .15f, .05f)); 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 = .1f; var neckToHeadBallSocketJoint = new BallSocketJoint(neck, head, neck.Position + new Vector3(0, .1f, .05f)); 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 = .1f; #endregion #region Left Arm var torsoTopToLeftArmBallSocketJoint = new BallSocketJoint(torsoTop, leftUpperArm, torsoTop.Position + new Vector3(-.3f, .1f, 0)); var torsoTopToLeftArmEllipseLimit = new EllipseSwingLimit(torsoTop, leftUpperArm, Vector3.Left, MathHelper.Pi * .75f, 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 = .2f; var leftUpperArmToLeftForearmSwivelHingeJoint = new SwivelHingeJoint(leftUpperArm, leftForearm, leftUpperArm.Position + new Vector3(-.28f, 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 * .8f; 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 = .3f; var leftForearmToLeftHandBallSocketJoint = new BallSocketJoint(leftForearm, leftHand, leftForearm.Position + new Vector3(-.2f, 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 = .4f; #endregion #region Right Arm var torsoTopToRightArmBallSocketJoint = new BallSocketJoint(torsoTop, rightUpperArm, torsoTop.Position + new Vector3(.3f, .1f, 0)); var torsoTopToRightArmEllipseLimit = new EllipseSwingLimit(torsoTop, rightUpperArm, Vector3.Right, MathHelper.Pi * .75f, 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 = .2f; var rightUpperArmToRightForearmSwivelHingeJoint = new SwivelHingeJoint(rightUpperArm, rightForearm, rightUpperArm.Position + new Vector3(.28f, 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 * .8f; //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 = .3f; var rightForearmToRightHandBallSocketJoint = new BallSocketJoint(rightForearm, rightHand, rightForearm.Position + new Vector3(.2f, 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 = .4f; #endregion #region Left Leg var pelvisToLeftThighBallSocketJoint = new BallSocketJoint(pelvis, leftThigh, pelvis.Position + new Vector3(-.15f, -.1f, 0)); var pelvisToLeftThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, leftThigh, Vector3.Normalize(new Vector3(-.2f, -1, -.6f)), MathHelper.Pi * .7f, 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 = .1f; var leftThighToLeftShinRevoluteJoint = new RevoluteJoint(leftThigh, leftShin, leftThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right); leftThighToLeftShinRevoluteJoint.Limit.IsActive = true; leftThighToLeftShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f; leftThighToLeftShinRevoluteJoint.Limit.MaximumAngle = 0; leftThighToLeftShinRevoluteJoint.Motor.IsActive = true; leftThighToLeftShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f; var leftShinToLeftFootBallSocketJoint = new BallSocketJoint(leftShin, leftFoot, leftShin.Position + new Vector3(0, -.3f, 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 = .2f; #endregion #region Right Leg var pelvisToRightThighBallSocketJoint = new BallSocketJoint(pelvis, rightThigh, pelvis.Position + new Vector3(.15f, -.1f, 0)); var pelvisToRightThighEllipseSwingLimit = new EllipseSwingLimit(pelvis, rightThigh, Vector3.Normalize(new Vector3(.2f, -1, -.6f)), MathHelper.Pi * .7f, 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 = .1f; var rightThighToRightShinRevoluteJoint = new RevoluteJoint(rightThigh, rightShin, rightThigh.Position + new Vector3(0, -.3f, 0), Vector3.Right); rightThighToRightShinRevoluteJoint.Limit.IsActive = true; rightThighToRightShinRevoluteJoint.Limit.MinimumAngle = -MathHelper.Pi * .8f; rightThighToRightShinRevoluteJoint.Limit.MaximumAngle = 0; rightThighToRightShinRevoluteJoint.Motor.IsActive = true; rightThighToRightShinRevoluteJoint.Motor.Settings.VelocityMotor.Softness = .2f; var rightShinToRightFootBallSocketJoint = new BallSocketJoint(rightShin, rightFoot, rightShin.Position + new Vector3(0, -.3f, 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 = .2f; #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 SelfCollidingClothDemo(DemosGame game) : base(game) { //Joints can also act like springs by modifying their springSettings. //Though using a bunch of DistanceJoint objects can be slower than just applying direct spring forces, //it is significantly more stable and allows rigid structures. //The extra stability can make it useful for cloth-like simulations. Entity latticePiece; BallSocketJoint joint; NarrowPhaseHelper.Factories.BoxBox.Count = 4000; NarrowPhaseHelper.Factories.BoxSphere.Count = 1000; int numColumns = 40; int numRows = 40; float xSpacing = 1.0f; float zSpacing = 1.0f; var lattice = new Entity[numRows, numColumns]; for (int i = 0; i < numRows; i++) for (int j = 0; j < numColumns; j++) { latticePiece = new Box( new Vector3( xSpacing * i - (numRows - 1) * xSpacing / 2f, 15.58f, 2 + zSpacing * j - (numColumns - 1) * zSpacing / 2f), xSpacing, .2f, zSpacing, 10); lattice[i, j] = latticePiece; Space.Add(latticePiece); } //The joints composing the cloth can have their max iterations set independently from the solver iterations. //More iterations (up to the solver's own max) will increase the quality at the cost of speed. int clothIterations = 3; //So while the above clamps joint iterations, setting the solver's iteration limit can lower the //rest of the solving load (collisions). Space.Solver.IterationLimit = 10; float damping = 20000, stiffness = 20000; float starchDamping = 5000, starchStiffness = 500; //Loop through the grid and set up the joints. for (int i = 0; i < numRows; i++) for (int j = 0; j < numColumns; j++) { if (i == 0 && j + 1 < numColumns) { //Add in column connections for left edge. joint = new BallSocketJoint(lattice[0, j], lattice[0, j + 1], lattice[0, j].Position + new Vector3(-xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } if (i == numRows - 1 && j + 1 < numColumns) { //Add in column connections for right edge. joint = new BallSocketJoint(lattice[numRows - 1, j], lattice[numRows - 1, j + 1], lattice[numRows - 1, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } if (i + 1 < numRows && j == 0) { //Add in row connections for top edge. joint = new BallSocketJoint(lattice[i, 0], lattice[i + 1, 0], lattice[i, 0].Position + new Vector3(xSpacing / 2, 0, -zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } if (i + 1 < numRows && j == numColumns - 1) { //Add in row connections for bottom edge. joint = new BallSocketJoint(lattice[i, numColumns - 1], lattice[i + 1, numColumns - 1], lattice[i, numColumns - 1].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } if (i + 1 < numRows && j + 1 < numColumns) { //Add in interior connections. joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); joint = new BallSocketJoint(lattice[i, j], lattice[i + 1, j + 1], lattice[i, j].Position + new Vector3(xSpacing / 2, 0, zSpacing / 2)); joint.SpringSettings.Damping = damping; joint.SpringSettings.Stiffness = stiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } if (i + 2 < numRows && j + 2 < numColumns) { //Add in skipping 'starch' connections. joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing)); joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); joint = new BallSocketJoint(lattice[i, j], lattice[i, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing)); joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); joint = new BallSocketJoint(lattice[i, j], lattice[i + 2, j + 2], lattice[i, j].Position + new Vector3(xSpacing, 0, zSpacing)); joint.SpringSettings.Damping = starchDamping; joint.SpringSettings.Stiffness = starchStiffness; joint.SolverSettings.MaximumIterationCount = clothIterations; Space.Add(joint); } //Add in collision rules. if (j - 1 >= 0) { if (i - 1 >= 0) CollisionRules.AddRule(lattice[i, j], lattice[i - 1, j - 1], CollisionRule.NoBroadPhase); CollisionRules.AddRule(lattice[i, j], lattice[i, j - 1], CollisionRule.NoBroadPhase); if (i + 1 < numRows) CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j - 1], CollisionRule.NoBroadPhase); } if (i + 1 < numRows) CollisionRules.AddRule(lattice[i, j], lattice[i + 1, j], CollisionRule.NoBroadPhase); } //Add some ground. var sphere = new Sphere(new Vector3(7, 0, 0), 10); sphere.Material.KineticFriction = .2f; Space.Add(sphere); Space.Add(new Box(new Vector3(0, -20.5f, 0), 100f, 10, 100f)); game.Camera.Position = new Vector3(0, 5, 25); }
/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public ReverseTrikeDemo(DemosGame game) : base(game) { game.Camera.Position = new Vector3(0, 2, 15); Space.Add(new Box(new Vector3(0, -5, 0), 20, 1, 20)); var body = new Box(new Vector3(0, 0, 0), 2, 1, 3, 10); body.CollisionInformation.LocalPosition = new Vector3(0, .8f, 0); Space.Add(body); #region First Wheel var wheel = new Cylinder(body.Position + new Vector3(-1.3f, 0, -1.5f), .2f, .5f, 4); wheel.Material = new Material(1.5f, 1.5f, 0); wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2); //Preventing the occasional pointless collision pair can speed things up. CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase); //Connect the wheel to the body. var ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position); var swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right); //Motorize the wheel. drivingMotor1 = new RevoluteMotor(body, wheel, Vector3.Left); drivingMotor1.Settings.VelocityMotor.Softness = .2f; //Let it roll when the user isn't giving specific commands. drivingMotor1.IsActive = false; steeringMotor1 = new RevoluteMotor(body, wheel, Vector3.Up); steeringMotor1.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. steeringMotor1.Basis.SetWorldAxes(Vector3.Up, Vector3.Right); steeringMotor1.TestAxis = Vector3.Right; //Add the wheel and connection to the space. Space.Add(wheel); Space.Add(ballSocketJoint); Space.Add(swivelHingeAngularJoint); Space.Add(drivingMotor1); Space.Add(steeringMotor1); #endregion #region Second Wheel wheel = new Cylinder(body.Position + new Vector3(1.3f, 0, -1.5f), .2f, .5f, 4); wheel.Material = new Material(1.5f, 1.5f, 0); wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2); //Preventing the occasional pointless collision pair can speed things up. CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase); //Connect the wheel to the body. ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position); swivelHingeAngularJoint = new SwivelHingeAngularJoint(body, wheel, Vector3.Up, Vector3.Right); //Motorize the wheel. drivingMotor2 = new RevoluteMotor(body, wheel, Vector3.Left); drivingMotor2.Settings.VelocityMotor.Softness = .2f; //Let it roll when the user isn't giving specific commands. drivingMotor2.IsActive = false; steeringMotor2 = new RevoluteMotor(body, wheel, Vector3.Up); steeringMotor2.Settings.Mode = MotorMode.Servomechanism; //Configure the motor. See wheel 1 for more description. steeringMotor2.Basis.SetWorldAxes(Vector3.Up, Vector3.Right); steeringMotor2.TestAxis = Vector3.Right; //Add the wheel and connection to the space. Space.Add(wheel); Space.Add(ballSocketJoint); Space.Add(swivelHingeAngularJoint); Space.Add(drivingMotor2); Space.Add(steeringMotor2); #endregion #region Third Wheel wheel = new Cylinder(body.Position + new Vector3(0, -.3f, 1.5f), .2f, .5f, 4); wheel.Material = new Material(1.5f, 1.5f, 0); wheel.Orientation = Quaternion.CreateFromAxisAngle(Vector3.Forward, MathHelper.PiOver2); //Preventing the occasional pointless collision pair can speed things up. CollisionRules.AddRule(body, wheel, CollisionRule.NoBroadPhase); //Connect the wheel to the body. ballSocketJoint = new BallSocketJoint(body, wheel, wheel.Position); //Notice that the third wheel isn't a swivel hinge, it's just a revolute axis. //This lets it roll, but prevents flopping around like the wheels of a grocery cart. //Could have used a RevoluteJoint solver group here, but this shows it's possible to do //the same things without using the combo-constraints. var revoluteAngularJoint = new RevoluteAngularJoint(body, wheel, Vector3.Right); //Add the wheel and connection to the space. Space.Add(wheel); Space.Add(ballSocketJoint); Space.Add(revoluteAngularJoint); #endregion int xLength = 180; int zLength = 180; float xSpacing = 8f; float zSpacing = 8f; var heights = new float[xLength, zLength]; for (int i = 0; i < xLength; i++) { for (int j = 0; j < zLength; j++) { float x = i - xLength / 2; float z = j - zLength / 2; //heights[i,j] = (float)(x * y / 1000f); heights[i, j] = (float)(10 * (Math.Sin(x / 8) + Math.Sin(z / 8))); //heights[i,j] = 3 * (float)Math.Sin(x * y / 100f); //heights[i,j] = (x * x * x * y - y * y * y * x) / 1000f; } } //Create the terrain. var terrain = new Terrain(heights, new AffineTransform( new Vector3(xSpacing, 1, zSpacing), Quaternion.Identity, new Vector3(-xLength * xSpacing / 2, -10, -zLength * zSpacing / 2))); Space.Add(terrain); game.ModelDrawer.Add(terrain); }
/// <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 void GrabEntity() { if (_grabbing) return; if (!grabbedEntity.IsDynamic) return; EntityTag eT = lastTouched.Tag as EntityTag; bsj = new BallSocketJoint(grabbedEntity, grabPairCollector, grabPairCollector.Position); bsj.SpringSettings.StiffnessConstant = 2000000.0f; nrj = new NoRotationJoint(grabbedEntity, grabPairCollector); Space.Add(bsj); Space.Add(nrj); _grabbing = true; eT.ParentComponent.Mesh.Highlight = true; }
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); }
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); }
/// <summary> /// Constructs a new demo. /// </summary> /// <param name="game">Game owning this demo.</param> public UnfortunateGuyDemo(DemosGame game) : base(game) { Entity ground = new Box(Vector3.Zero, 10, 1, 10); Space.Add(ground); //Rather than add basically redundant code for every limb, this demo //focuses on a single arm, showing some extra details and limits. //Make the torso. var bodies = new List<CompoundShapeEntry>() { new CompoundShapeEntry(new BoxShape(2, 1.5f, 1), new Vector3(-1, 3, 0), 50), new CompoundShapeEntry(new SphereShape(.45f), new Vector3(.4f, 3, 0), 1), new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-1.9f, 3.5f, 0), 1), new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-1.9f, 2.5f, 0), 1), new CompoundShapeEntry(new SphereShape(.25f), new Vector3(-.3f, 2.3f, 0), 1) }; var torso = new CompoundBody(bodies, 54); Space.Add(torso); //Make the upper arm. Entity upperArm = new Box(torso.Position + new Vector3(1, 1.4f, 0), .4f, 1.2f, .4f, 8); Space.Add(upperArm); //A ball socket joint will handle the linear degrees of freedom between the two entities. var ballSocketJoint = new BallSocketJoint(torso, upperArm, torso.Position + new Vector3(1, .7f, 0)); Space.Add(ballSocketJoint); //Shoulders don't have a simple limit. The EllipseSwingLimit allows angles within an ellipse, which is closer to how some joints function //than just flat planes (like two RevoluteLimits) or a single angle (like SwingLimits). var swingLimit = new EllipseSwingLimit(torso, upperArm, Vector3.Up, MathHelper.PiOver2, MathHelper.PiOver4 * 3); Space.Add(swingLimit); //Upper arms can't spin around forever. var twistLimit = new TwistLimit(torso, upperArm, Vector3.Up, Vector3.Up, -MathHelper.PiOver4 / 2, MathHelper.PiOver4); twistLimit.SpringSettings.StiffnessConstant = 100; twistLimit.SpringSettings.DampingConstant = 100; Space.Add(twistLimit); //Make the lower arm. Entity lowerArm = new Box(upperArm.Position + new Vector3(0, 1.4f, 0), .35f, 1.3f, .35f, 8); Space.Add(lowerArm); var elbow = new SwivelHingeJoint(upperArm, lowerArm, upperArm.Position + new Vector3(0, .6f, 0), Vector3.Forward); //Forearm can twist a little. elbow.TwistLimit.IsActive = true; elbow.TwistLimit.MinimumAngle = -MathHelper.PiOver4 / 2; elbow.TwistLimit.MaximumAngle = MathHelper.PiOver4 / 2; elbow.TwistLimit.SpringSettings.DampingConstant = 100; elbow.TwistLimit.SpringSettings.StiffnessConstant = 100; //The elbow is like a hinge, but it can't hyperflex. elbow.HingeLimit.IsActive = true; elbow.HingeLimit.MinimumAngle = 0; elbow.HingeLimit.MaximumAngle = MathHelper.Pi * .7f; Space.Add(elbow); Entity hand = new Box(lowerArm.Position + new Vector3(0, .9f, 0), .4f, .55f, .25f, 3); Space.Add(hand); ballSocketJoint = new BallSocketJoint(lowerArm, hand, lowerArm.Position + new Vector3(0, .7f, 0)); Space.Add(ballSocketJoint); //Wrists can use an ellipse limit too. swingLimit = new EllipseSwingLimit(lowerArm, hand, Vector3.Up, MathHelper.PiOver4, MathHelper.PiOver2); Space.Add(swingLimit); //Allow a little extra twist beyond the forearm. twistLimit = new TwistLimit(lowerArm, hand, Vector3.Up, Vector3.Up, -MathHelper.PiOver4 / 2, MathHelper.PiOver4 / 2); twistLimit.SpringSettings.StiffnessConstant = 100; twistLimit.SpringSettings.DampingConstant = 100; Space.Add(twistLimit); //The hand is pretty floppy without some damping. var angularMotor = new AngularMotor(lowerArm, hand); angularMotor.Settings.VelocityMotor.Softness = .5f; Space.Add(angularMotor); //Make sure the parts of the arm don't collide. CollisionRules.AddRule(torso, upperArm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(lowerArm, upperArm, CollisionRule.NoBroadPhase); CollisionRules.AddRule(lowerArm, hand, CollisionRule.NoBroadPhase); game.Camera.Position = new Vector3(0, 4, 20); }
void BuildStick(Vector3 position) { //Set up a bone chain. float fullLength = 20; int linkCount = 20; float linkLength = fullLength / linkCount; float linkRadius = linkLength * 0.2f; var previousBoneEntity = new Cylinder(position, linkLength, linkRadius, 100); var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height); bones.Add(new BoneRelationship(previousBone, previousBoneEntity)); Space.Add(previousBoneEntity); for (int i = 1; i < linkCount; i++) { var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, linkLength, 0), linkLength, linkRadius, 100); var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height); bones.Add(new BoneRelationship(bone, boneEntity)); Space.Add(boneEntity); //Make a relationship between the two bones and entities. CollisionRules.AddRule(previousBoneEntity, boneEntity, CollisionRule.NoBroadPhase); Vector3 anchor = (previousBoneEntity.Position + boneEntity.Position) / 2; var dynamicsBallSocketJoint = new BallSocketJoint(previousBoneEntity, boneEntity, anchor); var dynamicsAngularFriction = new NoRotationJoint(previousBoneEntity, boneEntity); Space.Add(dynamicsBallSocketJoint); Space.Add(dynamicsAngularFriction); var ikBallSocketJoint = new IKBallSocketJoint(previousBone, bone, anchor); //(the joint is auto-added to the bones; no solver-add is needed) var ikAngularJoint = new IKAngularJoint(previousBone, bone); joints.Add(ikBallSocketJoint); joints.Add(ikAngularJoint); previousBone = bone; previousBoneEntity = boneEntity; } }
void BuildRing(Vector3 position) { int incrementCount = 20; float radius = 5; float anglePerIncrement = MathHelper.TwoPi / incrementCount; Bone[] bonesList = new Bone[incrementCount]; for (int i = 0; i < incrementCount; i++) { Vector3 bonePosition; #if !WINDOWS bonePosition = new Vector3(); #endif bonePosition.X = (float)Math.Cos(anglePerIncrement * i); bonePosition.Y = 0; bonePosition.Z = (float)Math.Sin(anglePerIncrement * i); bonePosition = bonePosition * radius + position; bonesList[i] = new Bone(bonePosition, Quaternion.Concatenate(Quaternion.CreateFromAxisAngle(Vector3.Right, MathHelper.PiOver2), Quaternion.CreateFromAxisAngle(Vector3.Up, -anglePerIncrement * i)), 0.5f, MathHelper.Pi * radius * 2 / incrementCount); } for (int i = 0; i < bonesList.Length; i++) { var boneA = bonesList[i]; var boneB = bonesList[(i + 1) % incrementCount]; var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation); var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation); joints.Add(new IKBallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.HalfHeight + boneB.Position - upB * boneB.HalfHeight) * .5f)); joints.Add(new IKSwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f)); } Cylinder[] boneEntitiesList = new Cylinder[incrementCount]; for (int i = 0; i < incrementCount; i++) { var boneEntity = new Cylinder(new MotionState { Position = bonesList[i].Position, Orientation = bonesList[i].Orientation }, bonesList[i].Height, bonesList[i].Radius, 10); bones.Add(new BoneRelationship(bonesList[i], boneEntity)); boneEntitiesList[i] = boneEntity; Space.Add(boneEntity); } for (int i = 0; i < incrementCount; i++) { var boneA = boneEntitiesList[i]; var boneB = boneEntitiesList[(i + 1) % incrementCount]; var upA = Quaternion.Transform(Vector3.Up, boneA.Orientation); var upB = Quaternion.Transform(Vector3.Up, boneB.Orientation); var joint = new BallSocketJoint(boneA, boneB, (boneA.Position + upA * boneB.Height * 0.5f + boneB.Position - upB * boneB.Height * 0.5f) * .5f); var swingLimit = new SwingLimit(boneA, boneB, upA, upB, MathHelper.Pi * .5f); Space.Add(swingLimit); Space.Add(joint); CollisionRules.AddRule(boneA, boneB, CollisionRule.NoBroadPhase); } }
void BuildChain(Vector3 position) { //Set up a bone chain. int linkCount = 100; var previousBoneEntity = new Cylinder(position, 1, .2f, 10); var previousBone = new Bone(previousBoneEntity.Position, previousBoneEntity.Orientation, previousBoneEntity.Radius, previousBoneEntity.Height); bones.Add(new BoneRelationship(previousBone, previousBoneEntity)); Space.Add(previousBoneEntity); for (int i = 1; i < linkCount; i++) { var boneEntity = new Cylinder(previousBone.Position + new Vector3(0, 1, 0), 1, .2f, 10); var bone = new Bone(boneEntity.Position, boneEntity.Orientation, boneEntity.Radius, boneEntity.Height); bones.Add(new BoneRelationship(bone, boneEntity)); Space.Add(boneEntity); //Make a relationship between the two bones and entities. CollisionRules.AddRule(previousBoneEntity, boneEntity, CollisionRule.NoBroadPhase); Vector3 anchor = (previousBoneEntity.Position + boneEntity.Position) / 2; var dynamicsBallSocketJoint = new BallSocketJoint(previousBoneEntity, boneEntity, anchor); var dynamicsAngularFriction = new AngularMotor(previousBoneEntity, boneEntity); dynamicsAngularFriction.Settings.Mode = MotorMode.VelocityMotor; dynamicsAngularFriction.Settings.MaximumForce = 200; Space.Add(dynamicsBallSocketJoint); Space.Add(dynamicsAngularFriction); var ikBallSocketJoint = new IKBallSocketJoint(previousBone, bone, anchor); //(the joint is auto-added to the bones; no solver-add is needed) joints.Add(ikBallSocketJoint); previousBone = bone; previousBoneEntity = boneEntity; } }