PageRenderTime 61ms CodeModel.GetById 38ms RepoModel.GetById 0ms app.codeStats 0ms

/BEPUphysics/Constraints/SolverGroups/SwivelHingeJoint.cs

#
C# | 134 lines | 83 code | 14 blank | 37 comment | 9 complexity | 636ecad883d2f9a9401099a35faa4320 MD5 | raw file
  1. using BEPUphysics.Constraints.TwoEntity;
  2. using BEPUphysics.Constraints.TwoEntity.JointLimits;
  3. using BEPUphysics.Constraints.TwoEntity.Joints;
  4. using BEPUphysics.Constraints.TwoEntity.Motors;
  5. using BEPUphysics.Entities;
  6. using BEPUutilities;
  7. namespace BEPUphysics.Constraints.SolverGroups
  8. {
  9. /// <summary>
  10. /// Restricts linear motion while allowing one degree of angular freedom.
  11. /// Acts like a tablet pc monitor hinge.
  12. /// </summary>
  13. public class SwivelHingeJoint : SolverGroup
  14. {
  15. /// <summary>
  16. /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
  17. /// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner,
  18. /// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false).
  19. /// </summary>
  20. public SwivelHingeJoint()
  21. {
  22. IsActive = false;
  23. BallSocketJoint = new BallSocketJoint();
  24. AngularJoint = new SwivelHingeAngularJoint();
  25. HingeLimit = new RevoluteLimit();
  26. HingeMotor = new RevoluteMotor();
  27. TwistLimit = new TwistLimit();
  28. TwistMotor = new TwistMotor();
  29. Add(BallSocketJoint);
  30. Add(AngularJoint);
  31. Add(HingeLimit);
  32. Add(HingeMotor);
  33. Add(TwistLimit);
  34. Add(TwistMotor);
  35. }
  36. /// <summary>
  37. /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities.
  38. /// </summary>
  39. /// <param name="connectionA">First entity of the constraint pair.</param>
  40. /// <param name="connectionB">Second entity of the constraint pair.</param>
  41. /// <param name="anchor">Point around which both entities rotate.</param>
  42. /// <param name="hingeAxis">Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis.</param>
  43. public SwivelHingeJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 hingeAxis)
  44. {
  45. if (connectionA == null)
  46. connectionA = TwoEntityConstraint.WorldEntity;
  47. if (connectionB == null)
  48. connectionB = TwoEntityConstraint.WorldEntity;
  49. BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor);
  50. AngularJoint = new SwivelHingeAngularJoint(connectionA, connectionB, hingeAxis, -BallSocketJoint.OffsetB);
  51. HingeLimit = new RevoluteLimit(connectionA, connectionB);
  52. HingeMotor = new RevoluteMotor(connectionA, connectionB, hingeAxis);
  53. TwistLimit = new TwistLimit(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB, 0, 0);
  54. TwistMotor = new TwistMotor(connectionA, connectionB, BallSocketJoint.OffsetA, -BallSocketJoint.OffsetB);
  55. HingeLimit.IsActive = false;
  56. HingeMotor.IsActive = false;
  57. TwistLimit.IsActive = false;
  58. TwistMotor.IsActive = false;
  59. //Ensure that the base and test direction is perpendicular to the free axis.
  60. Vector3 baseAxis = anchor - connectionA.position;
  61. if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way.
  62. baseAxis = connectionB.position - anchor;
  63. baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
  64. if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
  65. {
  66. //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
  67. baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
  68. if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
  69. {
  70. baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
  71. }
  72. }
  73. HingeLimit.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
  74. HingeMotor.Basis.SetWorldAxes(hingeAxis, baseAxis, connectionA.orientationMatrix);
  75. baseAxis = connectionB.position - anchor;
  76. baseAxis -= Vector3.Dot(baseAxis, hingeAxis) * hingeAxis;
  77. if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
  78. {
  79. //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction.
  80. baseAxis = Vector3.Cross(hingeAxis, Vector3.Up);
  81. if (baseAxis.LengthSquared() < Toolbox.BigEpsilon)
  82. {
  83. baseAxis = Vector3.Cross(hingeAxis, Vector3.Right);
  84. }
  85. }
  86. HingeLimit.TestAxis = baseAxis;
  87. HingeMotor.TestAxis = baseAxis;
  88. Add(BallSocketJoint);
  89. Add(AngularJoint);
  90. Add(HingeLimit);
  91. Add(HingeMotor);
  92. Add(TwistLimit);
  93. Add(TwistMotor);
  94. }
  95. /// <summary>
  96. /// Gets the angular joint which removes one degree of freedom.
  97. /// </summary>
  98. public SwivelHingeAngularJoint AngularJoint { get; private set; }
  99. /// <summary>
  100. /// Gets the ball socket joint that restricts linear degrees of freedom.
  101. /// </summary>
  102. public BallSocketJoint BallSocketJoint { get; private set; }
  103. /// <summary>
  104. /// Gets the rotational limit of the hinge.
  105. /// </summary>
  106. public RevoluteLimit HingeLimit { get; private set; }
  107. /// <summary>
  108. /// Gets the motor of the hinge.
  109. /// </summary>
  110. public RevoluteMotor HingeMotor { get; private set; }
  111. /// <summary>
  112. /// Gets the rotational limit of the swivel hinge.
  113. /// </summary>
  114. public TwistLimit TwistLimit { get; private set; }
  115. /// <summary>
  116. /// Gets the twist motor of the swivel hinge.
  117. /// </summary>
  118. public TwistMotor TwistMotor { get; private set; }
  119. }
  120. }