Repository: etodd/Lemma Branch: master Commit: 061442bb5f9e Files: 1198 Total size: 147.8 MB Directory structure: gitextract_wzh98dju/ ├── .gitignore ├── .gitmodules ├── BEPUphysics/ │ ├── BEPUphysics.csproj │ ├── BroadPhaseEntries/ │ │ ├── BroadPhaseEntry.cs │ │ ├── Collidable.cs │ │ ├── CollidableCollection.cs │ │ ├── CollidablePair.cs │ │ ├── DetectorVolume.cs │ │ ├── EntityCollidableCollection.cs │ │ ├── Events/ │ │ │ ├── CollisionEventTypes.cs │ │ │ ├── CompoundEventManager.cs │ │ │ ├── ContactEventManager.cs │ │ │ ├── EntryEventManager.cs │ │ │ ├── IContactEventTriggerer.cs │ │ │ └── IEntryEventTriggerer.cs │ │ ├── InstancedMesh.cs │ │ ├── MobileCollidables/ │ │ │ ├── CompoundCollidable.cs │ │ │ ├── CompoundHelper.cs │ │ │ ├── CompoundHierarchy.cs │ │ │ ├── ConvexCollidable.cs │ │ │ ├── EntityCollidable.cs │ │ │ ├── MobileCollidable.cs │ │ │ ├── MobileMeshCollidable.cs │ │ │ └── TriangleCollidable.cs │ │ ├── StaticCollidable.cs │ │ ├── StaticGroup.cs │ │ ├── StaticMesh.cs │ │ └── Terrain.cs │ ├── BroadPhaseSystems/ │ │ ├── BroadPhase.cs │ │ ├── BroadPhaseOverlap.cs │ │ ├── BruteForce.cs │ │ ├── Hierarchies/ │ │ │ ├── DynamicHierarchy.cs │ │ │ ├── DynamicHierarchyNode.cs │ │ │ └── DynamicHierarchyQueryAccelerator.cs │ │ ├── IBoundingBoxOwner.cs │ │ ├── IBroadPhaseEntryOwner.cs │ │ ├── IQueryAccelerator.cs │ │ └── SortAndSweep/ │ │ ├── Grid2DEntry.cs │ │ ├── Grid2DSortAndSweep.cs │ │ ├── Grid2DSortAndSweepQueryAccelerator.cs │ │ ├── GridCell2D.cs │ │ ├── SortAndSweep1D.cs │ │ ├── SortedGrid2DSet.cs │ │ └── Testing/ │ │ └── SortAndSweep3D.cs │ ├── CollisionRuleManagement/ │ │ ├── CollisionGroup.cs │ │ ├── CollisionGroupPair.cs │ │ ├── CollisionRule.cs │ │ ├── CollisionRules.cs │ │ └── ICollisionRulesOwner.cs │ ├── CollisionShapes/ │ │ ├── CollisionShape.cs │ │ ├── CompoundShape.cs │ │ ├── ConvexShapes/ │ │ │ ├── BoxShape.cs │ │ │ ├── CapsuleShape.cs │ │ │ ├── ConeShape.cs │ │ │ ├── ConvexHullShape.cs │ │ │ ├── ConvexShape.cs │ │ │ ├── CylinderShape.cs │ │ │ ├── InertiaHelper.cs │ │ │ ├── MinkowskiSumShape.cs │ │ │ ├── SphereShape.cs │ │ │ ├── TransformableShape.cs │ │ │ ├── TriangleShape.cs │ │ │ └── WrappedShape.cs │ │ ├── EntityShape.cs │ │ ├── InstancedMeshShape.cs │ │ ├── MobileMeshShape.cs │ │ ├── ShapeDistributionInformation.cs │ │ ├── StaticGroupShape.cs │ │ ├── StaticMeshShape.cs │ │ └── TerrainShape.cs │ ├── CollisionTests/ │ │ ├── CollisionAlgorithms/ │ │ │ ├── BoxBoxCollider.cs │ │ │ ├── BoxSphereTester.cs │ │ │ ├── GJK/ │ │ │ │ ├── GJKToolbox.cs │ │ │ │ ├── PairSimplex.cs │ │ │ │ ├── RaySimplex.cs │ │ │ │ └── SimpleSimplex.cs │ │ │ ├── GeneralConvexPairTester.cs │ │ │ ├── MPRToolbox.cs │ │ │ ├── MinkowskiToolbox.cs │ │ │ ├── SphereTester.cs │ │ │ ├── TriangleConvexPairTester.cs │ │ │ ├── TrianglePairTester.cs │ │ │ ├── TriangleSpherePairTester.cs │ │ │ └── TriangleTrianglePairTester.cs │ │ ├── Contact.cs │ │ ├── ContactData.cs │ │ ├── ContactReducer.cs │ │ ├── ContactRefresher.cs │ │ ├── ContactSupplementData.cs │ │ └── Manifolds/ │ │ ├── BoxContactManifold.cs │ │ ├── BoxSphereContactManifold.cs │ │ ├── ContactManifold.cs │ │ ├── GeneralConvexContactManifold.cs │ │ ├── InstancedMeshContactManifold.cs │ │ ├── InstancedMeshConvexContactManifold.cs │ │ ├── InstancedMeshSphereContactManifold.cs │ │ ├── MobileMeshContactManifold.cs │ │ ├── MobileMeshConvexContactManifold.cs │ │ ├── MobileMeshSphereContactManifold.cs │ │ ├── MobileMeshTriangleContactManifold.cs │ │ ├── SphereContactManifold.cs │ │ ├── StaticMeshContactManifold.cs │ │ ├── StaticMeshConvexContactManifold.cs │ │ ├── StaticMeshSphereContactManifold.cs │ │ ├── TerrainContactManifold.cs │ │ ├── TerrainConvexContactManifold.cs │ │ ├── TerrainSphereContactManifold.cs │ │ ├── TriangleConvexContactManifold.cs │ │ └── TriangleMeshConvexContactManifold.cs │ ├── Constraints/ │ │ ├── Collision/ │ │ │ ├── ContactFrictionConstraint.cs │ │ │ ├── ContactManifoldConstraint.cs │ │ │ ├── ContactManifoldConstraintGroup.cs │ │ │ ├── ContactPenetrationConstraint.cs │ │ │ ├── ConvexContactManifoldConstraint.cs │ │ │ ├── NonConvexContactManifoldConstraint.cs │ │ │ ├── SlidingFrictionTwoAxis.cs │ │ │ ├── Testing/ │ │ │ │ ├── ContactPenetrationConstraintDETester.cs │ │ │ │ ├── DirectEnumerationSolver.cs │ │ │ │ ├── SlidingFrictionOneAxisConstraint.cs │ │ │ │ └── SlidingFrictionTwoAxisObsolete.cs │ │ │ └── TwistFrictionConstraint.cs │ │ ├── EntitySolverUpdateable.cs │ │ ├── IJacobians.cs │ │ ├── ISolverSettings.cs │ │ ├── ISpringConstraint.cs │ │ ├── IXDImpulseConstraint.cs │ │ ├── JointTransform.cs │ │ ├── SingleEntity/ │ │ │ ├── MaximumAngularVelocityConstraint.cs │ │ │ ├── MaximumLinearVelocityConstraint.cs │ │ │ ├── SingleEntityAngularMotor.cs │ │ │ ├── SingleEntityConstraint.cs │ │ │ └── SingleEntityLinearMotor.cs │ │ ├── SolverGroups/ │ │ │ ├── CustomizableSolverGroup.cs │ │ │ ├── LineSliderJoint.cs │ │ │ ├── PlaneSliderJoint.cs │ │ │ ├── PrismaticJoint.cs │ │ │ ├── RevoluteJoint.cs │ │ │ ├── SolverGroup.cs │ │ │ ├── SwivelHingeJoint.cs │ │ │ ├── UniversalJoint.cs │ │ │ └── WeldJoint.cs │ │ ├── SolverSettings.cs │ │ ├── SpringSettings.cs │ │ └── TwoEntity/ │ │ ├── JointLimits/ │ │ │ ├── DistanceLimit.cs │ │ │ ├── EllipseSwingLimit.cs │ │ │ ├── JointLimit.cs │ │ │ ├── LinearAxisLimit.cs │ │ │ ├── RevoluteLimit.cs │ │ │ ├── SwingLimit.cs │ │ │ └── TwistLimit.cs │ │ ├── Joints/ │ │ │ ├── BallSocketJoint.cs │ │ │ ├── DistanceJoint.cs │ │ │ ├── Joint.cs │ │ │ ├── NoRotationJoint.cs │ │ │ ├── PointOnLineJoint.cs │ │ │ ├── PointOnPlaneJoint.cs │ │ │ ├── RevoluteAngularJoint.cs │ │ │ ├── SwivelHingeAngularJoint.cs │ │ │ └── TwistJoint.cs │ │ ├── Motors/ │ │ │ ├── AngularMotor.cs │ │ │ ├── LinearAxisMotor.cs │ │ │ ├── Motor.cs │ │ │ ├── MotorSettings.cs │ │ │ ├── RevoluteMotor.cs │ │ │ └── TwistMotor.cs │ │ └── TwoEntityConstraint.cs │ ├── DataStructures/ │ │ ├── BoundingBoxTree.cs │ │ ├── MeshBoundingBoxTree.cs │ │ ├── MeshBoundingBoxTreeData.cs │ │ ├── StaticMeshData.cs │ │ ├── TransformableMeshData.cs │ │ ├── TreeOverlapPair.cs │ │ └── TriangleMesh.cs │ ├── DeactivationManagement/ │ │ ├── DeactivationManager.cs │ │ ├── ISimulationIslandConnection.cs │ │ ├── ISimulationIslandConnectionOwner.cs │ │ ├── ISimulationIslandMemberOwner.cs │ │ ├── SimulationIsland.cs │ │ ├── SimulationIslandConnection.cs │ │ ├── SimulationIslandMember.cs │ │ └── SimulationIslandMemberList.cs │ ├── Entities/ │ │ ├── Entity.cs │ │ ├── EntityBase.cs │ │ ├── EntityConstraintCollection.cs │ │ ├── EntitySolverUpdateableCollection.cs │ │ ├── MorphableEntity.cs │ │ └── Prefabs/ │ │ ├── Box.cs │ │ ├── Capsule.cs │ │ ├── CompoundBody.cs │ │ ├── Cone.cs │ │ ├── ConvexHull.cs │ │ ├── Cylinder.cs │ │ ├── MinkowskiSum.cs │ │ ├── MobileMesh.cs │ │ ├── Sphere.cs │ │ ├── TransformableEntity.cs │ │ ├── Triangle.cs │ │ └── WrappedBody.cs │ ├── EntityStateManagement/ │ │ ├── BufferedStatesAccessor.cs │ │ ├── BufferedStatesManager.cs │ │ ├── EntityBufferedStates.cs │ │ ├── EntityStateReadBuffers.cs │ │ ├── EntityStateWriteBuffer.cs │ │ ├── InterpolatedStatesAccessor.cs │ │ ├── InterpolatedStatesManager.cs │ │ └── MotionState.cs │ ├── ISpace.cs │ ├── ISpaceObject.cs │ ├── Materials/ │ │ ├── IMaterialOwner.cs │ │ ├── InteractionProperties.cs │ │ ├── Material.cs │ │ ├── MaterialManager.cs │ │ └── MaterialPair.cs │ ├── MultithreadedProcessingStage.cs │ ├── NarrowPhaseSystems/ │ │ ├── NarrowPhase.cs │ │ ├── NarrowPhaseHelper.cs │ │ ├── NarrowPhasePairFactory.cs │ │ └── Pairs/ │ │ ├── BoxPairHandler.cs │ │ ├── BoxSpherePairHandler.cs │ │ ├── CollidablePairHandler.cs │ │ ├── CompoundConvexPairHandler.cs │ │ ├── CompoundGroupPairHandler.cs │ │ ├── CompoundInstancedMeshPairHandler.cs │ │ ├── CompoundMobileMeshPairHandler.cs │ │ ├── CompoundPairHandler.cs │ │ ├── CompoundStaticMeshPairHandler.cs │ │ ├── CompoundTerrainPairHandler.cs │ │ ├── ContactCollection.cs │ │ ├── ContactInformation.cs │ │ ├── ConvexConstraintPairHandler.cs │ │ ├── ConvexPairHandler.cs │ │ ├── DetectorVolumeCompoundPairHandler.cs │ │ ├── DetectorVolumeConvexPairHandler.cs │ │ ├── DetectorVolumeGroupPairHandler.cs │ │ ├── DetectorVolumeMobileMeshPairHandler.cs │ │ ├── DetectorVolumePairHandler.cs │ │ ├── GeneralConvexPairHandler.cs │ │ ├── GroupPairHandler.cs │ │ ├── IDetectorVolumePairHandlerParent.cs │ │ ├── IPairHandlerParent.cs │ │ ├── InstancedMeshConvexPairHandler.cs │ │ ├── InstancedMeshPairHandler.cs │ │ ├── InstancedMeshSpherePairHandler.cs │ │ ├── MeshGroupPairHandler.cs │ │ ├── MobileMeshConvexPairHandler.cs │ │ ├── MobileMeshInstancedMeshPairHandler.cs │ │ ├── MobileMeshMeshPairHandler.cs │ │ ├── MobileMeshMobileMeshPairHandler.cs │ │ ├── MobileMeshPairHandler.cs │ │ ├── MobileMeshSpherePairHandler.cs │ │ ├── MobileMeshStaticMeshPairHandler.cs │ │ ├── MobileMeshTerrainPairHandler.cs │ │ ├── MobileMeshTrianglePairHandler.cs │ │ ├── NarrowPhasePair.cs │ │ ├── SpherePairHandler.cs │ │ ├── StandardPairHandler.cs │ │ ├── StaticGroupCompoundPairHandler.cs │ │ ├── StaticGroupConvexPairHandler.cs │ │ ├── StaticGroupMobileMeshPairHandler.cs │ │ ├── StaticGroupPairHandler.cs │ │ ├── StaticMeshConvexPairHandler.cs │ │ ├── StaticMeshPairHandler.cs │ │ ├── StaticMeshSpherePairHandler.cs │ │ ├── TerrainConvexPairHandler.cs │ │ ├── TerrainPairHandler.cs │ │ ├── TerrainSpherePairHandler.cs │ │ └── TriangleConvexPairHandler.cs │ ├── OtherSpaceStages/ │ │ ├── BoundingBoxUpdater.cs │ │ ├── DeferredEventDispatcher.cs │ │ ├── ForceUpdater.cs │ │ ├── IDeferredEventCreator.cs │ │ ├── IDeferredEventCreatorOwner.cs │ │ ├── IForceUpdateable.cs │ │ └── SpaceObjectBuffer.cs │ ├── Paths/ │ │ ├── CardinalSpline3D.cs │ │ ├── ConstantAngularSpeedCurve.cs │ │ ├── ConstantLinearSpeedCurve.cs │ │ ├── ConstantSpeedCurve.cs │ │ ├── Curve.cs │ │ ├── CurveControlPoint.cs │ │ ├── CurveControlPointList.cs │ │ ├── CurveEndpointBehavior.cs │ │ ├── FiniteDifferenceSpline3D.cs │ │ ├── HermiteCurve3D.cs │ │ ├── LinearInterpolationCurve3D.cs │ │ ├── Path following/ │ │ │ ├── EntityMover.cs │ │ │ └── EntityRotator.cs │ │ ├── Path.cs │ │ ├── QuaternionSlerpCurve.cs │ │ ├── SpeedControlledCurve.cs │ │ ├── StepCurve1D.cs │ │ ├── VariableAngularSpeedCurve.cs │ │ ├── VariableLinearSpeedCurve.cs │ │ └── VariableSpeedCurve.cs │ ├── PhysicsChecker.cs │ ├── PhysicsResources.cs │ ├── PositionUpdating/ │ │ ├── ContinuousPositionUpdater.cs │ │ ├── ICCDPositionUpdateable.cs │ │ ├── IPositionUpdateable.cs │ │ └── PositionUpdater.cs │ ├── ProcessingStage.cs │ ├── Properties/ │ │ ├── AppManifest.xml │ │ ├── AssemblyInfo.cs │ │ └── WMAppManifest.xml │ ├── RayCastResult.cs │ ├── Settings/ │ │ ├── CollisionDetectionSettings.cs │ │ ├── CollisionResponseSettings.cs │ │ └── MotionSettings.cs │ ├── SolverSystems/ │ │ ├── Solver.cs │ │ ├── SolverUpdateable.cs │ │ └── SolverUpdateableChange.cs │ ├── Space.cs │ ├── StyleCop.Cache │ ├── Threading/ │ │ ├── IThreadManager.cs │ │ ├── Modified Pool/ │ │ │ ├── ParallelLoopManager.cs │ │ │ ├── ParallelLoopWorker.cs │ │ │ ├── SpecializedThreadManager.cs │ │ │ └── ThreadTaskManager.cs │ │ ├── SimpleThreadManager.cs │ │ └── ThreadManagerTPL.cs │ ├── TimeStepSettings.cs │ ├── UpdateableSystems/ │ │ ├── CombinedUpdateable.cs │ │ ├── FluidVolume.cs │ │ ├── ForceFields/ │ │ │ ├── BoundingBoxForceFieldShape.cs │ │ │ ├── BoundingSphereForceFieldShape.cs │ │ │ ├── ForceField.cs │ │ │ ├── ForceFieldShape.cs │ │ │ ├── InfiniteForceFieldShape.cs │ │ │ └── VolumeForceFieldShape.cs │ │ ├── IBeforeNarrowPhaseUpdateable.cs │ │ ├── IBeforePositionUpdateUpdateable.cs │ │ ├── IBeforeSolverUpdateable.cs │ │ ├── IDuringForcesUpdateable.cs │ │ ├── IEndOfFrameUpdateable.cs │ │ ├── IEndOfTimeStepUpdateable.cs │ │ ├── ISpaceUpdateable.cs │ │ ├── Updateable.cs │ │ ├── UpdateableManager.cs │ │ └── UpdateableManagers.cs │ ├── Vehicle/ │ │ ├── CylinderCastWheelShape.cs │ │ ├── RaycastWheelShape.cs │ │ ├── Vehicle.cs │ │ ├── Wheel.cs │ │ ├── WheelBrake.cs │ │ ├── WheelDrivingMotor.cs │ │ ├── WheelFrictionBlender.cs │ │ ├── WheelShape.cs │ │ ├── WheelSlidingFriction.cs │ │ └── WheelSuspension.cs │ └── strongNameKey.snk ├── BEPUutilities/ │ ├── AffineTransform.cs │ ├── BEPUutilities.csproj │ ├── ConvexHullHelper.Pruning.cs │ ├── ConvexHullHelper.cs │ ├── DataStructures/ │ │ ├── ConcurrentDeque.cs │ │ ├── HashSet.cs │ │ ├── ObservableDictionary.cs │ │ ├── ObservableList.cs │ │ ├── RawList.cs │ │ ├── RawValueList.cs │ │ ├── ReadOnlyDictionary.cs │ │ ├── ReadOnlyEnumerable.cs │ │ ├── ReadOnlyList.cs │ │ ├── TinyList.cs │ │ └── TinyStructList.cs │ ├── MathChecker.cs │ ├── Matrix2x2.cs │ ├── Matrix2x3.cs │ ├── Matrix3x2.cs │ ├── Matrix3x3.cs │ ├── PermutationMapper.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── RayHit.cs │ ├── ResourceManagement/ │ │ ├── CommonResources.cs │ │ ├── LockingResourcePool.cs │ │ ├── ResourcePool.cs │ │ └── UnsafeResourcePool.cs │ ├── RigidTransform.cs │ ├── SpinLock.cs │ ├── Toolbox.cs │ ├── TriangleSidedness.cs │ ├── VoronoiRegion.cs │ └── strongNameKey.snk ├── ComponentBind/ │ ├── BaseMain.cs │ ├── Binding.cs │ ├── Command.cs │ ├── CommandBinding.cs │ ├── Component.cs │ ├── ComponentBind.csproj │ ├── Entity.cs │ ├── Factory.cs │ ├── Log.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── Property.cs │ └── Transform.cs ├── Dialogger/ │ ├── app.js │ ├── dialogger.js │ ├── index.css │ ├── index.html │ └── lib/ │ ├── joint.css │ ├── joint.js │ ├── joint.shapes.devs.js │ └── jquery.contextmenu.js ├── Excel/ │ ├── Build.Debug.CF.bat │ ├── Build.Release.CF.bat │ ├── Core/ │ │ ├── Binary12Format/ │ │ │ ├── Enums.cs │ │ │ ├── XlsbDimension.cs │ │ │ ├── XlsbNumFmt.cs │ │ │ ├── XlsbRecord.cs │ │ │ ├── XlsbSST.cs │ │ │ ├── XlsbStyles.cs │ │ │ ├── XlsbWorkbook.cs │ │ │ ├── XlsbWorksheet.cs │ │ │ └── XlsbXf.cs │ │ ├── BinaryFormat/ │ │ │ ├── Enums.cs │ │ │ ├── XlsBiffBOF.cs │ │ │ ├── XlsBiffBlankCell.cs │ │ │ ├── XlsBiffBoolErr.cs │ │ │ ├── XlsBiffBoundSheet.cs │ │ │ ├── XlsBiffContinue.cs │ │ │ ├── XlsBiffDbCell.cs │ │ │ ├── XlsBiffDimensions.cs │ │ │ ├── XlsBiffEOF.cs │ │ │ ├── XlsBiffFormatString.cs │ │ │ ├── XlsBiffFormulaCell.cs │ │ │ ├── XlsBiffFormulaString.cs │ │ │ ├── XlsBiffIndex.cs │ │ │ ├── XlsBiffIntegerCell.cs │ │ │ ├── XlsBiffInterfaceHdr.cs │ │ │ ├── XlsBiffLabelCell.cs │ │ │ ├── XlsBiffLabelSSTCell.cs │ │ │ ├── XlsBiffMulBlankCell.cs │ │ │ ├── XlsBiffMulRKCell.cs │ │ │ ├── XlsBiffNumberCell.cs │ │ │ ├── XlsBiffQuickTip.cs │ │ │ ├── XlsBiffRKCell.cs │ │ │ ├── XlsBiffRecord.cs │ │ │ ├── XlsBiffRow.cs │ │ │ ├── XlsBiffSST.cs │ │ │ ├── XlsBiffSimpleValueRecord.cs │ │ │ ├── XlsBiffStream.cs │ │ │ ├── XlsBiffUncalced.cs │ │ │ ├── XlsBiffWindow1.cs │ │ │ ├── XlsDirectoryEntry.cs │ │ │ ├── XlsFat.cs │ │ │ ├── XlsFormattedUnicodeString.cs │ │ │ ├── XlsHeader.cs │ │ │ ├── XlsRootDirectory.cs │ │ │ ├── XlsStream.cs │ │ │ ├── XlsWorkbookGlobals.cs │ │ │ └── XlsWorksheet.cs │ │ ├── FormatReader.cs │ │ ├── Helpers.cs │ │ ├── OpenXmlFormat/ │ │ │ ├── XlsxDimension.cs │ │ │ ├── XlsxNumFmt.cs │ │ │ ├── XlsxSST.cs │ │ │ ├── XlsxStyles.cs │ │ │ ├── XlsxWorkbook.cs │ │ │ ├── XlsxWorksheet.cs │ │ │ ├── XlsxXf.cs │ │ │ └── XmlReaderExtensions.cs │ │ ├── ReferenceHelper.cs │ │ └── ZipWorker.cs │ ├── Errors.cs │ ├── Excel.4.0.csproj │ ├── Excel.4.5.csproj │ ├── Excel.4.5.csproj.vspscc │ ├── Excel.csproj │ ├── Excel.csproj.vspscc │ ├── ExcelBinary12Reader.cs │ ├── ExcelBinaryReader.cs │ ├── ExcelDataReader.cs │ ├── ExcelFileType.cs │ ├── ExcelOpenXmlReader.cs │ ├── ExcelReaderFactory.cs │ ├── Exceptions/ │ │ ├── BiffRecordException.cs │ │ └── HeaderException.cs │ ├── Factory.cs │ ├── IExcelDataReader.cs │ ├── Log/ │ │ ├── ILog-4.5.cs │ │ ├── ILog.cs │ │ ├── Log.cs │ │ ├── LogExtensions.cs │ │ ├── LogManager.cs │ │ ├── Logger/ │ │ │ ├── NullLog-4.5.cs │ │ │ └── NullLog.cs │ │ └── StringExtensions.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ └── packages.config ├── Lemma/ │ ├── .gitignore │ ├── AdapterSelectorForm.Designer.cs │ ├── AdapterSelectorForm.cs │ ├── AdapterSelectorForm.resx │ ├── ComponentInterfaces.cs │ ├── Components/ │ │ ├── AI.cs │ │ ├── Agent.cs │ │ ├── AmbientSound.cs │ │ ├── Analytics.cs │ │ ├── AnimatedProp.cs │ │ ├── Animation.cs │ │ ├── Block.cs │ │ ├── BlockCloud.cs │ │ ├── Bouncer.cs │ │ ├── CameraStop.cs │ │ ├── Collectible.cs │ │ ├── Data.cs │ │ ├── DialogueFile.cs │ │ ├── EffectBlock.cs │ │ ├── EnemyBase.cs │ │ ├── Exploder.cs │ │ ├── Explosion.cs │ │ ├── FallingTower.cs │ │ ├── ImplodeBlock.cs │ │ ├── Joint.cs │ │ ├── Levitator.cs │ │ ├── MapExit.cs │ │ ├── Note.cs │ │ ├── PID.cs │ │ ├── ParticleWind.cs │ │ ├── PhysicsBlock.cs │ │ ├── PhysicsSphere.cs │ │ ├── PlayerCylinderTrigger.cs │ │ ├── PlayerData.cs │ │ ├── PlayerSpawn.cs │ │ ├── PlayerTrigger.cs │ │ ├── PostInitialization.cs │ │ ├── PowerBlockSocket.cs │ │ ├── Propagator.cs │ │ ├── Rain.cs │ │ ├── RaycastAI.cs │ │ ├── RaycastAIMovement.cs │ │ ├── Rift.cs │ │ ├── Rotator.cs │ │ ├── SceneryBlock.cs │ │ ├── Script.cs │ │ ├── Scriptlike/ │ │ │ ├── AnimatedSetter.cs │ │ │ ├── Binder.cs │ │ │ ├── ConsoleCommand.cs │ │ │ ├── Constant.cs │ │ │ ├── Counter.cs │ │ │ ├── LogicGate.cs │ │ │ ├── MessageDisplayer.cs │ │ │ ├── RandomTicker.cs │ │ │ ├── Scriptlike.cs │ │ │ ├── Sequence.cs │ │ │ ├── Setter.cs │ │ │ └── Ticker.cs │ │ ├── SignalTower.cs │ │ ├── Skybox.cs │ │ ├── Slider.cs │ │ ├── SliderCommon.cs │ │ ├── Smoke.cs │ │ ├── Snake.cs │ │ ├── Sound.cs │ │ ├── SoundBank.cs │ │ ├── SoundKiller.cs │ │ ├── Spawner.cs │ │ ├── Spinner.cs │ │ ├── Starter.cs │ │ ├── StaticSlider.cs │ │ ├── Switch.cs │ │ ├── TimeTrial.cs │ │ ├── TimeTrialUI.cs │ │ ├── Timer.cs │ │ ├── Trigger.cs │ │ ├── Turret.cs │ │ ├── Updater.cs │ │ ├── Voxel.cs │ │ ├── VoxelChaseAI.cs │ │ ├── VoxelFill.cs │ │ ├── VoxelSetter.cs │ │ ├── VoxelTrigger.cs │ │ ├── Water.cs │ │ ├── World.cs │ │ └── Zone.cs │ ├── Console/ │ │ ├── AutoConCommand.cs │ │ ├── AutoConVar.cs │ │ ├── ConCommand.cs │ │ ├── ConVar.cs │ │ ├── Console.cs │ │ └── ConsoleParser.cs │ ├── Content/ │ │ ├── AlphaModels/ │ │ │ ├── box.fbx │ │ │ ├── clouds.blend │ │ │ ├── clouds.fbx │ │ │ ├── cone.blend │ │ │ ├── cone.fbx │ │ │ ├── cylinder.blend │ │ │ ├── cylinder.fbx │ │ │ ├── distortion-box.fbx │ │ │ ├── light.blend │ │ │ ├── light.fbx │ │ │ ├── pyramid.blend │ │ │ ├── pyramid.fbx │ │ │ ├── selector.blend │ │ │ ├── selector.fbx │ │ │ ├── sphere.fbx │ │ │ ├── waterfall.blend │ │ │ └── waterfall.fbx │ │ ├── Effects/ │ │ │ ├── AdditiveClouds.fx │ │ │ ├── Animation.fx │ │ │ ├── AnimationCommon.fxh │ │ │ ├── AnimationNormalMap.fx │ │ │ ├── AnimationNormalMapCommon.fxh │ │ │ ├── AnimationUnlit.fx │ │ │ ├── ClipCommon.fxh │ │ │ ├── CloudCommon.fxh │ │ │ ├── Clouds.fx │ │ │ ├── Common.fxh │ │ │ ├── Default.fx │ │ │ ├── DefaultAlpha.fx │ │ │ ├── DefaultAlphaClip.fx │ │ │ ├── DefaultCommon.fxh │ │ │ ├── DefaultNormalMap.fx │ │ │ ├── DefaultSingleMaterial.fx │ │ │ ├── DefaultSolidColor.fx │ │ │ ├── DefaultSolidColorAlpha.fx │ │ │ ├── DistortionBox.fx │ │ │ ├── Environment.fx │ │ │ ├── EnvironmentBlock.fx │ │ │ ├── EnvironmentCommon.fxh │ │ │ ├── InstancedNormalMap.fx │ │ │ ├── InstancedSolidColor.fx │ │ │ ├── Lines.fx │ │ │ ├── Lines2D.fx │ │ │ ├── Oculus.fx │ │ │ ├── Particle.fx │ │ │ ├── ParticleCommon.fxh │ │ │ ├── ParticleFrameBufferDistortion.fx │ │ │ ├── ParticleOpaqueCommon.fxh │ │ │ ├── ParticleRain.fx │ │ │ ├── ParticleSnow.fx │ │ │ ├── ParticleVolume.fx │ │ │ ├── Player.fx │ │ │ ├── PostProcess/ │ │ │ │ ├── Bloom.fx │ │ │ │ ├── BloomCommon.fxh │ │ │ │ ├── Blur.fx │ │ │ │ ├── Clear.fx │ │ │ │ ├── Composite.fx │ │ │ │ ├── Downsample.fx │ │ │ │ ├── EffectCommon.fxh │ │ │ │ ├── EffectSamplers.fxh │ │ │ │ ├── Fog.fx │ │ │ │ ├── GlobalLight.fx │ │ │ │ ├── LightingCommon.fxh │ │ │ │ ├── MotionBlur.fx │ │ │ │ ├── PointLight.fx │ │ │ │ ├── SSAO.fx │ │ │ │ ├── Shadow2D.fxh │ │ │ │ └── SpotLight.fx │ │ │ ├── RenderCommon.fxh │ │ │ ├── RenderCommonAlpha.fxh │ │ │ ├── SkyDecal.fx │ │ │ ├── Skybox.fx │ │ │ ├── Unlit.fx │ │ │ ├── UnlitSolidColor.fx │ │ │ ├── VirtualUI.fx │ │ │ ├── Water.fx │ │ │ └── Waterfall.fx │ │ ├── EngineContent.XNA.contentproj │ │ ├── Font.spritefont │ │ ├── FontLarge.spritefont │ │ ├── FontLargeVR.spritefont │ │ ├── FontVR.spritefont │ │ ├── InstancedModels/ │ │ │ ├── block.blend │ │ │ ├── block.fbx │ │ │ └── position-model.fbx │ │ ├── InternalModels/ │ │ │ ├── pointlight.blend │ │ │ ├── pointlight.fbx │ │ │ ├── skybox.blend │ │ │ ├── skybox.fbx │ │ │ ├── spotlight.blend │ │ │ └── spotlight.fbx │ │ ├── Models/ │ │ │ ├── .gitignore │ │ │ ├── joan-firstperson.blend │ │ │ ├── joan-firstperson.fbx │ │ │ ├── joan.blend │ │ │ ├── joan.fbx │ │ │ ├── note.blend │ │ │ ├── note.fbx │ │ │ ├── papers.blend │ │ │ ├── papers.fbx │ │ │ ├── papers.xcf │ │ │ ├── phone.blend │ │ │ ├── phone.fbx │ │ │ ├── plane.blend │ │ │ ├── plane.fbx │ │ │ ├── sphere.blend │ │ │ └── sphere.fbx │ │ ├── Strings.xlsx │ │ ├── Textures/ │ │ │ ├── pattern1.xcf │ │ │ └── pattern2.xcf │ │ └── Wwise/ │ │ └── Windows/ │ │ ├── Init.bnk │ │ └── SFX_Bank_01.bnk │ ├── Editor/ │ │ ├── AnalyticsViewer.cs │ │ ├── Editor.cs │ │ └── EditorGeeUI.cs │ ├── ErrorForm.Designer.cs │ ├── ErrorForm.cs │ ├── ErrorForm.resx │ ├── Factories/ │ │ ├── AmbientLightFactory.cs │ │ ├── AmbientSoundFactory.cs │ │ ├── AnimatedPropFactory.cs │ │ ├── BinderFactory.cs │ │ ├── BlockFactory.cs │ │ ├── BouncerFactory.cs │ │ ├── CameraStopFactory.cs │ │ ├── CloudFactory.cs │ │ ├── CollectibleFactory.cs │ │ ├── ConsoleCommandFactory.cs │ │ ├── ConstantFactory.cs │ │ ├── CounterFactory.cs │ │ ├── DialogueFileFactory.cs │ │ ├── DirectionalLightFactory.cs │ │ ├── DustFactory.cs │ │ ├── EditorFactory.cs │ │ ├── EffectBlockFactory.cs │ │ ├── EmptyFactory.cs │ │ ├── EntityConnectable.cs │ │ ├── EvilBlocksFactory.cs │ │ ├── ExploderFactory.cs │ │ ├── ExplosionFactory.cs │ │ ├── FallingTowerFactory.cs │ │ ├── FileFilter.cs │ │ ├── ImplodeBlockFactory.cs │ │ ├── JointFactory.cs │ │ ├── LevitatorFactory.cs │ │ ├── LogicGateFactory.cs │ │ ├── LowerLimitFactory.cs │ │ ├── MapExitFactory.cs │ │ ├── MessageDisplayerFactory.cs │ │ ├── NoteFactory.cs │ │ ├── ParticleEmitterFactory.cs │ │ ├── PlayerDataFactory.cs │ │ ├── PlayerSpawnFactory.cs │ │ ├── PlayerTriggerFactory.cs │ │ ├── PointLightFactory.cs │ │ ├── PowerBlockSocketFactory.cs │ │ ├── PowerSoundFactory.cs │ │ ├── PropFactory.cs │ │ ├── RainFactory.cs │ │ ├── RandomTickerFactory.cs │ │ ├── RiftFactory.cs │ │ ├── RotatorFactory.cs │ │ ├── SceneryBlockFactory.cs │ │ ├── ScriptFactory.cs │ │ ├── SequenceFactory.cs │ │ ├── SetterFactory.cs │ │ ├── SignalTowerFactory.cs │ │ ├── SkyDecalFactory.cs │ │ ├── SkyboxFactory.cs │ │ ├── SliderFactory.cs │ │ ├── SmokeFactory.cs │ │ ├── SnakeFactory.cs │ │ ├── SnowFactory.cs │ │ ├── SoundBankFactory.cs │ │ ├── SoundFactory.cs │ │ ├── SpinnerFactory.cs │ │ ├── SplashOutTriggerFactory.cs │ │ ├── SpotLightFactory.cs │ │ ├── StarterFactory.cs │ │ ├── StaticSliderFactory.cs │ │ ├── SwitchFactory.cs │ │ ├── TargetFactory.cs │ │ ├── TickerFactory.cs │ │ ├── TimeTrialFactory.cs │ │ ├── TriggerFactory.cs │ │ ├── TurretFactory.cs │ │ ├── VoxelAttachable.cs │ │ ├── VoxelFactory.cs │ │ ├── VoxelFillFactory.cs │ │ ├── VoxelSetterFactory.cs │ │ ├── VoxelTriggerFactory.cs │ │ ├── WaterFactory.cs │ │ ├── WaterfallFactory.cs │ │ ├── WorldFactory.cs │ │ └── ZoneFactory.cs │ ├── GInterfaces/ │ │ ├── ConsoleUI.cs │ │ ├── TextPrompt.cs │ │ └── WorkShopInterface.cs │ ├── GeeUI/ │ │ ├── Composites/ │ │ │ └── ToolTip.cs │ │ ├── GeeUIMain.cs │ │ ├── Managers/ │ │ │ ├── ConversionManager.cs │ │ │ ├── DrawManager.cs │ │ │ └── InputManager.cs │ │ ├── Resource1.Designer.cs │ │ ├── Resource1.resx │ │ ├── Structs/ │ │ │ ├── BoundKey.cs │ │ │ ├── BoundMouse.cs │ │ │ ├── DropDownItem.cs │ │ │ ├── NinePatch.cs │ │ │ └── RadioButtonGroup.cs │ │ ├── ViewLayouts/ │ │ │ ├── BorderViewLayout.cs │ │ │ ├── ExpandToFitLayout.cs │ │ │ ├── HorizontalViewLayout.cs │ │ │ ├── SpinViewLayout.cs │ │ │ ├── VerticalViewLayout.cs │ │ │ └── ViewLayout.cs │ │ └── Views/ │ │ ├── ButtonView.cs │ │ ├── CheckBoxView.cs │ │ ├── DropDownView.cs │ │ ├── EmptyView.cs │ │ ├── ImageView.cs │ │ ├── ListView.cs │ │ ├── PanelView.cs │ │ ├── SliderView.cs │ │ ├── TabContainer.cs │ │ ├── TabHost.cs │ │ ├── TabView.cs │ │ ├── TextFieldView.cs │ │ ├── TextView.cs │ │ ├── View.cs │ │ └── WindowView.cs │ ├── Graphics/ │ │ ├── AmbientLight.cs │ │ ├── AnimatedModel.cs │ │ ├── Camera.cs │ │ ├── Cloud.cs │ │ ├── DirectionalLight.cs │ │ ├── DynamicModel.cs │ │ ├── FullscreenQuad.cs │ │ ├── LightingManager.cs │ │ ├── LineDrawer.cs │ │ ├── Model.cs │ │ ├── ModelInstance.cs │ │ ├── ParticleEmitter.cs │ │ ├── ParticleSystem.cs │ │ ├── PointLight.cs │ │ ├── Renderer.cs │ │ └── SpotLight.cs │ ├── IO/ │ │ ├── FPSInput.cs │ │ ├── MapLoader.cs │ │ └── PCInput.cs │ ├── LeaderboardProxy.cs │ ├── Lemma VR.lnk │ ├── Lemma.XNA.csproj │ ├── Main.cs │ ├── MainMenu.cs │ ├── Menu.cs │ ├── Oculus.cs │ ├── OvrCapi.cs │ ├── Player/ │ │ ├── AnimationController.cs │ │ ├── BlockPredictor.cs │ │ ├── CameraController.cs │ │ ├── Character.cs │ │ ├── FPSCamera.cs │ │ ├── FallDamage.cs │ │ ├── Footsteps.cs │ │ ├── Jump.cs │ │ ├── Phone.cs │ │ ├── PhoneNote.cs │ │ ├── Player.cs │ │ ├── PlayerFactory.cs │ │ ├── PlayerUI.cs │ │ ├── RollKickSlide.cs │ │ ├── RotationController.cs │ │ ├── Rumble.cs │ │ ├── TargetUI.cs │ │ ├── Vault.cs │ │ ├── VirtualReticle.cs │ │ ├── VoxelTools.cs │ │ └── WallRun.cs │ ├── Program.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── README.txt │ ├── Screenshot.cs │ ├── Scripts/ │ │ ├── Abilities.cs │ │ ├── DialogueLink.cs │ │ ├── DialogueTrigger.cs │ │ ├── RiftSpawner.cs │ │ └── SetPhoto.cs │ ├── Strings.cs │ ├── UI/ │ │ ├── Container.cs │ │ ├── LineDrawer2D.cs │ │ ├── ListContainer.cs │ │ ├── Scroller.cs │ │ ├── Sprite.cs │ │ ├── TextElement.cs │ │ ├── UIComponent.cs │ │ ├── UIFactory.cs │ │ └── UIRenderer.cs │ ├── Util/ │ │ ├── Algorithms.cs │ │ ├── AngleTools.cs │ │ ├── BSpline.cs │ │ ├── BitWorker.cs │ │ ├── BlockingQueue.cs │ │ ├── BoundingBoxExtensions.cs │ │ ├── CustomFluidVolume.cs │ │ ├── DialogueForest.cs │ │ ├── Direction.cs │ │ ├── LambdaComparer.cs │ │ ├── LargeObjectHeap.cs │ │ ├── Noise3D.cs │ │ ├── PriorityQueue.cs │ │ ├── RectangleExtensions.cs │ │ ├── ScriptBase.cs │ │ ├── SteamWorker.cs │ │ ├── VectorExtensions.cs │ │ ├── VoxelAStar.cs │ │ ├── VoxelRip.cs │ │ └── WwisePicker.cs │ ├── WwiseIDConverter.py │ ├── Wwise_IDs.cs │ ├── app.manifest │ ├── attribution.txt │ ├── build-demo.bat │ ├── build-nonsteam.bat │ ├── maps-compress.bat │ ├── maps-decompress.bat │ ├── maps-save.bat │ ├── steam_appid.txt │ ├── wwise-convert-ids.bat │ └── wwise-sync.bat ├── Lemma.XNA.sln ├── Newtonsoft.Json/ │ ├── Bson/ │ │ ├── BsonBinaryType.cs │ │ ├── BsonBinaryWriter.cs │ │ ├── BsonObjectId.cs │ │ ├── BsonReader.cs │ │ ├── BsonToken.cs │ │ ├── BsonType.cs │ │ └── BsonWriter.cs │ ├── ConstructorHandling.cs │ ├── Converters/ │ │ ├── BinaryConverter.cs │ │ ├── BsonObjectIdConverter.cs │ │ ├── CustomCreationConverter.cs │ │ ├── DataSetConverter.cs │ │ ├── DataTableConverter.cs │ │ ├── DateTimeConverterBase.cs │ │ ├── DiscriminatedUnionConverter.cs │ │ ├── EntityKeyMemberConverter.cs │ │ ├── ExpandoObjectConverter.cs │ │ ├── IsoDateTimeConverter.cs │ │ ├── JavaScriptDateTimeConverter.cs │ │ ├── JsonValueConverter.cs │ │ ├── KeyValuePairConverter.cs │ │ ├── RegexConverter.cs │ │ ├── StringEnumConverter.cs │ │ ├── VersionConverter.cs │ │ └── XmlNodeConverter.cs │ ├── DateFormatHandling.cs │ ├── DateParseHandling.cs │ ├── DateTimeZoneHandling.cs │ ├── DefaultValueHandling.cs │ ├── Dynamic.snk │ ├── FloatFormatHandling.cs │ ├── FloatParseHandling.cs │ ├── FormatterAssemblyStyle.cs │ ├── Formatting.cs │ ├── IJsonLineInfo.cs │ ├── JsonArrayAttribute.cs │ ├── JsonConstructorAttribute.cs │ ├── JsonContainerAttribute.cs │ ├── JsonConvert.cs │ ├── JsonConverter.cs │ ├── JsonConverterAttribute.cs │ ├── JsonConverterCollection.cs │ ├── JsonDictionaryAttribute.cs │ ├── JsonException.cs │ ├── JsonExtensionDataAttribute.cs │ ├── JsonIgnoreAttribute.cs │ ├── JsonObjectAttribute.cs │ ├── JsonPosition.cs │ ├── JsonPropertyAttribute.cs │ ├── JsonReader.cs │ ├── JsonReaderException.cs │ ├── JsonSerializationException.cs │ ├── JsonSerializer.cs │ ├── JsonSerializerSettings.cs │ ├── JsonTextReader.cs │ ├── JsonTextWriter.cs │ ├── JsonToken.cs │ ├── JsonValidatingReader.cs │ ├── JsonWriter.cs │ ├── JsonWriterException.cs │ ├── Linq/ │ │ ├── Extensions.cs │ │ ├── IJEnumerable.cs │ │ ├── JArray.cs │ │ ├── JConstructor.cs │ │ ├── JContainer.cs │ │ ├── JEnumerable.cs │ │ ├── JObject.cs │ │ ├── JProperty.cs │ │ ├── JPropertyDescriptor.cs │ │ ├── JPropertyKeyedCollection.cs │ │ ├── JRaw.cs │ │ ├── JToken.cs │ │ ├── JTokenEqualityComparer.cs │ │ ├── JTokenReader.cs │ │ ├── JTokenType.cs │ │ ├── JTokenWriter.cs │ │ ├── JValue.cs │ │ └── JsonPath/ │ │ ├── ArrayIndexFilter.cs │ │ ├── ArrayMultipleIndexFilter.cs │ │ ├── ArraySliceFilter.cs │ │ ├── FieldFilter.cs │ │ ├── FieldMultipleFilter.cs │ │ ├── JPath.cs │ │ ├── PathFilter.cs │ │ ├── QueryExpression.cs │ │ ├── QueryFilter.cs │ │ └── ScanFilter.cs │ ├── MemberSerialization.cs │ ├── MetadataPropertyHandling.cs │ ├── MissingMemberHandling.cs │ ├── Newtonsoft.Json.Net40.csproj │ ├── Newtonsoft.Json.ruleset │ ├── NullValueHandling.cs │ ├── ObjectCreationHandling.cs │ ├── PreserveReferencesHandling.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── ReferenceLoopHandling.cs │ ├── Required.cs │ ├── Schema/ │ │ ├── Extensions.cs │ │ ├── JsonSchema.cs │ │ ├── JsonSchemaBuilder.cs │ │ ├── JsonSchemaConstants.cs │ │ ├── JsonSchemaException.cs │ │ ├── JsonSchemaGenerator.cs │ │ ├── JsonSchemaModel.cs │ │ ├── JsonSchemaModelBuilder.cs │ │ ├── JsonSchemaNode.cs │ │ ├── JsonSchemaNodeCollection.cs │ │ ├── JsonSchemaResolver.cs │ │ ├── JsonSchemaType.cs │ │ ├── JsonSchemaWriter.cs │ │ ├── UndefinedSchemaIdHandling.cs │ │ ├── ValidationEventArgs.cs │ │ └── ValidationEventHandler.cs │ ├── Serialization/ │ │ ├── CachedAttributeGetter.cs │ │ ├── CamelCasePropertyNamesContractResolver.cs │ │ ├── DefaultContractResolver.cs │ │ ├── DefaultReferenceResolver.cs │ │ ├── DefaultSerializationBinder.cs │ │ ├── DiagnosticsTraceWriter.cs │ │ ├── DynamicValueProvider.cs │ │ ├── ErrorContext.cs │ │ ├── ErrorEventArgs.cs │ │ ├── ExpressionValueProvider.cs │ │ ├── IContractResolver.cs │ │ ├── IReferenceResolver.cs │ │ ├── ITraceWriter.cs │ │ ├── IValueProvider.cs │ │ ├── JsonArrayContract.cs │ │ ├── JsonContainerContract.cs │ │ ├── JsonContract.cs │ │ ├── JsonDictionaryContract.cs │ │ ├── JsonDynamicContract.cs │ │ ├── JsonFormatterConverter.cs │ │ ├── JsonISerializableContract.cs │ │ ├── JsonLinqContract.cs │ │ ├── JsonObjectContract.cs │ │ ├── JsonPrimitiveContract.cs │ │ ├── JsonProperty.cs │ │ ├── JsonPropertyCollection.cs │ │ ├── JsonSerializerInternalBase.cs │ │ ├── JsonSerializerInternalReader.cs │ │ ├── JsonSerializerInternalWriter.cs │ │ ├── JsonSerializerProxy.cs │ │ ├── JsonStringContract.cs │ │ ├── JsonTypeReflector.cs │ │ ├── LateBoundMetadataTypeAttribute.cs │ │ ├── MemoryTraceWriter.cs │ │ ├── ObjectConstructor.cs │ │ ├── OnErrorAttribute.cs │ │ ├── ReflectionValueProvider.cs │ │ ├── TraceJsonReader.cs │ │ └── TraceJsonWriter.cs │ ├── SerializationBinder.cs │ ├── StringEscapeHandling.cs │ ├── TraceLevel.cs │ ├── TypeNameHandling.cs │ ├── Utilities/ │ │ ├── Base64Encoder.cs │ │ ├── BidirectionalDictionary.cs │ │ ├── CollectionUtils.cs │ │ ├── CollectionWrapper.cs │ │ ├── ConvertUtils.cs │ │ ├── DateTimeParser.cs │ │ ├── DateTimeUtils.cs │ │ ├── DictionaryWrapper.cs │ │ ├── DynamicProxy.cs │ │ ├── DynamicProxyMetaObject.cs │ │ ├── DynamicReflectionDelegateFactory.cs │ │ ├── DynamicUtils.cs │ │ ├── DynamicWrapper.cs │ │ ├── EnumUtils.cs │ │ ├── EnumValue.cs │ │ ├── EnumValues.cs │ │ ├── ExpressionReflectionDelegateFactory.cs │ │ ├── FSharpUtils.cs │ │ ├── ILGeneratorExtensions.cs │ │ ├── ImmutableCollectionsUtils.cs │ │ ├── JavaScriptUtils.cs │ │ ├── LateBoundReflectionDelegateFactory.cs │ │ ├── LinqBridge.cs │ │ ├── MathUtils.cs │ │ ├── MethodCall.cs │ │ ├── MiscellaneousUtils.cs │ │ ├── ReflectionDelegateFactory.cs │ │ ├── ReflectionUtils.cs │ │ ├── StringBuffer.cs │ │ ├── StringReference.cs │ │ ├── StringUtils.cs │ │ ├── ThreadSafeStore.cs │ │ ├── TypeExtensions.cs │ │ └── ValidationUtils.cs │ └── WriteState.cs ├── PipelineExtensions/ │ ├── .gitignore │ ├── ByteImporter.cs │ ├── CubemapProcessor.cs │ ├── CustomModelProcessor.cs │ ├── PipelineExtensions.XNA.csproj │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── SkinnedModelImporter.cs │ ├── SkinnedModelProcessor.cs │ ├── TextImporter.cs │ ├── __init__.py │ └── export_fbx.py ├── README.md ├── SkinnedModel/ │ ├── .gitignore │ ├── Channel.cs │ ├── Clip.cs │ ├── Keyframe.cs │ ├── Properties/ │ │ └── AssemblyInfo.cs │ ├── SkinnedModel.csproj │ ├── SkinnedModelContent.csproj │ └── SkinningData.cs ├── Steam/ │ ├── app_build_300340.vdf │ ├── app_build_372580.vdf │ ├── build_demo.bat │ ├── depot_build_300341.vdf │ ├── depot_build_372581.vdf │ ├── depot_build_386000.vdf │ └── run_build.bat └── Wwise/ ├── AkAuxSend.cs ├── AkAuxSendAware.cs ├── AkBankPath.cs ├── AkGameObject.cs ├── AkGameObjectTracker.cs ├── AkGlobalSoundEngineInitializer.cs ├── AkGlobalSoundEngineTerminator.cs ├── AkInMemBankLoader.cs ├── AkListener.cs ├── Generated/ │ ├── Common/ │ │ ├── AKRESULT.cs │ │ ├── AkActionOnEventType.cs │ │ ├── AkArrayAllocatorAlignedSimd.cs │ │ ├── AkArrayAllocatorDefault.cs │ │ ├── AkAuxSendValue.cs │ │ ├── AkBankContent.cs │ │ ├── AkCallbackSerializer.cs │ │ ├── AkCallbackType.cs │ │ ├── AkChannelOrdering.cs │ │ ├── AkCurveInterpolation.cs │ │ ├── AkDeviceSettings.cs │ │ ├── AkExternalSourceInfo.cs │ │ ├── AkGroupType.cs │ │ ├── AkInitSettings.cs │ │ ├── AkListenerPosition.cs │ │ ├── AkMemSettings.cs │ │ ├── AkMusicPlaylistCallbackInfo.cs │ │ ├── AkMusicSettings.cs │ │ ├── AkNodeType.cs │ │ ├── AkObjectInfo.cs │ │ ├── AkOutputSettings.cs │ │ ├── AkPannerType.cs │ │ ├── AkPanningRule.cs │ │ ├── AkPlaylistArray.cs │ │ ├── AkPositionSourceType.cs │ │ ├── AkPositioningInfo.cs │ │ ├── AkSegmentInfo.cs │ │ ├── AkSoundPosition.cs │ │ ├── AkStreamMgrSettings.cs │ │ ├── AkVector.cs │ │ ├── AkVolumeOffset.cs │ │ ├── ArrayPoolDefault.cs │ │ ├── ArrayPoolLEngineDefault.cs │ │ ├── DynamicSequenceType.cs │ │ ├── EnvelopePoint.cs │ │ ├── ErrorCode.cs │ │ ├── ErrorLevel.cs │ │ ├── Iterator.cs │ │ ├── MultiPositionType.cs │ │ ├── Playlist.cs │ │ ├── PlaylistItem.cs │ │ ├── PreparationType.cs │ │ ├── RTPCValue_type.cs │ │ ├── WwiseObjectID.cs │ │ └── WwiseObjectIDext.cs │ └── Windows/ │ ├── AkMemPoolAttributes_Windows.cs │ ├── AkPlatformInitSettings_Windows.cs │ ├── AkSinkType_Windows.cs │ ├── AkSoundEnginePINVOKE_Windows.cs │ ├── AkSoundEngine_Windows.cs │ ├── AkSoundQuality_Windows.cs │ ├── AkSpeakerVolumes_Windows.cs │ └── AkThreadProperties_Windows.cs ├── Handwritten/ │ ├── AkAuxSendArray.cs │ ├── AkCallbackManager.cs │ └── AkPositionArray.cs ├── Properties/ │ └── AssemblyInfo.cs └── Wwise.csproj ================================================ FILE CONTENTS ================================================ ================================================ FILE: .gitignore ================================================ *.suo /Installer/Content *bin *obj *Debug *.opensdf *.sdf *.blend1 *.blend2 Thumbs.db /packages Steam/*.manifest Steam/*.manifest.txt Steam/*.log Steam/*.csd Steam/*.csm ================================================ FILE: .gitmodules ================================================ [submodule "Game"] path = Lemma/Game url = git@bitbucket.org:etodd/lemma-game-assets.git [submodule "LemmaAnalytics"] path = LemmaAnalytics url = git@bitbucket.org:etodd/lemmaanalytics.git [submodule "SharpZipLib"] path = SharpZipLib url = git@github.com:etodd/SharpZipLib.git [submodule "Steamworks.NET"] path = Steamworks.NET url = git@github.com:etodd/Steamworks.NET.git ignore = dirty ================================================ FILE: BEPUphysics/BEPUphysics.csproj ================================================  {2A9D2227-78D7-4804-B6D1-560BB43AE911} {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} Debug x86 Library Properties BEPUphysics BEPUphysics v4.0 Client v4.0 Windows HiDef 7370a280-2dc9-49be-8ea9-7b6a817e142c Library true full false bin\x86\Debug TRACE;DEBUG;WINDOWS;ALLOWUNSAFE prompt 4 true false x86 false true pdbonly true bin\x86\Release WINDOWS;ALLOWUNSAFE prompt 4 true false x86 true true bin\x86\Release\BEPUphysics.XML true true strongNameKey.snk False False False False False False False False False False False False 4.0 False False {E3AAEB61-D7DF-4E7E-A75B-B5282D2FF3F5} BEPUutilities ================================================ FILE: BEPUphysics/BroadPhaseEntries/BroadPhaseEntry.cs ================================================ using System; using BEPUphysics.BroadPhaseSystems; using Microsoft.Xna.Framework; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; namespace BEPUphysics.BroadPhaseEntries { /// /// Superclass of all objects which live inside the broad phase. /// The BroadPhase will generate pairs between BroadPhaseEntries. /// public abstract class BroadPhaseEntry : IBoundingBoxOwner, ICollisionRulesOwner { internal int hashCode; protected BroadPhaseEntry() { CollisionRules = new CollisionRules(); collisionRulesUpdatedDelegate = CollisionRulesUpdated; hashCode = (int)(base.GetHashCode() * 0xd8163841); } /// /// Gets the broad phase to which this broad phase entry belongs. /// public BroadPhase BroadPhase { get; internal set; } /// /// Gets the object's hash code. /// /// Hash code for the object. public override int GetHashCode() { return hashCode; } private Action collisionRulesUpdatedDelegate; protected abstract void CollisionRulesUpdated(); protected internal BoundingBox boundingBox; /// /// Gets or sets the bounding box of the entry. /// public BoundingBox BoundingBox { get { return boundingBox; } set { boundingBox = value; } } protected internal abstract bool IsActive { get; } internal CollisionRules collisionRules; /// /// Gets the entry's collision rules. /// public CollisionRules CollisionRules { get { return collisionRules; } set { if (collisionRules != value) { if (collisionRules != null) collisionRules.CollisionRulesChanged -= collisionRulesUpdatedDelegate; collisionRules = value; if (collisionRules != null) collisionRules.CollisionRulesChanged += collisionRulesUpdatedDelegate; CollisionRulesUpdated(); } } } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public abstract bool RayCast(Ray ray, float maximumLength, out RayHit rayHit); /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public virtual bool RayCast(Ray ray, float maximumLength, Func filter, out RayHit rayHit) { if (filter(this)) return RayCast(ray, maximumLength, out rayHit); rayHit = new RayHit(); return false; } /// /// Sweeps a convex shape against the entry. /// /// Swept shape. /// Beginning location and orientation of the cast shape. /// Sweep motion to apply to the cast shape. /// Hit data of the cast on the entry, if any. /// Whether or not the cast hit the entry. public abstract bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit); /// /// Sweeps a convex shape against the entry. /// /// Swept shape. /// Beginning location and orientation of the cast shape. /// Sweep motion to apply to the cast shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data of the cast on the entry, if any. /// Whether or not the cast hit the entry. public virtual bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayHit hit) { if (filter(this)) return ConvexCast(castShape, ref startingTransform, ref sweep, out hit); hit = new RayHit(); return false; } /// /// Updates the bounding box to the current state of the entry. /// public abstract void UpdateBoundingBox(); /// /// Gets or sets the user data associated with this entry. /// public object Tag { get; set; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Collidable.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.CollisionShapes; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.CollisionRuleManagement; using System; using BEPUutilities.DataStructures; namespace BEPUphysics.BroadPhaseEntries { /// /// Superclass of objects living in the collision detection pipeline /// that can result in contacts. /// public abstract class Collidable : BroadPhaseEntry { protected Collidable() { shapeChangedDelegate = OnShapeChanged; } internal CollisionShape shape; //Having this non-private allows for some very special-casey stuff; see TriangleShape initialization. /// /// Gets the shape used by the collidable. /// public CollisionShape Shape { get { return shape; } protected set { if (shape != null) shape.ShapeChanged -= shapeChangedDelegate; shape = value; if (shape != null) shape.ShapeChanged += shapeChangedDelegate; OnShapeChanged(shape); //TODO: Watch out for unwanted references in the delegate lists. } } protected internal abstract IContactEventTriggerer EventTriggerer { get; } /// /// Gets or sets whether or not to ignore shape changes. When true, changing the collision shape will not force the collidable to perform any updates. /// public bool IgnoreShapeChanges { get; set; } Action shapeChangedDelegate; protected virtual void OnShapeChanged(CollisionShape collisionShape) { } internal RawList pairs = new RawList(); /// /// Gets the list of pairs associated with the collidable. /// These pairs are found by the broad phase and are managed by the narrow phase; /// they can contain other collidables, entities, and contacts. /// public ReadOnlyList Pairs { get { return new ReadOnlyList(pairs); } } /// /// Gets a list of all other collidables that this collidable overlaps. /// public CollidableCollection OverlappedCollidables { get { return new CollidableCollection(this); } } protected override void CollisionRulesUpdated() { for (int i = 0; i < pairs.Count; i++) { pairs[i].CollisionRule = CollisionRules.CollisionRuleCalculator(pairs[i].BroadPhaseOverlap.entryA, pairs[i].BroadPhaseOverlap.entryB); } } internal void AddPair(CollidablePairHandler pair, ref int index) { index = pairs.Count; pairs.Add(pair); } internal void RemovePair(CollidablePairHandler pair, ref int index) { if (pairs.Count > index) { pairs.FastRemoveAt(index); if (pairs.Count > index) { var endPair = pairs.Elements[index]; if (endPair.CollidableA == this) endPair.listIndexA = index; else endPair.listIndexB = index; } } index = -1; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/CollidableCollection.cs ================================================ using System; using System.Collections.Generic; namespace BEPUphysics.BroadPhaseEntries { /// /// List of collidable objects overlapping another collidable. /// public struct CollidableCollection : IList { /// /// Enumerator for the CollidableCollection. /// public struct Enumerator : IEnumerator { CollidableCollection collection; int index; /// /// Constructs an enumerator. /// ///Collection to which the enumerator belongs. public Enumerator(CollidableCollection collection) { this.collection = collection; index = -1; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public Collidable Current { get { return collection[index]; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. /// T:System.InvalidOperationException /// 2 public bool MoveNext() { return ++index < collection.Count; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. /// T:System.InvalidOperationException /// 2 public void Reset() { index = -1; } } /// /// Constructs a new CollidableCollection. /// ///The collidable to which the collection belongs. public CollidableCollection(Collidable owner) { this.owner = owner; } internal Collidable owner; /// /// Gets an enumerator which can be used to enumerate over the list. /// ///Enumerator for the collection. public Enumerator GetEnumerator() { return new Enumerator(this); } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(this); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(this); } /// /// Determines the index of a specific item in the /// T:System.Collections.Generic.IList`1 /// . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the /// T:System.Collections.Generic.IList`1 /// . public int IndexOf(Collidable item) { for (int i = 0; i < Count; i++) { if (item == this[i]) return i; } return -1; } /// /// Gets or sets the element at the specified index. /// /// /// The element at the specified index. /// /// The zero-based index of the element to get or set. /// T:System.ArgumentOutOfRangeException /// is not a valid index in the /// T:System.Collections.Generic.IList`1 /// /// .The property is set and the /// T:System.NotSupportedException /// /// T:System.Collections.Generic.IList`1 /// /// is read-only. public Collidable this[int index] { get { //It's guaranteed to be a CollisionInformation, because it's a member of a CollidablePairHandler. return (Collidable)(owner.pairs[index].broadPhaseOverlap.entryA == owner ? owner.pairs[index].broadPhaseOverlap.entryB : owner.pairs[index].broadPhaseOverlap.entryA); } set { throw new NotSupportedException(); } } /// /// Determines whether the /// T:System.Collections.Generic.ICollection`1 /// contains a specific value. /// /// /// true if is found in the /// T:System.Collections.Generic.ICollection`1 /// ; otherwise, false. /// /// The object to locate in the /// T:System.Collections.Generic.ICollection`1 /// . public bool Contains(Collidable item) { for (int i = 0; i < Count; i++) { if (item == this[i]) return true; } return false; } /// /// Copies the elements of the /// T:System.Collections.Generic.ICollection`1 /// to an /// T:System.Array /// , starting at a particular /// T:System.Array /// index. /// /// The one-dimensional /// T:System.Array /// that is the destination of the elements copied from /// T:System.Collections.Generic.ICollection`1 /// . The /// T:System.Array /// must have zero-based indexing.The zero-based index in at which copying begins. is null. is less than 0. is multidimensional.-or-The number of elements in the source is greater than the available space from to the end of the destination .-or-Type cannot be cast automatically to the type of the destination . public void CopyTo(Collidable[] array, int arrayIndex) { for (int i = 0; i < Count; i++) { array[arrayIndex + i] = this[i]; } } /// /// Gets the number of elements contained in the /// T:System.Collections.Generic.ICollection`1 /// . /// /// /// The number of elements contained in the /// T:System.Collections.Generic.ICollection`1 /// . /// public int Count { get { return owner.pairs.Count; } } bool ICollection.IsReadOnly { get { return true; } } bool ICollection.Remove(Collidable item) { throw new NotSupportedException(); } void ICollection.Add(Collidable item) { throw new NotSupportedException(); } void ICollection.Clear() { throw new NotSupportedException(); } void IList.Insert(int index, Collidable item) { throw new NotSupportedException(); } void IList.RemoveAt(int index) { throw new NotSupportedException(); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/CollidablePair.cs ================================================ using System; namespace BEPUphysics.BroadPhaseEntries { /// /// Pair of collidables. /// public struct CollidablePair : IEquatable { internal Collidable collidableA; /// /// First collidable in the pair. /// public Collidable CollidableA { get { return collidableA; } } internal Collidable collidableB; /// /// Second collidable in the pair. /// public Collidable CollidableB { get { return collidableB; } } /// /// Constructs a new collidable pair. /// ///First collidable in the pair. ///Second collidable in the pair. public CollidablePair(Collidable collidableA, Collidable collidableB) { this.collidableA = collidableA; this.collidableB = collidableB; } /// /// Returns the hash code for this instance. /// /// /// A 32-bit signed integer that is the hash code for this instance. /// /// 2 public override int GetHashCode() { //TODO: Use old prime-based system? return collidableA.GetHashCode() + collidableB.GetHashCode(); } #region IEquatable Members /// /// Indicates whether the current object is equal to another object of the same type. /// /// /// true if the current object is equal to the parameter; otherwise, false. /// /// An object to compare with this object. public bool Equals(CollidablePair other) { return (other.collidableA == collidableA && other.collidableB == collidableB) || (other.collidableA == collidableB && other.collidableB == collidableA); } #endregion } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/DetectorVolume.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.DataStructures; using BEPUutilities.DataStructures; using BEPUphysics.Entities; using BEPUphysics.OtherSpaceStages; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.NarrowPhaseSystems.Pairs; using RigidTransform = BEPUutilities.RigidTransform; namespace BEPUphysics.BroadPhaseEntries { /// /// Stores flags regarding an object's degree of inclusion in a volume. /// public struct ContainmentState { /// /// Whether or not the object is fully contained. /// public bool IsContained; /// /// Whether or not the object is partially or fully contained. /// public bool IsTouching; /// /// Whether or not the entity associated with this state has been refreshed during the last update. /// internal bool StaleState; /// /// Constructs a new ContainmentState. /// /// Whether or not the object is partially or fully contained. /// Whether or not the object is fully contained. public ContainmentState(bool touching, bool contained) { IsTouching = touching; IsContained = contained; StaleState = false; } /// /// Constructs a new ContainmentState. /// /// Whether or not the object is partially or fully contained. /// Whether or not the object is fully contained. /// Whether or not the entity associated with this state has been refreshed in the previous update. internal ContainmentState(bool touching, bool contained, bool stale) { IsTouching = touching; IsContained = contained; StaleState = stale; } } /// /// Manages the detection of entities within an arbitrary closed triangle mesh. /// public class DetectorVolume : BroadPhaseEntry, ISpaceObject, IDeferredEventCreator { internal Dictionary pairs = new Dictionary(); /// /// Gets the list of pairs associated with the detector volume. /// public ReadOnlyDictionary Pairs { get { return new ReadOnlyDictionary(pairs); } } TriangleMesh triangleMesh; /// /// Gets or sets the triangle mesh data and acceleration structure. Must be a closed mesh with consistent winding. /// public TriangleMesh TriangleMesh { get { return triangleMesh; } set { triangleMesh = value; UpdateBoundingBox(); Reinitialize(); } } /// /// Creates a detector volume. /// /// Closed and consistently wound mesh defining the volume. public DetectorVolume(TriangleMesh triangleMesh) { TriangleMesh = triangleMesh; UpdateBoundingBox(); } /// /// Fires when an entity comes into contact with the volume. /// public event EntityBeginsTouchingVolumeEventHandler EntityBeganTouching; /// /// Fires when an entity ceases to intersect the volume. /// public event EntityStopsTouchingVolumeEventHandler EntityStoppedTouching; /// /// Fires when an entity becomes fully engulfed by a volume. /// public event VolumeBeginsContainingEntityEventHandler VolumeBeganContainingEntity; /// /// Fires when an entity ceases to be fully engulfed by a volume. /// public event VolumeStopsContainingEntityEventHandler VolumeStoppedContainingEntity; private ISpace space; ISpace ISpaceObject.Space { get { return space; } set { space = value; } } /// /// Space that owns the detector volume. /// public ISpace Space { get { return space; } } private bool innerFacingIsClockwise; /// /// Determines if a point is contained by the detector volume. /// /// Point to check for containment. /// Whether or not the point is contained by the detector volume. public bool IsPointContained(Vector3 point) { var triangles = CommonResources.GetIntList(); bool contained = IsPointContained(ref point, triangles); CommonResources.GiveBack(triangles); return contained; } internal bool IsPointContained(ref Vector3 point, RawList triangles) { Vector3 rayDirection; //Point from the approximate center of the mesh outwards. //This is a cheap way to reduce the number of unnecessary checks when objects are external to the mesh. Vector3.Add(ref boundingBox.Max, ref boundingBox.Min, out rayDirection); Vector3.Multiply(ref rayDirection, .5f, out rayDirection); Vector3.Subtract(ref point, ref rayDirection, out rayDirection); //If the point is right in the middle, we'll need a backup. if (rayDirection.LengthSquared() < .01f) rayDirection = Vector3.Up; var ray = new Ray(point, rayDirection); triangleMesh.Tree.GetOverlaps(ray, triangles); float minimumT = float.MaxValue; bool minimumIsClockwise = false; for (int i = 0; i < triangles.Count; i++) { Vector3 a, b, c; triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c); RayHit hit; bool hitClockwise; if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit)) { if (hit.T < minimumT) { minimumT = hit.T; minimumIsClockwise = hitClockwise; } } } triangles.Clear(); //If the first hit is on the inner surface, then the ray started inside the mesh. return minimumT < float.MaxValue && minimumIsClockwise == innerFacingIsClockwise; } protected override void CollisionRulesUpdated() { foreach (var pair in pairs.Values) pair.CollisionRule = CollisionRules.CollisionRuleCalculator(pair.BroadPhaseOverlap.entryA, pair.BroadPhaseOverlap.entryB); } protected internal override bool IsActive { get { return false; } } public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return triangleMesh.RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, out rayHit); } public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (triangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { triangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; } /// /// Sets the bounding box of the detector volume to the current hierarchy root bounding box. This is called automatically if the TriangleMesh property is set. /// public override void UpdateBoundingBox() { boundingBox = triangleMesh.Tree.BoundingBox; } /// /// Updates the detector volume's interpretation of the mesh. This should be called when the the TriangleMesh is changed significantly. This is called automatically if the TriangleMesh property is set. /// public void Reinitialize() { //Pick a point that is known to be outside the mesh as the origin. Vector3 origin = (triangleMesh.Tree.BoundingBox.Max - triangleMesh.Tree.BoundingBox.Min) * 1.5f + triangleMesh.Tree.BoundingBox.Min; //Pick a direction which will definitely hit the mesh. Vector3 a, b, c; triangleMesh.Data.GetTriangle(0, out a, out b, out c); var direction = (a + b + c) / 3 - origin; var ray = new Ray(origin, direction); var triangles = CommonResources.GetIntList(); triangleMesh.Tree.GetOverlaps(ray, triangles); float minimumT = float.MaxValue; for (int i = 0; i < triangles.Count; i++) { triangleMesh.Data.GetTriangle(triangles.Elements[i], out a, out b, out c); RayHit hit; bool hitClockwise; if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref a, ref b, ref c, out hitClockwise, out hit)) { if (hit.T < minimumT) { minimumT = hit.T; innerFacingIsClockwise = !hitClockwise; } } } CommonResources.GiveBack(triangles); } void ISpaceObject.OnAdditionToSpace(ISpace newSpace) { } void ISpaceObject.OnRemovalFromSpace(ISpace oldSpace) { } /// /// Used to protect against containment changes coming in from multithreaded narrowphase contexts. /// SpinLock locker = new SpinLock(); struct ContainmentChange { public Entity Entity; public ContainmentChangeType Change; } enum ContainmentChangeType : byte { BeganTouching, StoppedTouching, BeganContaining, StoppedContaining } private Queue containmentChanges = new Queue(); internal void BeganTouching(DetectorVolumePairHandler pair) { locker.Enter(); containmentChanges.Enqueue(new ContainmentChange { Change = ContainmentChangeType.BeganTouching, Entity = pair.Collidable.entity }); locker.Exit(); } internal void StoppedTouching(DetectorVolumePairHandler pair) { locker.Enter(); containmentChanges.Enqueue(new ContainmentChange { Change = ContainmentChangeType.StoppedTouching, Entity = pair.Collidable.entity }); locker.Exit(); } internal void BeganContaining(DetectorVolumePairHandler pair) { locker.Enter(); containmentChanges.Enqueue(new ContainmentChange { Change = ContainmentChangeType.BeganContaining, Entity = pair.Collidable.entity }); locker.Exit(); } internal void StoppedContaining(DetectorVolumePairHandler pair) { locker.Enter(); containmentChanges.Enqueue(new ContainmentChange { Change = ContainmentChangeType.StoppedContaining, Entity = pair.Collidable.entity }); locker.Exit(); } DeferredEventDispatcher IDeferredEventCreator.DeferredEventDispatcher { get; set; } bool IDeferredEventCreator.IsActive { get { return true; } set { throw new NotSupportedException("Detector volumes are always active deferred event generators."); } } void IDeferredEventCreator.DispatchEvents() { while (containmentChanges.Count > 0) { var change = containmentChanges.Dequeue(); switch (change.Change) { case ContainmentChangeType.BeganTouching: if (EntityBeganTouching != null) EntityBeganTouching(this, change.Entity); break; case ContainmentChangeType.StoppedTouching: if (EntityStoppedTouching != null) EntityStoppedTouching(this, change.Entity); break; case ContainmentChangeType.BeganContaining: if (VolumeBeganContainingEntity != null) VolumeBeganContainingEntity(this, change.Entity); break; case ContainmentChangeType.StoppedContaining: if (VolumeStoppedContainingEntity != null) VolumeStoppedContainingEntity(this, change.Entity); break; } } } int IDeferredEventCreator.ChildDeferredEventCreators { get { return 0; } set { throw new NotSupportedException("The detector volume does not allow child deferred event creators."); } } } /// /// Handles any special logic to perform when an entry begins touching a detector volume. /// Runs within an update loop for updateables; modifying the updateable listing during the event is disallowed. /// /// DetectorVolume being touched. /// Entry touching the volume. public delegate void EntityBeginsTouchingVolumeEventHandler(DetectorVolume volume, Entity toucher); /// /// Handles any special logic to perform when an entry stops touching a detector volume. /// Runs within an update loop for updateables; modifying the updateable listing during the event is disallowed. /// /// DetectorVolume no longer being touched. /// Entry no longer touching the volume. public delegate void EntityStopsTouchingVolumeEventHandler(DetectorVolume volume, Entity toucher); /// /// Handles any special logic to perform when an entity begins being contained by a detector volume. /// Runs within an update loop for updateables; modifying the updateable listing during the event is disallowed. /// /// DetectorVolume containing the entry. /// Entity contained by the volume. public delegate void VolumeBeginsContainingEntityEventHandler(DetectorVolume volume, Entity entity); /// /// Handles any special logic to perform when an entry stops being contained by a detector volume. /// Runs within an update loop for updateables; modifying the updateable listing during the event is disallowed. /// /// DetectorVolume no longer containing the entry. /// Entity no longer contained by the volume. public delegate void VolumeStopsContainingEntityEventHandler(DetectorVolume volume, Entity entity); } ================================================ FILE: BEPUphysics/BroadPhaseEntries/EntityCollidableCollection.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Entities; namespace BEPUphysics.BroadPhaseEntries { /// /// Enumerable collection of entities associated with a collidable. /// public struct EntityCollidableCollection : IEnumerable { /// /// Enumerator for the EntityCollidableCollection. /// public struct Enumerator : IEnumerator { EntityCollidableCollection collection; EntityCollidable current; int index; /// /// Constructs a new enumerator. /// ///Owning collection. public Enumerator(EntityCollidableCollection collection) { this.collection = collection; index = -1; current = null; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public Entity Current { get { return current.entity; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. /// T:System.InvalidOperationException /// 2 public bool MoveNext() { while (++index < collection.owner.pairs.Count) { if ((current = (collection.owner.pairs[index].broadPhaseOverlap.entryA == collection.owner ? collection.owner.pairs[index].broadPhaseOverlap.entryB : collection.owner.pairs[index].broadPhaseOverlap.entryA) as EntityCollidable) != null) return true; } return false; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. /// T:System.InvalidOperationException /// 2 public void Reset() { index = -1; } } /// /// Constructs a new EntityCollidableCollection. /// ///Owner of the collection. public EntityCollidableCollection(EntityCollidable owner) { this.owner = owner; } internal EntityCollidable owner; /// /// Gets an enumerator over the entities in the collection. /// ///Enumerator over the entities in the collection. public Enumerator GetEnumerator() { return new Enumerator(this); } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(this); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(this); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/CollisionEventTypes.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.CollisionTests; using BEPUphysics.NarrowPhaseSystems.Pairs; namespace BEPUphysics.BroadPhaseEntries.Events { //TODO: Contravariance isn't supported on all platforms... /// /// Handles any special logic when two objects' bounding boxes overlap as determined by the broadphase system. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. public delegate void PairCreatedEventHandler(T sender, BroadPhaseEntry other, NarrowPhasePair pair); /// /// Handles any special logic when two objects' bounding boxes overlap as determined by the broadphase system. /// Unlike PairCreatedEventHandler, this will be called as soon as a pair is created instead of at the end of the frame. /// This allows the pair's data to be adjusted prior to any usage, but some actions are not supported due to the execution stage. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. public delegate void CreatingPairEventHandler(T sender, BroadPhaseEntry other, NarrowPhasePair pair); /// /// Handles any special logic when two objects' bounding boxes cease to overlap as determined by the broadphase system. /// /// Entry sending the event. /// The entry formerly interacting with the sender via the deleted pair. public delegate void PairRemovedEventHandler(T sender, BroadPhaseEntry other); /// /// Handles any special logic when two objects' bounding boxes cease to overlap as determined by the broadphase system. /// Unlike PairRemovedEventHandler, this will trigger at the time of pair removal instead of at the end of the space's update. /// /// Entry sending the event. /// The entry formerly interacting with the sender via the deleted pair. public delegate void RemovingPairEventHandler(T sender, BroadPhaseEntry other); /// /// Handles any special logic when two bodies are touching and generate a contact point. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. /// Created contact data. public delegate void ContactCreatedEventHandler(T sender, Collidable other, CollidablePairHandler pair, ContactData contact); /// /// Handles any special logic when two bodies are touching and generate a contact point. /// Unlike ContactCreatedEventHandler, this will trigger at the time of contact generation instead of at the end of the space's update. /// This allows the contact's data to be adjusted prior to usage in the velocity solver, /// but other actions such as altering the owning space's pair or entry listings are unsafe. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. /// Newly generated contact point between the pair's two bodies. /// This reference cannot be safely kept outside of the scope of the handler; contacts can quickly return to the resource pool. public delegate void CreatingContactEventHandler(T sender, Collidable other, CollidablePairHandler pair, Contact contact); /// /// Handles any special logic when two bodies initally collide and generate a contact point. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. public delegate void InitialCollisionDetectedEventHandler(T sender, Collidable other, CollidablePairHandler pair); /// /// Handles any special logic when two bodies initally collide and generate a contact point. /// Unlike InitialCollisionDetectedEventHandler, this will trigger at the time of contact creation instead of at the end of the space's update. /// Performing operations outside of the scope of the pair is unsafe. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. public delegate void DetectingInitialCollisionEventHandler(T sender, Collidable other, CollidablePairHandler pair); /// /// Handles any special logic when a contact point between two bodies is removed. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies and data about the removed contact. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. /// Removed contact data. public delegate void ContactRemovedEventHandler(T sender, Collidable other, CollidablePairHandler pair, ContactData contact); /// /// Handles any special logic when a contact point between two bodies is removed. /// Unlike ContactRemovedEventHandler, this will trigger at the time of contact removal instead of at the end of the space's update. /// Performing operations outside of the scope of the controller is unsafe. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies and data about the removed contact. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. /// Contact between the two entries. This reference cannot be safely kept outside of the scope of the handler; /// it will be immediately returned to the resource pool after the event handler completes. public delegate void RemovingContactEventHandler(T sender, Collidable other, CollidablePairHandler pair, Contact contact); /// /// Handles any special logic when two bodies go from a touching state to a separated state. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair overseeing the collision. Note that this instance may be invalid if the entries' bounding boxes no longer overlap. public delegate void CollisionEndedEventHandler(T sender, Collidable other, CollidablePairHandler pair); /// /// Handles any special logic when two bodies go from a touching state to a separated state. /// Unlike CollisionEndedEventHandler, this will trigger at the time of contact removal instead of at the end of the space's update. /// Performing operations outside of the scope of the controller is unsafe. /// /// Entry sending the event. /// Other entry within the pair opposing the monitored entry. /// Pair presiding over the interaction of the two involved bodies. /// This reference cannot be safely kept outside of the scope of the handler; pairs can quickly return to the resource pool. public delegate void CollisionEndingEventHandler(T sender, Collidable other, CollidablePairHandler pair); /// /// Handles any special logic to perform at the end of a pair's UpdateContactManifold method. /// This is called every single update regardless if the process was quit early or did not complete due to interaction rules. /// /// Entry involved in the pair monitored for events. /// Other entry within the pair opposing the monitored entry. /// Pair that was updated. public delegate void PairUpdatedEventHandler(T sender, BroadPhaseEntry other, NarrowPhasePair pair); /// /// Handles any special logic to perform at the end of a pair's UpdateContactManifold method. /// This is called every single update regardless if the process was quit early or did not complete due to interaction rules. /// Unlike PairUpdatedEventHandler, this is called at the time of the collision detection update rather than at the end of the space's update. /// Other entries' information may not be up to date, and operations acting on data outside of the character controller may be unsafe. /// /// Entry involved in the pair monitored for events. /// Other entry within the pair opposing the monitored entry. /// Pair that was updated. public delegate void PairUpdatingEventHandler(T sender, BroadPhaseEntry other, NarrowPhasePair pair); /// /// Handles any special logic to perform at the end of a pair's UpdateContactManifold method if the two objects are colliding. /// This is called every single update regardless if the process was quit early or did not complete due to interaction rules. /// /// Entry involved in the pair monitored for events. /// Other entry within the pair opposing the monitored entry. /// Pair that was updated. public delegate void PairTouchedEventHandler(T sender, Collidable other, CollidablePairHandler pair); /// /// Handles any special logic to perform at the end of a pair's UpdateContactManifold method if the two objects are colliding. /// This is called every single update regardless if the process was quit early or did not complete due to interaction rules. /// Unlike PairTouchedEventHandler, this is called at the time of the collision detection update rather than at the end of the space's update. /// Other entries' information may not be up to date, and operations acting on data outside of the character controller may be unsafe. /// /// Entry involved in the pair monitored for events. /// Other entry within the pair opposing the monitored entry. /// Pair that was updated. public delegate void PairTouchingEventHandler(T sender, Collidable other, CollidablePairHandler pair); //Storage for deferred event dispatching internal struct EventStoragePairCreated { internal NarrowPhasePair pair; internal BroadPhaseEntry other; internal EventStoragePairCreated(BroadPhaseEntry other, NarrowPhasePair pair) { this.other = other; this.pair = pair; } } internal struct EventStoragePairRemoved { internal BroadPhaseEntry other; internal EventStoragePairRemoved(BroadPhaseEntry other) { this.other = other; } } internal struct EventStorageContactCreated { internal CollidablePairHandler pair; internal ContactData contactData; internal Collidable other; internal EventStorageContactCreated(Collidable other, CollidablePairHandler pair, ref ContactData contactData) { this.other = other; this.pair = pair; this.contactData = contactData; } } internal struct EventStorageInitialCollisionDetected { internal CollidablePairHandler pair; internal Collidable other; internal EventStorageInitialCollisionDetected(Collidable other, CollidablePairHandler pair) { this.pair = pair; this.other = other; } } internal struct EventStorageContactRemoved { internal CollidablePairHandler pair; internal ContactData contactData; internal Collidable other; internal EventStorageContactRemoved(Collidable other, CollidablePairHandler pair, ref ContactData contactData) { this.other = other; this.pair = pair; this.contactData = contactData; } } internal struct EventStorageCollisionEnded { internal CollidablePairHandler pair; internal Collidable other; internal EventStorageCollisionEnded(Collidable other, CollidablePairHandler pair) { this.other = other; this.pair = pair; } } internal struct EventStoragePairUpdated { internal NarrowPhasePair pair; internal BroadPhaseEntry other; internal EventStoragePairUpdated(BroadPhaseEntry other, NarrowPhasePair pair) { this.other = other; this.pair = pair; } } internal struct EventStoragePairTouched { internal CollidablePairHandler pair; internal Collidable other; internal EventStoragePairTouched(Collidable other, CollidablePairHandler pair) { this.other = other; this.pair = pair; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/CompoundEventManager.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.OtherSpaceStages; namespace BEPUphysics.BroadPhaseEntries.Events { /// /// Event manager for use with the CompoundCollidable. /// It's possible to use the ContactEventManager directly with a compound, /// but without using this class, any child event managers will fail to dispatch /// deferred events. /// public class CompoundEventManager : ContactEventManager { //TODO: This class can be generalized if there is ever another collidable which has similar requirements to the compound body. protected override void DispatchEvents() { //Go through all children and dispatch events. //They won't be touched by the primary event manager otherwise. var compound = this.owner as CompoundCollidable; if (compound != null) { foreach (var child in compound.children) { var deferredEventCreator = child.CollisionInformation.events as IDeferredEventCreator; if (deferredEventCreator.IsActive) deferredEventCreator.DispatchEvents(); } } else { throw new InvalidOperationException("Cannot use a CompoundEventManager with anything but a CompoundCollidable."); } base.DispatchEvents(); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/ContactEventManager.cs ================================================ using BEPUutilities; using BEPUphysics.CollisionTests; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUutilities.DataStructures; namespace BEPUphysics.BroadPhaseEntries.Events { /// /// Event manager for collidables (things which can create contact points). /// ///Some Collidable subclass. public class ContactEventManager : EntryEventManager, IContactEventTriggerer where T : Collidable { #region Events /// /// Fires when the entity stops touching another entity. /// public event CollisionEndedEventHandler CollisionEnded { add { InternalCollisionEnded += value; AddToEventfuls(); } remove { InternalCollisionEnded -= value; VerifyEventStatus(); } } /// /// Fires when the entity stops touching another entity. /// Unlike the CollisionEnded event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event CollisionEndingEventHandler CollisionEnding; /// /// Fires when a pair is updated and there are contact points in it. /// public event PairTouchedEventHandler PairTouched { add { InternalPairTouched += value; AddToEventfuls(); } remove { InternalPairTouched -= value; VerifyEventStatus(); } } /// /// Fires when a pair is updated and there are contact points in it. /// Unlike the PairTouched event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event PairTouchingEventHandler PairTouching; /// /// Fires when this entity gains a contact point with another entity. /// public event ContactCreatedEventHandler ContactCreated { add { InternalContactCreated += value; AddToEventfuls(); } remove { InternalContactCreated -= value; VerifyEventStatus(); } } /// /// Fires when this entity loses a contact point with another entity. /// public event ContactRemovedEventHandler ContactRemoved { add { InternalContactRemoved += value; AddToEventfuls(); } remove { InternalContactRemoved -= value; VerifyEventStatus(); } } /// /// Fires when this entity gains a contact point with another entity. /// Unlike the ContactCreated event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event CreatingContactEventHandler CreatingContact; /// /// Fires when a collision first occurs. /// Unlike the InitialCollisionDetected event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event DetectingInitialCollisionEventHandler DetectingInitialCollision; /// /// Fires when a collision first occurs. /// public event InitialCollisionDetectedEventHandler InitialCollisionDetected { add { InternalInitialCollisionDetected += value; AddToEventfuls(); } remove { InternalInitialCollisionDetected -= value; VerifyEventStatus(); } } /// /// Fires when this entity loses a contact point with another entity. /// Unlike the ContactRemoved event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event RemovingContactEventHandler RemovingContact; private event CollisionEndedEventHandler InternalCollisionEnded; private event PairTouchedEventHandler InternalPairTouched; private event ContactCreatedEventHandler InternalContactCreated; private event ContactRemovedEventHandler InternalContactRemoved; private event InitialCollisionDetectedEventHandler InternalInitialCollisionDetected; #endregion #region Supporting members protected override bool EventsAreInactive() { return InternalCollisionEnded == null && InternalPairTouched == null && InternalContactCreated == null && InternalContactRemoved == null && InternalInitialCollisionDetected == null && base.EventsAreInactive(); } readonly ConcurrentDeque eventStorageContactCreated = new ConcurrentDeque(0); readonly ConcurrentDeque eventStorageInitialCollisionDetected = new ConcurrentDeque(0); readonly ConcurrentDeque eventStorageContactRemoved = new ConcurrentDeque(0); readonly ConcurrentDeque eventStorageCollisionEnded = new ConcurrentDeque(0); readonly ConcurrentDeque eventStoragePairTouched = new ConcurrentDeque(0); protected override void DispatchEvents() { //Note: Deferred event creation should be performed sequentially with dispatching. //This means a event creation from this creator cannot occur ASYNCHRONOUSLY while DispatchEvents is running. //Note: If the deferred event handler is removed during the execution of the engine, the handler may be null. //In this situation, ignore the event. //This is not a particularly clean behavior, but it's better than just crashing. EventStorageContactCreated contactCreated; while (eventStorageContactCreated.TryUnsafeDequeueFirst(out contactCreated)) if (InternalContactCreated != null) InternalContactCreated(owner, contactCreated.other, contactCreated.pair, contactCreated.contactData); EventStorageInitialCollisionDetected initialCollisionDetected; while (eventStorageInitialCollisionDetected.TryUnsafeDequeueFirst(out initialCollisionDetected)) if (InternalInitialCollisionDetected != null) InternalInitialCollisionDetected(owner, initialCollisionDetected.other, initialCollisionDetected.pair); EventStorageContactRemoved contactRemoved; while (eventStorageContactRemoved.TryUnsafeDequeueFirst(out contactRemoved)) if (InternalContactRemoved != null) InternalContactRemoved(owner, contactRemoved.other, contactRemoved.pair, contactRemoved.contactData); EventStorageCollisionEnded collisionEnded; while (eventStorageCollisionEnded.TryUnsafeDequeueFirst(out collisionEnded)) if (InternalCollisionEnded != null) InternalCollisionEnded(owner, collisionEnded.other, collisionEnded.pair); EventStoragePairTouched collisionPairTouched; while (eventStoragePairTouched.TryUnsafeDequeueFirst(out collisionPairTouched)) if (InternalPairTouched != null) InternalPairTouched(owner, collisionPairTouched.other, collisionPairTouched.pair); base.DispatchEvents(); } public void OnCollisionEnded(Collidable other, CollidablePairHandler collisionPair) { if (InternalCollisionEnded != null) eventStorageCollisionEnded.Enqueue(new EventStorageCollisionEnded(other, collisionPair)); if (CollisionEnding != null) CollisionEnding(owner, other, collisionPair); } public void OnPairTouching(Collidable other, CollidablePairHandler collisionPair) { if (InternalPairTouched != null) eventStoragePairTouched.Enqueue(new EventStoragePairTouched(other, collisionPair)); if (PairTouching != null) PairTouching(owner, other, collisionPair); } public void OnContactCreated(Collidable other, CollidablePairHandler collisionPair, Contact contact) { if (InternalContactCreated != null) { ContactData contactData; contactData.Position = contact.Position; contactData.Normal = contact.Normal; contactData.PenetrationDepth = contact.PenetrationDepth; contactData.Id = contact.Id; eventStorageContactCreated.Enqueue(new EventStorageContactCreated(other, collisionPair, ref contactData)); } if (CreatingContact != null) CreatingContact(owner, other, collisionPair, contact); } public void OnContactRemoved(Collidable other, CollidablePairHandler collisionPair, Contact contact) { if (InternalContactRemoved != null) { ContactData contactData; contactData.Position = contact.Position; contactData.Normal = contact.Normal; contactData.PenetrationDepth = contact.PenetrationDepth; contactData.Id = contact.Id; eventStorageContactRemoved.Enqueue(new EventStorageContactRemoved(other, collisionPair, ref contactData)); } if (RemovingContact != null) RemovingContact(owner, other, collisionPair, contact); } public void OnInitialCollisionDetected(Collidable other, CollidablePairHandler collisionPair) { if (InternalInitialCollisionDetected != null) eventStorageInitialCollisionDetected.Enqueue(new EventStorageInitialCollisionDetected(other, collisionPair)); if (DetectingInitialCollision != null) DetectingInitialCollision(owner, other, collisionPair); } /// /// Removes all event hooks from the event manager. /// public override void RemoveAllEvents() { InternalCollisionEnded = null; InternalPairTouched = null; InternalContactCreated = null; InternalContactRemoved = null; InternalInitialCollisionDetected = null; CollisionEnding = null; DetectingInitialCollision = null; CreatingContact = null; RemovingContact = null; PairTouching = null; base.RemoveAllEvents(); } #endregion } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/EntryEventManager.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.OtherSpaceStages; using BEPUutilities; using System; using BEPUutilities.DataStructures; namespace BEPUphysics.BroadPhaseEntries.Events { /// /// Event manager for BroadPhaseEntries (all types that live in the broad phase). /// ///Some BroadPhaseEntry subclass. public class EntryEventManager : IDeferredEventCreator, IEntryEventTriggerer where T : BroadPhaseEntry { protected internal int childDeferredEventCreators; /// /// Number of child deferred event creators. /// int IDeferredEventCreator.ChildDeferredEventCreators { get { return childDeferredEventCreators; } set { int previousValue = childDeferredEventCreators; childDeferredEventCreators = value; if (childDeferredEventCreators == 0 && previousValue != 0) { //Deactivate! if (EventsAreInactive()) { //The events are inactive method tests to see if this event manager //has any events that need to be deferred. //If we get here, that means that there's zero children active, and we aren't active... ((IDeferredEventCreator)this).IsActive = false; } } else if (childDeferredEventCreators != 0 && previousValue == 0) { //Activate! //It doesn't matter if there are any events active in this instance, just try to activate anyway. //If it is already active, nothing will happen. ((IDeferredEventCreator)this).IsActive = true; } } } private CompoundEventManager parent; /// /// The parent of the event manager, if any. /// protected internal CompoundEventManager Parent { get { return parent; } set { //The child deferred event creator links must be maintained. if (parent != null && isActive) ((IDeferredEventCreator)parent).ChildDeferredEventCreators--; parent = value; if (parent != null && isActive) ((IDeferredEventCreator)parent).ChildDeferredEventCreators++; } } protected internal T owner; /// /// Owner of the event manager. /// public T Owner { get { return owner; } protected internal set { owner = value; } } #region Events /// /// Fires when this entity's bounding box newly overlaps another entity's bounding box. /// public event PairCreatedEventHandler PairCreated { add { InternalPairCreated += value; AddToEventfuls(); } remove { InternalPairCreated -= value; VerifyEventStatus(); } } /// /// Fires when this entity's bounding box no longer overlaps another entity's bounding box. /// public event PairRemovedEventHandler PairRemoved { add { InternalPairRemoved += value; AddToEventfuls(); } remove { InternalPairRemoved -= value; VerifyEventStatus(); } } /// /// Fires when a pair is updated. /// public event PairUpdatedEventHandler PairUpdated { add { InternalPairUpdated += value; AddToEventfuls(); } remove { InternalPairUpdated -= value; VerifyEventStatus(); } } /// /// Fires when a pair is updated. /// Unlike the PairUpdated event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event PairUpdatingEventHandler PairUpdating; /// /// Fires when this entity's bounding box newly overlaps another entity's bounding box. /// Unlike the PairCreated event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event CreatingPairEventHandler CreatingPair; /// /// Fires when this entity's bounding box no longer overlaps another entity's bounding box. /// Unlike the PairRemoved event, this event will run inline instead of at the end of the space's update. /// Some operations are unsupported while the engine is updating, and be especially careful if internal multithreading is enabled. /// public event RemovingPairEventHandler RemovingPair; private event PairCreatedEventHandler InternalPairCreated; private event PairRemovedEventHandler InternalPairRemoved; private event PairUpdatedEventHandler InternalPairUpdated; #endregion #region Supporting members /// /// Removes the entity from the space's list of eventful entities if no events are active. /// protected void VerifyEventStatus() { if (EventsAreInactive() && childDeferredEventCreators == 0) { ((IDeferredEventCreator)this).IsActive = false; } } protected virtual bool EventsAreInactive() { return InternalPairCreated == null && InternalPairRemoved == null && InternalPairUpdated == null; } protected void AddToEventfuls() { ((IDeferredEventCreator)this).IsActive = true; } private DeferredEventDispatcher deferredEventDispatcher; DeferredEventDispatcher IDeferredEventCreator.DeferredEventDispatcher { get { return deferredEventDispatcher; } set { deferredEventDispatcher = value; } } readonly ConcurrentDeque eventStoragePairCreated = new ConcurrentDeque(0); readonly ConcurrentDeque eventStoragePairRemoved = new ConcurrentDeque(0); readonly ConcurrentDeque eventStoragePairUpdated = new ConcurrentDeque(0); void IDeferredEventCreator.DispatchEvents() { DispatchEvents(); } protected virtual void DispatchEvents() { //Note: Deferred event creation should be performed sequentially with dispatching. //This means a event creation from this creator cannot occur ASYNCHRONOUSLY while DispatchEvents is running. //Note: If the deferred event handler is removed during the execution of the engine, the handler may be null. //In this situation, ignore the event. //This is not a particularly clean behavior, but it's better than just crashing. EventStoragePairCreated collisionPairCreated; while (eventStoragePairCreated.TryUnsafeDequeueFirst(out collisionPairCreated)) if (InternalPairCreated != null) InternalPairCreated(owner, collisionPairCreated.other, collisionPairCreated.pair); EventStoragePairRemoved collisionPairRemoved; while (eventStoragePairRemoved.TryUnsafeDequeueFirst(out collisionPairRemoved)) if (InternalPairRemoved != null) InternalPairRemoved(owner, collisionPairRemoved.other); EventStoragePairUpdated collisionPairUpdated; while (eventStoragePairUpdated.TryUnsafeDequeueFirst(out collisionPairUpdated)) if (InternalPairUpdated != null) InternalPairUpdated(owner, collisionPairUpdated.other, collisionPairUpdated.pair); } public void OnPairCreated(BroadPhaseEntry other, NarrowPhasePair collisionPair) { if (InternalPairCreated != null) eventStoragePairCreated.Enqueue(new EventStoragePairCreated(other, collisionPair)); if (CreatingPair != null) CreatingPair(owner, other, collisionPair); } public void OnPairRemoved(BroadPhaseEntry other) { if (InternalPairRemoved != null) { eventStoragePairRemoved.Enqueue(new EventStoragePairRemoved(other)); } if (RemovingPair != null) { RemovingPair(owner, other); } } public void OnPairUpdated(BroadPhaseEntry other, NarrowPhasePair collisionPair) { if (InternalPairUpdated != null) eventStoragePairUpdated.Enqueue(new EventStoragePairUpdated(other, collisionPair)); if (PairUpdating != null) PairUpdating(owner, other, collisionPair); } private bool isActive; //IsActive is enabled whenever this collision information can dispatch events. bool IDeferredEventCreator.IsActive { get { return isActive; } set { if (!isActive && value) { isActive = true; //Notify the parent that it needs to activate. if (parent != null) ((IDeferredEventCreator)parent).ChildDeferredEventCreators++; if (deferredEventDispatcher != null) deferredEventDispatcher.CreatorActivityChanged(this); } else if (isActive && !value) { isActive = false; //Notify the parent that it can deactivate. if (parent != null) ((IDeferredEventCreator)parent).ChildDeferredEventCreators--; if (deferredEventDispatcher != null) deferredEventDispatcher.CreatorActivityChanged(this); } } } /// /// Removes all event hooks from the manager. /// public virtual void RemoveAllEvents() { PairUpdating = null; CreatingPair = null; RemovingPair = null; InternalPairCreated = null; InternalPairRemoved = null; InternalPairUpdated = null; VerifyEventStatus(); } #endregion } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/IContactEventTriggerer.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.CollisionTests; namespace BEPUphysics.BroadPhaseEntries.Events { /// /// Manages triggers for events in an ContactEventManager. /// public interface IContactEventTriggerer : IEntryEventTriggerer { /// /// Fires collision ending events. /// /// Other collidable involved in the pair. /// Collidable pair handler that manages the two objects. void OnCollisionEnded(Collidable other, CollidablePairHandler collisionPair); /// /// Fires pair touching events. /// /// Other collidable involved in the pair. /// Collidable pair handler that manages the two objects. void OnPairTouching(Collidable other, CollidablePairHandler collisionPair); /// /// Fires contact creating events. /// /// Other collidable involved in the pair. /// Collidable pair handler that manages the two objects. /// Contact point of collision. void OnContactCreated(Collidable other, CollidablePairHandler collisionPair, Contact contact); /// /// Fires contact removal events. /// /// Other collidable involved in the pair. /// Collidable pair handler that manages the two objects. /// Contact point of collision. void OnContactRemoved(Collidable other, CollidablePairHandler collisionPair, Contact contact); /// /// Fires initial collision detected events. /// /// Other collidable involved in the pair. /// Collidable pair handler that manages the two objects. void OnInitialCollisionDetected(Collidable other, CollidablePairHandler collisionPair); } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Events/IEntryEventTriggerer.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.NarrowPhaseSystems.Pairs; namespace BEPUphysics.BroadPhaseEntries.Events { /// /// Manages triggers for events in an EntryEventManager. /// public interface IEntryEventTriggerer { /// /// Fires the event manager's pair creation events. /// /// Other entry involved in the pair. /// Narrow phase pair governing the two objects. void OnPairCreated(BroadPhaseEntry other, NarrowPhasePair collisionPair); /// /// Fires the event manager's pair removal events. /// /// Other entry involved in the pair. void OnPairRemoved(BroadPhaseEntry other); /// /// Fires the event manager's pair updated events. /// /// Other entry involved in the pair. /// Narrow phase pair governing the two objects. void OnPairUpdated(BroadPhaseEntry other, NarrowPhasePair collisionPair); } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/InstancedMesh.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.OtherSpaceStages; using AffineTransform = BEPUutilities.AffineTransform; using RigidTransform = BEPUutilities.RigidTransform; namespace BEPUphysics.BroadPhaseEntries { /// /// Collidable mesh which can be created from a reusable InstancedMeshShape. /// Very little data is needed for each individual InstancedMesh object, allowing /// a complicated mesh to be repeated many times. Since the hierarchy used to accelerate /// collisions is purely local, it may be marginally slower than an individual StaticMesh. /// public class InstancedMesh : StaticCollidable { internal AffineTransform worldTransform; /// /// Gets or sets the world transform of the mesh. /// public AffineTransform WorldTransform { get { return worldTransform; } set { worldTransform = value; Shape.ComputeBoundingBox(ref value, out boundingBox); } } /// /// Updates the bounding box to the current state of the entry. /// public override void UpdateBoundingBox() { Shape.ComputeBoundingBox(ref worldTransform, out boundingBox); } /// /// Constructs a new InstancedMesh. /// ///Shape to use for the instance. public InstancedMesh(InstancedMeshShape meshShape) : this(meshShape, AffineTransform.Identity) { } /// /// Constructs a new InstancedMesh. /// ///Shape to use for the instance. ///Transform to use for the instance. public InstancedMesh(InstancedMeshShape meshShape, AffineTransform worldTransform) { this.worldTransform = worldTransform; base.Shape = meshShape; Events = new ContactEventManager(); } /// /// Gets the shape used by the instanced mesh. /// public new InstancedMeshShape Shape { get { return (InstancedMeshShape)shape; } } internal TriangleSidedness sidedness = TriangleSidedness.DoubleSided; /// /// Gets or sets the sidedness of the mesh. This can be used to ignore collisions and rays coming from a direction relative to the winding of the triangle. /// public TriangleSidedness Sidedness { get { return sidedness; } set { sidedness = value; } } internal bool improveBoundaryBehavior = true; /// /// Gets or sets whether or not the collision system should attempt to improve contact behavior at the boundaries between triangles. /// This has a slight performance cost, but prevents objects sliding across a triangle boundary from 'bumping,' and otherwise improves /// the robustness of contacts at edges and vertices. /// public bool ImproveBoundaryBehavior { get { return improveBoundaryBehavior; } set { improveBoundaryBehavior = value; } } protected internal ContactEventManager events; /// /// Gets the event manager of the mesh. /// public ContactEventManager Events { get { return events; } set { if (value.Owner != null && //Can't use a manager which is owned by a different entity. value != events) //Stay quiet if for some reason the same event manager is being set. throw new ArgumentException("Event manager is already owned by a mesh; event managers cannot be shared."); if (events != null) events.Owner = null; events = value; if (events != null) events.Owner = this; } } protected internal override IContactEventTriggerer EventTriggerer { get { return events; } } protected override IDeferredEventCreator EventCreator { get { return events; } } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return RayCast(ray, maximumLength, sidedness, out rayHit); } /// /// Tests a ray against the instance. /// ///Ray to test. ///Maximum length of the ray to test; in units of the ray's direction's length. ///Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness. ///The hit location of the ray on the mesh, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit) { //Put the ray into local space. Ray localRay; AffineTransform inverse; AffineTransform.Invert(ref worldTransform, out inverse); Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction); AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position); if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit)) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.TransformTranspose(ref rayHit.Normal, ref inverse.LinearTransform, out rayHit.Normal); return true; } rayHit = new RayHit(); return false; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); AffineTransform.Transform(ref tri.vA, ref worldTransform, out tri.vA); AffineTransform.Transform(ref tri.vB, ref worldTransform, out tri.vB); AffineTransform.Transform(ref tri.vC, ref worldTransform, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/CompoundCollidable.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.CollisionShapes; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.Materials; using BEPUphysics.CollisionRuleManagement; using System; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Collidable used by compound shapes. /// public class CompoundCollidable : EntityCollidable { /// /// Gets or sets the event manager for the collidable. /// Compound collidables must use a special CompoundEventManager in order for the deferred events created /// by child collidables to be dispatched. /// If this method is bypassed and a different event manager is used, this method will return null and /// deferred events from children will fail. /// public new CompoundEventManager Events { get { return events as CompoundEventManager; } set { //Tell every child to update their parent references to the new object. foreach (var child in children) { child.CollisionInformation.events.Parent = value; } base.Events = value; } } /// /// Gets the shape of the collidable. /// public new CompoundShape Shape { get { return (CompoundShape)shape; } protected internal set { base.Shape = value; } } internal RawList children = new RawList(); /// /// Gets a list of the children in the collidable. /// public ReadOnlyList Children { get { return new ReadOnlyList(children); } } protected override void OnEntityChanged() { for (int i = 0; i < children.Count; i++) { children.Elements[i].CollisionInformation.Entity = entity; if (children.Elements[i].Material == null) children.Elements[i].Material = entity.material; } base.OnEntityChanged(); } private CompoundChild GetChild(CompoundChildData data, int index) { var instance = data.Entry.Shape.GetCollidableInstance(); if (data.Events != null) instance.Events = data.Events; //Establish the link between the child event manager and our event manager. instance.events.Parent = Events; if (data.CollisionRules != null) instance.CollisionRules = data.CollisionRules; instance.Tag = data.Tag; if (data.Material == null) data.Material = new Material(); return new CompoundChild(Shape, instance, data.Material, index); } private CompoundChild GetChild(CompoundShapeEntry entry, int index) { var instance = entry.Shape.GetCollidableInstance(); //Establish the link between the child event manager and our event manager. instance.events.Parent = Events; return new CompoundChild(Shape, instance, index); } //Used to efficiently split compounds. internal CompoundCollidable() { Events = new CompoundEventManager(); hierarchy = new CompoundHierarchy(this); } /// /// Constructs a compound collidable using additional information about the shapes in the compound. /// ///Data representing the children of the compound collidable. public CompoundCollidable(IList children) { Events = new CompoundEventManager(); var shapeList = new RawList(); //Create the shape first. for (int i = 0; i < children.Count; i++) { shapeList.Add(children[i].Entry); } base.Shape = new CompoundShape(shapeList); //Now create the actual child objects. for (int i = 0; i < children.Count; i++) { this.children.Add(GetChild(children[i], i)); } hierarchy = new CompoundHierarchy(this); } /// /// Constructs a compound collidable using additional information about the shapes in the compound. /// ///Data representing the children of the compound collidable. ///Location computed to be the center of the compound object. public CompoundCollidable(IList children, out Vector3 center) { Events = new CompoundEventManager(); var shapeList = new RawList(); //Create the shape first. for (int i = 0; i < children.Count; i++) { shapeList.Add(children[i].Entry); } base.Shape = new CompoundShape(shapeList, out center); //Now create the actual child objects. for (int i = 0; i < children.Count; i++) { this.children.Add(GetChild(children[i], i)); } hierarchy = new CompoundHierarchy(this); } /// /// Constructs a new CompoundCollidable. /// ///Compound shape to use for the collidable. public CompoundCollidable(CompoundShape compoundShape) : base(compoundShape) { Events = new CompoundEventManager(); for (int i = 0; i < compoundShape.shapes.Count; i++) { CompoundChild child = GetChild(compoundShape.shapes.Elements[i], i); this.children.Add(child); } hierarchy = new CompoundHierarchy(this); } internal CompoundHierarchy hierarchy; /// /// Gets the hierarchy of children used by the collidable. /// public CompoundHierarchy Hierarchy { get { return hierarchy; } } /// /// Updates the world transform of the collidable. /// ///Position to use for the calculation. ///Orientation to use for the calculation. public override void UpdateWorldTransform(ref Vector3 position, ref Quaternion orientation) { base.UpdateWorldTransform(ref position, ref orientation); var shapeList = Shape.shapes; for (int i = 0; i < children.Count; i++) { RigidTransform transform; RigidTransform.Transform(ref shapeList.Elements[children.Elements[i].shapeIndex].LocalTransform, ref worldTransform, out transform); children.Elements[i].CollisionInformation.UpdateWorldTransform(ref transform.Position, ref transform.Orientation); } } protected internal override void UpdateBoundingBoxInternal(float dt) { for (int i = 0; i < children.Count; i++) { children.Elements[i].CollisionInformation.UpdateBoundingBoxInternal(dt); } hierarchy.Tree.Refit(); boundingBox = hierarchy.Tree.BoundingBox; } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the collidable, if any. /// Whether or not the ray hit the collidable. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { CompoundChild hitChild; bool hit = RayCast(ray, maximumLength, out rayHit, out hitChild); return hit; } /// /// Tests a ray against the compound. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit data and the hit child collidable, if any. /// Whether or not the ray hit the entry. public bool RayCast(Ray ray, float maximumLength, out RayCastResult rayHit) { RayHit hitData; CompoundChild hitChild; bool hit = RayCast(ray, maximumLength, out hitData, out hitChild); rayHit = new RayCastResult { HitData = hitData, HitObject = hitChild.CollisionInformation }; return hit; } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit data, if any. /// Child collidable hit by the ray, if any. /// Whether or not the ray hit the entry. public bool RayCast(Ray ray, float maximumLength, out RayHit rayHit, out CompoundChild hitChild) { rayHit = new RayHit(); hitChild = null; var hitElements = PhysicsResources.GetCompoundChildList(); if (hierarchy.Tree.GetOverlaps(ray, maximumLength, hitElements)) { rayHit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { EntityCollidable candidate = hitElements.Elements[i].CollisionInformation; RayHit tempHit; if (candidate.RayCast(ray, maximumLength, out tempHit) && tempHit.T < rayHit.T) { rayHit = tempHit; hitChild = hitElements.Elements[i]; } } PhysicsResources.GiveBack(hitElements); return rayHit.T != float.MaxValue; } PhysicsResources.GiveBack(hitElements); return false; } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit location of the ray on the collidable, if any. /// Whether or not the ray hit the collidable. public override bool RayCast(Ray ray, float maximumLength, Func filter, out RayHit rayHit) { CompoundChild hitChild; bool hit = RayCast(ray, maximumLength, filter, out rayHit, out hitChild); return hit; } /// /// Tests a ray against the compound. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit data and the hit child collidable, if any. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Whether or not the ray hit the entry. public bool RayCast(Ray ray, float maximumLength, Func filter, out RayCastResult rayHit) { RayHit hitData; CompoundChild hitChild; bool hit = RayCast(ray, maximumLength, filter, out hitData, out hitChild); rayHit = new RayCastResult { HitData = hitData, HitObject = hitChild.CollisionInformation }; return hit; } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit location of the ray on the collidable, if any. /// Child hit by the ray. /// Whether or not the ray hit the collidable. public bool RayCast(Ray ray, float maximumLength, Func filter, out RayHit rayHit, out CompoundChild hitChild) { rayHit = new RayHit(); hitChild = null; if (filter(this)) { var hitElements = PhysicsResources.GetCompoundChildList(); if (hierarchy.Tree.GetOverlaps(ray, maximumLength, hitElements)) { rayHit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { RayHit tempHit; if (hitElements.Elements[i].CollisionInformation.RayCast(ray, maximumLength, filter, out tempHit) && tempHit.T < rayHit.T) { rayHit = tempHit; hitChild = hitElements.Elements[i]; } } PhysicsResources.GiveBack(hitElements); return rayHit.T != float.MaxValue; } PhysicsResources.GiveBack(hitElements); } return false; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit rayHit) { CompoundChild hitChild; bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, out rayHit, out hitChild); return hit; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Data and hit object from the first impact, if any. /// Whether or not the cast hit anything. public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayCastResult result) { CompoundChild hitChild; RayHit rayHit; bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, out rayHit, out hitChild); result = new RayCastResult { HitData = rayHit, HitObject = hitChild.CollisionInformation }; return hit; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Child hit by the cast. /// Whether or not the cast hit anything. public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit, out CompoundChild hitChild) { hit = new RayHit(); hitChild = null; BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); var hitElements = PhysicsResources.GetCompoundChildList(); if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { var candidate = hitElements.Elements[i].CollisionInformation; RayHit tempHit; if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, out tempHit) && tempHit.T < hit.T) { hit = tempHit; hitChild = hitElements.Elements[i]; } } PhysicsResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(hitElements); return false; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayHit rayHit) { CompoundChild hitChild; bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, out rayHit, out hitChild); return hit; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Data and hit object from the first impact, if any. /// Whether or not the cast hit anything. public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayCastResult result) { CompoundChild hitChild; RayHit rayHit; bool hit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, out rayHit, out hitChild); result = new RayCastResult { HitData = rayHit, HitObject = hitChild.CollisionInformation }; return hit; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data, if any. /// Child hit by the cast. /// Whether or not the cast hit anything. public bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayHit hit, out CompoundChild hitChild) { hit = new RayHit(); hitChild = null; BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); var hitElements = PhysicsResources.GetCompoundChildList(); if (hierarchy.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { var candidate = hitElements.Elements[i].CollisionInformation; RayHit tempHit; if (candidate.ConvexCast(castShape, ref startingTransform, ref sweep, filter, out tempHit) && tempHit.T < hit.T) { hit = tempHit; hitChild = hitElements.Elements[i]; } } PhysicsResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(hitElements); return false; } } /// /// Data which can be used to create a CompoundChild. /// This data is not itself a child yet; another system /// will use it as input to construct the children. /// public struct CompoundChildData { /// /// Shape entry of the compound child. /// public CompoundShapeEntry Entry; /// /// Event manager for the new child. /// public ContactEventManager Events; /// /// Collision rules for the new child. /// public CollisionRules CollisionRules; /// /// Material for the new child. /// public Material Material; /// /// Tag to assign to the collidable created for this child. /// public object Tag; } /// /// A collidable child of a compound. /// public class CompoundChild : IBoundingBoxOwner { CompoundShape shape; internal int shapeIndex; /// /// Gets the index of the shape used by this child in the CompoundShape's shapes list. /// public int ShapeIndex { get { return shapeIndex; } } private EntityCollidable collisionInformation; /// /// Gets the Collidable associated with the child. /// public EntityCollidable CollisionInformation { get { return collisionInformation; } } /// /// Gets or sets the material associated with the child. /// public Material Material { get; set; } /// /// Gets the index of the shape associated with this child in the CompoundShape's shapes list. /// public CompoundShapeEntry Entry { get { return shape.shapes.Elements[shapeIndex]; } } internal CompoundChild(CompoundShape shape, EntityCollidable collisionInformation, Material material, int index) { this.shape = shape; this.collisionInformation = collisionInformation; Material = material; this.shapeIndex = index; } internal CompoundChild(CompoundShape shape, EntityCollidable collisionInformation, int index) { this.shape = shape; this.collisionInformation = collisionInformation; this.shapeIndex = index; } /// /// Gets the bounding box of the child. /// public BoundingBox BoundingBox { get { return collisionInformation.boundingBox; } } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/CompoundHelper.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.CollisionShapes; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUphysics.Entities; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Contains methods to help with splitting compound objects into multiple pieces. /// public static class CompoundHelper { /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(Func splitPredicate, Entity a, out Entity b) { var childContributions = a.CollisionInformation.Shape.ComputeChildContributions(); return SplitCompound(childContributions, splitPredicate, a, out b); } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(IList childContributions, Func splitPredicate, Entity a, out Entity b) { ShapeDistributionInformation distributionInfoA, distributionInfoB; if (SplitCompound(childContributions, splitPredicate, a, out b, out distributionInfoA, out distributionInfoB)) { return true; } else return false; } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Volume, volume distribution, and center information about the new form of the original compound collidable. /// Volume, volume distribution, and center information about the new compound collidable. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(IList childContributions, Func splitPredicate, Entity a, out Entity b, out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB) { var bCollidable = new CompoundCollidable { Shape = a.CollisionInformation.Shape }; b = null; float weightA, weightB; if (SplitCompound(childContributions, splitPredicate, a.CollisionInformation, bCollidable, out distributionInfoA, out distributionInfoB, out weightA, out weightB)) { //Reconfigure the entities using the data computed in the split. float originalMass = a.mass; if (a.CollisionInformation.children.Count > 0) { float newMassA = (weightA / (weightA + weightB)) * originalMass; Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, newMassA * InertiaHelper.InertiaTensorScale, out distributionInfoA.VolumeDistribution); a.Initialize(a.CollisionInformation, newMassA, distributionInfoA.VolumeDistribution, distributionInfoA.Volume); } if (bCollidable.children.Count > 0) { float newMassB = (weightB / (weightA + weightB)) * originalMass; Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, newMassB * InertiaHelper.InertiaTensorScale, out distributionInfoB.VolumeDistribution); b = new Entity(); b.Initialize(bCollidable, newMassB, distributionInfoB.VolumeDistribution, distributionInfoB.Volume); } SplitReposition(a, b, ref distributionInfoA, ref distributionInfoB, weightA, weightB); return true; } else return false; } static void SplitReposition(Entity a, Entity b, ref ShapeDistributionInformation distributionInfoA, ref ShapeDistributionInformation distributionInfoB, float weightA, float weightB) { //The compounds are not aligned with the original's position yet. //In order to align them, first look at the centers the split method computed. //They are offsets from the center of the original shape in local space. //These can be used to reposition the objects in world space. Vector3 weightedA, weightedB; Vector3.Multiply(ref distributionInfoA.Center, weightA, out weightedA); Vector3.Multiply(ref distributionInfoB.Center, weightB, out weightedB); Vector3 newLocalCenter; Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter); Vector3.Divide(ref newLocalCenter, weightA + weightB, out newLocalCenter); Vector3 localOffsetA; Vector3 localOffsetB; Vector3.Subtract(ref distributionInfoA.Center, ref newLocalCenter, out localOffsetA); Vector3.Subtract(ref distributionInfoB.Center, ref newLocalCenter, out localOffsetB); Vector3 originalPosition = a.position; b.Orientation = a.Orientation; Vector3 offsetA = Vector3.Transform(localOffsetA, a.Orientation); Vector3 offsetB = Vector3.Transform(localOffsetB, a.Orientation); a.Position = originalPosition + offsetA; b.Position = originalPosition + offsetB; Vector3 originalLinearVelocity = a.linearVelocity; Vector3 originalAngularVelocity = a.angularVelocity; a.AngularVelocity = originalAngularVelocity; b.AngularVelocity = originalAngularVelocity; a.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetA); b.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offsetB); } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(Func splitPredicate, Entity a, Entity b) { var childContributions = a.CollisionInformation.Shape.ComputeChildContributions(); if (SplitCompound(childContributions, splitPredicate, a, b)) { return true; } else return false; } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(IList childContributions, Func splitPredicate, Entity a, Entity b) { ShapeDistributionInformation distributionInfoA, distributionInfoB; if (SplitCompound(childContributions, splitPredicate, a, b, out distributionInfoA, out distributionInfoB)) { return true; } else return false; } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Volume, volume distribution, and center information about the new form of the original compound collidable. /// Volume, volume distribution, and center information about the new compound collidable. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(IList childContributions, Func splitPredicate, Entity a, Entity b, out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB) { float weightA, weightB; if (SplitCompound(childContributions, splitPredicate, a.CollisionInformation, b.CollisionInformation, out distributionInfoA, out distributionInfoB, out weightA, out weightB)) { //Reconfigure the entities using the data computed in the split. float originalMass = a.mass; if (a.CollisionInformation.children.Count > 0) { float newMassA = (weightA / (weightA + weightB)) * originalMass; Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, newMassA * InertiaHelper.InertiaTensorScale, out distributionInfoA.VolumeDistribution); a.Initialize(a.CollisionInformation, newMassA, distributionInfoA.VolumeDistribution, distributionInfoA.Volume); } if (b.CollisionInformation.children.Count > 0) { float newMassB = (weightB / (weightA + weightB)) * originalMass; Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, newMassB * InertiaHelper.InertiaTensorScale, out distributionInfoB.VolumeDistribution); b.Initialize(b.CollisionInformation, newMassB, distributionInfoB.VolumeDistribution, distributionInfoB.Volume); } SplitReposition(a, b, ref distributionInfoA, ref distributionInfoB, weightA, weightB); return true; } else return false; } /// /// Splits a single compound collidable into two separate compound collidables and computes information needed by the simulation. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to be split. Children in this compound will be removed and added to the other compound. /// Compound to receive children removed from the original compound. /// Volume, volume distribution, and center information about the new form of the original compound collidable. /// Volume, volume distribution, and center information about the new compound collidable. /// Total weight associated with the new form of the original compound collidable. /// Total weight associated with the new compound collidable. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool SplitCompound(IList childContributions, Func splitPredicate, CompoundCollidable a, CompoundCollidable b, out ShapeDistributionInformation distributionInfoA, out ShapeDistributionInformation distributionInfoB, out float weightA, out float weightB) { bool splitOccurred = false; for (int i = a.children.Count - 1; i >= 0; i--) { //The shape doesn't change during this process. The entity could, though. //All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same. var child = a.children.Elements[i]; if (splitPredicate(child)) { splitOccurred = true; a.children.FastRemoveAt(i); b.children.Add(child); //The child event handler must be unhooked from the old compound and given to the new one. child.CollisionInformation.events.Parent = b.Events; } } if (!splitOccurred) { //No split occurred, so we cannot proceed. distributionInfoA = new ShapeDistributionInformation(); distributionInfoB = new ShapeDistributionInformation(); weightA = 0; weightB = 0; return false; } //Compute the contributions from the original shape to the new form of the original collidable. distributionInfoA = new ShapeDistributionInformation(); weightA = 0; distributionInfoB = new ShapeDistributionInformation(); weightB = 0; for (int i = a.children.Count - 1; i >= 0; i--) { var child = a.children.Elements[i]; var entry = child.Entry; var contribution = childContributions[child.shapeIndex]; Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center); Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center); Vector3.Add(ref contribution.Center, ref distributionInfoA.Center, out distributionInfoA.Center); distributionInfoA.Volume += contribution.Volume; weightA += entry.Weight; } for (int i = b.children.Count - 1; i >= 0; i--) { var child = b.children.Elements[i]; var entry = child.Entry; var contribution = childContributions[child.shapeIndex]; Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center); Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center); Vector3.Add(ref contribution.Center, ref distributionInfoB.Center, out distributionInfoB.Center); distributionInfoB.Volume += contribution.Volume; weightB += entry.Weight; } //Average the center out. if (weightA > 0) Vector3.Divide(ref distributionInfoA.Center, weightA, out distributionInfoA.Center); if (weightB > 0) Vector3.Divide(ref distributionInfoB.Center, weightB, out distributionInfoB.Center); //Note that the 'entry' is from the Shape, and so the translations are local to the shape's center. //That is not technically the center of the new collidable- distributionInfoA.Center is. //Offset the child collidables by -distributionInfoA.Center using their local offset. Vector3 offsetA; Vector3.Negate(ref distributionInfoA.Center, out offsetA); Vector3 offsetB; Vector3.Negate(ref distributionInfoB.Center, out offsetB); //Compute the unscaled inertia tensor. for (int i = a.children.Count - 1; i >= 0; i--) { var child = a.children.Elements[i]; var entry = child.Entry; Vector3 transformedOffset; Quaternion conjugate; Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate); Vector3.Transform(ref offsetA, ref conjugate, out transformedOffset); child.CollisionInformation.localPosition = transformedOffset; var contribution = childContributions[child.shapeIndex]; CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoA.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution); //Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position); Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfoA.VolumeDistribution, out distributionInfoA.VolumeDistribution); } for (int i = b.children.Count - 1; i >= 0; i--) { var child = b.children.Elements[i]; var entry = child.Entry; Vector3 transformedOffset; Quaternion conjugate; Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate); Vector3.Transform(ref offsetB, ref conjugate, out transformedOffset); child.CollisionInformation.localPosition = transformedOffset; var contribution = childContributions[child.shapeIndex]; CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfoB.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution); //Vector3.Add(ref entry.LocalTransform.Position, ref offsetB, out entry.LocalTransform.Position); Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfoB.VolumeDistribution, out distributionInfoB.VolumeDistribution); } //Normalize the volume distribution. Matrix3x3.Multiply(ref distributionInfoA.VolumeDistribution, 1 / weightA, out distributionInfoA.VolumeDistribution); Matrix3x3.Multiply(ref distributionInfoB.VolumeDistribution, 1 / weightB, out distributionInfoB.VolumeDistribution); //Update the hierarchies of the compounds. //TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types. a.hierarchy.Tree.Reconstruct(a.children); b.hierarchy.Tree.Reconstruct(b.children); return true; } static void RemoveReposition(Entity compound, ref ShapeDistributionInformation distributionInfo, float weight, float removedWeight, ref Vector3 removedCenter) { //The compounds are not aligned with the original's position yet. //In order to align them, first look at the centers the split method computed. //They are offsets from the center of the original shape in local space. //These can be used to reposition the objects in world space. Vector3 weightedA, weightedB; Vector3.Multiply(ref distributionInfo.Center, weight, out weightedA); Vector3.Multiply(ref removedCenter, removedWeight, out weightedB); Vector3 newLocalCenter; Vector3.Add(ref weightedA, ref weightedB, out newLocalCenter); Vector3.Divide(ref newLocalCenter, weight + removedWeight, out newLocalCenter); Vector3 localOffset; Vector3.Subtract(ref distributionInfo.Center, ref newLocalCenter, out localOffset); Vector3 originalPosition = compound.position; Vector3 offset = Vector3.Transform(localOffset, compound.orientation); compound.Position = originalPosition + offset; Vector3 originalLinearVelocity = compound.linearVelocity; Vector3 originalAngularVelocity = compound.angularVelocity; compound.AngularVelocity = originalAngularVelocity; compound.LinearVelocity = originalLinearVelocity + Vector3.Cross(originalAngularVelocity, offset); } /// /// Removes a child from a compound body. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Original compound to have a child removed. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool RemoveChildFromCompound(Entity compound, Func removalPredicate, IList childContributions) { ShapeDistributionInformation distributionInfo; if (RemoveChildFromCompound(compound, removalPredicate, childContributions, out distributionInfo)) { return true; } else return false; } /// /// Removes a child from a compound body. /// /// List of distribution information associated with each child shape of the whole compound shape used by the compound being split. /// Delegate which determines if a child in the original compound should be moved to the new compound. /// Volume, volume distribution, and center information about the new form of the original compound collidable. /// Original compound to have a child removed. /// Whether or not the predicate returned true for any element in the original compound and split the compound. public static bool RemoveChildFromCompound(Entity compound, Func removalPredicate, IList childContributions, out ShapeDistributionInformation distributionInfo) { float weight; float removedWeight; Vector3 removedCenter; if (RemoveChildFromCompound(compound.CollisionInformation, removalPredicate, childContributions, out distributionInfo, out weight, out removedWeight, out removedCenter)) { //Reconfigure the entities using the data computed in the split. //Only bother if there are any children left in the compound! if (compound.CollisionInformation.Children.Count > 0) { float originalMass = compound.mass; float newMass = (weight / (weight + removedWeight)) * originalMass; Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, newMass * InertiaHelper.InertiaTensorScale, out distributionInfo.VolumeDistribution); compound.Initialize(compound.CollisionInformation, newMass, distributionInfo.VolumeDistribution, distributionInfo.Volume); RemoveReposition(compound, ref distributionInfo, weight, removedWeight, ref removedCenter); } return true; } else return false; } /// /// Removes a child from a compound collidable. /// /// Compound collidable to remove a child from. /// Callback which analyzes a child and determines if it should be removed from the compound. /// Distribution contributions from all shapes in the compound shape. This can include shapes which are not represented in the compound. /// Distribution information of the new compound. /// Total weight of the new compound. /// Weight removed from the compound. /// Center of the chunk removed from the compound. /// Whether or not any removal took place. public static bool RemoveChildFromCompound(CompoundCollidable compound, Func removalPredicate, IList childContributions, out ShapeDistributionInformation distributionInfo, out float weight, out float removedWeight, out Vector3 removedCenter) { bool removalOccurred = false; removedWeight = 0; removedCenter = new Vector3(); for (int i = compound.children.Count - 1; i >= 0; i--) { //The shape doesn't change during this process. The entity could, though. //All of the other collidable information, like the Tag, CollisionRules, Events, etc. all stay the same. var child = compound.children.Elements[i]; if (removalPredicate(child)) { removalOccurred = true; var entry = child.Entry; removedWeight += entry.Weight; Vector3 toAdd; Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd); Vector3.Add(ref removedCenter, ref toAdd, out removedCenter); //The child event handler must be unhooked from the compound. child.CollisionInformation.events.Parent = null; compound.children.FastRemoveAt(i); } } if (!removalOccurred) { //No removal occurred, so we cannot proceed. distributionInfo = new ShapeDistributionInformation(); weight = 0; return false; } if (removedWeight > 0) { Vector3.Divide(ref removedCenter, removedWeight, out removedCenter); } //Compute the contributions from the original shape to the new form of the original collidable. distributionInfo = new ShapeDistributionInformation(); weight = 0; for (int i = compound.children.Count - 1; i >= 0; i--) { var child = compound.children.Elements[i]; var entry = child.Entry; var contribution = childContributions[child.shapeIndex]; Vector3.Add(ref contribution.Center, ref entry.LocalTransform.Position, out contribution.Center); Vector3.Multiply(ref contribution.Center, child.Entry.Weight, out contribution.Center); Vector3.Add(ref contribution.Center, ref distributionInfo.Center, out distributionInfo.Center); distributionInfo.Volume += contribution.Volume; weight += entry.Weight; } //Average the center out. Vector3.Divide(ref distributionInfo.Center, weight, out distributionInfo.Center); //Note that the 'entry' is from the Shape, and so the translations are local to the shape's center. //That is not technically the center of the new collidable- distributionInfo.Center is. //Offset the child collidables by -distributionInfo.Center using their local offset. Vector3 offset; Vector3.Negate(ref distributionInfo.Center, out offset); //Compute the unscaled inertia tensor. for (int i = compound.children.Count - 1; i >= 0; i--) { var child = compound.children.Elements[i]; var entry = child.Entry; Vector3 transformedOffset; Quaternion conjugate; Quaternion.Conjugate(ref entry.LocalTransform.Orientation, out conjugate); Vector3.Transform(ref offset, ref conjugate, out transformedOffset); child.CollisionInformation.localPosition = transformedOffset; var contribution = childContributions[child.shapeIndex]; CompoundShape.TransformContribution(ref entry.LocalTransform, ref distributionInfo.Center, ref contribution.VolumeDistribution, entry.Weight, out contribution.VolumeDistribution); //Vector3.Add(ref entry.LocalTransform.Position, ref offsetA, out entry.LocalTransform.Position); Matrix3x3.Add(ref contribution.VolumeDistribution, ref distributionInfo.VolumeDistribution, out distributionInfo.VolumeDistribution); } //Normalize the volume distribution. Matrix3x3.Multiply(ref distributionInfo.VolumeDistribution, 1 / weight, out distributionInfo.VolumeDistribution); //Update the hierarchies of the compounds. //TODO: Create a new method that does this quickly without garbage. Requires a new Reconstruct method which takes a pool which stores the appropriate node types. compound.hierarchy.Tree.Reconstruct(compound.children); return true; } /// /// Constructs a compound collidable containing only the specified subset of children. /// /// Shape to base the compound collidable on. /// Indices of child shapes from the CompoundShape to include in the compound collidable. /// Compound collidable containing only the specified subset of children. public static CompoundCollidable CreatePartialCompoundCollidable(CompoundShape shape, IList childIndices) { if (childIndices.Count == 0) throw new ArgumentException("Cannot create a compound from zero shapes."); CompoundCollidable compound = new CompoundCollidable(); Vector3 center = new Vector3(); float totalWeight = 0; for (int i = 0; i < childIndices.Count; i++) { //Create and add the child object itself. var entry = shape.shapes[childIndices[i]]; compound.children.Add(new CompoundChild(shape, entry.Shape.GetCollidableInstance(), childIndices[i])); //Grab its entry to compute the center of mass of this subset. Vector3 toAdd; Vector3.Multiply(ref entry.LocalTransform.Position, entry.Weight, out toAdd); Vector3.Add(ref center, ref toAdd, out center); totalWeight += entry.Weight; } if (totalWeight <= 0) { throw new ArgumentException("Compound has zero total weight; invalid configuration."); } Vector3.Divide(ref center, totalWeight, out center); //Our subset of the compound is not necessarily aligned with the shape's origin. //By default, an object will rotate around the center of the collision shape. //We can't modify the shape data itself since it could be shared, which leaves //modifying the local position of the collidable. //We have the subset position in shape space, so pull the collidable back into alignment //with the origin. //This approach matches the rest of the CompoundHelper's treatment of subsets. compound.LocalPosition = -center; //Recompute the hierarchy for the compound. compound.hierarchy.Tree.Reconstruct(compound.children); compound.Shape = shape; return compound; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/CompoundHierarchy.cs ================================================ using System; using BEPUphysics.DataStructures; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Hierarchy of children used to accelerate queries and tests for compound collidables. /// public class CompoundHierarchy { private BoundingBoxTree tree; /// /// Gets the bounding box tree of the hierarchy. /// public BoundingBoxTree Tree { get { return tree; } } private CompoundCollidable owner; /// /// Gets the CompoundCollidable that owns this hierarchy. /// public CompoundCollidable Owner { get { return owner; } } /// /// Constructs a new compound hierarchy. /// ///Owner of the hierarchy. public CompoundHierarchy(CompoundCollidable owner) { this.owner = owner; var children = new CompoundChild[owner.children.Count]; Array.Copy(owner.children.Elements, children, owner.children.Count); //In order to initialize a good tree, the local space bounding boxes should first be computed. //Otherwise, the tree would try to create a hierarchy based on a bunch of zeroed out bounding boxes! for (int i = 0; i < children.Length; i++) { children[i].CollisionInformation.worldTransform = owner.Shape.shapes.Elements[i].LocalTransform; children[i].CollisionInformation.UpdateBoundingBoxInternal(0); } tree = new BoundingBoxTree(children); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/ConvexCollidable.cs ================================================ using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; using System; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUphysics.CollisionTests.CollisionAlgorithms; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Collidable with a convex shape. /// public abstract class ConvexCollidable : EntityCollidable { protected ConvexCollidable(ConvexShape shape) : base(shape) { Events = new ContactEventManager(); } /// /// Gets the shape of the collidable. /// public new ConvexShape Shape { get { return (ConvexShape)shape; } } public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { return MPRToolbox.Sweep(castShape, Shape, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref worldTransform, out hit); } } /// /// Collidable with a convex shape of a particular type. /// ///ConvexShape type. public class ConvexCollidable : ConvexCollidable where T : ConvexShape { /// /// Gets the shape of the collidable. /// public new T Shape { get { return (T)shape; } } /// /// Constructs a new convex collidable. /// ///Shape to use in the collidable. public ConvexCollidable(T shape) : base(shape) { } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return Shape.RayTest(ref ray, ref worldTransform, maximumLength, out rayHit); } protected internal override void UpdateBoundingBoxInternal(float dt) { Shape.GetBoundingBox(ref worldTransform, out boundingBox); ExpandBoundingBox(ref boundingBox, dt); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/EntityCollidable.cs ================================================ using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.Settings; using System; using BEPUphysics.PositionUpdating; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Mobile collidable acting as a collision proxy for an entity. /// public abstract class EntityCollidable : MobileCollidable { protected EntityCollidable() { //This constructor is used when the subclass is going to set the shape after doing some extra initialization. } protected EntityCollidable(EntityShape shape) { base.Shape = shape; } /// /// Gets the shape of the collidable. /// public new EntityShape Shape { get { return (EntityShape)shape; } protected set { base.Shape = value; } } protected internal Entity entity; /// /// Gets the entity owning the collidable. /// public Entity Entity { get { return entity; } protected internal set { entity = value; OnEntityChanged(); } } protected virtual void OnEntityChanged() { } protected internal RigidTransform worldTransform; /// /// Gets or sets the world transform of the collidable. /// The EntityCollidable's LocalPosition is ignored for this process; the shape will end up /// centered exactly on the world transform. /// Setting this property also updates the bounding box. /// public RigidTransform WorldTransform { get { return worldTransform; } set { //Remove the local position. The UpdateBoundingBoxForTransform will reintroduce it; we want the final result to put the shape (i.e. the WorldTransform) right where defined. Quaternion conjugate; Quaternion.Conjugate(ref value.Orientation, out conjugate); Vector3 worldOffset; Vector3.Transform(ref localPosition, ref conjugate, out worldOffset); Vector3.Subtract(ref value.Position, ref worldOffset, out value.Position); UpdateBoundingBoxForTransform(ref value); } } protected internal override bool IsActive { get { return entity != null ? entity.activityInformation.IsActive : false; } } protected internal Vector3 localPosition; /// /// Gets or sets the local position of the collidable. /// The local position can be used to offset the collision geometry /// from an entity's center of mass. /// public Vector3 LocalPosition { get { return localPosition; } set { localPosition = value; localPosition.Validate(); } } /// /// Updates the bounding box of the mobile collidable according to the associated entity's current state. /// Do not use this if the EntityCollidable does not have an associated entity; consider using /// UpdateBoundingBoxForTransform instead. /// public override void UpdateBoundingBox() { UpdateBoundingBox(0); } /// /// Updates the bounding box of the mobile collidable according to the associated entity's current state. /// Do not use this if the EntityCollidable does not have an associated entity; consider using /// UpdateBoundingBoxForTransform instead. /// ///Timestep with which to update the bounding box. public override void UpdateBoundingBox(float dt) { //The world transform update isn't strictly required for uninterrupted simulation. //The entity update method manages the world transforms. //However, the redundancy allows a user to change the position in between frames. //If the order of the space update changes to position-update-first, this is completely unnecessary. UpdateWorldTransform(ref entity.position, ref entity.orientation); UpdateBoundingBoxInternal(dt); } /// /// Updates the world transform of the shape using the given position and orientation. /// The world transform of the shape is offset from the given position and orientation by the collidable's LocalPosition. /// ///Position to use for the calculation. ///Orientation to use for the calculation. public virtual void UpdateWorldTransform(ref Vector3 position, ref Quaternion orientation) { Vector3.Transform(ref localPosition, ref orientation, out worldTransform.Position); Vector3.Add(ref worldTransform.Position, ref position, out worldTransform.Position); worldTransform.Orientation = orientation; worldTransform.Validate(); } /// /// Updates the collidable's world transform and bounding box. The transform provided /// will be offset by the collidable's LocalPosition to get the shape transform. /// This is a convenience method for external modification of the collidable's data. /// /// Transform to use for the collidable. /// Duration of the simulation time step. Used to expand the /// bounding box using the owning entity's velocity. If the collidable /// does not have an owning entity, this must be zero. public void UpdateBoundingBoxForTransform(ref RigidTransform transform, float dt) { UpdateWorldTransform(ref transform.Position, ref transform.Orientation); UpdateBoundingBoxInternal(dt); } /// /// Updates the collidable's world transform and bounding box. /// This is a convenience method for external modification of the collidable's data. /// /// Transform to use for the collidable. public void UpdateBoundingBoxForTransform(ref RigidTransform transform) { UpdateBoundingBoxForTransform(ref transform, 0); } protected internal abstract void UpdateBoundingBoxInternal(float dt); //Helper method for mobile collidables. internal void ExpandBoundingBox(ref BoundingBox boundingBox, float dt) { //Expand bounding box with velocity. if (dt > 0) { bool useExtraExpansion = MotionSettings.UseExtraExpansionForContinuousBoundingBoxes && entity.PositionUpdateMode == PositionUpdateMode.Continuous; float velocityScaling = useExtraExpansion ? 2 : 1; if (entity.linearVelocity.X > 0) boundingBox.Max.X += entity.linearVelocity.X * dt * velocityScaling; else boundingBox.Min.X += entity.linearVelocity.X * dt * velocityScaling; if (entity.linearVelocity.Y > 0) boundingBox.Max.Y += entity.linearVelocity.Y * dt * velocityScaling; else boundingBox.Min.Y += entity.linearVelocity.Y * dt * velocityScaling; if (entity.linearVelocity.Z > 0) boundingBox.Max.Z += entity.linearVelocity.Z * dt * velocityScaling; else boundingBox.Min.Z += entity.linearVelocity.Z * dt * velocityScaling; if (useExtraExpansion) { float expansion = 0; //It's possible that an object could have a small bounding box since its own //velocity is low, but then a collision with a high velocity object sends //it way out of its bounding box. By taking into account high velocity objects //in danger of hitting us and expanding our own bounding box by their speed, //we stand a much better chance of not missing secondary collisions. foreach (var e in OverlappedEntities) { float velocity = e.linearVelocity.LengthSquared(); if (velocity > expansion) expansion = velocity; } expansion = (float)Math.Sqrt(expansion) * dt; boundingBox.Min.X -= expansion; boundingBox.Min.Y -= expansion; boundingBox.Min.Z -= expansion; boundingBox.Max.X += expansion; boundingBox.Max.Y += expansion; boundingBox.Max.Z += expansion; } //Could use this to incorporate angular motion. Since the bounding box is an approximation to begin with, //this isn't too important. If an updating system is used where the bounding box MUST fully contain the frame's motion //then the commented area should be used. //Math.Min(entity.angularVelocity.Length() * dt, Shape.maximumRadius) * velocityScaling; //TODO: consider using minimum radius } boundingBox.Validate(); } protected override void CollisionRulesUpdated() { //Try to activate the entity since our collision rules just changed; broadphase might need to update some stuff. //Beware, though; if this collidable is still being constructed, then the entity won't be available. if (entity != null) entity.activityInformation.Activate(); } protected internal ContactEventManager events; /// /// Gets or sets the event manager of the collidable. /// public ContactEventManager Events { get { return events; } set { if (value.Owner != null && //Can't use a manager which is owned by a different entity. value != events) //Stay quiet if for some reason the same event manager is being set. throw new ArgumentException("Event manager is already owned by an entity; event managers cannot be shared."); //Must pass on the link to the parent event manager to the new event manager in case we are the child of a compound. CompoundEventManager oldParent = null; if (events != null) { events.Owner = null; oldParent = events.Parent; events.Parent = null; } events = value; if (events != null) { events.Owner = this; events.Parent = oldParent; } } } protected internal override IContactEventTriggerer EventTriggerer { get { return events; } } /// /// Gets an enumerable collection of all entities overlapping this collidable. /// public EntityCollidableCollection OverlappedEntities { get { return new EntityCollidableCollection(this); } } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/MobileCollidable.cs ================================================ namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { //This is implemented by anything which wants the engine to update its bounding box every frame (so long as it is 'active'). /// /// Superclass of all collidables which are capable of movement, and thus need bounding box updates every frame. /// public abstract class MobileCollidable : Collidable { //TODO: Imagine needing to calculate the bounding box for a data structure that is not axis-aligned. Being able to return BB without 'setting' would be helpful. //Possibly require second method. The parameterless one uses 'self data' to do the calculation, as a sort of convenience. The parameterful would return without setting. /// /// Updates the bounding box of the mobile collidable. /// ///Timestep with which to update the bounding box. public abstract void UpdateBoundingBox(float dt); } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/MobileMeshCollidable.cs ================================================ using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionTests.CollisionAlgorithms; using System; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Collidable used by compound shapes. /// public class MobileMeshCollidable : EntityCollidable { /// /// Gets the shape of the collidable. /// public new MobileMeshShape Shape { get { return (MobileMeshShape)shape; } } /// /// Constructs a new mobile mesh collidable. /// /// Shape to use in the collidable. public MobileMeshCollidable(MobileMeshShape shape) : base(shape) { Events = new ContactEventManager(); } internal bool improveBoundaryBehavior = true; /// /// Gets or sets whether or not the collision system should attempt to improve contact behavior at the boundaries between triangles. /// This has a slight performance cost, but prevents objects sliding across a triangle boundary from 'bumping,' and otherwise improves /// the robustness of contacts at edges and vertices. /// public bool ImproveBoundaryBehavior { get { return improveBoundaryBehavior; } set { improveBoundaryBehavior = value; } } protected internal override void UpdateBoundingBoxInternal(float dt) { Shape.GetBoundingBox(ref worldTransform, out boundingBox); //This DOES NOT EXPAND the local hierarchy. //The bounding boxes of queries against the local hierarchy //should be expanded using the relative velocity. ExpandBoundingBox(ref boundingBox, dt); } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { //Put the ray into local space. Ray localRay; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation); Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction); Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position); Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position); if (Shape.solidity == MobileMeshSolidity.Solid) { //Find all hits. Use the count to determine the ray started inside or outside. //If it starts inside and we're in 'solid' mode, then return the ray start. //The raycast must be of infinite length at first. This allows it to determine //if it is inside or outside. if (Shape.IsLocalRayOriginInMesh(ref localRay, out rayHit)) { //It was inside! rayHit = new RayHit() { Location = ray.Position, Normal = Vector3.Zero, T = 0 }; return true; } else { if (rayHit.T < maximumLength) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal); } else { //The hit was too far away, or there was no hit (in which case T would be float.MaxValue). return false; } return true; } } else { //Just do a normal raycast since the object isn't solid. TriangleSidedness sidedness; switch (Shape.solidity) { case MobileMeshSolidity.Clockwise: sidedness = TriangleSidedness.Clockwise; break; case MobileMeshSolidity.Counterclockwise: sidedness = TriangleSidedness.Counterclockwise; break; default: sidedness = TriangleSidedness.DoubleSided; break; } if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit)) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal); return true; } } rayHit = new RayHit(); return false; } /// /// Tests a ray against the surface of the mesh. This does not take into account solidity. /// ///Ray to test. ///Maximum length of the ray to test; in units of the ray's direction's length. ///Sidedness to use during the ray cast. This does not have to be the same as the mesh's sidedness. ///The hit location of the ray on the mesh, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit) { //Put the ray into local space. Ray localRay; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out orientation); Matrix3x3.TransformTranspose(ref ray.Direction, ref orientation, out localRay.Direction); Vector3.Subtract(ref ray.Position, ref worldTransform.Position, out localRay.Position); Matrix3x3.TransformTranspose(ref localRay.Position, ref orientation, out localRay.Position); if (Shape.TriangleMesh.RayCast(localRay, maximumLength, sidedness, out rayHit)) { //Transform the hit into world space. Vector3.Multiply(ref ray.Direction, rayHit.T, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref ray.Position, out rayHit.Location); Matrix3x3.Transform(ref rayHit.Normal, ref orientation, out rayHit.Normal); return true; } rayHit = new RayHit(); return false; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { if (Shape.solidity == MobileMeshSolidity.Solid) { //If the convex cast is inside the mesh and the mesh is solid, it should return t = 0. var ray = new Ray() { Position = startingTransform.Position, Direction = Toolbox.UpVector }; if (Shape.IsLocalRayOriginInMesh(ref ray, out hit)) { hit = new RayHit() { Location = startingTransform.Position, Normal = new Vector3(), T = 0 }; return true; } } hit = new RayHit(); BoundingBox boundingBox; var transform = new AffineTransform {Translation = worldTransform.Position}; Matrix3x3.CreateFromQuaternion(ref worldTransform.Orientation, out transform.LinearTransform); castShape.GetSweptLocalBoundingBox(ref startingTransform, ref transform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (this.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.TriangleMesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); AffineTransform.Transform(ref tri.vA, ref transform, out tri.vA); AffineTransform.Transform(ref tri.vB, ref transform, out tri.vB); AffineTransform.Transform(ref tri.vC, ref transform, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center}; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/MobileCollidables/TriangleCollidable.cs ================================================ using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseEntries.MobileCollidables { /// /// Special case collidable for reuseable triangles. /// public class TriangleCollidable : ConvexCollidable { /// /// Constructs a new shapeless collidable. /// public TriangleCollidable() : base(new TriangleShape()) { } /// /// Constructs the triangle collidable using the given shape. /// ///TriangleShape to use in the collidable. public TriangleCollidable(TriangleShape shape) : base(shape) { } /// /// Initializes the collidable using the new triangle shape, but does NOT /// fire any shape-changed events. /// ///First vertex in the triangle. ///Second vertex in the triangle. ///Third vertex in the triangle. public void Initialize(ref Vector3 a, ref Vector3 b, ref Vector3 c) { var shape = Shape; shape.collisionMargin = 0; shape.sidedness = TriangleSidedness.DoubleSided; shape.vA = a; shape.vB = b; shape.vC = c; } /// /// Cleans up the collidable by removing all events. /// public void CleanUp() { events.RemoveAllEvents(); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/StaticCollidable.cs ================================================ using System; using BEPUphysics.CollisionShapes; using BEPUphysics.Materials; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.OtherSpaceStages; namespace BEPUphysics.BroadPhaseEntries { /// /// Superclass of static collidable objects which can be added directly to a space. Static objects cannot move. /// public abstract class StaticCollidable : Collidable, ISpaceObject, IMaterialOwner, IDeferredEventCreatorOwner { /// /// Performs common initialization. /// protected StaticCollidable() { collisionRules.group = CollisionRules.DefaultKinematicCollisionGroup; //Note that the Events manager is not created here. That is left for subclasses to implement so that the type is more specific. //Entities can get away with having EntityCollidable specificity since you generally care more about the entity than the collidable, //but with static objects, the collidable is the only important object. It would be annoying to cast to the type you know it is every time //just to get access to some type-specific properties. material = new Material(); materialChangedDelegate = OnMaterialChanged; material.MaterialChanged += materialChangedDelegate; } protected override void OnShapeChanged(CollisionShape collisionShape) { if (!IgnoreShapeChanges) UpdateBoundingBox(); } internal Material material; //NOT thread safe due to material change pair update. /// /// Gets or sets the material used by the collidable. /// public Material Material { get { return material; } set { if (material != null) material.MaterialChanged -= materialChangedDelegate; material = value; if (material != null) material.MaterialChanged += materialChangedDelegate; OnMaterialChanged(material); } } Action materialChangedDelegate; protected virtual void OnMaterialChanged(Material newMaterial) { for (int i = 0; i < pairs.Count; i++) { pairs[i].UpdateMaterialProperties(); } } protected internal override bool IsActive { get { return false; } } ISpace space; ISpace ISpaceObject.Space { get { return space; } set { space = value; } } /// /// Gets the space that owns the mesh. /// public ISpace Space { get { return space; } } void ISpaceObject.OnAdditionToSpace(ISpace newSpace) { } void ISpaceObject.OnRemovalFromSpace(ISpace oldSpace) { } IDeferredEventCreator IDeferredEventCreatorOwner.EventCreator { get { return EventCreator; } } /// /// Gets the event creator associated with this collidable. /// protected abstract IDeferredEventCreator EventCreator { get; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/StaticGroup.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.OtherSpaceStages; using System.Collections.Generic; using RigidTransform = BEPUutilities.RigidTransform; namespace BEPUphysics.BroadPhaseEntries { /// /// Collection of unmoving collidable objects. /// /// /// Batching multiple static objects together into a StaticGroup as opposed to adding them separately to the Space avoids BroadPhase pollution, improving performance. /// public class StaticGroup : StaticCollidable { /// /// Constructs a new static mesh. /// ///List of collidables in the static group. public StaticGroup(IList collidables) { shape = new StaticGroupShape(collidables, this); Events = new ContactEventManager(); } /// /// Gets the shape used by the mesh. Unlike most collidable-shape pairs, StaticGroupShapes cannot be shared between multiple StaticGroups. /// public new StaticGroupShape Shape { get { return (StaticGroupShape)shape; } } protected internal ContactEventManager events; /// /// Gets the event manager used by the mesh. /// public ContactEventManager Events { get { return events; } set { if (value.Owner != null && //Can't use a manager which is owned by a different entity. value != events) //Stay quiet if for some reason the same event manager is being set. throw new ArgumentException("Event manager is already owned by a mesh; event managers cannot be shared."); if (events != null) events.Owner = null; events = value; if (events != null) events.Owner = this; } } protected internal override IContactEventTriggerer EventTriggerer { get { return events; } } protected override IDeferredEventCreator EventCreator { get { return events; } } /// /// Updates the bounding box to the current state of the entry. /// public override void UpdateBoundingBox() { boundingBox = Shape.CollidableTree.BoundingBox; } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { RayCastResult result; bool toReturn = Shape.RayCast(ray, maximumLength, out result); rayHit = result.HitData; return toReturn; } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, Func filter, out RayHit rayHit) { RayCastResult result; bool toReturn = Shape.RayCast(ray, maximumLength, filter, out result); rayHit = result.HitData; return toReturn; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { RayCastResult result; bool toReturn = Shape.ConvexCast(castShape, ref startingTransform, ref sweep, out result); hit = result.HitData; return toReturn; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayHit hit) { RayCastResult result; bool toReturn = Shape.ConvexCast(castShape, ref startingTransform, ref sweep, filter, out result); hit = result.HitData; return toReturn; } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/StaticMesh.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.Events; using BEPUphysics.CollisionShapes; using BEPUphysics.DataStructures; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.OtherSpaceStages; namespace BEPUphysics.BroadPhaseEntries { /// /// Unmoving, collidable triangle mesh. /// /// /// The acceleration structure for the mesh is created individually for each /// StaticMesh; if you want to create many meshes of the same model, consider using the /// InstancedMesh. /// public class StaticMesh : StaticCollidable { TriangleMesh mesh; /// /// Gets the TriangleMesh acceleration structure used by the StaticMesh. /// public TriangleMesh Mesh { get { return mesh; } } /// /// Gets or sets the world transform of the mesh. /// public AffineTransform WorldTransform { get { return ((TransformableMeshData)mesh.Data).worldTransform; } set { ((TransformableMeshData)mesh.Data).WorldTransform = value; mesh.Tree.Refit(); UpdateBoundingBox(); } } /// /// Constructs a new static mesh. /// ///Vertex positions of the mesh. ///Index list of the mesh. public StaticMesh(Vector3[] vertices, uint[] indices, int indexCount) { base.Shape = new StaticMeshShape(vertices, indices, indexCount); Events = new ContactEventManager(); } /// /// Constructs a new static mesh. /// ///Vertex positions of the mesh. ///Index list of the mesh. /// Transform to use to create the mesh initially. public StaticMesh(Vector3[] vertices, uint[] indices, int indexCount, AffineTransform worldTransform) { base.Shape = new StaticMeshShape(vertices, indices, indexCount, worldTransform); Events = new ContactEventManager(); } /// /// Gets the shape used by the mesh. /// public new StaticMeshShape Shape { get { return (StaticMeshShape)shape; } } internal TriangleSidedness sidedness = TriangleSidedness.DoubleSided; /// /// Gets or sets the sidedness of the mesh. This can be used to ignore collisions and rays coming from a direction relative to the winding of the triangle. /// public TriangleSidedness Sidedness { get { return sidedness; } set { sidedness = value; } } internal bool improveBoundaryBehavior = true; /// /// Gets or sets whether or not the collision system should attempt to improve contact behavior at the boundaries between triangles. /// This has a slight performance cost, but prevents objects sliding across a triangle boundary from 'bumping,' and otherwise improves /// the robustness of contacts at edges and vertices. /// public bool ImproveBoundaryBehavior { get { return improveBoundaryBehavior; } set { improveBoundaryBehavior = value; } } protected internal ContactEventManager events; /// /// Gets the event manager used by the mesh. /// public ContactEventManager Events { get { return events; } set { if (value.Owner != null && //Can't use a manager which is owned by a different entity. value != events) //Stay quiet if for some reason the same event manager is being set. throw new ArgumentException("Event manager is already owned by a mesh; event managers cannot be shared."); if (events != null) events.Owner = null; events = value; if (events != null) events.Owner = this; } } protected internal override IContactEventTriggerer EventTriggerer { get { return events; } } protected override IDeferredEventCreator EventCreator { get { return events; } } protected override void OnShapeChanged(CollisionShape collisionShape) { if (!IgnoreShapeChanges) { mesh = new TriangleMesh(Shape.TriangleMeshData); UpdateBoundingBox(); } } /// /// Updates the bounding box to the current state of the entry. /// public override void UpdateBoundingBox() { boundingBox = mesh.Tree.BoundingBox; } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return mesh.RayCast(ray, maximumLength, sidedness, out rayHit); } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = CommonResources.GetIntList(); if (Mesh.Tree.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { mesh.Data.GetTriangle(hitElements[i], out tri.vA, out tri.vB, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform {Orientation = Quaternion.Identity, Position = center}; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); CommonResources.GiveBack(hitElements); return false; } /// /// Tests a ray against the mesh. /// ///Ray to test. ///Maximum length to test in units of the ray direction's length. ///Sidedness to use when raycasting. Doesn't have to be the same as the mesh's own sidedness. ///Data about the ray's intersection with the mesh, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit) { return mesh.RayCast(ray, maximumLength, sidedness, out rayHit); } } } ================================================ FILE: BEPUphysics/BroadPhaseEntries/Terrain.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.Events; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.OtherSpaceStages; namespace BEPUphysics.BroadPhaseEntries { /// /// Heightfield-based unmovable collidable object. /// public class Terrain : StaticCollidable { /// /// Gets the shape of this collidable. /// public new TerrainShape Shape { get { return (TerrainShape)shape; } set { base.Shape = value; } } internal AffineTransform worldTransform; /// /// Gets or sets the affine transform of the terrain. /// public AffineTransform WorldTransform { get { return worldTransform; } set { worldTransform = value; } } internal bool improveBoundaryBehavior = true; /// /// Gets or sets whether or not the collision system should attempt to improve contact behavior at the boundaries between triangles. /// This has a slight performance cost, but prevents objects sliding across a triangle boundary from 'bumping,' and otherwise improves /// the robustness of contacts at edges and vertices. /// public bool ImproveBoundaryBehavior { get { return improveBoundaryBehavior; } set { improveBoundaryBehavior = value; } } protected internal ContactEventManager events; /// /// Gets the event manager used by the Terrain. /// public ContactEventManager Events { get { return events; } set { if (value.Owner != null && //Can't use a manager which is owned by a different entity. value != events) //Stay quiet if for some reason the same event manager is being set. throw new ArgumentException("Event manager is already owned by a Terrain; event managers cannot be shared."); if (events != null) events.Owner = null; events = value; if (events != null) events.Owner = this; } } protected internal override IContactEventTriggerer EventTriggerer { get { return events; } } protected override IDeferredEventCreator EventCreator { get { return events; } } internal float thickness; /// /// Gets or sets the thickness of the terrain. This defines how far below the triangles of the terrain's surface the terrain 'body' extends. /// Anything within the body of the terrain will be pulled back up to the surface. /// public float Thickness { get { return thickness; } set { if (value < 0) throw new ArgumentException("Cannot use a negative thickness value."); //Modify the bounding box to include the new thickness. Vector3 down = Vector3.Normalize(worldTransform.LinearTransform.Down); Vector3 thicknessOffset = down * (value - thickness); //Use the down direction rather than the thicknessOffset to determine which //component of the bounding box to subtract, since the down direction contains all //previous extra thickness. if (down.X < 0) boundingBox.Min.X += thicknessOffset.X; else boundingBox.Max.X += thicknessOffset.X; if (down.Y < 0) boundingBox.Min.Y += thicknessOffset.Y; else boundingBox.Max.Y += thicknessOffset.Y; if (down.Z < 0) boundingBox.Min.Z += thicknessOffset.Z; else boundingBox.Max.Z += thicknessOffset.Z; thickness = value; } } /// /// Constructs a new Terrain. /// ///Shape to use for the terrain. ///Transform to use for the terrain. public Terrain(TerrainShape shape, AffineTransform worldTransform) { this.worldTransform = worldTransform; Shape = shape; Events = new ContactEventManager(); } /// /// Constructs a new Terrain. /// ///Height data to use to create the TerrainShape. ///Transform to use for the terrain. public Terrain(float[,] heights, AffineTransform worldTransform) : this(new TerrainShape(heights), worldTransform) { } /// /// Updates the bounding box of the terrain. /// public override void UpdateBoundingBox() { Shape.GetBoundingBox(ref worldTransform, out boundingBox); //Include the thickness of the terrain. Vector3 thicknessOffset = Vector3.Normalize(worldTransform.LinearTransform.Down) * thickness; if (thicknessOffset.X < 0) boundingBox.Min.X += thicknessOffset.X; else boundingBox.Max.X += thicknessOffset.X; if (thicknessOffset.Y < 0) boundingBox.Min.Y += thicknessOffset.Y; else boundingBox.Max.Y += thicknessOffset.Y; if (thicknessOffset.Z < 0) boundingBox.Min.Z += thicknessOffset.Z; else boundingBox.Max.Z += thicknessOffset.Z; } /// /// Tests a ray against the entry. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit location of the ray on the entry, if any. /// Whether or not the ray hit the entry. public override bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return Shape.RayCast(ref ray, maximumLength, ref worldTransform, out rayHit); } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public override bool ConvexCast(CollisionShapes.ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit) { hit = new RayHit(); BoundingBox boundingBox; castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out boundingBox); var tri = PhysicsResources.GetTriangle(); var hitElements = PhysicsResources.GetTriangleIndicesList(); if (Shape.GetOverlaps(boundingBox, hitElements)) { hit.T = float.MaxValue; for (int i = 0; i < hitElements.Count; i++) { Shape.GetTriangle(ref hitElements.Elements[i], ref worldTransform, out tri.vA, out tri.vB, out tri.vC); Vector3 center; Vector3.Add(ref tri.vA, ref tri.vB, out center); Vector3.Add(ref center, ref tri.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); Vector3.Subtract(ref tri.vA, ref center, out tri.vA); Vector3.Subtract(ref tri.vB, ref center, out tri.vB); Vector3.Subtract(ref tri.vC, ref center, out tri.vC); tri.maximumRadius = tri.vA.LengthSquared(); float radius = tri.vB.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; radius = tri.vC.LengthSquared(); if (tri.maximumRadius < radius) tri.maximumRadius = radius; tri.maximumRadius = (float)Math.Sqrt(tri.maximumRadius); tri.collisionMargin = 0; var triangleTransform = new RigidTransform { Orientation = Quaternion.Identity, Position = center }; RayHit tempHit; if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T) { hit = tempHit; } } tri.maximumRadius = 0; PhysicsResources.GiveBack(tri); PhysicsResources.GiveBack(hitElements); return hit.T != float.MaxValue; } PhysicsResources.GiveBack(tri); PhysicsResources.GiveBack(hitElements); return false; } /// /// Gets the normal of a vertex at the given indices. /// ///First dimension index into the heightmap array. ///Second dimension index into the heightmap array. ///Normal at the given indices. public void GetNormal(int i, int j, out Vector3 normal) { Shape.GetNormal(i, j, ref worldTransform, out normal); } /// /// Gets the position of a vertex at the given indices. /// ///First dimension index into the heightmap array. ///Second dimension index into the heightmap array. ///Position at the given indices. public void GetPosition(int i, int j, out Vector3 position) { Shape.GetPosition(i, j, ref worldTransform, out position); } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/BroadPhase.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Threading; using BEPUutilities; using BEPUphysics.CollisionRuleManagement; using BEPUutilities.DataStructures; using System; namespace BEPUphysics.BroadPhaseSystems { /// /// Superclass of all broad phases. Broad phases collect overlapping broad phase entry pairs. /// public abstract class BroadPhase : MultithreadedProcessingStage { readonly SpinLock overlapAddLock = new SpinLock(); /// /// Gets the object which is locked by the broadphase during synchronized update processes. /// public object Locker { get; protected set; } protected BroadPhase() { Locker = new object(); Enabled = true; } protected BroadPhase(IThreadManager threadManager) : this() { ThreadManager = threadManager; AllowMultithreading = true; } //TODO: Initial capacity? Special collection type other than list due to structs? RawList? Clear at beginning of each frame? readonly RawList overlaps = new RawList(); /// /// Gets the list of overlaps identified in the previous broad phase update. /// public RawList Overlaps { get { return overlaps; } } /// /// Gets an interface to the broad phase's support for volume-based queries. /// public IQueryAccelerator QueryAccelerator { get; protected set; } /// /// Adds an entry to the broad phase. /// /// Entry to add. public virtual void Add(BroadPhaseEntry entry) { if (entry.BroadPhase == null) entry.BroadPhase = this; else throw new ArgumentException("Cannot add entry; it already belongs to a broad phase."); } /// /// Removes an entry from the broad phase. /// /// Entry to remove. public virtual void Remove(BroadPhaseEntry entry) { if (entry.BroadPhase == this) entry.BroadPhase = null; else throw new ArgumentException("Cannot remove entry; it does not belong to this broad phase."); } protected internal void AddOverlap(BroadPhaseOverlap overlap) { overlapAddLock.Enter(); overlaps.Add(overlap); overlapAddLock.Exit(); } /// /// Adds a broad phase overlap if the collision rules permit it. /// /// First entry of the overlap. /// Second entry of the overlap. protected internal void TryToAddOverlap(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { CollisionRule rule; if ((rule = GetCollisionRule(entryA, entryB)) < CollisionRule.NoBroadPhase) { overlapAddLock.Enter(); overlaps.Add(new BroadPhaseOverlap(entryA, entryB, rule)); overlapAddLock.Exit(); } } protected internal CollisionRule GetCollisionRule(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { if (entryA.IsActive || entryB.IsActive) return CollisionRules.collisionRuleCalculator(entryA, entryB); return CollisionRule.NoBroadPhase; } //TODO: Consider what happens when an overlap is found twice. How should it be dealt with? //Can the DBH spit out redundancies? //The PUG definitely can- consider two entities that are both in two adjacent cells. //Could say 'whatever' to it and handle it in the narrow phase- use the NeedsUpdate property. //If NeedsUpdate is false, that means it's already been updated once. Consider multithreaded problems. //Would require an interlocked compare exchange or something similar to protect it. //Slightly ruins the whole 'embarassingly parallel' aspect. //Need a something which has O(1) add, O(1) contains check, and fast iteration without requiring external nodes since everything gets regenerated each frame. } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/BroadPhaseOverlap.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.CollisionRuleManagement; namespace BEPUphysics.BroadPhaseSystems { /// /// A pair of overlapping BroadPhaseEntries. /// public struct BroadPhaseOverlap : IEquatable { internal BroadPhaseEntry entryA; /// /// First entry in the pair. /// public BroadPhaseEntry EntryA { get { return entryA; } } internal BroadPhaseEntry entryB; /// /// Second entry in the pair. /// public BroadPhaseEntry EntryB { get { return entryB; } } internal CollisionRule collisionRule; /// /// Constructs an overlap. /// /// First entry in the pair. /// Second entry in the pair. public BroadPhaseOverlap(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { this.entryA = entryA; this.entryB = entryB; collisionRule = CollisionRules.DefaultCollisionRule; } /// /// Constructs an overlap. /// /// First entry in the pair. /// Second entry in the pair. /// Collision rule calculated for the pair. public BroadPhaseOverlap(BroadPhaseEntry entryA, BroadPhaseEntry entryB, CollisionRule collisionRule) { this.entryA = entryA; this.entryB = entryB; this.collisionRule = collisionRule; } /// /// Gets the collision rule calculated for the pair. /// public CollisionRule CollisionRule { get { return collisionRule; } } /// /// Gets the hash code of the object. /// /// Hash code of the object. public override int GetHashCode() { //TODO: Use old prime-based system? return (int)((entryA.hashCode + entryB.hashCode) * 0xd8163841); } #region IEquatable Members /// /// Compares the overlaps for equality based on the involved entries. /// /// Overlap to compare. /// Whether or not the overlaps were equal. public bool Equals(BroadPhaseOverlap other) { return (other.entryA == entryA && other.entryB == entryB) || (other.entryA == entryB && other.entryB == entryA); } #endregion public override string ToString() { return "{" + entryA + ", " + entryB + "}"; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/BruteForce.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.BroadPhaseSystems { public class BruteForce : BroadPhase { public List entries = new List(); public override void Add(BroadPhaseEntry entry) { entries.Add(entry); } public override void Remove(BroadPhaseEntry entry) { entries.Remove(entry); } protected override void UpdateMultithreaded() { UpdateSingleThreaded(); } protected override void UpdateSingleThreaded() { Overlaps.Clear(); for (int i = 0; i < entries.Count; i++) { for (int j = i + 1; j < entries.Count; j++) { if (entries[i].boundingBox.Intersects(entries[j].boundingBox)) base.TryToAddOverlap(entries[i], entries[j]); } } } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/Hierarchies/DynamicHierarchy.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Threading; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.BroadPhaseSystems.Hierarchies { /// /// Broad phase that incrementally updates the internal tree acceleration structure. /// /// /// This is a good all-around broad phase; its performance is consistent and all queries are supported and speedy. /// The memory usage is higher than simple one-axis sort and sweep, but a bit lower than the Grid2DSortAndSweep option. /// public class DynamicHierarchy : BroadPhase { internal Node root; /// /// Constructs a new dynamic hierarchy broad phase. /// public DynamicHierarchy() { multithreadedRefit = MultithreadedRefit; multithreadedOverlap = MultithreadedOverlap; QueryAccelerator = new DynamicHierarchyQueryAccelerator(this); } /// /// Constructs a new dynamic hierarchy broad phase. /// /// Thread manager to use in the broad phase. public DynamicHierarchy(IThreadManager threadManager) : base(threadManager) { multithreadedRefit = MultithreadedRefit; multithreadedOverlap = MultithreadedOverlap; QueryAccelerator = new DynamicHierarchyQueryAccelerator(this); } /// /// This is a few test-based values which help threaded scaling. /// By going deeper into the trees, a better distribution of work is achieved. /// Going above the tested core count theoretically benefits from a '0 if power of 2, 2 otherwise' rule of thumb. /// private int[] threadSplitOffsets = new[] #if !XBOX360 { 0, 0, 4, 1, 2, 2, 2, 0, 2, 2, 2, 2 }; #else { 2, 2, 2, 1}; #endif #if PROFILE /// /// Gets the time used in refitting the acceleration structure and making any necessary incremental improvements. /// public double RefitTime { get { return (endRefit - startRefit) / (double)Stopwatch.Frequency; } } /// /// Gets the time used in testing the tree against itself to find overlapping pairs. /// public double OverlapTime { get { return (endOverlap - endRefit) / (double)Stopwatch.Frequency; } } long startRefit, endRefit; long endOverlap; #endif #region Multithreading private void MultithreadedRefitPhase(int splitDepth) { if (splitDepth > 0) { root.CollectMultithreadingNodes(splitDepth, 1, multithreadingSourceNodes); //Go through every node and refit it. ThreadManager.ForLoop(0, multithreadingSourceNodes.Count, multithreadedRefit); multithreadingSourceNodes.Clear(); //Now that the subtrees belonging to the source nodes are refit, refit the top nodes. //Sometimes, this will go deeper than necessary because the refit process may require an extremely high level (nonmultithreaded) revalidation. //The waste cost is a matter of nanoseconds due to the simplicity of the operations involved. root.PostRefit(splitDepth, 1); } else { SingleThreadedRefitPhase(); } } private void MultithreadedOverlapPhase(int splitDepth) { if (splitDepth > 0) { //The trees are now fully refit (and revalidated, if the refit process found it to be necessary). //The overlap traversal is conceptually similar to the multithreaded refit, but is a bit easier since there's no need to go back up the stack. if (!root.IsLeaf) //If the root is a leaf, it's alone- nothing to collide against! This test is required by the assumptions of the leaf-leaf test. { root.GetMultithreadedOverlaps(root, splitDepth, 1, this, multithreadingSourceOverlaps); ThreadManager.ForLoop(0, multithreadingSourceOverlaps.Count, multithreadedOverlap); multithreadingSourceOverlaps.Clear(); } } else { SingleThreadedOverlapPhase(); } } protected override void UpdateMultithreaded() { lock (Locker) { Overlaps.Clear(); if (root != null) { //To multithread the tree traversals, we have to do a little single threaded work. //Dive down into the tree far enough that there are enough nodes to split amongst all the threads in the thread manager. //The depth to which we dive is offset by some precomputed values (when available) or a guess based on whether or not the //thread count is a power of 2. Thread counts which are a power of 2 match well to the binary tree, while other thread counts //require going deeper for better distributions. int offset = ThreadManager.ThreadCount <= threadSplitOffsets.Length ? threadSplitOffsets[ThreadManager.ThreadCount - 1] : (ThreadManager.ThreadCount & (ThreadManager.ThreadCount - 1)) == 0 ? 0 : 2; int splitDepth = offset + (int)Math.Ceiling(Math.Log(ThreadManager.ThreadCount, 2)); #if PROFILE startRefit = Stopwatch.GetTimestamp(); #endif MultithreadedRefitPhase(splitDepth); #if PROFILE endRefit = Stopwatch.GetTimestamp(); #endif MultithreadedOverlapPhase(splitDepth); #if PROFILE endOverlap = Stopwatch.GetTimestamp(); #endif } } } internal struct NodePair { internal Node a; internal Node b; } RawList multithreadingSourceNodes = new RawList(4); Action multithreadedRefit; void MultithreadedRefit(int i) { multithreadingSourceNodes.Elements[i].Refit(); } RawList multithreadingSourceOverlaps = new RawList(10); Action multithreadedOverlap; void MultithreadedOverlap(int i) { var overlap = multithreadingSourceOverlaps.Elements[i]; //Note: It's okay not to check to see if a and b are equal and leaf nodes, because the systems which added nodes to the list already did it. overlap.a.GetOverlaps(overlap.b, this); } #endregion private void SingleThreadedRefitPhase() { root.Refit(); } private void SingleThreadedOverlapPhase() { if (!root.IsLeaf) //If the root is a leaf, it's alone- nothing to collide against! This test is required by the assumptions of the leaf-leaf test. root.GetOverlaps(root, this); } protected override void UpdateSingleThreaded() { lock (Locker) { Overlaps.Clear(); if (root != null) { #if PROFILE startRefit = Stopwatch.GetTimestamp(); #endif SingleThreadedRefitPhase(); #if PROFILE endRefit = Stopwatch.GetTimestamp(); #endif SingleThreadedOverlapPhase(); #if PROFILE endOverlap = Stopwatch.GetTimestamp(); #endif } } } UnsafeResourcePool leafNodes = new UnsafeResourcePool(); /// /// Adds an entry to the hierarchy. /// /// Entry to remove. public override void Add(BroadPhaseEntry entry) { base.Add(entry); //Entities do not set up their own bounding box before getting stuck in here. If they're all zeroed out, the tree will be horrible. Vector3 offset; Vector3.Subtract(ref entry.boundingBox.Max, ref entry.boundingBox.Min, out offset); if (offset.X * offset.Y * offset.Z == 0) entry.UpdateBoundingBox(); //Could buffer additions to get a better construction in the tree. var node = leafNodes.Take(); node.Initialize(entry); if (root == null) { //Empty tree. This is the first and only node. root = node; } else { if (root.IsLeaf) //Root is alone. root.TryToInsert(node, out root); else { BoundingBox.CreateMerged(ref node.BoundingBox, ref root.BoundingBox, out root.BoundingBox); var internalNode = (InternalNode)root; Vector3.Subtract(ref root.BoundingBox.Max, ref root.BoundingBox.Min, out offset); internalNode.currentVolume = offset.X * offset.Y * offset.Z; //internalNode.maximumVolume = internalNode.currentVolume * InternalNode.MaximumVolumeScale; //The caller is responsible for the merge. var treeNode = root; while (!treeNode.TryToInsert(node, out treeNode)) ;//TryToInsert returns the next node, if any, and updates node bounding box. } } } /// /// Removes an entry from the hierarchy. /// /// Entry to remove. public override void Remove(BroadPhaseEntry entry) { if (root == null) throw new InvalidOperationException("Entry not present in the hierarchy."); //Attempt to search for the entry with a boundingbox lookup first. if (!RemoveFast(entry)) { //Oof, could not locate it with the fast method; it must have been force-moved or something. //Fall back to a slow brute force approach. if (!RemoveBrute(entry)) { throw new InvalidOperationException("Entry not present in the hierarchy."); } } } internal bool RemoveFast(BroadPhaseEntry entry) { LeafNode leafNode; //Update the root with the replacement just in case the removal triggers a root change. if (root.RemoveFast(entry, out leafNode, out root)) { leafNode.CleanUp(); leafNodes.GiveBack(leafNode); base.Remove(entry); return true; } return false; } internal bool RemoveBrute(BroadPhaseEntry entry) { LeafNode leafNode; //Update the root with the replacement just in case the removal triggers a root change. if (root.Remove(entry, out leafNode, out root)) { leafNode.CleanUp(); leafNodes.GiveBack(leafNode); base.Remove(entry); return true; } return false; } #region Debug internal void Analyze(List depths, out int nodeCount) { nodeCount = 0; root.Analyze(depths, 0, ref nodeCount); } internal void ForceRevalidation() { ((InternalNode)root).Revalidate(); } #endregion } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/Hierarchies/DynamicHierarchyNode.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.BroadPhaseSystems.Hierarchies { internal abstract class Node { internal BoundingBox BoundingBox; internal abstract void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements); internal abstract void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements); internal abstract void GetOverlaps(Node node, DynamicHierarchy owner); internal abstract bool IsLeaf { get; } internal abstract Node ChildA { get; } internal abstract Node ChildB { get; } internal abstract BroadPhaseEntry Element { get; } internal abstract bool TryToInsert(LeafNode node, out Node treeNode); internal abstract void Analyze(List depths, int depth, ref int nodeCount); internal abstract void Refit(); internal abstract void RetrieveNodes(RawList leafNodes); internal abstract void CollectMultithreadingNodes(int splitDepth, int currentDepth, RawList multithreadingSourceNodes); internal abstract void PostRefit(int splitDepth, int currentDepth); internal abstract void GetMultithreadedOverlaps(Node opposingNode, int splitDepth, int currentDepth, DynamicHierarchy owner, RawList multithreadingSourceOverlaps); internal abstract bool Remove(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode); internal abstract bool RemoveFast(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode); } internal sealed class InternalNode : Node { internal Node childA; internal Node childB; internal float currentVolume; internal float maximumVolume; internal static float MaximumVolumeScale = 1.4f; internal override Node ChildA { get { return childA; } } internal override Node ChildB { get { return childB; } } internal override BroadPhaseEntry Element { get { return default(BroadPhaseEntry); } } internal override bool IsLeaf { get { return false; } } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Users of the GetOverlaps method will have to check the bounding box before calling //root.getoverlaps. This is actually desired in some cases, since the outer bounding box is used //to determine a pair, and further overlap tests shouldn'BroadPhaseEntry bother retesting the root. bool intersects; childA.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) childA.GetOverlaps(ref boundingBox, outputOverlappedElements); childB.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) childB.GetOverlaps(ref boundingBox, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { bool intersects; childA.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) childA.GetOverlaps(ref boundingSphere, outputOverlappedElements); childB.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) childB.GetOverlaps(ref boundingSphere, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { bool intersects; boundingFrustum.Intersects(ref childA.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(ref boundingFrustum, outputOverlappedElements); boundingFrustum.Intersects(ref childB.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(ref boundingFrustum, outputOverlappedElements); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { float? result; ray.Intersects(ref childA.BoundingBox, out result); if (result != null && result < maximumLength) childA.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); ray.Intersects(ref childB.BoundingBox, out result); if (result != null && result < maximumLength) childB.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); } internal override void GetOverlaps(Node opposingNode, DynamicHierarchy owner) { bool intersects; if (this == opposingNode) { //We are being compared against ourselves! //Obviously we're an internal node, so spawn three children: //A versus A: if (!childA.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. childA.GetOverlaps(childA, owner); //B versus B: if (!childB.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. childB.GetOverlaps(childB, owner); //A versus B (if they intersect): childA.BoundingBox.Intersects(ref childB.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(childB, owner); } else { //Two different nodes. The other one may be a leaf. if (opposingNode.IsLeaf) { //If it's a leaf, go deeper in our hierarchy, but not the opposition. childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingNode, owner); childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingNode, owner); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in both hierarchies. childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingChildA, owner); childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingChildB, owner); childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingChildA, owner); childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingChildB, owner); } } } internal static LockingResourcePool nodePool = new LockingResourcePool(); internal override bool TryToInsert(LeafNode node, out Node treeNode) { //Since we are an internal node, we know we have two children. //Regardless of what kind of nodes they are, figure out which would be a better choice to merge the new node with. //Use the path which produces the smallest 'volume.' BoundingBox mergedA, mergedB; BoundingBox.CreateMerged(ref childA.BoundingBox, ref node.BoundingBox, out mergedA); BoundingBox.CreateMerged(ref childB.BoundingBox, ref node.BoundingBox, out mergedB); Vector3 offset; float originalAVolume, originalBVolume; Vector3.Subtract(ref childA.BoundingBox.Max, ref childA.BoundingBox.Min, out offset); originalAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref childB.BoundingBox.Max, ref childB.BoundingBox.Min, out offset); originalBVolume = offset.X * offset.Y * offset.Z; float mergedAVolume, mergedBVolume; Vector3.Subtract(ref mergedA.Max, ref mergedA.Min, out offset); mergedAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref mergedB.Max, ref mergedB.Min, out offset); mergedBVolume = offset.X * offset.Y * offset.Z; //Could use factor increase or absolute difference if (mergedAVolume - originalAVolume < mergedBVolume - originalBVolume) { //merging A produces a better result. if (childA.IsLeaf) { var newChildA = nodePool.Take(); newChildA.BoundingBox = mergedA; newChildA.childA = this.childA; newChildA.childB = node; newChildA.currentVolume = mergedAVolume; //newChildA.maximumVolume = newChildA.currentVolume * MaximumVolumeScale; childA = newChildA; treeNode = null; return true; } else { childA.BoundingBox = mergedA; var internalNode = (InternalNode)childA; internalNode.currentVolume = mergedAVolume; //internalNode.maximumVolume = internalNode.currentVolume * MaximumVolumeScale; treeNode = childA; return false; } } else { //merging B produces a better result. if (childB.IsLeaf) { //Target is a leaf! Return. var newChildB = nodePool.Take(); newChildB.BoundingBox = mergedB; newChildB.childA = node; newChildB.childB = this.childB; newChildB.currentVolume = mergedBVolume; //newChildB.maximumVolume = newChildB.currentVolume * MaximumVolumeScale; childB = newChildB; treeNode = null; return true; } else { childB.BoundingBox = mergedB; treeNode = childB; var internalNode = (InternalNode)childB; internalNode.currentVolume = mergedBVolume; //internalNode.maximumVolume = internalNode.currentVolume * MaximumVolumeScale; return false; } } } public override string ToString() { return "{" + childA + ", " + childB + "}"; } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; childA.Analyze(depths, depth + 1, ref nodeCount); childB.Analyze(depths, depth + 1, ref nodeCount); } internal override void Refit() { if (currentVolume > maximumVolume) { Revalidate(); return; } childA.Refit(); childB.Refit(); BoundingBox.CreateMerged(ref childA.BoundingBox, ref childB.BoundingBox, out BoundingBox); //float DEBUGlastVolume = currentVolume; currentVolume = (BoundingBox.Max.X - BoundingBox.Min.X) * (BoundingBox.Max.Y - BoundingBox.Min.Y) * (BoundingBox.Max.Z - BoundingBox.Min.Z); //if (Math.Abs(currentVolume - DEBUGlastVolume) > .000001 * (DEBUGlastVolume + currentVolume)) // Debug.WriteLine(":Break>:)"); } internal static LockingResourcePool> nodeListPool = new LockingResourcePool>(); internal void Revalidate() { //The revalidation procedure 'reconstructs' a portion of the tree that has expanded beyond its old limits. //To reconstruct the tree, the nodes (internal and leaf) currently in use need to be retrieved. //The internal nodes can be put back into the nodePool. LeafNodes are reinserted one by one into the new tree. //To retrieve the nodes, a depth-first search is used. //Given that an internal node is being revalidated, it is known that there are at least two children. var oldChildA = childA; var oldChildB = childB; childA = null; childB = null; var leafNodes = nodeListPool.Take(); oldChildA.RetrieveNodes(leafNodes); oldChildB.RetrieveNodes(leafNodes); for (int i = 0; i < leafNodes.Count; i++) leafNodes.Elements[i].Refit(); Reconstruct(leafNodes, 0, leafNodes.Count); leafNodes.Clear(); nodeListPool.GiveBack(leafNodes); } void Reconstruct(RawList leafNodes, int begin, int end) { //It is known that we have 2 children; this is safe. //This is because this is only an internal node if the parent figured out it involved more than 2 leaf nodes, OR //this node was the initiator of the revalidation (in which case, it was an internal node with 2+ children). BoundingBox.CreateMerged(ref leafNodes.Elements[begin].BoundingBox, ref leafNodes.Elements[begin + 1].BoundingBox, out BoundingBox); for (int i = begin + 2; i < end; i++) { BoundingBox.CreateMerged(ref BoundingBox, ref leafNodes.Elements[i].BoundingBox, out BoundingBox); } Vector3 offset; Vector3.Subtract(ref BoundingBox.Max, ref BoundingBox.Min, out offset); currentVolume = offset.X * offset.Y * offset.Z; maximumVolume = currentVolume * MaximumVolumeScale; //Pick an axis and sort along it. if (offset.X > offset.Y && offset.X > offset.Z) { //Maximum variance axis is X. Array.Sort(leafNodes.Elements, begin, end - begin, xComparer); } else if (offset.Y > offset.Z) { //Maximum variance axis is Y. Array.Sort(leafNodes.Elements, begin, end - begin, yComparer); } else { //Maximum variance axis is Z. Array.Sort(leafNodes.Elements, begin, end - begin, zComparer); } //Find the median index. int median = (begin + end) / 2; if (median - begin >= 2) { //There are 2 or more leaf nodes remaining in the first half. The next childA will be an internal node. var newChildA = nodePool.Take(); newChildA.Reconstruct(leafNodes, begin, median); childA = newChildA; } else { //There is only 1 leaf node remaining in this half. It's a leaf node. childA = leafNodes.Elements[begin]; } if (end - median >= 2) { //There are 2 or more leaf nodes remaining in the second half. The next childB will be an internal node. var newChildB = nodePool.Take(); newChildB.Reconstruct(leafNodes, median, end); childB = newChildB; } else { //There is only 1 leaf node remaining in this half. It's a leaf node. childB = leafNodes.Elements[median]; } } internal override void RetrieveNodes(RawList leafNodes) { var oldChildA = childA; var oldChildB = childB; childA = null; childB = null; nodePool.GiveBack(this); //Give internal nodes back to the pool before going deeper to minimize the creation of additional internal instances. oldChildA.RetrieveNodes(leafNodes); oldChildB.RetrieveNodes(leafNodes); } internal override void CollectMultithreadingNodes(int splitDepth, int currentDepth, RawList multithreadingSourceNodes) { if (currentVolume > maximumVolume) { //Very rarely, one of these extremely high level nodes will need to be revalidated. This isn't great. //We may lose a frame. This could be independently multithreaded, but the benefit is unknown. Revalidate(); return; } if (currentDepth == splitDepth) { //We are deep enough in the tree where our children will act as the starting point for multithreaded refits. //The split depth ensures that we have enough tasks to thread across our core count. multithreadingSourceNodes.Add(childA); multithreadingSourceNodes.Add(childB); } else { childA.CollectMultithreadingNodes(splitDepth, currentDepth + 1, multithreadingSourceNodes); childB.CollectMultithreadingNodes(splitDepth, currentDepth + 1, multithreadingSourceNodes); } } internal override void PostRefit(int splitDepth, int currentDepth) { if (splitDepth > currentDepth) { //We are not yet back to the nodes that triggered the multithreaded split. //Need to go deeper into the tree. childA.PostRefit(splitDepth, currentDepth + 1); childB.PostRefit(splitDepth, currentDepth + 1); } BoundingBox.CreateMerged(ref childA.BoundingBox, ref childB.BoundingBox, out BoundingBox); currentVolume = (BoundingBox.Max.X - BoundingBox.Min.X) * (BoundingBox.Max.Y - BoundingBox.Min.Y) * (BoundingBox.Max.Z - BoundingBox.Min.Z); } internal override void GetMultithreadedOverlaps(Node opposingNode, int splitDepth, int currentDepth, DynamicHierarchy owner, RawList multithreadingSourceOverlaps) { bool intersects; if (currentDepth == splitDepth) { //We've reached the depth where our child comparisons will be multithreaded. if (this == opposingNode) { //We are being compared against ourselves! //Obviously we're an internal node, so spawn three children: //A versus A: if (!childA.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childA, b = childA }); //B versus B: if (!childB.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childB, b = childB }); //A versus B (if they intersect): childA.BoundingBox.Intersects(ref childB.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childA, b = childB }); } else { //Two different nodes. The other one may be a leaf. if (opposingNode.IsLeaf) { //If it's a leaf, go deeper in our hierarchy, but not the opposition. childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childA, b = opposingNode }); childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childB, b = opposingNode }); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in both hierarchies. childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childA, b = opposingChildA }); childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childA, b = opposingChildB }); childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childB, b = opposingChildA }); childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = childB, b = opposingChildB }); } } return; } if (this == opposingNode) { //We are being compared against ourselves! //Obviously we're an internal node, so spawn three children: //A versus A: if (!childA.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. childA.GetMultithreadedOverlaps(childA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); //B versus B: if (!childB.IsLeaf) //This is performed in the child method usually by convention, but this saves some time. childB.GetMultithreadedOverlaps(childB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); //A versus B (if they intersect): childA.BoundingBox.Intersects(ref childB.BoundingBox, out intersects); if (intersects) childA.GetMultithreadedOverlaps(childB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); } else { //Two different nodes. The other one may be a leaf. if (opposingNode.IsLeaf) { //If it's a leaf, go deeper in our hierarchy, but not the opposition. childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childA.GetMultithreadedOverlaps(opposingNode, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childB.GetMultithreadedOverlaps(opposingNode, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in both hierarchies. childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childA.GetMultithreadedOverlaps(opposingChildA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childA.GetMultithreadedOverlaps(opposingChildB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childB.GetMultithreadedOverlaps(opposingChildA, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childB.GetMultithreadedOverlaps(opposingChildB, splitDepth, currentDepth + 1, owner, multithreadingSourceOverlaps); } } } internal override bool Remove(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode) { if (childA.Remove(entry, out leafNode, out replacementNode)) { if (childA.IsLeaf) replacementNode = childB; else { //It was not a leaf node, but a child found the leaf. //Change the child to the replacement node. childA = replacementNode; replacementNode = this; //We don't need to be replaced! } return true; } if (childB.Remove(entry, out leafNode, out replacementNode)) { if (childB.IsLeaf) replacementNode = childA; else { //It was not a leaf node, but a child found the leaf. //Change the child to the replacement node. childB = replacementNode; replacementNode = this; //We don't need to be replaced! } return true; } replacementNode = this; return false; } internal override bool RemoveFast(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode) { //Only bother checking deeper in the path if the entry and child have overlapping bounding boxes. bool intersects; childA.BoundingBox.Intersects(ref entry.boundingBox, out intersects); if (intersects && childA.RemoveFast(entry, out leafNode, out replacementNode)) { if (childA.IsLeaf) replacementNode = childB; else { //It was not a leaf node, but a child found the leaf. //Change the child to the replacement node. childA = replacementNode; replacementNode = this; //We don't need to be replaced! } return true; } childB.BoundingBox.Intersects(ref entry.boundingBox, out intersects); if (intersects && childB.RemoveFast(entry, out leafNode, out replacementNode)) { if (childB.IsLeaf) replacementNode = childA; else { //It was not a leaf node, but a child found the leaf. //Change the child to the replacement node. childB = replacementNode; replacementNode = this; //We don't need to be replaced! } return true; } replacementNode = this; leafNode = null; return false; } static XComparer xComparer = new XComparer(); static YComparer yComparer = new YComparer(); static ZComparer zComparer = new ZComparer(); //Try using Comparer instead of IComparer- is there some tricky hardcoded optimization? class XComparer : IComparer { public int Compare(LeafNode x, LeafNode y) { return x.BoundingBox.Min.X < y.BoundingBox.Min.X ? -1 : 1; } } class YComparer : IComparer { public int Compare(LeafNode x, LeafNode y) { return x.BoundingBox.Min.Y < y.BoundingBox.Min.Y ? -1 : 1; } } class ZComparer : IComparer { public int Compare(LeafNode x, LeafNode y) { return x.BoundingBox.Min.Z < y.BoundingBox.Min.Z ? -1 : 1; } } } internal sealed class LeafNode : Node { BroadPhaseEntry element; internal override Node ChildA { get { return null; } } internal override Node ChildB { get { return null; } } internal override BroadPhaseEntry Element { get { return element; } } internal override bool IsLeaf { get { return true; } } internal void Initialize(BroadPhaseEntry element) { this.element = element; BoundingBox = element.BoundingBox; } internal void CleanUp() { element = null; } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Our parent already tested the bounding box. All that's left is to add myself to the list. outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(Node opposingNode, DynamicHierarchy owner) { bool intersects; //note: This is never executed when the opposing node is the current node. if (opposingNode.IsLeaf) { //We're both leaves! Our parents have already done the testing for us, so we know we're overlapping. owner.TryToAddOverlap(element, opposingNode.Element); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in the opposing hierarchy. BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildA, owner); BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildB, owner); } } internal override bool TryToInsert(LeafNode node, out Node treeNode) { var newTreeNode = InternalNode.nodePool.Take(); BoundingBox.CreateMerged(ref BoundingBox, ref node.BoundingBox, out newTreeNode.BoundingBox); Vector3 offset; Vector3.Subtract(ref newTreeNode.BoundingBox.Max, ref newTreeNode.BoundingBox.Min, out offset); newTreeNode.currentVolume = offset.X * offset.Y * offset.Z; //newTreeNode.maximumVolume = newTreeNode.currentVolume * InternalNode.MaximumVolumeScale; newTreeNode.childA = this; newTreeNode.childB = node; treeNode = newTreeNode; return true; } public override string ToString() { return element.ToString(); } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; depths.Add(depth); } internal override void Refit() { BoundingBox = element.boundingBox; } internal override void RetrieveNodes(RawList leafNodes) { Refit(); leafNodes.Add(this); } internal override void CollectMultithreadingNodes(int splitDepth, int currentDepth, RawList multithreadingSourceNodes) { //This could happen if there are almost no elements in the tree. No biggie- do nothing! } internal override void PostRefit(int splitDepth, int currentDepth) { //This could happen if there are almost no elements in the tree. Just do a normal leaf refit. BoundingBox = element.boundingBox; } internal override void GetMultithreadedOverlaps(Node opposingNode, int splitDepth, int currentDepth, DynamicHierarchy owner, RawList multithreadingSourceOverlaps) { bool intersects; //note: This is never executed when the opposing node is the current node. if (opposingNode.IsLeaf) { //We're both leaves! Our parents have already done the testing for us, so we know we're overlapping. owner.TryToAddOverlap(element, opposingNode.Element); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; if (splitDepth == currentDepth) { //Time to add the child overlaps to the multithreading set! BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = this, b = opposingChildA }); BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) multithreadingSourceOverlaps.Add(new DynamicHierarchy.NodePair() { a = this, b = opposingChildB }); return; } //If it's not a leaf, try to go deeper in the opposing hierarchy. BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildA, owner); BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildB, owner); } } internal override bool Remove(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode) { replacementNode = null; if (element == entry) { leafNode = this; return true; } leafNode = null; return false; } internal override bool RemoveFast(BroadPhaseEntry entry, out LeafNode leafNode, out Node replacementNode) { //The fastremove leaf node procedure is identical to the brute force approach. //We don't need to perform any bounding box test here; if they're equal, they're equal! replacementNode = null; if (element == entry) { leafNode = this; return true; } leafNode = null; return false; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/Hierarchies/DynamicHierarchyQueryAccelerator.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems.Hierarchies { /// /// Interface to the DynamicHierarchy's volume query systems. /// public class DynamicHierarchyQueryAccelerator : IQueryAccelerator { private readonly DynamicHierarchy hierarchy; internal DynamicHierarchyQueryAccelerator(DynamicHierarchy hierarchy) { this.hierarchy = hierarchy; } /// /// Gets the broad phase associated with this query accelerator. /// public BroadPhase BroadPhase { get { return hierarchy; } } /// /// Collects all entries with bounding boxes which intersect the given bounding box. /// /// Bounding box to test against the world. /// Entries of the space which intersect the bounding box. public void GetEntries(BoundingBox box, IList entries) { if (hierarchy.root != null) hierarchy.root.GetOverlaps(ref box, entries); } /// /// Collects all entries with bounding boxes which intersect the given frustum. /// /// Frustum to test against the world. /// Entries of the space which intersect the frustum. public void GetEntries(BoundingFrustum frustum, IList entries) { if (hierarchy.root != null) hierarchy.root.GetOverlaps(ref frustum, entries); } /// /// Collects all entries with bounding boxes which intersect the given sphere. /// /// Sphere to test against the world. /// Entries of the space which intersect the sphere. public void GetEntries(BoundingSphere sphere, IList entries) { if (hierarchy.root != null) hierarchy.root.GetOverlaps(ref sphere, entries); } /// /// Finds all intersections between the ray and broad phase entries. /// /// Ray to test against the structure. /// Maximum length of the ray in units of the ray's direction's length. /// Entries which have bounding boxes that overlap the ray. public bool RayCast(Ray ray, float maximumLength, IList entries) { if (hierarchy.root != null) { hierarchy.root.GetOverlaps(ref ray, maximumLength, entries); return entries.Count > 0; } return false; } /// /// Finds all intersections between the ray and broad phase entries. /// /// Ray to test against the structure. /// Entries which have bounding boxes that overlap the ray. public bool RayCast(Ray ray, IList entries) { if (hierarchy.root != null) { hierarchy.root.GetOverlaps(ref ray, float.MaxValue, entries); return entries.Count > 0; } return false; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/IBoundingBoxOwner.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems { /// /// Requires that a class have a BoundingBox. /// public interface IBoundingBoxOwner { /// /// Gets the bounding box of the object. /// BoundingBox BoundingBox { get; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/IBroadPhaseEntryOwner.cs ================================================ using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.BroadPhaseSystems { /// /// Requires that a class own a BroadPhaseEntry. /// public interface IBroadPhaseEntryOwner { /// /// Gets the broad phase entry associated with this object. /// BroadPhaseEntry Entry { get; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/IQueryAccelerator.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems { /// /// Defines a system that accelerates bounding volume and ray cast queries. /// public interface IQueryAccelerator { /// /// Gets the broad phase associated with this query accelerator, if any. /// BroadPhase BroadPhase { get; } /// /// Gets the broad phase entries overlapping the ray. /// ///Ray to test. ///Overlapped entries. ///Whether or not the ray hit anything. bool RayCast(Ray ray, IList outputIntersections); /// /// Gets the broad phase entries overlapping the ray. /// ///Ray to test. /// Maximum length of the ray in units of the ray's direction's length. ///Overlapped entries. ///Whether or not the ray hit anything. bool RayCast(Ray ray, float maximumLength, IList outputIntersections); //There's no single-hit version because the TOI on queries isn't really meaningful. //TODO: IQueryAccelerator + BroadPhase. Both have add methods. A user might expect to be able to add separately, but that doesn't really work. //Consider pulling the query accelerator into the broadphase so people consider it to be a part of the broadphase- it accelerates queries against the broadphase. //If someone wanted to raycast against something other than the broadphase, they can create an IQueryAccelerator of some kind in isolation. /// /// Gets the entries with bounding boxes which overlap the bounding shape. /// /// Bounding shape to test. /// Overlapped entries. void GetEntries(BoundingBox boundingShape, IList overlaps); /// /// Gets the entries with bounding boxes which overlap the bounding shape. /// /// Bounding shape to test. /// Overlapped entries. void GetEntries(BoundingSphere boundingShape, IList overlaps); /// /// Gets the entries with bounding boxes which overlap the bounding shape. /// /// Bounding shape to test. /// Overlapped entries. void GetEntries(BoundingFrustum boundingShape, IList overlaps); } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/Grid2DEntry.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { class Grid2DEntry { internal void Initialize(BroadPhaseEntry entry) { this.item = entry; Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Min, out previousMin); Grid2DSortAndSweep.ComputeCell(ref entry.boundingBox.Max, out previousMax); } internal BroadPhaseEntry item; internal Int2 previousMin; internal Int2 previousMax; } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/Grid2DSortAndSweep.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Threading; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { /// /// Broad phase implementation that partitions objects into a 2d grid, and then performs a sort and sweep on the final axis. /// /// /// This broad phase typically has very good collision performance and scales well with multithreading, but its query times can sometimes be worse than tree-based systems /// since it must scan cells. Keeping rays as short as possible helps avoid unnecessary cell checks. /// The performance can degrade noticeably in some situations involving significant off-axis motion. /// public class Grid2DSortAndSweep : BroadPhase { /// /// Gets or sets the width of cells in the 2D grid. For sparser, larger scenes, increasing this can help performance. /// For denser scenes, decreasing this may help. /// public static float CellSize { get { return 1 / cellSizeInverse; } set { cellSizeInverse = 1 / value; } } //TODO: Try different values for this. internal static float cellSizeInverse = 1 / 8f; internal static void ComputeCell(ref Vector3 v, out Int2 cell) { cell.Y = (int)Math.Floor(v.Y * cellSizeInverse); cell.Z = (int)Math.Floor(v.Z * cellSizeInverse); } internal SortedGrid2DSet cellSet = new SortedGrid2DSet(); RawList entries = new RawList(); Action updateEntry, updateCell; /// /// Constructs a grid-based sort and sweep broad phase. /// /// Thread manager to use for the broad phase. public Grid2DSortAndSweep(IThreadManager threadManager) :base(threadManager) { updateEntry = UpdateEntry; updateCell = UpdateCell; QueryAccelerator = new Grid2DSortAndSweepQueryAccelerator(this); } /// /// Constructs a grid-based sort and sweep broad phase. /// public Grid2DSortAndSweep() { updateEntry = UpdateEntry; updateCell = UpdateCell; QueryAccelerator = new Grid2DSortAndSweepQueryAccelerator(this); } UnsafeResourcePool entryPool = new UnsafeResourcePool(); /// /// Adds an entry to the broad phase. /// /// Entry to add. public override void Add(BroadPhaseEntry entry) { base.Add(entry); //Entities do not set up their own bounding box before getting stuck in here. If they're all zeroed out, the tree will be horrible. Vector3 offset; Vector3.Subtract(ref entry.boundingBox.Max, ref entry.boundingBox.Min, out offset); if (offset.X * offset.Y * offset.Z == 0) entry.UpdateBoundingBox(); var newEntry = entryPool.Take(); newEntry.Initialize(entry); entries.Add(newEntry); //Add the object to the grid. for (int i = newEntry.previousMin.Y; i <= newEntry.previousMax.Y; i++) { for (int j = newEntry.previousMin.Z; j <= newEntry.previousMax.Z; j++) { var index = new Int2 {Y = i, Z = j}; cellSet.Add(ref index, newEntry); } } } /// /// Removes an entry from the broad phase. /// /// Entry to remove. public override void Remove(BroadPhaseEntry entry) { base.Remove(entry); for (int i = 0; i < entries.Count; i++) { if (entries.Elements[i].item == entry) { var gridEntry = entries.Elements[i]; entries.RemoveAt(i); //Remove the object from any cells that it is held by. for (int j = gridEntry.previousMin.Y; j <= gridEntry.previousMax.Y; j++) { for (int k = gridEntry.previousMin.Z; k <= gridEntry.previousMax.Z; k++) { var index = new Int2 {Y = j, Z = k}; cellSet.Remove(ref index, gridEntry); } } gridEntry.item = null; entryPool.GiveBack(gridEntry); return; } } } protected override void UpdateMultithreaded() { lock (Locker) { Overlaps.Clear(); //Update the entries! ThreadManager.ForLoop(0, entries.Count, updateEntry); //Update the cells! ThreadManager.ForLoop(0, cellSet.count, updateCell); } } protected override void UpdateSingleThreaded() { lock (Locker) { Overlaps.Clear(); //Update the placement of objects. for (int i = 0; i < entries.Count; i++) { //Compute the current cells occupied by the entry. var entry = entries.Elements[i]; Int2 min, max; ComputeCell(ref entry.item.boundingBox.Min, out min); ComputeCell(ref entry.item.boundingBox.Max, out max); //For any cell that used to be occupied (defined by the previous min/max), //remove the entry. for (int j = entry.previousMin.Y; j <= entry.previousMax.Y; j++) { for (int k = entry.previousMin.Z; k <= entry.previousMax.Z; k++) { if (j >= min.Y && j <= max.Y && k >= min.Z && k <= max.Z) continue; //This cell is currently occupied, do not remove. var index = new Int2 {Y = j, Z = k}; cellSet.Remove(ref index, entry); } } //For any cell that is newly occupied (was not previously contained), //add the entry. for (int j = min.Y; j <= max.Y; j++) { for (int k = min.Z; k <= max.Z; k++) { if (j >= entry.previousMin.Y && j <= entry.previousMax.Y && k >= entry.previousMin.Z && k <= entry.previousMax.Z) continue; //This cell is already occupied, do not add. var index = new Int2 {Y = j, Z = k}; cellSet.Add(ref index, entry); } } entry.previousMin = min; entry.previousMax = max; } //Update each cell to find the overlaps. for (int i = 0; i < cellSet.count; i++) { cellSet.cells.Elements[i].UpdateOverlaps(this); } } } //TODO: Cell change operations take a while. Spin lock can't efficiently wait that long. //This causes some pretty horrible scaling problems in some scenarios. //Improving the cell set operations directly should improve that problem and the query times noticeably. SpinLock cellSetLocker = new SpinLock(); void UpdateEntry(int i) { //Compute the current cells occupied by the entry. var entry = entries.Elements[i]; Int2 min, max; ComputeCell(ref entry.item.boundingBox.Min, out min); ComputeCell(ref entry.item.boundingBox.Max, out max); //For any cell that used to be occupied (defined by the previous min/max), //remove the entry. for (int j = entry.previousMin.Y; j <= entry.previousMax.Y; j++) { for (int k = entry.previousMin.Z; k <= entry.previousMax.Z; k++) { if (j >= min.Y && j <= max.Y && k >= min.Z && k <= max.Z) continue; //This cell is currently occupied, do not remove. var index = new Int2 {Y = j, Z = k}; cellSetLocker.Enter(); cellSet.Remove(ref index, entry); cellSetLocker.Exit(); } } //For any cell that is newly occupied (was not previously contained), //add the entry. for (int j = min.Y; j <= max.Y; j++) { for (int k = min.Z; k <= max.Z; k++) { if (j >= entry.previousMin.Y && j <= entry.previousMax.Y && k >= entry.previousMin.Z && k <= entry.previousMax.Z) continue; //This cell is already occupied, do not add. var index = new Int2 {Y = j, Z = k}; cellSetLocker.Enter(); cellSet.Add(ref index, entry); cellSetLocker.Exit(); } } entry.previousMin = min; entry.previousMax = max; } void UpdateCell(int i) { //TODO: Consider permuting. //In some simulations, there may be a ton of unoccupied cells. //It would be best to distribute these over the threads. //(int)((i * 122949823L) % cellSet.count) //(i * 122949823L) % cellSet.count cellSet.cells.Elements[i].UpdateOverlaps(this); } } struct Int2 { internal int Y; internal int Z; public override int GetHashCode() { return Y + Z; } internal int GetSortingHash() { return (int)(Y * 15485863L + Z * 32452843L); } public override string ToString() { return "{" + Y + ", " + Z + "}"; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/Grid2DSortAndSweepQueryAccelerator.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { public class Grid2DSortAndSweepQueryAccelerator : IQueryAccelerator { Grid2DSortAndSweep owner; public Grid2DSortAndSweepQueryAccelerator(Grid2DSortAndSweep owner) { this.owner = owner; } /// /// Gets the broad phase associated with this query accelerator. /// public BroadPhase BroadPhase { get { return owner; } } public bool RayCast(Microsoft.Xna.Framework.Ray ray, IList outputIntersections) { throw new NotSupportedException("The Grid2DSortAndSweep broad phase cannot accelerate infinite ray casts. Consider specifying a maximum length or using a broad phase which supports infinite ray casts."); } public bool RayCast(Microsoft.Xna.Framework.Ray ray, float maximumLength, IList outputIntersections) { if (maximumLength == float.MaxValue) throw new NotSupportedException("The Grid2DSortAndSweep broad phase cannot accelerate infinite ray casts. Consider specifying a maximum length or using a broad phase which supports infinite ray casts."); //Use 2d line rasterization. //Compute the exit location in the cell. //Test against each bounding box up until the exit value is reached. float length = 0; Int2 cellIndex; Vector3 currentPosition = ray.Position; Grid2DSortAndSweep.ComputeCell(ref currentPosition, out cellIndex); while (true) { float cellWidth = 1 / Grid2DSortAndSweep.cellSizeInverse; float nextT; //Distance along ray to next boundary. float nextTy; //Distance along ray to next boundary along y axis. float nextTz; //Distance along ray to next boundary along z axis. //Find the next cell. if (ray.Direction.Y > 0) nextTy = ((cellIndex.Y + 1) * cellWidth - currentPosition.Y) / ray.Direction.Y; else if (ray.Direction.Y < 0) nextTy = ((cellIndex.Y) * cellWidth - currentPosition.Y) / ray.Direction.Y; else nextTy = 10e10f; if (ray.Direction.Z > 0) nextTz = ((cellIndex.Z + 1) * cellWidth - currentPosition.Z) / ray.Direction.Z; else if (ray.Direction.Z < 0) nextTz = ((cellIndex.Z) * cellWidth - currentPosition.Z) / ray.Direction.Z; else nextTz = 10e10f; bool yIsMinimum = nextTy < nextTz; nextT = yIsMinimum ? nextTy : nextTz; //Grab the cell that we are currently in. GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { float endingX; if(ray.Direction.X < 0) endingX = currentPosition.X; else endingX = currentPosition.X + ray.Direction.X * nextT; //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int i = 0; i < cell.entries.Count && cell.entries.Elements[i].item.boundingBox.Min.X <= endingX; i++) //TODO: Try additional x axis pruning? { float? intersects; var item = cell.entries.Elements[i].item; ray.Intersects(ref item.boundingBox, out intersects); if (intersects != null && intersects < maximumLength && !outputIntersections.Contains(item)) { outputIntersections.Add(item); } } } //Move the position forward. length += nextT; if (length > maximumLength) //Note that this catches the case in which the ray is pointing right down the middle of a row (resulting in a nextT of 10e10f). break; Vector3 offset; Vector3.Multiply(ref ray.Direction, nextT, out offset); Vector3.Add(ref offset, ref currentPosition, out currentPosition); if (yIsMinimum) if (ray.Direction.Y < 0) cellIndex.Y -= 1; else cellIndex.Y += 1; else if (ray.Direction.Z < 0) cellIndex.Z -= 1; else cellIndex.Z += 1; } return outputIntersections.Count > 0; } public void GetEntries(Microsoft.Xna.Framework.BoundingBox boundingShape, IList overlaps) { //Compute the min and max of the bounding box. //Loop through the cells and select bounding boxes which overlap the x axis. Int2 min, max; Grid2DSortAndSweep.ComputeCell(ref boundingShape.Min, out min); Grid2DSortAndSweep.ComputeCell(ref boundingShape.Max, out max); for (int i = min.Y; i <= max.Y; i++) { for (int j = min.Z; j <= max.Z; j++) { //Grab the cell that we are currently in. Int2 cellIndex; cellIndex.Y = i; cellIndex.Z = j; GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int k = 0; k < cell.entries.Count && cell.entries.Elements[k].item.boundingBox.Min.X <= boundingShape.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test. { bool intersects; var item = cell.entries.Elements[k].item; boundingShape.Intersects(ref item.boundingBox, out intersects); if (intersects && !overlaps.Contains(item)) { overlaps.Add(item); } } } } } } public void GetEntries(Microsoft.Xna.Framework.BoundingSphere boundingShape, IList overlaps) { //Create a bounding box based on the bounding sphere. //Compute the min and max of the bounding box. //Loop through the cells and select bounding boxes which overlap the x axis. #if !WINDOWS Vector3 offset = new Vector3(); #else Vector3 offset; #endif offset.X = boundingShape.Radius; offset.Y = offset.X; offset.Z = offset.Y; BoundingBox box; Vector3.Add(ref boundingShape.Center, ref offset, out box.Max); Vector3.Subtract(ref boundingShape.Center, ref offset, out box.Min); Int2 min, max; Grid2DSortAndSweep.ComputeCell(ref box.Min, out min); Grid2DSortAndSweep.ComputeCell(ref box.Max, out max); for (int i = min.Y; i <= max.Y; i++) { for (int j = min.Z; j <= max.Z; j++) { //Grab the cell that we are currently in. Int2 cellIndex; cellIndex.Y = i; cellIndex.Z = j; GridCell2D cell; if (owner.cellSet.TryGetCell(ref cellIndex, out cell)) { //To fully accelerate this, the entries list would need to contain both min and max interval markers. //Since it only contains the sorted min intervals, we can't just start at a point in the middle of the list. //Consider some giant bounding box that spans the entire list. for (int k = 0; k < cell.entries.Count && cell.entries.Elements[k].item.boundingBox.Min.X <= box.Max.X; k++) //TODO: Try additional x axis pruning? A bit of optimization potential due to overlap with AABB test. { bool intersects; var item = cell.entries.Elements[k].item; boundingShape.Intersects(ref item.boundingBox, out intersects); if (intersects && !overlaps.Contains(item)) { overlaps.Add(item); } } } } } } public void GetEntries(Microsoft.Xna.Framework.BoundingFrustum boundingShape, IList overlaps) { throw new NotSupportedException("The Grid2DSortAndSweep broad phase cannot accelerate frustum tests. Consider using a broad phase which supports frustum tests or using a custom solution."); } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/GridCell2D.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; using System.Diagnostics; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { class GridCell2D { internal RawList entries = new RawList(); internal Int2 cellIndex; internal int sortingHash; internal void Initialize(ref Int2 cellIndex, int hash) { this.cellIndex = cellIndex; sortingHash = hash; } internal int GetIndex(float x) { int minIndex = 0; //inclusive int maxIndex = entries.Count; //exclusive int index = 0; while (maxIndex - minIndex > 0) { index = (maxIndex + minIndex) / 2; if (entries.Elements[index].item.boundingBox.Min.X > x) maxIndex = index; else if (entries.Elements[index].item.boundingBox.Min.X < x) minIndex = ++index; else break; //Found an equal value! } return index; } internal void Add(Grid2DEntry entry) { //binary search for the approximately correct location. This helps prevent large first-frame sort times. entries.Insert(GetIndex(entry.item.boundingBox.Min.X), entry); } internal void Remove(Grid2DEntry entry) { entries.Remove(entry); } internal void UpdateOverlaps(Grid2DSortAndSweep owner) { //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entries.Count; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.item.boundingBox.Min.X < entries.Elements[j].item.boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else break; } } //Sweep the list looking for overlaps. for (int i = 0; i < entries.Count; i++) { Grid2DEntry a = entries.Elements[i]; Grid2DEntry b; //TODO: Microoptimize for (int j = i + 1; j < entries.Count && a.item.boundingBox.Max.X >= (b = entries.Elements[j]).item.boundingBox.Min.X; j++) { if (!(a.item.boundingBox.Min.Y > b.item.boundingBox.Max.Y || a.item.boundingBox.Max.Y < b.item.boundingBox.Min.Y || a.item.boundingBox.Min.Z > b.item.boundingBox.Max.Z || a.item.boundingBox.Max.Z < b.item.boundingBox.Min.Z)) { //Now we know this pair is overlapping, but we do not know if this overlap is already added. //Rather than use a hashset or other heavy structure to check, rely on the rules of the grid. //It's possible to avoid adding pairs entirely unless we are the designated 'responsible' cell. //All other cells will defer to the cell 'responsible' for a pair. //A simple rule for determining the cell which is responsible is to choose the cell which is the //smallest index in the shared cells. So first, compute that cell's index. Int2 minimumSharedIndex = a.previousMin; if (minimumSharedIndex.Y < b.previousMin.Y) minimumSharedIndex.Y = b.previousMin.Y; if (minimumSharedIndex.Y > b.previousMax.Y) minimumSharedIndex.Y = b.previousMax.Y; if (minimumSharedIndex.Z < b.previousMin.Z) minimumSharedIndex.Z = b.previousMin.Z; if (minimumSharedIndex.Z > b.previousMax.Z) minimumSharedIndex.Z = b.previousMax.Z; //Is our cell the minimum cell? if (minimumSharedIndex.Y == cellIndex.Y && minimumSharedIndex.Z == cellIndex.Z) owner.TryToAddOverlap(a.item, b.item); } } } } public override string ToString() { return "{" + cellIndex.Y + ", " + cellIndex.Z + "}: " + entries.Count; } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/SortAndSweep1D.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Threading; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { /// /// Simple and standard implementation of the one-axis sort and sweep (sweep and prune) algorithm. /// /// /// In small scenarios, it can be the quickest option. It uses very little memory. /// However, it tends to scale poorly relative to other options and can slow down significantly when entries cluster along the axis. /// Additionally, it supports no queries at all. /// public class SortAndSweep1D : BroadPhase { /// /// Constructs a new sort and sweep broad phase. /// /// Thread manager to use in the broad phase. public SortAndSweep1D(IThreadManager threadManager) : base(threadManager) { sweepSegment = Sweep; backbuffer = new RawList(); } /// /// Constructs a new sort and sweep broad phase. /// public SortAndSweep1D() { sweepSegment = Sweep; backbuffer = new RawList(); } RawList entries = new RawList(); /// /// Adds an entry to the broad phase. /// /// Entry to add. public override void Add(BroadPhaseEntry entry) { base.Add(entry); //Entities do not set up their own bounding box before getting stuck in here. If they're all zeroed out, the tree will be horrible. Vector3 offset; Vector3.Subtract(ref entry.boundingBox.Max, ref entry.boundingBox.Min, out offset); if (offset.X * offset.Y * offset.Z == 0) entry.UpdateBoundingBox(); //binary search for the approximately correct location. This helps prevent large first-frame sort times. int minIndex = 0; //inclusive int maxIndex = entries.Count; //exclusive int index = 0; while (maxIndex - minIndex > 0) { index = (maxIndex + minIndex) / 2; if (entries.Elements[index].boundingBox.Min.X > entry.boundingBox.Min.X) maxIndex = index; else if (entries.Elements[index].boundingBox.Min.X < entry.boundingBox.Min.X) minIndex = ++index; else break; //Found an equal value! } entries.Insert(index, entry); } /// /// Removes an entry from the broad phase. /// /// Entry to remove. public override void Remove(BroadPhaseEntry entry) { base.Remove(entry); entries.Remove(entry); } Action sweepSegment; protected override void UpdateMultithreaded() { if (backbuffer.Count != entries.Count) { backbuffer.Capacity = entries.Capacity; backbuffer.Count = entries.Count; } Overlaps.Clear(); //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entries.Count; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.X < entries.Elements[j].boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else break; } } //TODO: Multithreaded sorting could help in some large cases. //The overhead involved in this implementation is way too high for reasonable object counts. //for (int i = 0; i < sortSegmentCount; i++) // SortSection(i); ////MergeSections(0, 1); ////MergeSections(2, 3); ////MergeSections(0, 2); ////MergeSections(1, 3); //MergeSections(0, 1); //MergeSections(2, 3); //MergeSections(4, 5); //MergeSections(6, 7); //MergeSections(0, 2); //MergeSections(1, 3); //MergeSections(4, 6); //MergeSections(5, 7); //MergeSections(0, 4); //MergeSections(1, 5); //MergeSections(2, 6); //MergeSections(3, 7); //var temp = backbuffer; //backbuffer = entries; //entries = temp; ThreadManager.ForLoop(0, sweepSegmentCount, sweepSegment); } protected override void UpdateSingleThreaded() { Overlaps.Clear(); //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entries.Count; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.X < entries.Elements[j].boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else break; } } //Sweep the list looking for overlaps. for (int i = 0; i < entries.Count; i++) { BoundingBox a = entries.Elements[i].boundingBox; for (int j = i + 1; j < entries.Count && a.Max.X >= entries.Elements[j].boundingBox.Min.X; j++) { if (!(a.Min.Y > entries.Elements[j].boundingBox.Max.Y || a.Max.Y < entries.Elements[j].boundingBox.Min.Y || a.Min.Z > entries.Elements[j].boundingBox.Max.Z || a.Max.Z < entries.Elements[j].boundingBox.Min.Z)) { TryToAddOverlap(entries.Elements[i], entries.Elements[j]); } } } } //TODO: It is possible to distribute things a bit better. Instead of lumping all of the remainder into the final, put and int sweepSegmentCount = 32; void Sweep(int segment) { int intervalLength = entries.Count / sweepSegmentCount; int end; if (segment == sweepSegmentCount - 1) end = entries.Count; else end = intervalLength * (segment + 1); for (int i = intervalLength * segment; i < end; i++) { BoundingBox a = entries.Elements[i].boundingBox; for (int j = i + 1; j < entries.Count && a.Max.X >= entries.Elements[j].boundingBox.Min.X; j++) { if (!(a.Min.Y > entries.Elements[j].boundingBox.Max.Y || a.Max.Y < entries.Elements[j].boundingBox.Min.Y || a.Min.Z > entries.Elements[j].boundingBox.Max.Z || a.Max.Z < entries.Elements[j].boundingBox.Min.Z)) { TryToAddOverlap(entries.Elements[i], entries.Elements[j]); } } } } int sortSegmentCount = 4; void SortSection(int section) { int intervalLength = entries.Count / sortSegmentCount; int start = section * intervalLength; int end; if (section == sortSegmentCount - 1) end = entries.Count; else end = intervalLength * (section + 1); for (int i = start + 1; i < end; i++) { var entry = entries.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.X < entries.Elements[j].boundingBox.Min.X) { entries.Elements[j + 1] = entries.Elements[j]; entries.Elements[j] = entry; } else break; } } } RawList backbuffer; void MergeSections(int a, int b) { int intervalLength = entries.Count / sortSegmentCount; //'a' is known to be less than b, which means it cannot be the last section. int aStart = intervalLength * a; int aEnd = intervalLength * (a + 1); int bStart = intervalLength * b; int bEnd; int length; if (b == sortSegmentCount - 1) { bEnd = entries.Count; length = intervalLength + entries.Count - bStart; } else { bEnd = intervalLength * (b + 1); length = intervalLength * 2; } int aIndex = aStart, bIndex = bStart; int i = 0; while (i < length) { //Compute the location in the buffer array to put the minimum element. int bufferIndex; if (i >= intervalLength) //a length is intervalLength. bufferIndex = bStart + i - intervalLength; else bufferIndex = aStart + i; if (aIndex < aEnd && bIndex < bEnd) { //Compare the element at a to the one at b. if (entries.Elements[aIndex].boundingBox.Min.X < entries.Elements[bIndex].boundingBox.Min.X) { //a was the minimum element. Put it into the buffer and increment the considered a index. backbuffer.Elements[bufferIndex] = entries.Elements[aIndex++]; } else { //b was the minimum element. Put it into the buffer and increment the considered b index. backbuffer.Elements[bufferIndex] = entries.Elements[bIndex++]; } } else if (aIndex < aEnd) { //B is at the max, so just use a. backbuffer.Elements[bufferIndex] = entries.Elements[aIndex++]; } else { //A is at the max, so just use b. backbuffer.Elements[bufferIndex] = entries.Elements[bIndex++]; } i++; } } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/SortedGrid2DSet.cs ================================================ using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep { internal class SortedGrid2DSet { //TODO: The cell set is the number one reason why Grid2DSortAndSweep fails in corner cases. //One option: //Instead of trying to maintain a sorted set, stick to a dictionary + RawList combo. //The update phase can add active cell-object pairs to a raw list. Could do bottom-up recreation too, though contention might be an issue. //Another option: Some other parallel-enumerable set, possibly with tricky hashing. internal RawList cells = new RawList(); UnsafeResourcePool cellPool = new UnsafeResourcePool(); internal int count; internal bool TryGetIndex(ref Int2 cellIndex, out int index, out int sortingHash) { sortingHash = cellIndex.GetSortingHash(); int minIndex = 0; //inclusive int maxIndex = count; //exclusive index = 0; while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can. { index = (maxIndex + minIndex) / 2; if (cells.Elements[index].sortingHash > sortingHash) maxIndex = index; else if (cells.Elements[index].sortingHash < sortingHash) minIndex = ++index; else { //Found an equal sorting hash! //The hash can collide, and we cannot add an entry to //an incorrect index. It would break the 'cell responsibility' //used by the cell update process to avoid duplicate overlaps. //So, check if the index we found is ACTUALLY correct. if (cells.Elements[index].cellIndex.Y == cellIndex.Y && cells.Elements[index].cellIndex.Z == cellIndex.Z) { return true; } //If it was not the correct index, let it continue searching. } } return false; } internal bool TryGetCell(ref Int2 cellIndex, out GridCell2D cell) { int index; int sortingHash; if (TryGetIndex(ref cellIndex, out index, out sortingHash)) { cell = cells.Elements[index]; return true; } cell = null; return false; } internal void Add(ref Int2 index, Grid2DEntry entry) { int cellIndex; int sortingHash; if (TryGetIndex(ref index, out cellIndex, out sortingHash)) { cells.Elements[cellIndex].Add(entry); return; } var cell = cellPool.Take(); cell.Initialize(ref index, sortingHash); cell.Add(entry); cells.Insert(cellIndex, cell); count++; ////Take an index. See if it's taken in the set. ////If it's already there, then add the entry to the cell. ////If it's not already there, create a new cell and add the entry to the cell and insert it at the index located. //int sortingHash = index.GetSortingHash(); //int minIndex = 0; //inclusive //int maxIndex = count; //exclusive //int i = 0; //while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can. //{ // i = (maxIndex + minIndex) / 2; // if (cells.Elements[i].sortingHash > sortingHash) // maxIndex = i; // else if (cells.Elements[i].sortingHash < sortingHash) // minIndex = ++i; // else // { // //Found an equal sorting hash! // //The hash can collide, and we cannot add an entry to // //an incorrect index. It would break the 'cell responsibility' // //used by the cell update process to avoid duplicate overlaps. // //So, check if the index we found is ACTUALLY correct. // if (cells.Elements[i].cellIndex.Y == index.Y && cells.Elements[i].cellIndex.Z == index.Z) // { // cells.Elements[i].Add(entry); // return; // } // //If it was not the correct index, let it continue searching. // } //} //var cell = cellPool.Take(); //cell.Initialize(ref index, sortingHash); //cell.Add(entry); //cells.Insert(i, cell); //count++; } internal void Remove(ref Int2 index, Grid2DEntry entry) { int cellIndex; int sortingHash; if (TryGetIndex(ref index, out cellIndex, out sortingHash)) { cells.Elements[cellIndex].Remove(entry); if (cells.Elements[cellIndex].entries.Count == 0) { //The cell is now empty. Give it back to the pool. var toRemove = cells.Elements[cellIndex]; //There's no cleanup to do on the grid cell. //Its list is empty, and the rest is just value types. cells.RemoveAt(cellIndex); cellPool.GiveBack(toRemove); count--; } } //int sortingHash = index.GetSortingHash(); //int minIndex = 0; //inclusive //int maxIndex = count; //exclusive //int i = 0; //while (maxIndex - minIndex > 0) //If the testing interval has a length of zero, we've done as much as we can. //{ // i = (maxIndex + minIndex) / 2; // if (cells.Elements[i].sortingHash > sortingHash) // maxIndex = i; // else if (cells.Elements[i].sortingHash < sortingHash) // minIndex = ++i; // else // { // //Found an equal sorting hash! // //The hash can collide, and we cannot add an entry to // //an incorrect index. It would break the 'cell responsibility' // //used by the cell update process to avoid duplicate overlaps. // //So, check if the index we found is ACTUALLY correct. // if (cells.Elements[i].cellIndex.Y == index.Y && cells.Elements[i].cellIndex.Z == index.Z) // { // cells.Elements[i].Remove(entry); // if (cells.Elements[i].entries.count == 0) // { // //The cell is now empty. Give it back to the pool. // var toRemove = cells.Elements[i]; // //There's no cleanup to do on the grid cell. // //Its list is empty, and the rest is just value types. // cells.RemoveAt(i); // cellPool.GiveBack(toRemove); // count--; // } // return; // } // //If it was not the correct index, let it continue searching. // } //} ////Getting here should be impossible. } } } ================================================ FILE: BEPUphysics/BroadPhaseSystems/SortAndSweep/Testing/SortAndSweep3D.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.BroadPhaseEntries; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUphysics.BroadPhaseSystems.SortAndSweep.Testing { internal class SortAndSweep3D : BroadPhase { RawList entriesX = new RawList(); RawList entriesY = new RawList(); RawList entriesZ = new RawList(); public override void Add(BroadPhaseEntry entry) { base.Add(entry); //binary search for the approximately correct location. This helps prevent large first-frame sort times. //X Axis: int minIndex = 0; //inclusive int maxIndex = entriesX.Count; //exclusive int index = 0; while (maxIndex - minIndex > 0) { index = (maxIndex + minIndex) / 2; if (entriesX.Elements[index].boundingBox.Min.X > entry.boundingBox.Min.X) maxIndex = index; else if (entriesX.Elements[index].boundingBox.Min.X < entry.boundingBox.Min.X) minIndex = ++index; else break; //Found an equal value! } entriesX.Insert(index, entry); //Y Axis: minIndex = 0; //inclusive maxIndex = entriesY.Count; //exclusive while (maxIndex - minIndex > 0) { index = (maxIndex + minIndex) / 2; if (entriesY.Elements[index].boundingBox.Min.Y > entry.boundingBox.Min.Y) maxIndex = index; else if (entriesY.Elements[index].boundingBox.Min.Y < entry.boundingBox.Min.Y) minIndex = ++index; else break; //Found an equal value! } entriesY.Insert(index, entry); //Z Axis: minIndex = 0; //inclusive maxIndex = entriesZ.Count; //exclusive while (maxIndex - minIndex > 0) { index = (maxIndex + minIndex) / 2; if (entriesZ.Elements[index].boundingBox.Min.Z > entry.boundingBox.Min.Z) maxIndex = index; else if (entriesZ.Elements[index].boundingBox.Min.Z < entry.boundingBox.Min.Z) minIndex = ++index; else break; //Found an equal value! } entriesZ.Insert(index, entry); } public override void Remove(BroadPhaseEntry entry) { base.Remove(entry); entriesX.Remove(entry); entriesY.Remove(entry); entriesZ.Remove(entry); } protected override void UpdateMultithreaded() { UpdateSingleThreaded(); } HashSet overlapCandidatesX = new HashSet(); HashSet overlapCandidatesY = new HashSet(); protected override void UpdateSingleThreaded() { overlapCandidatesX.Clear(); overlapCandidatesY.Clear(); Overlaps.Clear(); //Sort along x axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entriesX.Count; i++) { var entry = entriesX.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.X < entriesX.Elements[j].boundingBox.Min.X) { entriesX.Elements[j + 1] = entriesX.Elements[j]; entriesX.Elements[j] = entry; } else break; } } //Sort along y axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entriesY.Count; i++) { var entry = entriesY.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.Y < entriesY.Elements[j].boundingBox.Min.Y) { entriesY.Elements[j + 1] = entriesY.Elements[j]; entriesY.Elements[j] = entry; } else break; } } //Sort along z axis using insertion sort; the list will be nearly sorted, so very few swaps are necessary. for (int i = 1; i < entriesZ.Count; i++) { var entry = entriesZ.Elements[i]; for (int j = i - 1; j >= 0; j--) { if (entry.boundingBox.Min.Z < entriesZ.Elements[j].boundingBox.Min.Z) { entriesZ.Elements[j + 1] = entriesZ.Elements[j]; entriesZ.Elements[j] = entry; } else break; } } //Hash-set based sweeping is way too slow. 3D sap is really best suited to an incremental approach. //Sweep the list looking for overlaps. //Sweep the X axis first; in this phase, add overlaps to the hash set if they exist. for (int i = 0; i < entriesX.Count; i++) { BoundingBox a = entriesX.Elements[i].boundingBox; for (int j = i + 1; j < entriesX.Count && a.Max.X > entriesX.Elements[j].boundingBox.Min.X; j++) { overlapCandidatesX.Add(new BroadPhaseOverlap(entriesX.Elements[i], entriesX.Elements[j])); } } //Sweep the Y axis second; same thing for (int i = 0; i < entriesY.Count; i++) { BoundingBox a = entriesY.Elements[i].boundingBox; for (int j = i + 1; j < entriesY.Count && a.Max.Y > entriesY.Elements[j].boundingBox.Min.Y; j++) { overlapCandidatesY.Add(new BroadPhaseOverlap(entriesY.Elements[i], entriesY.Elements[j])); } } //Sweep the Z axis last for (int i = 0; i < entriesZ.Count; i++) { BoundingBox a = entriesZ.Elements[i].boundingBox; for (int j = i + 1; j < entriesZ.Count && a.Max.Z > entriesZ.Elements[j].boundingBox.Min.Z; j++) { var overlap = new BroadPhaseOverlap(entriesZ.Elements[i], entriesZ.Elements[j]); if (overlapCandidatesX.Contains(overlap) && overlapCandidatesY.Contains(overlap)) TryToAddOverlap(entriesZ.Elements[i], entriesZ.Elements[j]); } } } } } ================================================ FILE: BEPUphysics/CollisionRuleManagement/CollisionGroup.cs ================================================ using System.Collections.Generic; namespace BEPUphysics.CollisionRuleManagement { /// /// A group which can have interaction rules created between it and other collision groups. /// Every entity has a collision group and considers the group's interaction rules in collisions with other entities. /// public class CollisionGroup { private readonly int hashCode; /// /// Constructs a new collision group. /// public CollisionGroup() { const ulong prime = 0xd8163841; var hash = (ulong)(base.GetHashCode()); hash = hash * hash * hash * hash * hash * prime; hashCode = (int)(hash); } //Equals is not overriden because the hashcode because the hashcode is the default hashcode, just modified a bit. /// /// Defines the CollisionRule between the two groups for a given space. /// /// First CollisionGroup of the pair. /// Second CollisionGroup of the pair. /// CollisionRule to use between the pair. public static void DefineCollisionRule(CollisionGroup groupA, CollisionGroup groupB, CollisionRule rule) { var pair = new CollisionGroupPair(groupA, groupB); if (CollisionRules.CollisionGroupRules.ContainsKey(pair)) CollisionRules.CollisionGroupRules[pair] = rule; else CollisionRules.CollisionGroupRules.Add(pair, rule); } /// /// Defines a CollisionRule between every group in the first set and every group in the second set for a given space. /// /// First set of groups. /// Second set of groups. /// Collision rule to define between the sets. public static void DefineCollisionRulesBetweenSets(List aGroups, List bGroups, CollisionRule rule) { foreach (CollisionGroup group in aGroups) { DefineCollisionRulesWithSet(group, bGroups, rule); } } /// /// Defines a CollisionRule between every group in a set with itself and the others in the set for a given space. /// /// Set of CollisionGroups. /// CollisionRule between each group and itself. /// CollisionRule between each group and every other group in the set. public static void DefineCollisionRulesInSet(List groups, CollisionRule self, CollisionRule other) { for (int i = 0; i < groups.Count; i++) { DefineCollisionRule(groups[i], groups[i], self); } for (int i = 0; i < groups.Count - 1; i++) { for (int j = i + 1; j < groups.Count; j++) { DefineCollisionRule(groups[i], groups[j], other); } } } /// /// Defines a CollisionRule between a group and every group in a set of groups for a given space. /// /// First CollisionGroup of the pair. /// Set of CollisionGroups; each group will have its CollisionRule with the first group defined. /// CollisionRule to use between the pairs. public static void DefineCollisionRulesWithSet(CollisionGroup group, List groups, CollisionRule rule) { foreach (CollisionGroup g in groups) { DefineCollisionRule(group, g, rule); } } /// /// Removes any rule between the two groups in the space. /// /// First CollisionGroup of the pair. /// SecondCollisionGroup of the pair. public static void RemoveCollisionRule(CollisionGroup groupA, CollisionGroup groupB) { Dictionary dictionary = CollisionRules.CollisionGroupRules; var pair = new CollisionGroupPair(groupA, groupB); if (dictionary.ContainsKey(pair)) dictionary.Remove(pair); } /// /// Removes any rule between every group in the first set and every group in the second set for a given space. /// /// First set of groups. /// Second set of groups. public static void RemoveCollisionRulesBetweenSets(List aGroups, List bGroups) { foreach (CollisionGroup group in aGroups) { RemoveCollisionRulesWithSet(group, bGroups); } } /// /// Removes any rule between every group in a set with itself and the others in the set for a given space. /// /// Set of CollisionGroups. public static void RemoveCollisionRulesInSet(List groups) { for (int i = 0; i < groups.Count; i++) { RemoveCollisionRule(groups[i], groups[i]); } for (int i = 0; i < groups.Count - 1; i++) { for (int j = i + 1; j < groups.Count; j++) { RemoveCollisionRule(groups[i], groups[j]); } } } /// /// Removes any rule between a group and every group in a set of groups for a given space. /// /// First CollisionGroup of the pair. /// Set of CollisionGroups; each group will have its CollisionRule with the first group removed. public static void RemoveCollisionRulesWithSet(CollisionGroup group, List groups) { foreach (CollisionGroup g in groups) { RemoveCollisionRule(group, g); } } /// /// Gets a hash code for the object. /// /// Hash code for the object. public override int GetHashCode() { return hashCode; } } } ================================================ FILE: BEPUphysics/CollisionRuleManagement/CollisionGroupPair.cs ================================================ using System; namespace BEPUphysics.CollisionRuleManagement { /// /// Storage strucure containing two CollisionGroup instances used as a key in a collision rules dictionary. /// public struct CollisionGroupPair : IEquatable { /// /// First collision group in the pair. /// public readonly CollisionGroup A; /// /// Second collision group in the pair. /// public readonly CollisionGroup B; private readonly int hashCode; /// /// Constructs a new collision group pair. /// /// First collision group in the pair. /// Second collision group in the pair. public CollisionGroupPair(CollisionGroup groupA, CollisionGroup groupB) { if (groupA == null) throw new ArgumentNullException("groupA", "The first collision group in the pair is null. If this pair was being created for CollisionRule calculation purposes, simply consider the rule to be CollisionRule.Defer."); if (groupB == null) throw new ArgumentNullException("groupB", "The second collision group in the pair is null. If this pair was being created for CollisionRule calculation purposes, simply consider the rule to be CollisionRule.Defer."); A = groupA; B = groupB; const ulong prime = 0xd8163841; //Note that the order of the pair is irrelevant- this is required ulong hash = ((ulong)(groupA.GetHashCode()) + (ulong)(groupB.GetHashCode())) * prime; hashCode = (int)(hash); // % (int.MaxValue - 1)); } #region IEquatable Members bool IEquatable.Equals(CollisionGroupPair other) { return (other.A == A && other.B == B) || (other.B == A && other.A == B); } #endregion /// /// Determines whether or not the two objects are equal. /// Use the IEquatable interface implementation if possible. /// /// Object to compare. /// Whether or not the two objects are equal. public override bool Equals(object obj) { //This method requires boxing, so make sure any attempt to call it is caught. var other = (CollisionGroupPair)obj; return (other.A == A && other.B == B) || (other.B == A && other.A == B); } /// /// Gets the hash code of the entity type pair. /// /// Hash code of the entity type pair. public override int GetHashCode() { return hashCode; } } } ================================================ FILE: BEPUphysics/CollisionRuleManagement/CollisionRule.cs ================================================ namespace BEPUphysics.CollisionRuleManagement { /// /// Defines a set of rules that collisions can adhere to. /// public enum CollisionRule { /// /// Yields the interaction type's determination to a later stage. /// Defer, /// /// Uses all of collision detection, including creating a collision pair, creating contacts when appropriate, and responding to those contacts physically. /// If a collision pair is forced to use a 'normal' interaction but both entities in the pair are kinematic, the collision response will be skipped. /// Normal, /// /// Creates a collision pair and undergoes narrow phase testing, but does not collision response in the solver. /// NoSolver, /// /// Creates a broad phase overlap and narrow phase pair but the collision is never updated. It cannot generate contacts nor undergo solving. /// NoNarrowPhaseUpdate, /// /// Creates a broad phase overlap but does not create any narrow phase pairs. It cannot generate contacts nor undergo solving. /// NoNarrowPhasePair, /// /// Does not create a broad phase overlap. No further collision detection or response takes place. /// NoBroadPhase } } ================================================ FILE: BEPUphysics/CollisionRuleManagement/CollisionRules.cs ================================================ using System; using System.Collections.Generic; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionRuleManagement { /// /// Stores how an object can interact with other objects through collisions. /// public class CollisionRules { /// /// Fires when the contained collision rules are altered. /// public event Action CollisionRulesChanged; /// /// Constructs a new CollisionRules instance. /// public CollisionRules() { hashCode = (int)(base.GetHashCode() * 0x8da6b343); OnChangedDelegate = OnChanged; } int hashCode; /// /// Serves as a hash function for a particular type. /// /// /// A hash code for the current . /// /// 2 public override int GetHashCode() { return hashCode; } private Action OnChangedDelegate; protected void OnChanged() { if (CollisionRulesChanged != null) CollisionRulesChanged(); } internal CollisionGroup group; /// /// The collision group to which the object owning this instance belongs to. /// This is overridden by any relationships defined in the Specific collection with CollisionRules other than CollisionRule.Defer. /// This is also overriden by the Personal CollisionRule if it is anything but CollisionRule.Defer. /// If the interaction type between the group is defined as CollisionRule.Defer, it is considered to be CollisionRule.normal as the collision group is the final stage. /// public CollisionGroup Group { get { return group; } set { group = value; OnChanged(); } } internal CollisionRule personal = CollisionRule.Defer; /// /// Determines in general how the object owning this instance should react to other objects. /// This is overridden by any relationships defined in the Specific collection with CollisionRules other than CollisionRule.Defer. /// If this is not set to CollisionRule.Defer, it will override the collision group's collision rules. /// public CollisionRule Personal { get { return personal; } set { personal = value; OnChanged(); } } internal ObservableDictionary specific = new ObservableDictionary(); /// /// Specifies how the object owning this instance should react to other individual objects. /// Any rules defined in this collection will take priority over the Personal collision rule and the collision group's collision rules. /// Objects that are not in this collection are considered to have a relationship of CollisionRule.Defer. /// public ObservableDictionary Specific { get { return specific; } set { if (value != specific) { if (specific != null) specific.Changed -= OnChangedDelegate; if (value != null) value.Changed += OnChangedDelegate; specific = value; OnChanged(); } } } //Pure convenience method. /// /// Adds an entry in ownerA's Specific relationships list about ownerB. /// ///Owner of the collision rules that will gain an entry in its Specific relationships. ///Owner of the collision rules that will be added to ownerA's Specific relationships. ///Rule assigned to the pair. public static void AddRule(ICollisionRulesOwner ownerA, ICollisionRulesOwner ownerB, CollisionRule rule) { ownerA.CollisionRules.specific.Add(ownerB.CollisionRules, rule); } /// /// Adds an entry in rulesA's Specific relationships list about ownerB. /// ///Collision rules that will gain an entry in its Specific relationships. ///Owner of the collision rules that will be added to ownerA's Specific relationships. ///Rule assigned to the pair. public static void AddRule(CollisionRules rulesA, ICollisionRulesOwner ownerB, CollisionRule rule) { rulesA.specific.Add(ownerB.CollisionRules, rule); } /// /// Adds an entry in rulesA's Specific relationships list about ownerB. /// ///Owner of the collision rules that will gain an entry in its Specific relationships. ///Collision rules that will be added to ownerA's Specific relationships. ///Rule assigned to the pair. public static void AddRule(ICollisionRulesOwner ownerA, CollisionRules rulesB, CollisionRule rule) { ownerA.CollisionRules.specific.Add(rulesB, rule); } /// /// Tries to remove a relationship about ownerB from ownerA's Specific list. /// ///Owner of the collision rules that will lose an entry in its Specific relationships. ///Owner of the collision rules that will be removed from ownerA's Specific relationships. public static void RemoveRule(ICollisionRulesOwner ownerA, ICollisionRulesOwner ownerB) { if (!ownerA.CollisionRules.specific.Remove(ownerB.CollisionRules)) ownerB.CollisionRules.specific.Remove(ownerA.CollisionRules); } /// /// Tries to remove a relationship about ownerB from rulesA's Specific list. /// ///Collision rules that will lose an entry in its Specific relationships. ///Owner of the collision rules that will be removed from ownerA's Specific relationships. public static void RemoveRule(CollisionRules rulesA, ICollisionRulesOwner ownerB) { if (!rulesA.specific.Remove(ownerB.CollisionRules)) ownerB.CollisionRules.specific.Remove(rulesA); } /// /// Tries to remove a relationship about rulesB from ownerA's Specific list. /// ///Owner of the collision rules that will lose an entry in its Specific relationships. ///Collision rules that will be removed from ownerA's Specific relationships. public static void RemoveRule(ICollisionRulesOwner ownerA, CollisionRules rulesB) { if (!ownerA.CollisionRules.specific.Remove(rulesB)) rulesB.specific.Remove(ownerA.CollisionRules); } static CollisionRules() { CollisionGroupRules.Add(new CollisionGroupPair(DefaultKinematicCollisionGroup, DefaultKinematicCollisionGroup), CollisionRule.NoBroadPhase); } internal static Func collisionRuleCalculator = GetCollisionRuleDefault; /// /// Gets or sets the delegate used to calculate collision rules. /// Defaults to CollisionRules.GetCollisionRuleDefault. /// public static Func CollisionRuleCalculator { get { return collisionRuleCalculator; } set { collisionRuleCalculator = value; } } /// /// Uses the CollisionRuleCalculator to get the collision rule between two collision rules owners. /// /// First owner of the pair. /// Second owner of the pair. /// CollisionRule between the pair, according to the CollisionRuleCalculator. public static CollisionRule GetCollisionRule(ICollisionRulesOwner ownerA, ICollisionRulesOwner ownerB) { return collisionRuleCalculator(ownerA, ownerB); } /// /// Defines any special collision rules between collision groups. /// public static Dictionary CollisionGroupRules = new Dictionary(); /// /// If a CollisionRule calculation between two colliding objects results in no defined CollisionRule, this value will be used. /// public static CollisionRule DefaultCollisionRule = CollisionRule.Normal; /// /// When a dynamic entity is created and added to a space without having a specific collision group set beforehand, it inherits this collision group. /// There are no special rules associated with this group by default; entities within this group have normal, full interaction with all other entities. /// Collision group interaction rules can be overriden by entity personal collision rules or entity-to-entity specific collision rules. /// public static CollisionGroup DefaultDynamicCollisionGroup = new CollisionGroup(); /// /// When a kinematic entity is created and added to a space without having a specific collision group set beforehand, it inherits this collision group. /// Entities in this collision group will not create collision pairs with other entities of this collision group by default. All other interactions are normal. /// Collision group interaction rules can be overriden by entity personal collision rules or entity-to-entity specific collision rules. /// /// Non-entity collidable objects like static triangle meshes also use this collision group by default. /// public static CollisionGroup DefaultKinematicCollisionGroup = new CollisionGroup(); /// /// Determines what collision rule governs the interaction between the two objects. /// /// First ruleset owner in the pair. This entity's space is used to determine the collision detection settings that contain special collision group interaction rules. /// Second ruleset owner in the pair. /// Collision rule governing the interaction between the pair. public static CollisionRule GetCollisionRuleDefault(ICollisionRulesOwner aOwner, ICollisionRulesOwner bOwner) { var a = aOwner.CollisionRules; var b = bOwner.CollisionRules; CollisionRule pairRule = GetSpecificCollisionRuleDefault(a, b); if (pairRule == CollisionRule.Defer) { pairRule = GetPersonalCollisionRuleDefault(a, b); if (pairRule == CollisionRule.Defer) pairRule = GetGroupCollisionRuleDefault(a, b); } if (pairRule == CollisionRule.Defer) pairRule = DefaultCollisionRule; return pairRule; } /// /// Default implementation used to calculate collision rules due to the rulesets' specific relationships. /// ///First ruleset in the pair. ///Second ruleset in the pair. ///Collision rule governing the interaction between the pair. public static CollisionRule GetSpecificCollisionRuleDefault(CollisionRules a, CollisionRules b) { CollisionRule aToB; a.specific.WrappedDictionary.TryGetValue(b, out aToB); CollisionRule bToA; b.specific.WrappedDictionary.TryGetValue(a, out bToA); return aToB > bToA ? aToB : bToA; } /// /// Default implementation used to calculate collision rules due to the rulesets' collision groups. /// ///First ruleset in the pair. ///Second ruleset in the pair. ///Collision rule governing the interaction between the pair. public static CollisionRule GetGroupCollisionRuleDefault(CollisionRules a, CollisionRules b) { if (a.group == null || b.group == null) return CollisionRule.Defer; //This can happen occasionally when objects aren't in a space or are being handled uniquely (like in compound bodies). CollisionRule pairRule; CollisionGroupRules.TryGetValue(new CollisionGroupPair(a.group, b.group), out pairRule); return pairRule; } /// /// Default implementation used to calculate collision rules due to the rulesets' personal rules. /// ///First ruleset in the pair. ///Second ruleset in the pair. ///Collision rule governing the interaction between the pair. public static CollisionRule GetPersonalCollisionRuleDefault(CollisionRules a, CollisionRules b) { return a.personal > b.personal ? a.personal : b.personal; } } } ================================================ FILE: BEPUphysics/CollisionRuleManagement/ICollisionRulesOwner.cs ================================================ namespace BEPUphysics.CollisionRuleManagement { /// /// Defines a class which must own CollisionRules. /// public interface ICollisionRulesOwner { /// /// Collision rules owned by the object. /// CollisionRules CollisionRules { get; set; } } } ================================================ FILE: BEPUphysics/CollisionShapes/CollisionShape.cs ================================================ using System; namespace BEPUphysics.CollisionShapes { /// /// Superclass of all collision shapes. /// Collision shapes are composed entirely of local space information. /// Collidables provide the world space information needed to use the shapes to do collision detection. /// public abstract class CollisionShape { /// /// Fires when some of the local space information in the shape changes. /// public event Action ShapeChanged; protected virtual void OnShapeChanged() { if (ShapeChanged != null) ShapeChanged(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/CompoundShape.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; namespace BEPUphysics.CollisionShapes { /// /// Contains a shape and its local transform relative to its owning compound shape. /// This is used to construct compound shapes. /// public struct CompoundShapeEntry { /// /// Local transform of the shape relative to its owning compound shape. /// public RigidTransform LocalTransform; /// /// Shape used by the compound. /// public EntityShape Shape; /// /// Weight of the entry. This defines how much the entry contributes to its owner /// for the purposes of center of rotation computation. /// public float Weight; /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local transform of the shape. ///Weight of the entry. This defines how much the entry contributes to its owner /// for the purposes of center of rotation computation. public CompoundShapeEntry(EntityShape shape, RigidTransform localTransform, float weight) { localTransform.Validate(); LocalTransform = localTransform; Shape = shape; Weight = weight; } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local position of the shape. ///Weight of the entry. This defines how much the entry contributes to its owner /// for the purposes of center of mass and inertia computation. public CompoundShapeEntry(EntityShape shape, Vector3 position, float weight) { position.Validate(); LocalTransform = new RigidTransform(position); Shape = shape; Weight = weight; } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local orientation of the shape. ///Weight of the entry. This defines how much the entry contributes to its owner /// for the purposes of center of rotation computation. public CompoundShapeEntry(EntityShape shape, Quaternion orientation, float weight) { orientation.Validate(); LocalTransform = new RigidTransform(orientation); Shape = shape; Weight = weight; } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Weight of the entry. This defines how much the entry contributes to its owner /// for the purposes of center of rotation computation. public CompoundShapeEntry(EntityShape shape, float weight) { LocalTransform = RigidTransform.Identity; Shape = shape; Weight = weight; } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local transform of the shape. public CompoundShapeEntry(EntityShape shape, RigidTransform localTransform) { localTransform.Validate(); LocalTransform = localTransform; Shape = shape; Weight = shape.ComputeVolume(); } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local position of the shape. public CompoundShapeEntry(EntityShape shape, Vector3 position) { position.Validate(); LocalTransform = new RigidTransform(position); Shape = shape; Weight = shape.ComputeVolume(); } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. ///Local orientation of the shape. public CompoundShapeEntry(EntityShape shape, Quaternion orientation) { orientation.Validate(); LocalTransform = new RigidTransform(orientation); Shape = shape; Weight = shape.ComputeVolume(); } /// /// Constructs a new compound shape entry using the volume of the shape as a weight. /// ///Shape to use. public CompoundShapeEntry(EntityShape shape) { LocalTransform = RigidTransform.Identity; Shape = shape; Weight = shape.ComputeVolume(); } } /// /// Shape composed of multiple other shapes. /// public class CompoundShape : EntityShape { internal RawList shapes; /// /// Gets the list of shapes in the compound shape. /// public ReadOnlyList Shapes { get { return new ReadOnlyList(shapes); } } /// /// Constructs a compound shape. /// ///Shape entries used to create the compound. /// Computed center of the compound shape, using the entry weights. public CompoundShape(IList shapes, out Vector3 center) { if (shapes.Count > 0) { center = ComputeCenter(shapes); this.shapes = new RawList(shapes); for (int i = 0; i < this.shapes.Count; i++) { this.shapes.Elements[i].LocalTransform.Position -= center; } } else { throw new ArgumentException("Compound shape must have at least 1 subshape."); } } /// /// Constructs a compound shape. /// ///Shape entries used to create the compound. public CompoundShape(IList shapes) { if (shapes.Count > 0) { Vector3 center = ComputeCenter(shapes); this.shapes = new RawList(shapes); for (int i = 0; i < this.shapes.Count; i++) { this.shapes.Elements[i].LocalTransform.Position -= center; } } else { throw new ArgumentException("Compound shape must have at least 1 subshape."); } } #region EntityShape members and support /// /// Computes the center of the shape. This can be considered its /// center of mass, based on the weightings of entries in the shape. /// For properly calibrated compound shapes, this will return a zero vector, /// since the shape recenters itself on construction. /// /// Center of the shape. public override Vector3 ComputeCenter() { float totalWeight = 0; var center = new Vector3(); for (int i = 0; i < shapes.Count; i++) { totalWeight += shapes.Elements[i].Weight; Vector3 centerContribution; Vector3.Multiply(ref shapes.Elements[i].LocalTransform.Position, shapes.Elements[i].Weight, out centerContribution); Vector3.Add(ref center, ref centerContribution, out center); } if (totalWeight <= 0) throw new NotFiniteNumberException("Cannot compute center; the total weight of a compound shape must be positive."); Vector3.Divide(ref center, totalWeight, out center); center.Validate(); return center; } /// /// Computes the center of a compound using its child data. /// ///Child data to use to compute the center. ///Center of the children. public static Vector3 ComputeCenter(IList childData) { var center = new Vector3(); float totalWeight = 0; for (int i = 0; i < childData.Count; i++) { float weight = childData[i].Entry.Weight; totalWeight += weight; center += childData[i].Entry.LocalTransform.Position * weight; } if (totalWeight <= 0) throw new NotFiniteNumberException("Cannot compute center; the total weight of a compound shape must be positive."); Vector3.Divide(ref center, totalWeight, out center); center.Validate(); return center; } /// /// Computes the center of a compound using its child data. /// Children are weighted using their volumes for contribution to the center of 'mass.' /// ///Child data to use to compute the center. ///Center of the children. public static Vector3 ComputeCenter(IList childData) { var center = new Vector3(); float totalWeight = 0; for (int i = 0; i < childData.Count; i++) { float weight = childData[i].Weight; totalWeight += weight; center += childData[i].LocalTransform.Position * weight; } if (totalWeight <= 0) throw new NotFiniteNumberException("Cannot compute center; the total weight of a compound shape must be positive."); Vector3.Divide(ref center, totalWeight, out center); center.Validate(); return center; } /// /// Computes the volume of the shape. /// This is approximate; it will double count the intersection of multiple subshapes. /// /// Volume of the shape. public override float ComputeVolume() { float volume = 0; for (int i = 0; i < shapes.Count; i++) { volume += shapes.Elements[i].Shape.ComputeVolume(); } return volume; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { volume = ComputeVolume(); return ComputeVolumeDistribution(); } /// /// Computes the volume distribution of the shape. /// /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution() { var volumeDistribution = new Matrix3x3(); float totalWeight = 0; for (int i = 0; i < shapes.Count; i++) { totalWeight += shapes.Elements[i].Weight; Matrix3x3 contribution; GetContribution(shapes.Elements[i].Shape, ref shapes.Elements[i].LocalTransform, ref Toolbox.ZeroVector, shapes.Elements[i].Weight, out contribution); Matrix3x3.Add(ref contribution, ref volumeDistribution, out volumeDistribution); } if (totalWeight <= 0) throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive."); Matrix3x3.Multiply(ref volumeDistribution, 1 / totalWeight, out volumeDistribution); volumeDistribution.Validate(); return volumeDistribution; } /// /// Computes the volume distribution and center of the shape. /// /// Mass-weighted entries of the compound. /// Center of the compound. /// Volume distribution of the shape. public static Matrix3x3 ComputeVolumeDistribution(IList entries, out Vector3 center) { center = new Vector3(); float totalWeight = 0; for (int i = 0; i < entries.Count; i++) { center += entries[i].LocalTransform.Position * entries[i].Weight; totalWeight += entries[i].Weight; } if (totalWeight <= 0) throw new NotFiniteNumberException("Cannot compute distribution; the total weight of a compound shape must be positive."); float totalWeightInverse = 1 / totalWeight; totalWeightInverse.Validate(); center *= totalWeightInverse; var volumeDistribution = new Matrix3x3(); for (int i = 0; i < entries.Count; i++) { RigidTransform transform = entries[i].LocalTransform; Matrix3x3 contribution; GetContribution(entries[i].Shape, ref transform, ref center, entries[i].Weight, out contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } Matrix3x3.Multiply(ref volumeDistribution, totalWeightInverse, out volumeDistribution); volumeDistribution.Validate(); return volumeDistribution; } /// /// Gets the volume distribution contributed by a single shape. /// ///Shape to use to compute a contribution. ///Transform of the shape. ///Center to use when computing the distribution. ///Weighting to apply to the contribution. ///Volume distribution of the contribution. public static void GetContribution(EntityShape shape, ref RigidTransform transform, ref Vector3 center, float weight, out Matrix3x3 contribution) { contribution = shape.ComputeVolumeDistribution(); TransformContribution(ref transform, ref center, ref contribution, weight, out contribution); //return TransformContribution(ref transform, ref center, ref contribution, weight); } /// /// Modifies a contribution using a transform, position, and weight. /// /// Transform to use to modify the contribution. /// Center to use to modify the contribution. /// Original unmodified contribution. /// Weight of the contribution. /// Transformed contribution. public static void TransformContribution(ref RigidTransform transform, ref Vector3 center, ref Matrix3x3 baseContribution, float weight, out Matrix3x3 contribution) { Matrix3x3 rotation; Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out rotation); Matrix3x3 temp; //TODO: Verify contribution //Do angular transformed contribution first... Matrix3x3.MultiplyTransposed(ref rotation, ref baseContribution, out temp); Matrix3x3.Multiply(ref temp, ref rotation, out temp); contribution = temp; //Now add in the offset from the origin. Vector3 offset; Vector3.Subtract(ref transform.Position, ref center, out offset); Matrix3x3 innerProduct; Matrix3x3.CreateScale(offset.LengthSquared(), out innerProduct); Matrix3x3 outerProduct; Matrix3x3.CreateOuterProduct(ref offset, ref offset, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out temp); Matrix3x3.Add(ref contribution, ref temp, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new CompoundCollidable(this); } /// /// Computes the center of the shape and its volume. /// /// Volume of the compound. /// Volume of the compound. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes a variety of shape information all at once. /// /// Properties of the shape. public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo) { shapeInfo.VolumeDistribution = ComputeVolumeDistribution(out shapeInfo.Volume); shapeInfo.Center = ComputeCenter(); } #endregion /// /// Computes and returns the volume, volume distribution, and center contributions from each child shape in the compound shape. /// /// Volume, volume distribution, and center contributions from each child shape in the compound shape. public ShapeDistributionInformation[] ComputeChildContributions() { var toReturn = new ShapeDistributionInformation[shapes.Count]; for (int i = 0; i < shapes.Count; i++) { shapes.Elements[i].Shape.ComputeDistributionInformation(out toReturn[i]); } return toReturn; } /// /// Computes a bounding box for the shape given the specified transform. /// /// Transform to apply to the shape to compute the bounding box. /// Bounding box for the shape given the transform. public override void GetBoundingBox(ref RigidTransform transform, out BoundingBox boundingBox) { RigidTransform combinedTransform; RigidTransform.Transform(ref shapes.Elements[0].LocalTransform, ref transform, out combinedTransform); shapes.Elements[0].Shape.GetBoundingBox(ref combinedTransform, out boundingBox); for (int i = 0; i < shapes.Count; i++) { RigidTransform.Transform(ref shapes.Elements[i].LocalTransform, ref transform, out combinedTransform); BoundingBox childBoundingBox; shapes.Elements[i].Shape.GetBoundingBox(ref combinedTransform, out childBoundingBox); BoundingBox.CreateMerged(ref boundingBox, ref childBoundingBox, out boundingBox); } } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/BoxShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Convex shape with width, length, and height. /// public class BoxShape : ConvexShape { internal float halfWidth; internal float halfHeight; internal float halfLength; /// /// Constructs a new box shape. /// ///Width of the box. ///Height of the box. ///Length of the box. public BoxShape(float width, float height, float length) { halfWidth = width * .5f; halfHeight = height * .5f; halfLength = length * .5f; OnShapeChanged(); } /// /// Width of the box divided by two. /// public float HalfWidth { get { return halfWidth; } set { halfWidth = value; OnShapeChanged(); } } /// /// Height of the box divided by two. /// public float HalfHeight { get { return halfHeight; } set { halfHeight = value; OnShapeChanged(); } } /// /// Length of the box divided by two. /// public float HalfLength { get { return halfLength; } set { halfLength = value; OnShapeChanged(); } } /// /// Width of the box. /// public float Width { get { return halfWidth * 2; } set { halfWidth = value / 2; OnShapeChanged(); } } /// /// Height of the box. /// public float Height { get { return halfHeight * 2; } set { halfHeight = value / 2; OnShapeChanged(); } } /// /// Length of the box. /// public float Length { get { return halfLength * 2; } set { halfLength = value / 2; OnShapeChanged(); } } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); //Sample the local directions from the orientation matrix, implicitly transposed. //Notice only three directions are used. Due to box symmetry, 'left' is just -right. var right = new Vector3(Math.Sign(o.M11) * halfWidth, Math.Sign(o.M21) * halfHeight, Math.Sign(o.M31) * halfLength); var up = new Vector3(Math.Sign(o.M12) * halfWidth, Math.Sign(o.M22) * halfHeight, Math.Sign(o.M32) * halfLength); var backward = new Vector3(Math.Sign(o.M13) * halfWidth, Math.Sign(o.M23) * halfHeight, Math.Sign(o.M33) * halfLength); Matrix3x3.Transform(ref right, ref o, out right); Matrix3x3.Transform(ref up, ref o, out up); Matrix3x3.Transform(ref backward, ref o, out backward); //These right/up/backward represent the extreme points in world space along the world space axes. boundingBox.Max.X = shapeTransform.Position.X + right.X; boundingBox.Max.Y = shapeTransform.Position.Y + up.Y; boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z; boundingBox.Min.X = shapeTransform.Position.X - right.X; boundingBox.Min.Y = shapeTransform.Position.Y - up.Y; boundingBox.Min.Z = shapeTransform.Position.Z - backward.Z; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { extremePoint = new Vector3(Math.Sign(direction.X) * (halfWidth - collisionMargin), Math.Sign(direction.Y) * (halfHeight - collisionMargin), Math.Sign(direction.Z) * (halfLength - collisionMargin)); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { return Math.Min(halfWidth, Math.Min(halfHeight, halfLength)); } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { return (float)Math.Sqrt(halfWidth * halfWidth + halfHeight * halfHeight + halfLength * halfLength); } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { var volumeDistribution = new Matrix3x3(); float halfWidthSquared = halfWidth * halfWidth; float halfHeightSquared = halfHeight * halfHeight; float halfLengthSquared = halfLength * halfLength; const float inv3 = 1 / 3f; volumeDistribution.M11 = (halfHeightSquared + halfLengthSquared) * inv3; volumeDistribution.M22 = (halfWidthSquared + halfLengthSquared) * inv3; volumeDistribution.M33 = (halfWidthSquared + halfHeightSquared) * inv3; volume = ComputeVolume(); return volumeDistribution; } /// /// Gets the intersection between the box and the ray. /// /// Ray to test against the box. /// Transform of the shape. /// Maximum distance to travel in units of the direction vector's length. /// Hit data for the raycast, if any. /// Whether or not the ray hit the target. public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { hit = new RayHit(); Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3 localOrigin; Vector3.Subtract(ref ray.Position, ref transform.Position, out localOrigin); Vector3.Transform(ref localOrigin, ref conjugate, out localOrigin); Vector3 localDirection; Vector3.Transform(ref ray.Direction, ref conjugate, out localDirection); Vector3 normal = Toolbox.ZeroVector; float temp, tmin = 0, tmax = maximumLength; if (Math.Abs(localDirection.X) < Toolbox.Epsilon && (localOrigin.X < -halfWidth || localOrigin.X > halfWidth)) return false; float inverseDirection = 1 / localDirection.X; float t1 = (-halfWidth - localOrigin.X) * inverseDirection; float t2 = (halfWidth - localOrigin.X) * inverseDirection; var tempNormal = new Vector3(-1, 0, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; if (Math.Abs(localDirection.Y) < Toolbox.Epsilon && (localOrigin.Y < -halfHeight || localOrigin.Y > halfHeight)) return false; inverseDirection = 1 / localDirection.Y; t1 = (-halfHeight - localOrigin.Y) * inverseDirection; t2 = (halfHeight - localOrigin.Y) * inverseDirection; tempNormal = new Vector3(0, -1, 0); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; if (Math.Abs(localDirection.Z) < Toolbox.Epsilon && (localOrigin.Z < -halfLength || localOrigin.Z > halfLength)) return false; inverseDirection = 1 / localDirection.Z; t1 = (-halfLength - localOrigin.Z) * inverseDirection; t2 = (halfLength - localOrigin.Z) * inverseDirection; tempNormal = new Vector3(0, 0, -1); if (t1 > t2) { temp = t1; t1 = t2; t2 = temp; tempNormal *= -1; } temp = tmin; tmin = Math.Max(tmin, t1); if (temp != tmin) normal = tempNormal; tmax = Math.Min(tmax, t2); if (tmin > tmax) return false; hit.T = tmin; Vector3.Multiply(ref ray.Direction, tmin, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Vector3.Transform(ref normal, ref transform.Orientation, out normal); hit.Normal = normal; return true; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return 8 * halfWidth * halfLength * halfHeight; } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/CapsuleShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Sphere-expanded line segment. Another way of looking at it is a cylinder with half-spheres on each end. /// public class CapsuleShape : ConvexShape { /// /// Constructs a new capsule shape. /// ///Length of the capsule's inner line segment. ///Radius to expand the line segment width. public CapsuleShape(float length, float radius) { halfLength = length * .5f; Radius = radius; } float halfLength; /// /// Gets or sets the length of the capsule's inner line segment. /// public float Length { get { return halfLength * 2; } set { halfLength = value / 2; OnShapeChanged(); } } //This is a convenience method. People expect to see a 'radius' of some kind. /// /// Gets or sets the radius of the capsule. /// public float Radius { get { return collisionMargin; } set { CollisionMargin = value; } } public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif var upExtreme = new Vector3(0, halfLength, 0); var downExtreme = new Vector3(0, -halfLength, 0); Vector3.Transform(ref upExtreme, ref shapeTransform.Orientation, out upExtreme); Vector3.Transform(ref downExtreme, ref shapeTransform.Orientation, out downExtreme); if (upExtreme.X > downExtreme.X) { boundingBox.Max.X = upExtreme.X; boundingBox.Min.X = downExtreme.X; } else { boundingBox.Max.X = downExtreme.X; boundingBox.Min.X = upExtreme.X; } if (upExtreme.Y > downExtreme.Y) { boundingBox.Max.Y = upExtreme.Y; boundingBox.Min.Y = downExtreme.Y; } else { boundingBox.Max.Y = downExtreme.Y; boundingBox.Min.Y = upExtreme.Y; } if (upExtreme.Z > downExtreme.Z) { boundingBox.Max.Z = upExtreme.Z; boundingBox.Min.Z = downExtreme.Z; } else { boundingBox.Max.Z = downExtreme.Z; boundingBox.Min.Z = upExtreme.Z; } boundingBox.Min.X += shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X += shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { if (direction.Y > 0) extremePoint = new Vector3(0, halfLength, 0); else if (direction.Y < 0) extremePoint = new Vector3(0, -halfLength, 0); else extremePoint = Toolbox.ZeroVector; } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { return Radius; } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { return halfLength + Radius; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { volume = ComputeVolume(); //Calculate inertia tensor. var volumeDistribution = new Matrix3x3(); float effectiveLength = Length + Radius / 2; float diagValue = (.0833333333f * effectiveLength * effectiveLength + .25f * Radius * Radius); volumeDistribution.M11 = diagValue; volumeDistribution.M22 = .5f * Radius * Radius; volumeDistribution.M33 = diagValue; return volumeDistribution; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return (float)(Math.PI * Radius * Radius * Length + 1.333333 * Math.PI * Radius * Radius * Radius); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } /// /// Gets the intersection between the convex shape and the ray. /// /// Ray to test. /// Transform of the convex shape. /// Maximum distance to travel in units of the ray direction's length. /// Ray hit data, if any. /// Whether or not the ray hit the target. public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { //Put the ray into local space. Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Ray localRay; Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); //Check for containment in the cylindrical portion of the capsule. if (localRay.Position.Y >= -halfLength && localRay.Position.Y <= halfLength && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= collisionMargin * collisionMargin) { //It's inside! hit.T = 0; hit.Location = localRay.Position; hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } //Project the ray direction onto the plane where the cylinder is a circle. //The projected ray is then tested against the circle to compute the time of impact. //That time of impact is used to compute the 3d hit location. Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z); float planeDirectionLengthSquared = planeDirection.LengthSquared(); if (planeDirectionLengthSquared < Toolbox.Epsilon) { //The ray is nearly parallel with the axis. //Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside //and won't hit the sides. if (localRay.Position.Y > halfLength) goto upperSphereTest; if (localRay.Position.Y < -halfLength) goto lowerSphereTest; hit = new RayHit(); return false; } Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z); float dot; Vector2.Dot(ref planeDirection, ref planeOrigin, out dot); float closestToCenterT = -dot / planeDirectionLengthSquared; Vector2 closestPoint; Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint); Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint); //How close does the ray come to the circle? float squaredDistance = closestPoint.LengthSquared(); if (squaredDistance > collisionMargin * collisionMargin) { //It's too far! The ray cannot possibly hit the capsule. hit = new RayHit(); return false; } //With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis. float backwardsDistance = collisionMargin * (float)Math.Sqrt(1 - squaredDistance / (collisionMargin * collisionMargin)); float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared); hit.T = closestToCenterT - tOffset; //Compute the impact point on the infinite cylinder in 3d local space. Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location); //Is it intersecting the cylindrical portion of the capsule? if (hit.Location.Y <= halfLength && hit.Location.Y >= -halfLength && hit.T < maximumLength) { //Yup! hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } if (hit.Location.Y < halfLength) goto lowerSphereTest; upperSphereTest: //Nope! It may be intersecting the ends of the capsule though. //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. var spherePosition = new Vector3(0, halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return false; lowerSphereTest: //Okay, what about the bottom sphere? //We're above the capsule, so cast a ray against the upper sphere. //We don't have to worry about it hitting the bottom of the sphere since it would have hit the cylinder portion first. spherePosition = new Vector3(0, -halfLength, 0); if (Toolbox.RayCastSphere(ref localRay, ref spherePosition, collisionMargin, maximumLength, out hit)) { //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return false; } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/ConeShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Symmetrical shape with a circular base and a point at the top. /// public class ConeShape : ConvexShape { float radius; float height; /// /// Gets or sets the height of the cone. /// public float Height { get { return height; } set { height = value; OnShapeChanged(); } } /// /// Gets or sets the radius of the cone base. /// public float Radius { get { return radius; } set { radius = value; OnShapeChanged(); } } /// /// Constructs a new cone shape. /// ///Height of the cone. ///Radius of the cone base. public ConeShape(float height, float radius) { this.height = height; Radius = radius; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { //Is it the tip of the cone? float sinThetaSquared = radius * radius / (radius * radius + height * height); //If d.Y * d.Y / d.LengthSquared >= sinthetaSquared if (direction.Y > 0 && direction.Y * direction.Y >= direction.LengthSquared() * sinThetaSquared) { extremePoint = new Vector3(0, .75f * height, 0); return; } //Is it a bottom edge of the cone? float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z; if (horizontalLengthSquared > Toolbox.Epsilon) { var radOverSigma = radius / Math.Sqrt(horizontalLengthSquared); extremePoint = new Vector3((float)(radOverSigma * direction.X), -.25f * height, (float)(radOverSigma * direction.Z)); } else // It's pointing almost straight down... extremePoint = new Vector3(0, -.25f * height, 0); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { double denominator = radius / height; denominator = denominator / Math.Sqrt(denominator * denominator + 1); return (float)(collisionMargin + Math.Min(.25f * height, denominator * .75 * height)); } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { return (float)(collisionMargin + Math.Max(.75 * Height, Math.Sqrt(.0625f * Height * Height + Radius * Radius))); } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { volume = ComputeVolume(); //Calculate inertia tensor. var volumeDistribution = new Matrix3x3(); float diagValue = (.1f * Height * Height + .15f * Radius * Radius); volumeDistribution.M11 = diagValue; volumeDistribution.M22 = .3f * Radius * Radius; volumeDistribution.M33 = diagValue; return volumeDistribution; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return (float)(.333333 * Math.PI * Radius * Radius * Height); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/ConvexHullShape.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Convex wrapping around a point set. /// public class ConvexHullShape : ConvexShape { /// /// Gets the point set of the convex hull. /// public ReadOnlyList Vertices { get { return new ReadOnlyList(vertices); } } RawList vertices; /// /// Constructs a new convex hull shape. /// The point set will be recentered on the local origin. /// If that offset is needed, use the other constructor which outputs the computed center. /// ///Point set to use to construct the convex hull. ///Thrown when the point set is empty. public ConvexHullShape(IList vertices) { if (vertices.Count == 0) throw new ArgumentException("Vertices list used to create a ConvexHullShape cannot be empty."); var surfaceVertices = CommonResources.GetVectorList(); ComputeCenter(vertices, surfaceVertices); this.vertices = new RawList(surfaceVertices); CommonResources.GiveBack(surfaceVertices); OnShapeChanged(); } /// /// Constructs a new convex hull shape. /// The point set will be recentered on the local origin. /// ///Point set to use to construct the convex hull. /// Computed center of the convex hull shape prior to recentering. ///Thrown when the point set is empty. public ConvexHullShape(IList vertices, out Vector3 center) { if (vertices.Count == 0) throw new ArgumentException("Vertices list used to create a ConvexHullShape cannot be empty."); var surfaceVertices = CommonResources.GetVectorList(); center = ComputeCenter(vertices, surfaceVertices); this.vertices = new RawList(surfaceVertices); CommonResources.GiveBack(surfaceVertices); OnShapeChanged(); } /// /// Constructs a new convex hull shape. /// The point set will be recentered on the local origin. /// ///Point set to use to construct the convex hull. /// Computed center of the convex hull shape prior to recentering. /// Triangle indices computed on the surface of the point set. /// Unique vertices on the surface of the convex hull. ///Thrown when the point set is empty. public ConvexHullShape(IList vertices, out Vector3 center, IList outputHullTriangleIndices, IList outputUniqueSurfaceVertices) { if (vertices.Count == 0) throw new ArgumentException("Vertices list used to create a ConvexHullShape cannot be empty."); //Ensure that the convex hull is centered on its local origin. center = ComputeCenter(vertices, outputHullTriangleIndices, outputUniqueSurfaceVertices); this.vertices = new RawList(outputUniqueSurfaceVertices); OnShapeChanged(); } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); float minX, maxX; float minY, maxY; float minZ, maxZ; var right = new Vector3(o.M11, o.M21, o.M31); var up = new Vector3(o.M12, o.M22, o.M32); var backward = new Vector3(o.M13, o.M23, o.M33); Vector3.Dot(ref vertices.Elements[0], ref right, out maxX); minX = maxX; Vector3.Dot(ref vertices.Elements[0], ref up, out maxY); minY = maxY; Vector3.Dot(ref vertices.Elements[0], ref backward, out maxZ); minZ = maxZ; int minXIndex = 0; int maxXIndex = 0; int minYIndex = 0; int maxYIndex = 0; int minZIndex = 0; int maxZIndex = 0; for (int i = 1; i < vertices.Count; i++) { float dot; Vector3.Dot(ref vertices.Elements[i], ref right, out dot); if (dot < minX) { minX = dot; minXIndex = i; } else if (dot > maxX) { maxX = dot; maxXIndex = i; } Vector3.Dot(ref vertices.Elements[i], ref up, out dot); if (dot < minY) { minY = dot; minYIndex = i; } else if (dot > maxY) { maxY = dot; maxYIndex = i; } Vector3.Dot(ref vertices.Elements[i], ref backward, out dot); if (dot < minZ) { minZ = dot; minZIndex = i; } else if (dot > maxZ) { maxZ = dot; maxZIndex = i; } } Vector3 minXpoint, maxXpoint, minYpoint, maxYpoint, minZpoint, maxZpoint; Matrix3x3.Transform(ref vertices.Elements[minXIndex], ref o, out minXpoint); Matrix3x3.Transform(ref vertices.Elements[maxXIndex], ref o, out maxXpoint); Matrix3x3.Transform(ref vertices.Elements[minYIndex], ref o, out minYpoint); Matrix3x3.Transform(ref vertices.Elements[maxYIndex], ref o, out maxYpoint); Matrix3x3.Transform(ref vertices.Elements[minZIndex], ref o, out minZpoint); Matrix3x3.Transform(ref vertices.Elements[maxZIndex], ref o, out maxZpoint); boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + maxXpoint.X; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + maxYpoint.Y; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + maxZpoint.Z; boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + minXpoint.X; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + minYpoint.Y; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + minZpoint.Z; } public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { float max; Vector3.Dot(ref vertices.Elements[0], ref direction, out max); int maxIndex = 0; for (int i = 1; i < vertices.Count; i++) { float dot; Vector3.Dot(ref vertices.Elements[i], ref direction, out dot); if (dot > max) { max = dot; maxIndex = i; } } extremePoint = vertices.Elements[maxIndex]; } #region Shape Information /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return ComputeCenter(vertices); } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { return ComputeCenter(vertices, out volume); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { float volume; ComputeCenter(out volume); return volume; } /// /// Computes the center, volume, and surface triangles of the convex hull shape. /// ///Volume of the hull. ///Surface triangles of the hull. ///Surface vertices recentered on the center of volume. ///Center of the hull. public Vector3 ComputeCenter(out float volume, IList outputSurfaceTriangles, IList outputLocalSurfaceVertices) { return ComputeCenter(vertices, out volume, outputSurfaceTriangles, outputLocalSurfaceVertices); } /// /// Computes the center of a convex hull defined by the point set. /// ///Point set defining the convex hull. ///Center of the convex hull. public static Vector3 ComputeCenter(IList vertices) { float volume; return ComputeCenter(vertices, out volume); } /// /// Computes the center and volume of a convex hull defined by a pointset. /// ///Point set defining the convex hull. ///Volume of the convex hull. ///Center of the convex hull. public static Vector3 ComputeCenter(IList vertices, out float volume) { var localSurfaceVertices = CommonResources.GetVectorList(); var surfaceTriangles = CommonResources.GetIntList(); Vector3 toReturn = ComputeCenter(vertices, out volume, surfaceTriangles, localSurfaceVertices); CommonResources.GiveBack(localSurfaceVertices); CommonResources.GiveBack(surfaceTriangles); return toReturn; } /// /// Computes the center and surface triangles of a convex hull defined by a point set. /// ///Point set defining the convex hull. ///Local positions of vertices on the convex hull. ///Center of the convex hull. public static Vector3 ComputeCenter(IList vertices, IList outputLocalSurfaceVertices) { float volume; var indices = CommonResources.GetIntList(); Vector3 toReturn = ComputeCenter(vertices, out volume, indices, outputLocalSurfaceVertices); CommonResources.GiveBack(indices); return toReturn; } /// /// Computes the center and surface triangles of a convex hull defined by a point set. /// ///Point set defining the convex hull. ///Indices of surface triangles of the convex hull. ///Local positions of vertices on the convex hull. ///Center of the convex hull. public static Vector3 ComputeCenter(IList vertices, IList outputSurfaceTriangles, IList outputLocalSurfaceVertices) { float volume; Vector3 toReturn = ComputeCenter(vertices, out volume, outputSurfaceTriangles, outputLocalSurfaceVertices); return toReturn; } /// /// Computes the center, volume, and surface triangles of a convex hull defined by a point set. /// ///Point set defining the convex hull. ///Volume of the convex hull. ///Indices of surface triangles of the convex hull. ///Local positions of vertices on the convex hull. ///Center of the convex hull. public static Vector3 ComputeCenter(IList vertices, out float volume, IList outputSurfaceTriangles, IList outputLocalSurfaceVertices) { Vector3 centroid = Toolbox.ZeroVector; for (int k = 0; k < vertices.Count; k++) { centroid += vertices[k]; } centroid /= vertices.Count; //Toolbox.GetConvexHull(vertices, outputSurfaceTriangles, outputLocalSurfaceVertices); ConvexHullHelper.GetConvexHull(vertices, outputSurfaceTriangles, outputLocalSurfaceVertices); volume = 0; var volumes = CommonResources.GetFloatList(); var centroids = CommonResources.GetVectorList(); for (int k = 0; k < outputSurfaceTriangles.Count; k += 3) { volumes.Add(Vector3.Dot( Vector3.Cross(vertices[outputSurfaceTriangles[k + 1]] - vertices[outputSurfaceTriangles[k]], vertices[outputSurfaceTriangles[k + 2]] - vertices[outputSurfaceTriangles[k]]), centroid - vertices[outputSurfaceTriangles[k]])); volume += volumes[k / 3]; centroids.Add((vertices[outputSurfaceTriangles[k]] + vertices[outputSurfaceTriangles[k + 1]] + vertices[outputSurfaceTriangles[k + 2]] + centroid) / 4); } Vector3 center = Toolbox.ZeroVector; for (int k = 0; k < centroids.Count; k++) { center += centroids[k] * (volumes[k] / volume); } volume /= 6; for (int k = 0; k < outputLocalSurfaceVertices.Count; k++) { outputLocalSurfaceVertices[k] -= center; } CommonResources.GiveBack(centroids); CommonResources.GiveBack(volumes); return center; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { var surfaceTriangles = CommonResources.GetIntList(); var surfaceVertices = CommonResources.GetVectorList(); ComputeCenter(out volume, surfaceTriangles, surfaceVertices); Matrix3x3 toReturn = ComputeVolumeDistribution(volume, surfaceTriangles); CommonResources.GiveBack(surfaceTriangles); CommonResources.GiveBack(surfaceVertices); return toReturn; } /// /// Computes the volume distribution of the convex hull, its volume, and its surface triangles. /// ///Volume of the convex hull. ///Surface triangles of the convex hull. ///Volume distribution of the convex hull. public Matrix3x3 ComputeVolumeDistribution(float volume, IList localSurfaceTriangles) { //TODO: This method has a lot of overlap with the volume calculation. Conceptually very similar, could bundle tighter. //Source: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates //http://www.scipub.org/fulltext/jms2/jms2118-11.pdf //x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3 //Looking to find inertia tensor matrix of the form // [ a -b' -c' ] // [ -b' b -a' ] // [ -c' -a' c ] float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0; Vector3 v2, v3, v4; float density = 1 / volume; float diagonalFactor = density / 60; float offFactor = -density / 120; for (int i = 0; i < localSurfaceTriangles.Count; i += 3) { v2 = vertices[localSurfaceTriangles[i]]; v3 = vertices[localSurfaceTriangles[i + 1]]; v4 = vertices[localSurfaceTriangles[i + 2]]; float determinant = Math.Abs(v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) - v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) + v4.X * (v2.Y * v3.Z - v2.Z * v3.Y)); //Determinant is 6 * volume. a += determinant * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); b += determinant * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); c += determinant * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y); ao += determinant * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z); bo += determinant * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z); co += determinant * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y); /*subInertiaTensor = new Matrix(a, bo, co, 0, bo, b, ao, 0, co, ao, c, 0, 0, 0, 0, 0); localInertiaTensor += subInertiaTensor;// +(offset.LengthSquared() * Matrix.Identity - Toolbox.getOuterProduct(offset, offset));// *(determinant * density / 6);*/ } a *= diagonalFactor; b *= diagonalFactor; c *= diagonalFactor; ao *= offFactor; bo *= offFactor; co *= offFactor; var distribution = new Matrix3x3(a, bo, co, bo, b, ao, co, ao, c); return distribution; } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { float maximumRadius = 0; for (int i = 0; i < vertices.Count; i++) { float tempDist = vertices.Elements[i].Length(); if (maximumRadius < tempDist) maximumRadius = tempDist; } maximumRadius += collisionMargin; return maximumRadius; } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { //Sample the shape in directions pointing to the vertices of a regular tetrahedron. Vector3 a, b, c, d; var direction = new Vector3(1, 1, 1); GetLocalExtremePointWithoutMargin(ref direction, out a); direction = new Vector3(-1, -1, 1); GetLocalExtremePointWithoutMargin(ref direction, out b); direction = new Vector3(-1, 1, -1); GetLocalExtremePointWithoutMargin(ref direction, out c); direction = new Vector3(1, -1, -1); GetLocalExtremePointWithoutMargin(ref direction, out d); Vector3 ab, cb, ac, ad, cd; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref b, ref c, out cb); Vector3.Subtract(ref c, ref a, out ac); Vector3.Subtract(ref d, ref a, out ad); Vector3.Subtract(ref d, ref c, out cd); //Find normals of triangles: ABC, CBD, ACD, ADB Vector3 nABC, nCBD, nACD, nADB; Vector3.Cross(ref ac, ref ab, out nABC); Vector3.Cross(ref cd, ref cb, out nCBD); Vector3.Cross(ref ad, ref ac, out nACD); Vector3.Cross(ref ab, ref ad, out nADB); //Find distances to planes. float dABC, dCBD, dACD, dADB; Vector3.Dot(ref a, ref nABC, out dABC); Vector3.Dot(ref c, ref nCBD, out dCBD); Vector3.Dot(ref a, ref nACD, out dACD); Vector3.Dot(ref a, ref nADB, out dADB); dABC /= nABC.Length(); dCBD /= nCBD.Length(); dACD /= nACD.Length(); dADB /= nADB.Length(); return collisionMargin + Math.Min(dABC, Math.Min(dCBD, Math.Min(dACD, dADB))); } #endregion /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/ConvexShape.cs ================================================ using System; using System.Diagnostics; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUphysics.Settings; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Superclass of convex collision shapes. /// public abstract class ConvexShape : EntityShape { protected internal float collisionMargin = CollisionDetectionSettings.DefaultMargin; /// /// Collision margin of the convex shape. The margin is a small spherical expansion around /// entities which allows specialized collision detection algorithms to be used. /// It's recommended that this be left unchanged. /// public float CollisionMargin { get { return collisionMargin; } set { if (value < 0) throw new ArgumentException("Collision margin must be nonnegative.."); collisionMargin = value; OnShapeChanged(); } } protected internal float minimumRadius; /// /// Gets or sets the minimum radius of the collidable's shape. This is initialized to a value that is /// guaranteed to be equal to or smaller than the actual minimum radius. When setting this property, /// ensure that the inner sphere formed by the new minimum radius is fully contained within the shape. /// public float MinimumRadius { get { return minimumRadius; } set { minimumRadius = value; } } protected internal float maximumRadius; /// /// Gets the maximum radius of the collidable's shape. This is initialized to a value that is /// guaranteed to be equal to or larger than the actual maximum radius. /// public float MaximumRadius { get { return maximumRadius; } } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public abstract void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint); /// /// Gets the extreme point of the shape in world space in a given direction. /// ///Direction to find the extreme point in. /// Transform to use for the shape. ///Extreme point on the shape. public void GetExtremePointWithoutMargin(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint) { Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Vector3.Transform(ref direction, ref conjugate, out direction); GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3.Transform(ref extremePoint, ref shapeTransform.Orientation, out extremePoint); Vector3.Add(ref extremePoint, ref shapeTransform.Position, out extremePoint); } /// /// Gets the extreme point of the shape in world space in a given direction with margin expansion. /// ///Direction to find the extreme point in. /// Transform to use for the shape. ///Extreme point on the shape. public void GetExtremePoint(Vector3 direction, ref RigidTransform shapeTransform, out Vector3 extremePoint) { GetExtremePointWithoutMargin(direction, ref shapeTransform, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3.Add(ref extremePoint, ref direction, out extremePoint); } } /// /// Gets the extreme point of the shape in local space in a given direction with margin expansion. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public void GetLocalExtremePoint(Vector3 direction, out Vector3 extremePoint) { GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); float directionLength = direction.LengthSquared(); if (directionLength > Toolbox.Epsilon) { Vector3.Multiply(ref direction, collisionMargin / (float)Math.Sqrt(directionLength), out direction); Vector3.Add(ref extremePoint, ref direction, out extremePoint); } } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); //Sample the local directions from the orientation matrix, implicitly transposed. Vector3 right; var direction = new Vector3(o.M11, o.M21, o.M31); GetLocalExtremePointWithoutMargin(ref direction, out right); Vector3 left; direction = new Vector3(-o.M11, -o.M21, -o.M31); GetLocalExtremePointWithoutMargin(ref direction, out left); Vector3 up; direction = new Vector3(o.M12, o.M22, o.M32); GetLocalExtremePointWithoutMargin(ref direction, out up); Vector3 down; direction = new Vector3(-o.M12, -o.M22, -o.M32); GetLocalExtremePointWithoutMargin(ref direction, out down); Vector3 backward; direction = new Vector3(o.M13, o.M23, o.M33); GetLocalExtremePointWithoutMargin(ref direction, out backward); Vector3 forward; direction = new Vector3(-o.M13, -o.M23, -o.M33); GetLocalExtremePointWithoutMargin(ref direction, out forward); Matrix3x3.Transform(ref right, ref o, out right); Matrix3x3.Transform(ref left, ref o, out left); Matrix3x3.Transform(ref up, ref o, out up); Matrix3x3.Transform(ref down, ref o, out down); Matrix3x3.Transform(ref backward, ref o, out backward); Matrix3x3.Transform(ref forward, ref o, out forward); //These right/up/backward represent the extreme points in world space along the world space axes. boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z; boundingBox.Min.X = shapeTransform.Position.X - collisionMargin + left.X; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin + down.Y; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin + forward.Z; } /// /// Gets the intersection between the convex shape and the ray. /// /// Ray to test. /// Transform of the convex shape. /// Maximum distance to travel in units of the ray direction's length. /// Ray hit data, if any. /// Whether or not the ray hit the target. public virtual bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { //RayHit newHit; //bool newBool = GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out newHit); //RayHit oldHit; //bool oldBool = OldGJKVerifier.RayCastGJK(ray.Position, ray.Direction, maximumLength, this, transform, out oldHit.Location, out oldHit.Normal, out oldHit.T); //bool mprBool = MPRToolbox.RayCast(ray, maximumLength, this, ref transform, out hit); ////if (newBool != oldBool || ((newBool && oldBool) && Vector3.DistanceSquared(newHit.Location, hit.Location) > .01f)) //// Debug.WriteLine("break."); //return mprBool; //if (GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out hit)) //{ // //GJK toolbox doesn't normalize the hit normal; it's unnecessary for some other systems so it just saves on time. // //It would be nice if ray tests always normalized it though. // float length = hit.Normal.LengthSquared(); // if (length > Toolbox.Epsilon) // Vector3.Divide(ref hit.Normal, (float) Math.Sqrt(length), out hit.Normal); // else // hit.Normal = new Vector3(); // return true; //} //return false; return MPRToolbox.RayCast(ray, maximumLength, this, ref transform, out hit); } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return InertiaHelper.ComputeCenter(this); } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { return InertiaHelper.ComputeCenter(this, out volume); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { float volume; ComputeVolumeDistribution(out volume); return volume; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { return InertiaHelper.ComputeVolumeDistribution(this, out volume); } protected override void OnShapeChanged() { minimumRadius = ComputeMinimumRadius(); maximumRadius = ComputeMaximumRadius(); base.OnShapeChanged(); } /// /// Computes the volume distribution of the shape. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution() { float volume; return ComputeVolumeDistribution(out volume); } public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo) { shapeInfo.VolumeDistribution = ComputeVolumeDistribution(out shapeInfo.Volume); shapeInfo.Center = ComputeCenter(); } /// /// Computes a bounding box for the shape and expands it. /// /// Transform to use to position the shape. /// Extra to add to the bounding box. /// Expanded bounding box. public void GetSweptBoundingBox(ref RigidTransform transform, ref Vector3 sweep, out BoundingBox boundingBox) { GetBoundingBox(ref transform, out boundingBox); Toolbox.ExpandBoundingBox(ref boundingBox, ref sweep); } /// /// Gets the bounding box of the convex shape transformed first into world space, and then into the local space of another affine transform. /// /// Transform to use to put the shape into world space. /// Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space. /// Vector to expand the bounding box with in local space. /// Bounding box in the local space. public void GetSweptLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, ref Vector3 sweep, out BoundingBox boundingBox) { GetLocalBoundingBox(ref shapeTransform, ref spaceTransform, out boundingBox); Vector3 expansion; Matrix3x3.TransformTranspose(ref sweep, ref spaceTransform.LinearTransform, out expansion); Toolbox.ExpandBoundingBox(ref boundingBox, ref expansion); } //Transform the convex into the space of something else. /// /// Gets the bounding box of the convex shape transformed first into world space, and then into the local space of another affine transform. /// /// Transform to use to put the shape into world space. /// Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space. /// Bounding box in the local space. public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif //TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step. //There should be a better way to do this. //Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can //slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case). //Move forward into convex's space, backwards into the new space's local space. AffineTransform transform; AffineTransform.Invert(ref spaceTransform, out transform); AffineTransform.Multiply(ref shapeTransform, ref transform, out transform); //Sample the local directions from the orientation matrix, implicitly transposed. Vector3 right; var direction = new Vector3(transform.LinearTransform.M11, transform.LinearTransform.M21, transform.LinearTransform.M31); GetLocalExtremePoint(direction, out right); Vector3 left; direction = new Vector3(-transform.LinearTransform.M11, -transform.LinearTransform.M21, -transform.LinearTransform.M31); GetLocalExtremePoint(direction, out left); Vector3 up; direction = new Vector3(transform.LinearTransform.M12, transform.LinearTransform.M22, transform.LinearTransform.M32); GetLocalExtremePoint(direction, out up); Vector3 down; direction = new Vector3(-transform.LinearTransform.M12, -transform.LinearTransform.M22, -transform.LinearTransform.M32); GetLocalExtremePoint(direction, out down); Vector3 backward; direction = new Vector3(transform.LinearTransform.M13, transform.LinearTransform.M23, transform.LinearTransform.M33); GetLocalExtremePoint(direction, out backward); Vector3 forward; direction = new Vector3(-transform.LinearTransform.M13, -transform.LinearTransform.M23, -transform.LinearTransform.M33); GetLocalExtremePoint(direction, out forward); //This could be optimized. Unnecessary transformation information gets computed. Matrix3x3.Transform(ref right, ref transform.LinearTransform, out right); Matrix3x3.Transform(ref left, ref transform.LinearTransform, out left); Matrix3x3.Transform(ref up, ref transform.LinearTransform, out up); Matrix3x3.Transform(ref down, ref transform.LinearTransform, out down); Matrix3x3.Transform(ref backward, ref transform.LinearTransform, out backward); Matrix3x3.Transform(ref forward, ref transform.LinearTransform, out forward); //These right/up/backward represent the extreme points in world space along the world space axes. boundingBox.Max.X = transform.Translation.X + right.X; boundingBox.Max.Y = transform.Translation.Y + up.Y; boundingBox.Max.Z = transform.Translation.Z + backward.Z; boundingBox.Min.X = transform.Translation.X + left.X; boundingBox.Min.Y = transform.Translation.Y + down.Y; boundingBox.Min.Z = transform.Translation.Z + forward.Z; } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public abstract float ComputeMinimumRadius(); /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public abstract float ComputeMaximumRadius(); } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/CylinderShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Symmetrical object with a circular bottom and top. /// public class CylinderShape : ConvexShape { private float halfHeight; private float radius; /// /// Constructs a new cylinder shape. /// ///Height of the cylinder. ///Radius of the cylinder. public CylinderShape(float height, float radius) { this.halfHeight = height * .5f; Radius = radius; } /// /// Gets or sets the radius of the cylinder. /// public float Radius { get { return radius; } set { radius = value; OnShapeChanged(); } } /// /// Gets or sets the height of the cylinder. /// public float Height { get { return halfHeight * 2; } set { halfHeight = value / 2; OnShapeChanged(); } } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); //Sample the local directions from the orientation matrix, implicitly transposed. //Notice only three directions are used. Due to box symmetry, 'left' is just -right. var direction = new Vector3(o.M11, o.M21, o.M31); Vector3 right; GetLocalExtremePointWithoutMargin(ref direction, out right); direction = new Vector3(o.M12, o.M22, o.M32); Vector3 up; GetLocalExtremePointWithoutMargin(ref direction, out up); direction = new Vector3(o.M13, o.M23, o.M33); Vector3 backward; GetLocalExtremePointWithoutMargin(ref direction, out backward); Matrix3x3.Transform(ref right, ref o, out right); Matrix3x3.Transform(ref up, ref o, out up); Matrix3x3.Transform(ref backward, ref o, out backward); //These right/up/backward represent the extreme points in world space along the world space axes. boundingBox.Max.X = shapeTransform.Position.X + collisionMargin + right.X; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin + up.Y; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin + backward.Z; boundingBox.Min.X = shapeTransform.Position.X - collisionMargin - right.X; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin - up.Y; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin - backward.Z; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { float horizontalLengthSquared = direction.X * direction.X + direction.Z * direction.Z; if (horizontalLengthSquared > Toolbox.Epsilon) { float multiplier = (radius - collisionMargin) / (float)Math.Sqrt(horizontalLengthSquared); extremePoint = new Vector3(direction.X * multiplier, Math.Sign(direction.Y) * (halfHeight - collisionMargin), direction.Z * multiplier); } else { extremePoint = new Vector3(0, Math.Sign(direction.Y) * (halfHeight - collisionMargin), 0); } } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { return (float)Math.Sqrt(radius * radius + halfHeight * halfHeight); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { return Math.Min(radius, halfHeight); } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { volume = ComputeVolume(); var volumeDistribution = new Matrix3x3(); float diagValue = (.0833333333f * Height * Height + .25f * Radius * Radius); volumeDistribution.M11 = diagValue; volumeDistribution.M22 = .5f * Radius * Radius; volumeDistribution.M33 = diagValue; return volumeDistribution; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return (float)(Math.PI * Radius * Radius * Height); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } /// /// Gets the intersection between the convex shape and the ray. /// /// Ray to test. /// Transform of the convex shape. /// Maximum distance to travel in units of the ray direction's length. /// Ray hit data, if any. /// Whether or not the ray hit the target. public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { //Put the ray into local space. Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Ray localRay; Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); //Check for containment. if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius) { //It's inside! hit.T = 0; hit.Location = localRay.Position; hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } //Project the ray direction onto the plane where the cylinder is a circle. //The projected ray is then tested against the circle to compute the time of impact. //That time of impact is used to compute the 3d hit location. Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z); float planeDirectionLengthSquared = planeDirection.LengthSquared(); if (planeDirectionLengthSquared < Toolbox.Epsilon) { //The ray is nearly parallel with the axis. //Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside //and won't hit the sides. if (localRay.Position.Y > halfHeight) goto upperTest; if (localRay.Position.Y < -halfHeight) goto lowerTest; hit = new RayHit(); return false; } Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z); float dot; Vector2.Dot(ref planeDirection, ref planeOrigin, out dot); float closestToCenterT = -dot / planeDirectionLengthSquared; Vector2 closestPoint; Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint); Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint); //How close does the ray come to the circle? float squaredDistance = closestPoint.LengthSquared(); if (squaredDistance > radius * radius) { //It's too far! The ray cannot possibly hit the capsule. hit = new RayHit(); return false; } //With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis. float backwardsDistance = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius)); float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared); hit.T = closestToCenterT - tOffset; //Compute the impact point on the infinite cylinder in 3d local space. Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location); //Is it intersecting the cylindrical portion of the capsule? if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength) { //Yup! hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z); float normalLengthSquared = hit.Normal.LengthSquared(); if (normalLengthSquared > 1e-9f) Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); //Pull the hit into world space. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location); return true; } if (hit.Location.Y < halfHeight) goto lowerTest; upperTest: //Nope! It may be intersecting the ends of the cylinder though. //We're above the cylinder, so cast a ray against the upper cap. if (localRay.Direction.Y > -1e-9) { //Can't hit the upper cap if the ray isn't pointing down. hit = new RayHit(); return false; } float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y; Vector3 planeIntersection; Vector3.Multiply(ref localRay.Direction, t, out planeIntersection); Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection); if(planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength) { //Pull the hit into world space. Vector3.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location); hit.T = t; return true; } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return false; lowerTest: //Is it intersecting the bottom cap? if (localRay.Direction.Y < 1e-9) { //Can't hit the bottom cap if the ray isn't pointing up. hit = new RayHit(); return false; } t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y; Vector3.Multiply(ref localRay.Direction, t, out planeIntersection); Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection); if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength) { //Pull the hit into world space. Vector3.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal); RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location); hit.T = t; return true; } //No intersection! We can't be hitting the other sphere, so it's over! hit = new RayHit(); return false; } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/InertiaHelper.cs ================================================ using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Helper class used to compute volume distribution information, which is in turn used to compute inertia tensor information. /// public class InertiaHelper { /// /// Value to scale any created entities' inertia tensors by. /// Larger tensors (above 1) improve stiffness of constraints and contacts, while smaller values (towards 1) are closer to 'realistic' behavior. /// Defaults to 2.5. /// public static float InertiaTensorScale = 2.5f; /// /// Number of samples the system takes along a side of an object's AABB when voxelizing it. /// public static int NumberOfSamplesPerDimension = 10; /// /// Computes the center of a convex shape. /// ///Shape to compute the center of. ///Center of the shape. public static Vector3 ComputeCenter(ConvexShape shape) { float volume; return ComputeCenter(shape, out volume); } /// /// Computes the center and volume of a convex shape. /// ///Shape to compute the center of. ///Volume of the shape. ///Center of the shape. public static Vector3 ComputeCenter(ConvexShape shape, out float volume) { var pointContributions = CommonResources.GetVectorList(); GetPoints(shape, out volume, pointContributions); Vector3 center = AveragePoints(pointContributions); CommonResources.GiveBack(pointContributions); MathChecker.Validate(center); return center; } /// /// Averages together all the points in the point list. /// ///Point list to average. ///Averaged point. public static Vector3 AveragePoints(RawList pointContributions) { var center = new Vector3(); for (int i = 0; i < pointContributions.Count; i++) { center += pointContributions[i]; //Every point has equal weight. } return center / pointContributions.Count; } /// /// Computes the volume and volume distribution of a shape. /// ///Shape to compute the volume information of. ///Volume of the shape. ///Volume distribution of the shape. public static Matrix3x3 ComputeVolumeDistribution(ConvexShape shape, out float volume) { var pointContributions = CommonResources.GetVectorList(); GetPoints(shape, out volume, pointContributions); Vector3 center = AveragePoints(pointContributions); Matrix3x3 volumeDistribution = ComputeVolumeDistribution(pointContributions, ref center); CommonResources.GiveBack(pointContributions); return volumeDistribution; } /// /// Computes the volume and volume distribution of a shape based on a given center. /// ///Shape to compute the volume information of. ///Location to use as the center of the shape when computing the volume distribution. ///Volume of the shape. ///Volume distribution of the shape. public static Matrix3x3 ComputeVolumeDistribution(ConvexShape shape, ref Vector3 center, out float volume) { var pointContributions = CommonResources.GetVectorList(); GetPoints(shape, out volume, pointContributions); Matrix3x3 volumeDistribution = ComputeVolumeDistribution(pointContributions, ref center); CommonResources.GiveBack(pointContributions); return volumeDistribution; } /// /// Computes a volume distribution based on a bunch of point contributions. /// ///Point contributions to the volume distribution. ///Location to use as the center for purposes of computing point contributions. ///Volume distribution of the point contributions. public static Matrix3x3 ComputeVolumeDistribution(RawList pointContributions, ref Vector3 center) { var volumeDistribution = new Matrix3x3(); float pointWeight = 1f / pointContributions.Count; for (int i = 0; i < pointContributions.Count; i++) { Matrix3x3 contribution; GetPointContribution(pointWeight, ref center, pointContributions[i], out contribution); Matrix3x3.Add(ref volumeDistribution, ref contribution, out volumeDistribution); } return volumeDistribution; } /// /// Gets the point contributions within a convex shape. /// ///Shape to compute the point contributions of. ///Volume of the shape. ///Point contributions of the shape. public static void GetPoints(ConvexShape shape, out float volume, RawList outputPointContributions) { RigidTransform transform = RigidTransform.Identity; BoundingBox boundingBox; shape.GetBoundingBox(ref transform, out boundingBox); //Find the direction which maximizes the possible hits. Generally, this is the smallest area axis. //Possible options are: //YZ -> use X //XZ -> use Y //XY -> use Z Ray ray; float width = boundingBox.Max.X - boundingBox.Min.X; float height = boundingBox.Max.Y - boundingBox.Min.Y; float length = boundingBox.Max.Z - boundingBox.Min.Z; float yzArea = height * length; float xzArea = width * length; float xyArea = width * height; Vector3 increment1, increment2; float incrementMultiplier = 1f / NumberOfSamplesPerDimension; float maxLength; float rayIncrement; if (yzArea > xzArea && yzArea > xyArea) { //use the x axis as the direction. ray.Direction = Vector3.Right; ray.Position = new Vector3(boundingBox.Min.X, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(0, incrementMultiplier * height, 0); increment2 = new Vector3(0, 0, incrementMultiplier * length); rayIncrement = incrementMultiplier * width; maxLength = width; } else if (xzArea > xyArea) //yz is not the max, given by the previous if. Is xz or xy the max? { //use the y axis as the direction. ray.Direction = Vector3.Up; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y, boundingBox.Min.Z + .5f * incrementMultiplier * length); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, 0, incrementMultiplier * height); rayIncrement = incrementMultiplier * height; maxLength = height; } else { //use the z axis as the direction. ray.Direction = Vector3.Backward; ray.Position = new Vector3(boundingBox.Min.X + .5f * incrementMultiplier * width, boundingBox.Min.Y + .5f * incrementMultiplier * height, boundingBox.Min.Z); increment1 = new Vector3(incrementMultiplier * width, 0, 0); increment2 = new Vector3(0, incrementMultiplier * height, 0); rayIncrement = incrementMultiplier * length; maxLength = length; } Ray oppositeRay; volume = 0; for (int i = 0; i < NumberOfSamplesPerDimension; i++) { for (int j = 0; j < NumberOfSamplesPerDimension; j++) { //Ray cast from one direction. If it succeeds, try the other way. This forms an interval in which inertia tensor contributions are contained. RayHit hit; if (shape.RayTest(ref ray, ref transform, maxLength, out hit)) { Vector3.Multiply(ref ray.Direction, maxLength, out oppositeRay.Position); Vector3.Add(ref oppositeRay.Position, ref ray.Position, out oppositeRay.Position); Vector3.Negate(ref ray.Direction, out oppositeRay.Direction); RayHit oppositeHit; if (shape.RayTest(ref oppositeRay, ref transform, maxLength, out oppositeHit)) { //It should always get here if one direction casts, but there may be numerical issues. float scanVolume; ScanObject(rayIncrement, maxLength, ref increment1, ref increment2, ref ray, ref hit, ref oppositeHit, outputPointContributions, out scanVolume); volume += scanVolume; } } Vector3.Add(ref ray.Position, ref increment2, out ray.Position); } Vector3.Add(ref ray.Position, ref increment1, out ray.Position); //Move the ray back to the starting position along the other axis. Vector3 subtract; Vector3.Multiply(ref increment2, NumberOfSamplesPerDimension, out subtract); Vector3.Subtract(ref ray.Position, ref subtract, out ray.Position); } } private static void ScanObject(float rayIncrement, float maxLength, ref Vector3 increment1, ref Vector3 increment2, ref Ray ray, ref RayHit startHit, ref RayHit endHit, RawList pointContributions, out float volume) { Vector3 cell; Vector3.Multiply(ref ray.Direction, rayIncrement, out cell); Vector3.Add(ref increment1, ref cell, out cell); Vector3.Add(ref increment2, ref cell, out cell); float perCellVolume = cell.X * cell.Y * cell.Z; volume = 0; for (int i = (int)(startHit.T / rayIncrement); i <= (int)((maxLength - endHit.T) / rayIncrement); i++) { Vector3 position; Vector3.Multiply(ref ray.Direction, (i + .5f) * rayIncrement, out position); Vector3.Add(ref position, ref ray.Position, out position); pointContributions.Add(position); volume += perCellVolume; } } /// /// Computes the volume contribution of a point. /// ///Weight of the point. ///Location to use as the center for the purposes of computing the contribution. ///Point to compute the contribution of. ///Contribution of the point. public static void GetPointContribution(float pointWeight, ref Vector3 center, Vector3 p, out Matrix3x3 contribution) { Vector3.Subtract(ref p, ref center, out p); float xx = pointWeight * p.X * p.X; float yy = pointWeight * p.Y * p.Y; float zz = pointWeight * p.Z * p.Z; contribution.M11 = yy + zz; contribution.M22 = xx + zz; contribution.M33 = xx + yy; contribution.M12 = -pointWeight * p.X * p.Y; contribution.M13 = -pointWeight * p.X * p.Z; contribution.M23 = -pointWeight * p.Y * p.Z; contribution.M21 = contribution.M12; contribution.M31 = contribution.M13; contribution.M32 = contribution.M23; } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/MinkowskiSumShape.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// A shape associated with an orientation. /// public struct OrientedConvexShapeEntry { /// /// The entry's shape. /// public ConvexShape CollisionShape; /// /// The entry's orientation. /// public Quaternion Orientation; /// /// Constructs a new entry. /// ///Orientation of the entry. ///Shape of the entry. public OrientedConvexShapeEntry(Quaternion orientation, ConvexShape shape) { Orientation = orientation; CollisionShape = shape; } /// /// Constructs a new entry with identity orientation. /// ///Shape of the entry. public OrientedConvexShapeEntry(ConvexShape shape) { Orientation = Quaternion.Identity; CollisionShape = shape; } } /// /// A shape composed of the pointwise summation of all points in child shapes. /// For example, the minkowski sum of two spheres would be a sphere with the radius of both spheres combined. /// The minkowski sum of a box and a sphere would be a rounded box. /// public class MinkowskiSumShape : ConvexShape { ObservableList shapes = new ObservableList(); /// /// Gets the list of shapes in the minkowski sum. /// public ObservableList Shapes { get { return shapes; } } //Local offset is needed to ensure that the minkowski sum is centered on the local origin. Vector3 localOffset; /// /// Gets the local offset of the elements in the minkowski sum. /// This is required because convex shapes need to be centered on their local origin. /// public Vector3 LocalOffset { get { return localOffset; } } /// /// Constructs a minkowski sum shape. /// A minkowski sum can be created from more than two objects; use the other constructors. /// The sum will be recentered on its local origin. /// /// First entry in the sum. /// Second entry in the sum. /// Center of the minkowski sum computed pre-recentering. public MinkowskiSumShape(OrientedConvexShapeEntry firstShape, OrientedConvexShapeEntry secondShape, out Vector3 center) : this(firstShape, secondShape) { center = -localOffset; } /// /// Constructs a minkowski sum shape. /// The sum will be recentered on its local origin. /// /// Entries composing the minkowski sum. /// Center of the minkowski sum computed pre-recentering. public MinkowskiSumShape(IList shapeEntries, out Vector3 center) : this(shapeEntries) { center = -localOffset; } /// /// Constructs a minkowski sum shape. /// A minkowski sum can be created from more than two objects; use the other constructors. /// The sum will be recentered on its local origin. The computed center is outputted by the other constructor. /// /// First entry in the sum. /// Second entry in the sum. public MinkowskiSumShape(OrientedConvexShapeEntry firstShape, OrientedConvexShapeEntry secondShape) { shapes.Add(firstShape); shapes.Add(secondShape); shapes.Changed += ShapesChanged; OnShapeChanged(); localOffset = -ComputeCenter(); } /// /// Constructs a minkowski sum shape. /// The sum will be recentered on its local origin. The computed center is outputted by the other constructor. /// /// Entries composing the minkowski sum. public MinkowskiSumShape(IList shapeEntries) { if (shapeEntries.Count == 0) throw new ArgumentException("Cannot create a wrapped shape with no contained shapes."); for (int i = 0; i < shapeEntries.Count; i++) { shapes.Add(shapeEntries[i]); } shapes.Changed += ShapesChanged; OnShapeChanged(); localOffset = -ComputeCenter(); } void ShapesChanged(ObservableList list) { OnShapeChanged(); //Computing the center uses extreme point calculations. //Extreme point calculations make use of the localOffset. //So, set the local offset to zero before doing the computation. //The new offset is then computed. localOffset = new Vector3(); localOffset = -ComputeCenter(); } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { var transform = new RigidTransform { Orientation = shapes.WrappedList.Elements[0].Orientation }; shapes.WrappedList.Elements[0].CollisionShape.GetExtremePoint(direction, ref transform, out extremePoint); for (int i = 1; i < shapes.WrappedList.Count; i++) { Vector3 temp; transform.Orientation = shapes.WrappedList.Elements[i].Orientation; shapes.WrappedList.Elements[i].CollisionShape.GetExtremePoint(direction, ref transform, out temp); Vector3.Add(ref extremePoint, ref temp, out extremePoint); } Vector3.Add(ref extremePoint, ref localOffset, out extremePoint); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { float minRadius = 0; for (int i = 0; i < shapes.Count; i++) { minRadius += shapes.WrappedList.Elements[i].CollisionShape.ComputeMinimumRadius(); } return minRadius + collisionMargin; } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { //This can overestimate the actual maximum radius, but such is the defined behavior of the ComputeMaximumRadius function. It's not exact; it's an upper bound on the actual maximum. float maxRadius = 0; for (int i = 0; i < shapes.Count; i++) { maxRadius += shapes.WrappedList.Elements[i].CollisionShape.ComputeMaximumRadius(); } return maxRadius + collisionMargin; } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/SphereShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Ball-like shape. /// public class SphereShape : ConvexShape { /// /// Constructs a new sphere shape. /// ///Radius of the sphere. public SphereShape(float radius) { Radius = radius; } //This is a convenience method. People expect to see a 'radius' of some kind. /// /// Gets or sets the radius of the sphere. /// public float Radius { get { return collisionMargin; } set { CollisionMargin = value; } } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif boundingBox.Min.X = shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y = shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z = shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X = shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y = shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z = shapeTransform.Position.Z + collisionMargin; } //TODO: Could do a little optimizing. If the methods were virtual, could override and save a conjugate/transform. /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { extremePoint = Toolbox.ZeroVector; } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { return Radius; } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { return Radius; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { var volumeDistribution = new Matrix3x3(); float diagValue = ((2f / 5f) * Radius * Radius); volumeDistribution.M11 = diagValue; volumeDistribution.M22 = diagValue; volumeDistribution.M33 = diagValue; volume = ComputeVolume(); return volumeDistribution; } /// /// Gets the intersection between the sphere and the ray. /// /// Ray to test against the sphere. /// Transform applied to the convex for the test. /// Maximum distance to travel in units of the ray direction's length. /// Ray hit data, if any. /// Whether or not the ray hit the target. public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { return Toolbox.RayCastSphere(ref ray, ref transform.Position, collisionMargin, maximumLength, out hit); //Vector3 normalizedDirection; //float length = ray.Direction.Length(); //Vector3.Divide(ref ray.Direction, length, out normalizedDirection); //maximumLength *= length; //hit = new RayHit(); //Vector3 m; //Vector3.Subtract(ref ray.Position, ref transform.Position, out m); //float b = Vector3.Dot(m, normalizedDirection); //float c = m.LengthSquared() - collisionMargin * collisionMargin; //if (c > 0 && b > 0) // return false; //float discriminant = b * b - c; //if (discriminant < 0) // return false; //hit.T = -b - (float)Math.Sqrt(discriminant); //if (hit.T < 0) // hit.T = 0; //if (hit.T > maximumLength) // return false; //Vector3.Multiply(ref normalizedDirection, hit.T, out hit.Location); //Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); //Vector3.Subtract(ref hit.Location, ref transform.Position, out hit.Normal); //hit.Normal.Normalize(); //return true; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return (float)(1.333333 * Math.PI * Radius * Radius * Radius); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/TransformableShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Shape which can take any convex shape and use a linear transform to shear, scale, and rotate it. /// public class TransformableShape : ConvexShape { protected ConvexShape shape; /// /// Gets or sets the convex shape to be transformed. /// public ConvexShape Shape { get { return shape; } set { shape = value; OnShapeChanged(); } } protected Matrix3x3 transform; /// /// Gets or sets the linear transform used to transform the convex shape. /// public Matrix3x3 Transform { get { return transform; } set { transform = value; OnShapeChanged(); } } /// /// Constructs a new transformable shape. /// ///Base shape to transform. ///Transform to use. public TransformableShape(ConvexShape shape, Matrix3x3 transform) { this.shape = shape; Transform = transform; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { Vector3 d; Matrix3x3.TransformTranspose(ref direction, ref transform, out d); shape.GetLocalExtremePoint(d, out extremePoint); Matrix3x3.Transform(ref extremePoint, ref transform, out extremePoint); } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { //This will overestimate the actual maximum radius, but such is the defined behavior of the ComputeMaximumRadius function. It's not exact; it's an upper bound on the actual maximum. RigidTransform transform = RigidTransform.Identity; BoundingBox boundingBox; GetBoundingBox(ref transform, out boundingBox); Vector3 diameter; Vector3.Subtract(ref boundingBox.Max, ref boundingBox.Min, out diameter); return diameter.Length(); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { //Sample the shape in directions pointing to the vertices of a regular tetrahedron. Vector3 a, b, c, d; var direction = new Vector3(1, 1, 1); GetLocalExtremePointWithoutMargin(ref direction, out a); direction = new Vector3(-1, -1, 1); GetLocalExtremePointWithoutMargin(ref direction, out b); direction = new Vector3(-1, 1, -1); GetLocalExtremePointWithoutMargin(ref direction, out c); direction = new Vector3(1, -1, -1); GetLocalExtremePointWithoutMargin(ref direction, out d); Vector3 ab, cb, ac, ad, cd; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref b, ref c, out cb); Vector3.Subtract(ref c, ref a, out ac); Vector3.Subtract(ref d, ref a, out ad); Vector3.Subtract(ref d, ref c, out cd); //Find normals of triangles: ABC, CBD, ACD, ADB Vector3 nABC, nCBD, nACD, nADB; Vector3.Cross(ref ac, ref ab, out nABC); Vector3.Cross(ref cd, ref cb, out nCBD); Vector3.Cross(ref ad, ref ac, out nACD); Vector3.Cross(ref ab, ref ad, out nADB); //Find distances to planes. float dABC, dCBD, dACD, dADB; Vector3.Dot(ref a, ref nABC, out dABC); Vector3.Dot(ref c, ref nCBD, out dCBD); Vector3.Dot(ref a, ref nACD, out dACD); Vector3.Dot(ref a, ref nADB, out dADB); dABC /= nABC.Length(); dCBD /= nCBD.Length(); dACD /= nACD.Length(); dADB /= nADB.Length(); return collisionMargin + Math.Min(dABC, Math.Min(dCBD, Math.Min(dACD, dADB))); } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { //All convexes under acceptably normal circumstances are centered on the local origin. //The linear transform performs rotation, scaling, and shearing. All of these operations will not move the origin. return Vector3.Zero; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/TriangleShape.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; using Microsoft.Xna.Framework; using RigidTransform = BEPUutilities.RigidTransform; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Triangle collision shape. /// public class TriangleShape : ConvexShape { internal Vector3 vA, vB, vC; /// /// Constructs a triangle shape without initializing it. /// This is useful for systems that re-use a triangle shape repeatedly. /// public TriangleShape() { //Triangles are often used in special situations where the vertex locations are changed directly. This constructor assists with that. } /// /// Constructs a triangle shape. /// The vertices will be recentered. /// ///First vertex in the triangle. ///Second vertex in the triangle. ///Third vertex in the triangle. ///Computed center of the triangle. public TriangleShape(Vector3 vA, Vector3 vB, Vector3 vC, out Vector3 center) { //Recenter. Convexes should contain the origin. center = (vA + vB + vC) / 3; this.vA = vA - center; this.vB = vB - center; this.vC = vC - center; OnShapeChanged(); } /// /// Constructs a triangle shape. /// The vertices will be recentered. If the center is needed, use the other constructor. /// ///First vertex in the triangle. ///Second vertex in the triangle. ///Third vertex in the triangle. public TriangleShape(Vector3 vA, Vector3 vB, Vector3 vC) { //Recenter. Convexes should contain the origin. Vector3 center = (vA + vB + vC) / 3; this.vA = vA - center; this.vB = vB - center; this.vC = vC - center; OnShapeChanged(); } /// /// Gets or sets the first vertex of the triangle shape. /// public Vector3 VertexA { get { return vA; } set { vA = value; OnShapeChanged(); } } /// /// Gets or sets the second vertex of the triangle shape. /// public Vector3 VertexB { get { return vB; } set { vB = value; OnShapeChanged(); } } /// /// Gets or sets the third vertex of the triangle shape. /// public Vector3 VertexC { get { return vC; } set { vC = value; OnShapeChanged(); } } internal TriangleSidedness sidedness; /// /// Gets or sets the sidedness of the triangle. /// public TriangleSidedness Sidedness { get { return sidedness; } set { sidedness = value; OnShapeChanged(); } } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { Vector3 a, b, c; Vector3.Transform(ref vA, ref shapeTransform.Orientation, out a); Vector3.Transform(ref vB, ref shapeTransform.Orientation, out b); Vector3.Transform(ref vC, ref shapeTransform.Orientation, out c); Vector3.Min(ref a, ref b, out boundingBox.Min); Vector3.Min(ref c, ref boundingBox.Min, out boundingBox.Min); Vector3.Max(ref a, ref b, out boundingBox.Max); Vector3.Max(ref c, ref boundingBox.Max, out boundingBox.Max); boundingBox.Min.X += shapeTransform.Position.X - collisionMargin; boundingBox.Min.Y += shapeTransform.Position.Y - collisionMargin; boundingBox.Min.Z += shapeTransform.Position.Z - collisionMargin; boundingBox.Max.X += shapeTransform.Position.X + collisionMargin; boundingBox.Max.Y += shapeTransform.Position.Y + collisionMargin; boundingBox.Max.Z += shapeTransform.Position.Z + collisionMargin; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { float dotA, dotB, dotC; Vector3.Dot(ref direction, ref vA, out dotA); Vector3.Dot(ref direction, ref vB, out dotB); Vector3.Dot(ref direction, ref vC, out dotC); if (dotA > dotB && dotA > dotC) { extremePoint = vA; } else if (dotB > dotC) //vA is not the most extreme point. { extremePoint = vB; } else { extremePoint = vC; } } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { Vector3 center = ComputeCenter(); return collisionMargin + Math.Max((vA - center).Length(), Math.Max((vB - center).Length(), (vC - center).Length())); } /// /// Computes the minimum radius of the shape. /// This is often smaller than the actual minimum radius; /// it is simply an approximation that avoids overestimating. /// ///Minimum radius of the shape. public override float ComputeMinimumRadius() { return 0; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public override Matrix3x3 ComputeVolumeDistribution(out float volume) { Vector3 center = ComputeCenter(); volume = ComputeVolume(); //Just approximate. //Calculate distribution of mass. const float massPerPoint = .333333333f; //Subtract the position from the distribution, moving into a 'body space' relative to itself. // [ (j * j + z * z) (-j * j) (-j * z) ] //I = I + [ (-j * j) (j * j + z * z) (-j * z) ] // [ (-j * z) (-j * z) (j * j + j * j) ] float i = vA.X - center.X; float j = vA.Y - center.Y; float k = vA.Z - center.Z; //localInertiaTensor += new Matrix(j * j + k * k, -j * j, -j * k, 0, -j * j, j * j + k * k, -j * k, 0, -j * k, -j * k, j * j + j * j, 0, 0, 0, 0, 0); //No mass per point. var volumeDistribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); i = vB.X - center.X; j = vB.Y - center.Y; k = vB.Z - center.Z; var pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); i = vC.X - center.X; j = vC.Y - center.Y; k = vC.Z - center.Z; pointContribution = new Matrix3x3(massPerPoint * (j * j + k * k), massPerPoint * (-i * j), massPerPoint * (-i * k), massPerPoint * (-i * j), massPerPoint * (i * i + k * k), massPerPoint * (-j * k), massPerPoint * (-i * k), massPerPoint * (-j * k), massPerPoint * (i * i + j * j)); Matrix3x3.Add(ref volumeDistribution, ref pointContribution, out volumeDistribution); return volumeDistribution; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public override Vector3 ComputeCenter() { return (vA + vB + vC) / 3; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public override Vector3 ComputeCenter(out float volume) { volume = ComputeVolume(); return ComputeCenter(); } /// /// Computes the volume of the shape. /// /// Volume of the shape. public override float ComputeVolume() { return Vector3.Cross(vB - vA, vC - vA).Length() * collisionMargin; } /// /// Gets the normal of the triangle shape in its local space. /// ///The local normal. public Vector3 GetLocalNormal() { Vector3 normal; Vector3 vAvB; Vector3 vAvC; Vector3.Subtract(ref vB, ref vA, out vAvB); Vector3.Subtract(ref vC, ref vA, out vAvC); Vector3.Cross(ref vAvB, ref vAvC, out normal); normal.Normalize(); return normal; } /// /// Gets the normal of the triangle in world space. /// /// World transform. /// Normal of the triangle in world space. public Vector3 GetNormal(RigidTransform transform) { Vector3 normal = GetLocalNormal(); Vector3.Transform(ref normal, ref transform.Orientation, out normal); return normal; } /// /// Gets the intersection between the triangle and the ray. /// /// Ray to test against the triangle. /// Transform to apply to the triangle shape for the test. /// Maximum distance to travel in units of the direction vector's length. /// Hit data of the ray cast, if any. /// Whether or not the ray hit the target. public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit) { Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref transform.Orientation, out orientation); Ray localRay; Quaternion conjugate; Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); bool toReturn = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref vA, ref vB, ref vC, out hit); //Move the hit back into world space. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location); Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); return toReturn; } /// /// Returns a that represents the current . /// /// /// A that represents the current . /// /// 2 public override string ToString() { return vA + ", " + vB + ", " + vC; } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/ConvexShapes/WrappedShape.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionShapes.ConvexShapes { /// /// Convex shape entry to a WrappedShape. /// public struct ConvexShapeEntry { /// /// Convex shape of the entry. /// public ConvexShape CollisionShape; /// /// Local transform of the entry. /// public RigidTransform Transform; /// /// Constructs a convex shape entry. /// /// Local position of the entry. /// Shape of the entry. public ConvexShapeEntry(Vector3 position, ConvexShape shape) { Transform = new RigidTransform(position); CollisionShape = shape; } /// /// Constructs a convex shape entry. /// /// Local orientation of the entry. /// Shape of the entry. public ConvexShapeEntry(Quaternion orientation, ConvexShape shape) { Transform = new RigidTransform(orientation); CollisionShape = shape; } /// /// Constructs a convex shape entry. /// /// Local transform of the entry. /// Shape of the entry. public ConvexShapeEntry(RigidTransform transform, ConvexShape shape) { Transform = transform; CollisionShape = shape; } /// /// Constructs a convex shape entry with identity transformation. /// ///Shape of the entry. public ConvexShapeEntry(ConvexShape shape) { Transform = RigidTransform.Identity; CollisionShape = shape; } } /// /// Shape that wraps other convex shapes in a convex hull. /// One way to think of it is to collect a bunch of items and wrap shrinkwrap around them. /// That surface is the shape of the WrappedShape. /// public class WrappedShape : ConvexShape { ObservableList shapes = new ObservableList(); /// /// Gets the shapes in wrapped shape. /// public ObservableList Shapes { get { return shapes; } } void Recenter(out Vector3 center) { //When first constructed, a wrapped shape may not actually be centered on its local origin. //It is helpful to many systems if this is addressed. center = ComputeCenter(); for (int i = 0; i < shapes.Count; i++) { shapes.WrappedList.Elements[i].Transform.Position -= center; } } /// /// Constructs a wrapped shape. /// A constructor is also available which takes a list of objects rather than just a pair. /// The shape will be recentered. If the center is needed, use the other constructor. /// ///First shape in the wrapped shape. ///Second shape in the wrapped shape. public WrappedShape(ConvexShapeEntry firstShape, ConvexShapeEntry secondShape) { shapes.Add(firstShape); shapes.Add(secondShape); OnShapeChanged(); Vector3 v; Recenter(out v); shapes.Changed += ShapesChanged; } /// /// Constructs a wrapped shape. /// A constructor is also available which takes a list of objects rather than just a pair. /// The shape will be recentered. /// ///First shape in the wrapped shape. ///Second shape in the wrapped shape. ///Center of the shape before recentering.. public WrappedShape(ConvexShapeEntry firstShape, ConvexShapeEntry secondShape, out Vector3 center) { shapes.Add(firstShape); shapes.Add(secondShape); OnShapeChanged(); Recenter(out center); shapes.Changed += ShapesChanged; } /// /// Constructs a wrapped shape. /// The shape will be recentered; if the center is needed, use the other constructor. /// ///Shape entries used to construct the shape. ///Thrown when the shape list is empty. public WrappedShape(IList shapeEntries) { if (shapeEntries.Count == 0) throw new ArgumentException("Cannot create a wrapped shape with no contained shapes."); for (int i = 0; i < shapeEntries.Count; i++) { shapes.Add(shapeEntries[i]); } Vector3 v; OnShapeChanged(); Recenter(out v); shapes.Changed += ShapesChanged; } /// /// Constructs a wrapped shape. /// The shape will be recentered. /// ///Shape entries used to construct the shape. /// Center of the shape before recentering. ///Thrown when the shape list is empty. public WrappedShape(IList shapeEntries, out Vector3 center) { if (shapeEntries.Count == 0) throw new ArgumentException("Cannot create a wrapped shape with no contained shapes."); for (int i = 0; i < shapeEntries.Count; i++) { shapes.Add(shapeEntries[i]); } OnShapeChanged(); Recenter(out center); shapes.Changed += ShapesChanged; } void ShapesChanged(ObservableList list) { OnShapeChanged(); } /// /// Gets the bounding box of the shape given a transform. /// /// Transform to use. /// Bounding box of the transformed shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { RigidTransform subTransform; RigidTransform.Transform(ref shapes.WrappedList.Elements[0].Transform, ref shapeTransform, out subTransform); shapes.WrappedList.Elements[0].CollisionShape.GetBoundingBox(ref subTransform, out boundingBox); for (int i = 1; i < shapes.WrappedList.Count; i++) { RigidTransform.Transform(ref shapes.WrappedList.Elements[i].Transform, ref shapeTransform, out subTransform); BoundingBox toMerge; shapes.WrappedList.Elements[i].CollisionShape.GetBoundingBox(ref subTransform, out toMerge); BoundingBox.CreateMerged(ref boundingBox, ref toMerge, out boundingBox); } boundingBox.Min.X -= collisionMargin; boundingBox.Min.Y -= collisionMargin; boundingBox.Min.Z -= collisionMargin; boundingBox.Max.X += collisionMargin; boundingBox.Max.Y += collisionMargin; boundingBox.Max.Z += collisionMargin; } /// /// Gets the extreme point of the shape in local space in a given direction. /// ///Direction to find the extreme point in. ///Extreme point on the shape. public override void GetLocalExtremePointWithoutMargin(ref Vector3 direction, out Vector3 extremePoint) { shapes.WrappedList.Elements[0].CollisionShape.GetExtremePoint(direction, ref shapes.WrappedList.Elements[0].Transform, out extremePoint); float maxDot; Vector3.Dot(ref extremePoint, ref direction, out maxDot); for (int i = 1; i < shapes.WrappedList.Count; i++) { float dot; Vector3 temp; shapes.WrappedList.Elements[i].CollisionShape.GetExtremePoint(direction, ref shapes.WrappedList.Elements[i].Transform, out temp); Vector3.Dot(ref direction, ref temp, out dot); if (dot > maxDot) { extremePoint = temp; maxDot = dot; } } } /// /// Computes the maximum radius of the shape. /// This is often larger than the actual maximum radius; /// it is simply an approximation that avoids underestimating. /// /// Maximum radius of the shape. public override float ComputeMaximumRadius() { //This can overestimate the actual maximum radius, but such is the defined behavior of the ComputeMaximumRadius function. It's not exact; it's an upper bound on the actual maximum. float maxRadius = 0; for (int i = 0; i < shapes.Count; i++) { float radius = shapes.WrappedList.Elements[i].CollisionShape.ComputeMaximumRadius() + shapes.WrappedList.Elements[i].Transform.Position.Length(); if (radius > maxRadius) maxRadius = radius; } return maxRadius + collisionMargin; } public override float ComputeMinimumRadius() { //Could also use the tetrahedron approximation approach. float minRadius = 0; for (int i = 0; i < shapes.Count; i++) { float radius = shapes.WrappedList.Elements[i].CollisionShape.ComputeMinimumRadius(); if (radius < minRadius) minRadius = radius; } return minRadius + collisionMargin; } /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public override EntityCollidable GetCollidableInstance() { return new ConvexCollidable(this); } } } ================================================ FILE: BEPUphysics/CollisionShapes/EntityShape.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionShapes { /// /// Superclass of all collision shapes that are used by Entities. /// public abstract class EntityShape : CollisionShape { /// /// Computes the volume of the shape. /// /// Volume of the shape. public virtual float ComputeVolume() { ShapeDistributionInformation shapeInfo; ComputeDistributionInformation(out shapeInfo); return shapeInfo.Volume; } /// /// Computes the volume distribution of the shape as well as its volume. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume of the shape. /// Volume distribution of the shape. public virtual Matrix3x3 ComputeVolumeDistribution(out float volume) { ShapeDistributionInformation shapeInfo; ComputeDistributionInformation(out shapeInfo); volume = shapeInfo.Volume; return shapeInfo.VolumeDistribution; } /// /// Computes the volume distribution of the shape. /// The volume distribution can be used to compute inertia tensors when /// paired with mass and other tuning factors. /// /// Volume distribution of the shape. public virtual Matrix3x3 ComputeVolumeDistribution() { ShapeDistributionInformation shapeInfo; ComputeDistributionInformation(out shapeInfo); return shapeInfo.VolumeDistribution; } /// /// Computes the center of the shape. This can be considered its /// center of mass. /// /// Center of the shape. public virtual Vector3 ComputeCenter() { ShapeDistributionInformation shapeInfo; ComputeDistributionInformation(out shapeInfo); return shapeInfo.Center; } /// /// Computes the center of the shape. This can be considered its /// center of mass. This calculation is often associated with the /// volume calculation, which is given by this method as well. /// /// Volume of the shape. /// Center of the shape. public virtual Vector3 ComputeCenter(out float volume) { ShapeDistributionInformation shapeInfo; ComputeDistributionInformation(out shapeInfo); volume = shapeInfo.Volume; return shapeInfo.Center; } /// /// Computes a variety of shape information all at once. /// /// Properties of the shape. public abstract void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo); /// /// Retrieves an instance of an EntityCollidable that uses this EntityShape. Mainly used by compound bodies. /// /// EntityCollidable that uses this shape. public abstract EntityCollidable GetCollidableInstance(); /// /// Computes a bounding box for the shape given the specified transform. /// /// Transform to apply to the shape to compute the bounding box. /// Bounding box for the shape given the transform. public abstract void GetBoundingBox(ref RigidTransform transform, out BoundingBox boundingBox); } } ================================================ FILE: BEPUphysics/CollisionShapes/InstancedMeshShape.cs ================================================ using BEPUphysics.DataStructures; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; namespace BEPUphysics.CollisionShapes { /// /// Local space data associated with an instanced mesh. /// This contains a hierarchy and all the other heavy data needed /// by an InstancedMesh. /// public class InstancedMeshShape : CollisionShape { TriangleMesh triangleMesh; /// /// Gets or sets the TriangleMesh data structure used by this shape. /// public TriangleMesh TriangleMesh { get { return triangleMesh; } set { triangleMesh = value; OnShapeChanged(); } } /// /// Constructs a new instanced mesh shape. /// ///Vertices of the mesh. ///Indices of the mesh. public InstancedMeshShape(Vector3[] vertices, uint[] indices) { TriangleMesh = new TriangleMesh(new StaticMeshData(vertices, indices, indices.Length)); } /// /// Computes the bounding box of the transformed mesh shape. /// ///Transform to apply to the shape during the bounding box calculation. ///Bounding box containing the transformed mesh shape. public void ComputeBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif float minX = float.MaxValue; float minY = float.MaxValue; float minZ = float.MaxValue; float maxX = -float.MaxValue; float maxY = -float.MaxValue; float maxZ = -float.MaxValue; for (int i = 0; i < triangleMesh.Data.vertices.Length; i++) { Vector3 vertex; triangleMesh.Data.GetVertexPosition(i, out vertex); Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex); if (vertex.X < minX) minX = vertex.X; if (vertex.X > maxX) maxX = vertex.X; if (vertex.Y < minY) minY = vertex.Y; if (vertex.Y > maxY) maxY = vertex.Y; if (vertex.Z < minZ) minZ = vertex.Z; if (vertex.Z > maxZ) maxZ = vertex.Z; } boundingBox.Min.X = transform.Translation.X + minX; boundingBox.Min.Y = transform.Translation.Y + minY; boundingBox.Min.Z = transform.Translation.Z + minZ; boundingBox.Max.X = transform.Translation.X + maxX; boundingBox.Max.Y = transform.Translation.Y + maxY; boundingBox.Max.Z = transform.Translation.Z + maxZ; } } } ================================================ FILE: BEPUphysics/CollisionShapes/MobileMeshShape.cs ================================================ using BEPUphysics.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using System; using BEPUphysics.Settings; namespace BEPUphysics.CollisionShapes { /// /// Local space data associated with a mobile mesh. /// This contains a hierarchy and all the other heavy data needed /// by an MobileMesh. /// public class MobileMeshShape : EntityShape { private float meshCollisionMargin = CollisionDetectionSettings.DefaultMargin; /// /// Gets or sets the margin of the mobile mesh to use when colliding with other meshes. /// When colliding with non-mesh shapes, the mobile mesh has no margin. /// public float MeshCollisionMargin { get { return meshCollisionMargin; } set { if (value < 0) throw new ArgumentException("Mesh margin must be nonnegative."); meshCollisionMargin = value; OnShapeChanged(); } } TriangleMesh triangleMesh; /// /// Gets or sets the TriangleMesh data structure used by this shape. /// public TriangleMesh TriangleMesh { get { return triangleMesh; } } /// /// Gets the transform used by the local mesh shape. /// public AffineTransform Transform { get { return ((TransformableMeshData)triangleMesh.Data).worldTransform; } } RawList surfaceVertices = new RawList(); internal MobileMeshSolidity solidity = MobileMeshSolidity.DoubleSided; /// /// Gets the solidity of the mesh. /// public MobileMeshSolidity Solidity { get { return solidity; } } /// /// Gets or sets the sidedness of the shape. This is a convenience property based on the Solidity property. /// If the shape is solid, this returns whatever sidedness is computed to make the triangles of the shape face outward. /// If the shape is solid, setting this property will change the sidedness that is used while the shape is solid. /// public TriangleSidedness Sidedness { get { switch (solidity) { case MobileMeshSolidity.Clockwise: return TriangleSidedness.Clockwise; case MobileMeshSolidity.Counterclockwise: return TriangleSidedness.Counterclockwise; case MobileMeshSolidity.DoubleSided: return TriangleSidedness.DoubleSided; case MobileMeshSolidity.Solid: return solidSidedness; } return TriangleSidedness.DoubleSided; } set { if (solidity == MobileMeshSolidity.Solid) solidSidedness = value; else { switch (value) { case TriangleSidedness.Clockwise: solidity = MobileMeshSolidity.Clockwise; break; case TriangleSidedness.Counterclockwise: solidity = MobileMeshSolidity.Counterclockwise; break; case TriangleSidedness.DoubleSided: solidity = MobileMeshSolidity.DoubleSided; break; } } } } /// /// Constructs a new mobile mesh shape. /// ///Vertices of the mesh. ///Indices of the mesh. ///Local transform to apply to the shape. ///Solidity state of the shape. public MobileMeshShape(Vector3[] vertices, uint[] indices, AffineTransform localTransform, MobileMeshSolidity solidity) { this.solidity = solidity; var data = new TransformableMeshData(vertices, indices, indices.Length, localTransform); ShapeDistributionInformation distributionInfo; ComputeShapeInformation(data, out distributionInfo); for (int i = 0; i < surfaceVertices.Count; i++) { Vector3.Subtract(ref surfaceVertices.Elements[i], ref distributionInfo.Center, out surfaceVertices.Elements[i]); } triangleMesh = new TriangleMesh(data); ComputeSolidSidedness(); } /// /// Constructs a new mobile mesh shape. /// ///Vertices of the mesh. ///Indices of the mesh. ///Local transform to apply to the shape. ///Solidity state of the shape. ///Information computed about the shape during construction. public MobileMeshShape(Vector3[] vertices, uint[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, out ShapeDistributionInformation distributionInfo) { this.solidity = solidity; var data = new TransformableMeshData(vertices, indices, indices.Length, localTransform); ComputeShapeInformation(data, out distributionInfo); for (int i = 0; i < surfaceVertices.Count; i++) { Vector3.Subtract(ref surfaceVertices.Elements[i], ref distributionInfo.Center, out surfaceVertices.Elements[i]); } triangleMesh = new TriangleMesh(data); ComputeSolidSidedness(); //ComputeBoundingHull(); } /// /// Sidedness required if the mesh is in solid mode. /// If the windings were reversed or double sided, /// the solidity would fight against shell contacts, /// leading to very bad jittering. /// internal TriangleSidedness solidSidedness; /// /// Tests to see if a ray's origin is contained within the mesh. /// If it is, the hit location is found. /// If it isn't, the hit location is still valid if a hit occurred. /// If the origin isn't inside and there was no hit, the hit has a T value of float.MaxValue. /// /// Ray in the local space of the shape to test. /// The first hit against the mesh, if any. /// Whether or not the ray origin was in the mesh. public bool IsLocalRayOriginInMesh(ref Ray ray, out RayHit hit) { var overlapList = CommonResources.GetIntList(); hit = new RayHit(); hit.T = float.MaxValue; if (triangleMesh.Tree.GetOverlaps(ray, overlapList)) { bool minimumClockwise = false; for (int i = 0; i < overlapList.Count; i++) { Vector3 vA, vB, vC; triangleMesh.Data.GetTriangle(overlapList[i], out vA, out vB, out vC); bool hitClockwise; RayHit tempHit; if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, ref vA, ref vB, ref vC, out hitClockwise, out tempHit) && tempHit.T < hit.T) { hit = tempHit; minimumClockwise = hitClockwise; } } CommonResources.GiveBack(overlapList); //If the mesh is hit from behind by the ray on the first hit, then the ray is inside. return hit.T < float.MaxValue && ((solidSidedness == TriangleSidedness.Clockwise && !minimumClockwise) || (solidSidedness == TriangleSidedness.Counterclockwise && minimumClockwise)); } CommonResources.GiveBack(overlapList); return false; } /// /// The difference in t parameters in a ray cast under which two hits are considered to be redundant. /// public static float MeshHitUniquenessThreshold = .0001f; internal bool IsHitUnique(RawList hits, ref RayHit hit) { for (int i = 0; i < hits.Count; i++) { if (Math.Abs(hits.Elements[i].T - hit.T) < MeshHitUniquenessThreshold) return false; } hits.Add(hit); return true; } void ComputeSolidSidedness() { //Raycast against the mesh. //If there's an even number of hits, then the ray start point is outside. //If there's an odd number of hits, then the ray start point is inside. //If the start is outside, then take the earliest toi hit and calibrate sidedness based on it. //If the start is inside, then take the latest toi hit and calibrate sidedness based on it. //This test assumes consistent winding across the entire mesh as well as a closed surface. //If those assumptions are not correct, then the raycast cannot determine inclusion or exclusion, //or there exists no calibration that will work across the entire surface. //Pick a ray direction that goes to a random location on the mesh. //A vertex would work, but targeting the middle of a triangle avoids some edge cases. var ray = new Ray(); Vector3 vA, vB, vC; triangleMesh.Data.GetTriangle(((triangleMesh.Data.indices.Length / 3) / 2) * 3, out vA, out vB, out vC); ray.Direction = (vA + vB + vC) / 3; ray.Direction.Normalize(); solidSidedness = ComputeSolidSidednessHelper(ray); //TODO: Positions need to be valid for the verifying directions to work properly. ////Find another direction and test it to corroborate the first test. //Ray alternateRay; //alternateRay.Position = ray.Position; //Vector3.Cross(ref ray.Direction, ref Toolbox.UpVector, out alternateRay.Direction); //float lengthSquared = alternateRay.Direction.LengthSquared(); //if (lengthSquared < Toolbox.Epsilon) //{ // Vector3.Cross(ref ray.Direction, ref Toolbox.RightVector, out alternateRay.Direction); // lengthSquared = alternateRay.Direction.LengthSquared(); //} //Vector3.Divide(ref alternateRay.Direction, (float)Math.Sqrt(lengthSquared), out alternateRay.Direction); //var sidednessCandidate2 = ComputeSolidSidednessHelper(alternateRay); //if (sidednessCandidate == sidednessCandidate2) //{ // //The two tests agreed! It's very likely that the sidedness is, in fact, in this direction. // solidSidedness = sidednessCandidate; //} //else //{ // //The two tests disagreed. Tiebreaker! // Vector3.Cross(ref alternateRay.Direction, ref ray.Direction, out alternateRay.Direction); // solidSidedness = ComputeSolidSidednessHelper(alternateRay); //} } TriangleSidedness ComputeSolidSidednessHelper(Ray ray) { TriangleSidedness toReturn; var hitList = CommonResources.GetIntList(); if (triangleMesh.Tree.GetOverlaps(ray, hitList)) { Vector3 vA, vB, vC; var hits = CommonResources.GetRayHitList(); //Identify the first and last hits. int minimum = 0; int maximum = 0; float minimumT = float.MaxValue; float maximumT = -1; for (int i = 0; i < hitList.Count; i++) { triangleMesh.Data.GetTriangle(hitList[i], out vA, out vB, out vC); RayHit hit; if (Toolbox.FindRayTriangleIntersection(ref ray, float.MaxValue, TriangleSidedness.DoubleSided, ref vA, ref vB, ref vC, out hit) && IsHitUnique(hits, ref hit)) { if (hit.T < minimumT) { minimumT = hit.T; minimum = hitList[i]; } if (hit.T > maximumT) { maximumT = hit.T; maximum = hitList[i]; } } } if (hits.Count % 2 == 0) { //Since we were outside, the first hit triangle should be calibrated //such that it faces towards us. triangleMesh.Data.GetTriangle(minimum, out vA, out vB, out vC); var normal = Vector3.Cross(vA - vB, vA - vC); if (Vector3.Dot(normal, ray.Direction) < 0) toReturn = TriangleSidedness.Clockwise; else toReturn = TriangleSidedness.Counterclockwise; } else { //Since we were inside, the last hit triangle should be calibrated //such that it faces away from us. triangleMesh.Data.GetTriangle(maximum, out vA, out vB, out vC); var normal = Vector3.Cross(vA - vB, vA - vC); if (Vector3.Dot(normal, ray.Direction) < 0) toReturn = TriangleSidedness.Counterclockwise; else toReturn = TriangleSidedness.Clockwise; } CommonResources.GiveBack(hits); } else toReturn = TriangleSidedness.DoubleSided; //This is a problem... CommonResources.GiveBack(hitList); return toReturn; } void ComputeShapeInformation(TransformableMeshData data, out ShapeDistributionInformation shapeInformation) { //Compute the surface vertices of the shape. surfaceVertices.Clear(); try { ConvexHullHelper.GetConvexHull(data.vertices, surfaceVertices); for (int i = 0; i < surfaceVertices.Count; i++) { AffineTransform.Transform(ref surfaceVertices.Elements[i], ref data.worldTransform, out surfaceVertices.Elements[i]); } } catch { surfaceVertices.Clear(); //If the convex hull failed, then the point set has no volume. A mobile mesh is allowed to have zero volume, however. //In this case, compute the bounding box of all points. BoundingBox box = new BoundingBox(); for (int i = 0; i < data.vertices.Length; i++) { Vector3 v; data.GetVertexPosition(i, out v); if (v.X > box.Max.X) box.Max.X = v.X; if (v.X < box.Min.X) box.Min.X = v.X; if (v.Y > box.Max.Y) box.Max.Y = v.Y; if (v.Y < box.Min.Y) box.Min.Y = v.Y; if (v.Z > box.Max.Z) box.Max.Z = v.Z; if (v.Z < box.Min.Z) box.Min.Z = v.Z; } //Add the corners. This will overestimate the size of the surface a bit. surfaceVertices.Add(box.Min); surfaceVertices.Add(box.Max); surfaceVertices.Add(new Vector3(box.Min.X, box.Min.Y, box.Max.Z)); surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Min.Z)); surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Min.Z)); surfaceVertices.Add(new Vector3(box.Min.X, box.Max.Y, box.Max.Z)); surfaceVertices.Add(new Vector3(box.Max.X, box.Max.Y, box.Min.Z)); surfaceVertices.Add(new Vector3(box.Max.X, box.Min.Y, box.Max.Z)); } shapeInformation.Center = new Vector3(); if (solidity == MobileMeshSolidity.Solid) { //The following inertia tensor calculation assumes a closed mesh. shapeInformation.Volume = 0; for (int i = 0; i < data.IndexCount; i += 3) { Vector3 v2, v3, v4; data.GetTriangle(i, out v2, out v3, out v4); //Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin. float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) - v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) + v4.X * (v2.Y * v3.Z - v2.Z * v3.Y); shapeInformation.Volume += tetrahedronVolume; shapeInformation.Center += tetrahedronVolume * (v2 + v3 + v4); } shapeInformation.Center /= shapeInformation.Volume * 4; shapeInformation.Volume /= 6; shapeInformation.Volume = Math.Abs(shapeInformation.Volume); data.worldTransform.Translation -= shapeInformation.Center; //Source: Explicit Exact Formulas for the 3-D Tetrahedron Inertia Tensor in Terms of its Vertex Coordinates //http://www.scipub.org/fulltext/jms2/jms2118-11.pdf //x1, x2, x3, x4 are origin, triangle1, triangle2, triangle3 //Looking to find inertia tensor matrix of the form // [ a -b' -c' ] // [ -b' b -a' ] // [ -c' -a' c ] float a = 0, b = 0, c = 0, ao = 0, bo = 0, co = 0; float totalWeight = 0; for (int i = 0; i < data.IndexCount; i += 3) { Vector3 v2, v3, v4; data.GetTriangle(i, out v2, out v3, out v4); //Determinant is 6 * volume. It's signed, though; this is because the mesh isn't necessarily convex nor centered on the origin. float tetrahedronVolume = v2.X * (v3.Y * v4.Z - v3.Z * v4.Y) - v3.X * (v2.Y * v4.Z - v2.Z * v4.Y) + v4.X * (v2.Y * v3.Z - v2.Z * v3.Y); totalWeight += tetrahedronVolume; a += tetrahedronVolume * (v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); b += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Z * v2.Z + v2.Z * v3.Z + v3.Z * v3.Z + v2.Z * v4.Z + v3.Z * v4.Z + v4.Z * v4.Z); c += tetrahedronVolume * (v2.X * v2.X + v2.X * v3.X + v3.X * v3.X + v2.X * v4.X + v3.X * v4.X + v4.X * v4.X + v2.Y * v2.Y + v2.Y * v3.Y + v3.Y * v3.Y + v2.Y * v4.Y + v3.Y * v4.Y + v4.Y * v4.Y); ao += tetrahedronVolume * (2 * v2.Y * v2.Z + v3.Y * v2.Z + v4.Y * v2.Z + v2.Y * v3.Z + 2 * v3.Y * v3.Z + v4.Y * v3.Z + v2.Y * v4.Z + v3.Y * v4.Z + 2 * v4.Y * v4.Z); bo += tetrahedronVolume * (2 * v2.X * v2.Z + v3.X * v2.Z + v4.X * v2.Z + v2.X * v3.Z + 2 * v3.X * v3.Z + v4.X * v3.Z + v2.X * v4.Z + v3.X * v4.Z + 2 * v4.X * v4.Z); co += tetrahedronVolume * (2 * v2.X * v2.Y + v3.X * v2.Y + v4.X * v2.Y + v2.X * v3.Y + 2 * v3.X * v3.Y + v4.X * v3.Y + v2.X * v4.Y + v3.X * v4.Y + 2 * v4.X * v4.Y); } float density = 1 / totalWeight; float diagonalFactor = density / 10; float offFactor = -density / 20; a *= diagonalFactor; b *= diagonalFactor; c *= diagonalFactor; ao *= offFactor; bo *= offFactor; co *= offFactor; shapeInformation.VolumeDistribution = new Matrix3x3(a, bo, co, bo, b, ao, co, ao, c); } else { shapeInformation.Center = new Vector3(); float totalWeight = 0; for (int i = 0; i < data.IndexCount; i += 3) { //Configure the inertia tensor to be local. Vector3 vA, vB, vC; data.GetTriangle(i, out vA, out vB, out vC); Vector3 vAvB; Vector3 vAvC; Vector3.Subtract(ref vB, ref vA, out vAvB); Vector3.Subtract(ref vC, ref vA, out vAvC); Vector3 cross; Vector3.Cross(ref vAvB, ref vAvC, out cross); float weight = cross.Length(); totalWeight += weight; shapeInformation.Center += weight * (vA + vB + vC) / 3; } shapeInformation.Center /= totalWeight; shapeInformation.Volume = 0; data.worldTransform.Translation -= shapeInformation.Center; shapeInformation.VolumeDistribution = new Matrix3x3(); for (int i = 0; i < data.IndexCount; i += 3) { //Configure the inertia tensor to be local. Vector3 vA, vB, vC; data.GetTriangle(i, out vA, out vB, out vC); Vector3 vAvB; Vector3 vAvC; Vector3.Subtract(ref vB, ref vA, out vAvB); Vector3.Subtract(ref vC, ref vA, out vAvC); Vector3 cross; Vector3.Cross(ref vAvB, ref vAvC, out cross); float weight = cross.Length(); totalWeight += weight; Matrix3x3 innerProduct; Matrix3x3.CreateScale(vA.LengthSquared(), out innerProduct); Matrix3x3 outerProduct; Matrix3x3.CreateOuterProduct(ref vA, ref vA, out outerProduct); Matrix3x3 contribution; Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution); Matrix3x3.CreateScale(vB.LengthSquared(), out innerProduct); Matrix3x3.CreateOuterProduct(ref vB, ref vB, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out outerProduct); Matrix3x3.Multiply(ref contribution, weight, out contribution); Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution); Matrix3x3.CreateScale(vC.LengthSquared(), out innerProduct); Matrix3x3.CreateOuterProduct(ref vC, ref vC, out outerProduct); Matrix3x3.Subtract(ref innerProduct, ref outerProduct, out contribution); Matrix3x3.Multiply(ref contribution, weight, out contribution); Matrix3x3.Add(ref shapeInformation.VolumeDistribution, ref contribution, out shapeInformation.VolumeDistribution); } Matrix3x3.Multiply(ref shapeInformation.VolumeDistribution, 1 / (6 * totalWeight), out shapeInformation.VolumeDistribution); } ////Configure the inertia tensor to be local. //Vector3 finalOffset = shapeInformation.Center; //Matrix3X3 finalInnerProduct; //Matrix3X3.CreateScale(finalOffset.LengthSquared(), out finalInnerProduct); //Matrix3X3 finalOuterProduct; //Matrix3X3.CreateOuterProduct(ref finalOffset, ref finalOffset, out finalOuterProduct); //Matrix3X3 finalContribution; //Matrix3X3.Subtract(ref finalInnerProduct, ref finalOuterProduct, out finalContribution); //Matrix3X3.Subtract(ref shapeInformation.VolumeDistribution, ref finalContribution, out shapeInformation.VolumeDistribution); } ///// ///// Defines two planes that bound the mesh shape in local space. ///// //struct Extent //{ // internal Vector3 Direction; // internal float Minimum; // internal float Maximum; // internal void Clamp(ref Vector3 v) // { // float dot; // Vector3.Dot(ref v, ref Direction, out dot); // float difference; // if (dot < Minimum) // { // difference = dot - Minimum; // } // else if (dot > Maximum) // { // difference = dot - Maximum; // } // else return; // //Subtract the component of v which is parallel to the normal. // v.X -= difference * Direction.X; // v.Y -= difference * Direction.Y; // v.Z -= difference * Direction.Z; // } //} //RawList extents = new RawList(); //void ComputeBoundingHull() //{ // //TODO: // //While we have computed a convex hull of the shape already, we don't really // //need the full tightness of the convex hull. // extents.Add(new Extent() { Direction = new Vector3(1, 0, 0) }); // extents.Add(new Extent() { Direction = new Vector3(0, 1, 0) }); // extents.Add(new Extent() { Direction = new Vector3(0, 0, 1) }); // //extents.Add(new Extent() { Direction = new Vector3(1, 1, 0) }); // //extents.Add(new Extent() { Direction = new Vector3(-1, 1, 0) }); // //extents.Add(new Extent() { Direction = new Vector3(0, 1, 1) }); // //extents.Add(new Extent() { Direction = new Vector3(0, 1, -1) }); // extents.Add(new Extent() { Direction = Vector3.Normalize(new Vector3(1, 0, 1)) }); // extents.Add(new Extent() { Direction = Vector3.Normalize(new Vector3(1, 0, -1)) }); // //Add more extents for a tighter volume // //Initialize the max and mins. // for (int i = 0; i < extents.count; i++) // { // extents.Elements[i].Minimum = float.MaxValue; // extents.Elements[i].Maximum = -float.MaxValue; // } // for (int i = 0; i < triangleMesh.Data.vertices.Length; i++) // { // Vector3 v; // triangleMesh.Data.GetVertexPosition(i, out v); // for (int j = 0; j < extents.count; j++) // { // float dot; // Vector3.Dot(ref v, ref extents.Elements[j].Direction, out dot); // if (dot < extents.Elements[j].Minimum) // extents.Elements[j].Minimum = dot; // if (dot > extents.Elements[j].Maximum) // extents.Elements[j].Maximum = dot; // } // } //} private void GetBoundingBox(ref Matrix3x3 o, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif //Sample the local directions from the matrix, implicitly transposed. var rightDirection = new Vector3(o.M11, o.M21, o.M31); var upDirection = new Vector3(o.M12, o.M22, o.M32); var backDirection = new Vector3(o.M13, o.M23, o.M33); int right = 0, left = 0, up = 0, down = 0, backward = 0, forward = 0; float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue; for (int i = 0; i < surfaceVertices.Count; i++) { float dotX, dotY, dotZ; Vector3.Dot(ref rightDirection, ref surfaceVertices.Elements[i], out dotX); Vector3.Dot(ref upDirection, ref surfaceVertices.Elements[i], out dotY); Vector3.Dot(ref backDirection, ref surfaceVertices.Elements[i], out dotZ); if (dotX < minX) { minX = dotX; left = i; } if (dotX > maxX) { maxX = dotX; right = i; } if (dotY < minY) { minY = dotY; down = i; } if (dotY > maxY) { maxY = dotY; up = i; } if (dotZ < minZ) { minZ = dotZ; forward = i; } if (dotZ > maxZ) { maxZ = dotZ; backward = i; } } //Incorporate the collision margin. Vector3.Multiply(ref rightDirection, meshCollisionMargin / (float)Math.Sqrt(rightDirection.Length()), out rightDirection); Vector3.Multiply(ref upDirection, meshCollisionMargin / (float)Math.Sqrt(upDirection.Length()), out upDirection); Vector3.Multiply(ref backDirection, meshCollisionMargin / (float)Math.Sqrt(backDirection.Length()), out backDirection); var rightElement = surfaceVertices.Elements[right]; var leftElement = surfaceVertices.Elements[left]; var upElement = surfaceVertices.Elements[up]; var downElement = surfaceVertices.Elements[down]; var backwardElement = surfaceVertices.Elements[backward]; var forwardElement = surfaceVertices.Elements[forward]; Vector3.Add(ref rightElement, ref rightDirection, out rightElement); Vector3.Subtract(ref leftElement, ref rightDirection, out leftElement); Vector3.Add(ref upElement, ref upDirection, out upElement); Vector3.Subtract(ref downElement, ref upDirection, out downElement); Vector3.Add(ref backwardElement, ref backDirection, out backwardElement); Vector3.Subtract(ref forwardElement, ref backDirection, out forwardElement); //TODO: This could be optimized. Unnecessary transformation information gets computed. Vector3 vMinX, vMaxX, vMinY, vMaxY, vMinZ, vMaxZ; Matrix3x3.Transform(ref rightElement, ref o, out vMaxX); Matrix3x3.Transform(ref leftElement, ref o, out vMinX); Matrix3x3.Transform(ref upElement, ref o, out vMaxY); Matrix3x3.Transform(ref downElement, ref o, out vMinY); Matrix3x3.Transform(ref backwardElement, ref o, out vMaxZ); Matrix3x3.Transform(ref forwardElement, ref o, out vMinZ); boundingBox.Max.X = vMaxX.X; boundingBox.Max.Y = vMaxY.Y; boundingBox.Max.Z = vMaxZ.Z; boundingBox.Min.X = vMinX.X; boundingBox.Min.Y = vMinY.Y; boundingBox.Min.Z = vMinZ.Z; } /// /// Computes the bounding box of the transformed mesh shape. /// ///Transform to apply to the shape during the bounding box calculation. ///Bounding box containing the transformed mesh shape. public override void GetBoundingBox(ref RigidTransform shapeTransform, out BoundingBox boundingBox) { ////TODO: Could use an approximate bounding volume. Would be cheaper at runtime and use less memory, though the box would be bigger. //Matrix3X3 o; //Matrix3X3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); ////Sample the local directions from the orientation matrix, implicitly transposed. //Vector3 right = new Vector3(o.M11 * 100000, o.M21 * 100000, o.M31 * 100000); //Vector3 up = new Vector3(o.M12 * 100000, o.M22 * 100000, o.M32 * 100000); //Vector3 backward = new Vector3(o.M13 * 100000, o.M23 * 100000, o.M33 * 100000); //Vector3 left, down, forward; //Vector3.Negate(ref right, out left); //Vector3.Negate(ref up, out down); //Vector3.Negate(ref backward, out forward); //for (int i = 0; i < extents.count; i++) //{ // extents.Elements[i].Clamp(ref right); // extents.Elements[i].Clamp(ref left); // extents.Elements[i].Clamp(ref up); // extents.Elements[i].Clamp(ref down); // extents.Elements[i].Clamp(ref backward); // extents.Elements[i].Clamp(ref forward); //} //Matrix3X3.Transform(ref right, ref o, out right); //Matrix3X3.Transform(ref left, ref o, out left); //Matrix3X3.Transform(ref down, ref o, out down); //Matrix3X3.Transform(ref up, ref o, out up); //Matrix3X3.Transform(ref forward, ref o, out forward); //Matrix3X3.Transform(ref backward, ref o, out backward); //boundingBox.Max.X = shapeTransform.Position.X + right.X; //boundingBox.Max.Y = shapeTransform.Position.Y + up.Y; //boundingBox.Max.Z = shapeTransform.Position.Z + backward.Z; //boundingBox.Min.X = shapeTransform.Position.X + left.X; //boundingBox.Min.Y = shapeTransform.Position.Y + down.Y; //boundingBox.Min.Z = shapeTransform.Position.Z + forward.Z; Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref shapeTransform.Orientation, out o); GetBoundingBox(ref o, out boundingBox); boundingBox.Max.X += shapeTransform.Position.X; boundingBox.Max.Y += shapeTransform.Position.Y; boundingBox.Max.Z += shapeTransform.Position.Z; boundingBox.Min.X += shapeTransform.Position.X; boundingBox.Min.Y += shapeTransform.Position.Y; boundingBox.Min.Z += shapeTransform.Position.Z; } /// /// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform. /// /// Transform to use to put the shape into world space. /// Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space. /// Bounding box in the local space. public void GetLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif //TODO: This method peforms quite a few sqrts because the collision margin can get scaled, and so cannot be applied as a final step. //There should be a better way to do this. //Additionally, this bounding box is not consistent in all cases with the post-add version. Adding the collision margin at the end can //slightly overestimate the size of a margin expanded shape at the corners, which is fine (and actually important for the box-box special case). //Move forward into convex's space, backwards into the new space's local space. AffineTransform transform; AffineTransform.Invert(ref spaceTransform, out transform); AffineTransform.Multiply(ref shapeTransform, ref transform, out transform); GetBoundingBox(ref transform.LinearTransform, out boundingBox); boundingBox.Max.X += transform.Translation.X; boundingBox.Max.Y += transform.Translation.Y; boundingBox.Max.Z += transform.Translation.Z; boundingBox.Min.X += transform.Translation.X; boundingBox.Min.Y += transform.Translation.Y; boundingBox.Min.Z += transform.Translation.Z; } /// /// Gets the bounding box of the mesh transformed first into world space, and then into the local space of another affine transform. /// /// Transform to use to put the shape into world space. /// Used as the frame of reference to compute the bounding box. /// In effect, the shape is transformed by the inverse of the space transform to compute its bounding box in local space. /// World space sweep direction to transform and add to the bounding box. /// Bounding box in the local space. public void GetSweptLocalBoundingBox(ref RigidTransform shapeTransform, ref AffineTransform spaceTransform, ref Vector3 sweep, out BoundingBox boundingBox) { GetLocalBoundingBox(ref shapeTransform, ref spaceTransform, out boundingBox); Vector3 expansion; Matrix3x3.TransformTranspose(ref sweep, ref spaceTransform.LinearTransform, out expansion); Toolbox.ExpandBoundingBox(ref boundingBox, ref expansion); } /// /// Computes the volume, center of mass, and volume distribution of the shape. /// /// Data about the shape. public override void ComputeDistributionInformation(out ShapeDistributionInformation shapeInfo) { ComputeShapeInformation(this.TriangleMesh.Data as TransformableMeshData, out shapeInfo); } public override EntityCollidable GetCollidableInstance() { return new MobileMeshCollidable(this); } } /// /// Solidity of a triangle or mesh. /// A triangle can be double sided, or allow one of its sides to let interacting objects through. /// The entire mesh can be made solid, which means objects on the interior still generate contacts even if there aren't any triangles to hit. /// Solidity requires the mesh to be closed. /// public enum MobileMeshSolidity { /// /// The mesh will interact with objects coming from both directions. /// DoubleSided, /// /// The mesh will interact with objects from which the winding of the triangles appears to be clockwise. /// Clockwise, /// /// The mesh will interact with objects from which the winding of the triangles appears to be counterclockwise. /// Counterclockwise, /// /// The mesh will treat objects inside of its concave shell as if the mesh had volume. Mesh must be closed for this to work properly. /// Solid } } ================================================ FILE: BEPUphysics/CollisionShapes/ShapeDistributionInformation.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionShapes { /// /// Contains data about the distribution of volume in a shape. /// public struct ShapeDistributionInformation { /// /// The distribution of volume in a shape. /// This can be scaled to create an inertia tensor for a shape. /// public Matrix3x3 VolumeDistribution; /// /// The center of a shape. /// public Vector3 Center; /// /// The volume of a shape. /// public float Volume; } } ================================================ FILE: BEPUphysics/CollisionShapes/StaticGroupShape.cs ================================================ using System; using BEPUphysics.DataStructures; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.CollisionShapes { /// /// The shape information used by a StaticGroup. /// Unlike most shapes, a StaticGroupShape cannot be shared between multiple StaticGroups; /// a StaticGroupShape is linked to a single StaticGroup. /// public class StaticGroupShape : CollisionShape { //Technically, while this is superior to the 'put a bunch of stuff in the Space' approach, it is still //inferior in maximum performance to an approach which allows for the direct comparison of a static tree against a nonstatic tree. //This would require tighter integration with the broad phase, though. It's not clear that the performance boost is worth it. /// /// Gets the StaticGroup associated with this StaticGroupShape. Unlike most shapes, there is a one-to-one relationship /// between StaticGroupShapes and StaticGroups. /// public StaticGroup StaticGroup { get; private set; } /// /// Gets the bounding box tree associated with this shape. /// Contains Collidable instances as opposed to shapes. /// public BoundingBoxTree CollidableTree { get; private set; } /// /// Constructs a new StaticGroupShape. /// ///List of collidables in the StaticGroup. ///StaticGroup directly associated with this shape. public StaticGroupShape(IList collidables, StaticGroup owner) { this.StaticGroup = owner; CollidableTree = new BoundingBoxTree(collidables); //Rather than hooking up a bunch of ShapeChanged events here that don't capture the full capacity of change //in our child collidables, we will rely on the user telling the collidable tree to reformat itself directly. } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Hit data, if any. /// Whether or not the ray hit the entry. public bool RayCast(Ray ray, float maximumLength, out RayCastResult result) { var outputOverlappedElements = PhysicsResources.GetCollidableList(); CollidableTree.GetOverlaps(ray, maximumLength, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = float.MaxValue; for (int i = 0; i < outputOverlappedElements.Count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].RayCast(ray, maximumLength, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } PhysicsResources.GiveBack(outputOverlappedElements); return result.HitData.T < float.MaxValue; } /// /// Tests a ray against the collidable. /// /// Ray to test. /// Maximum length, in units of the ray's direction's length, to test. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data, if any. /// Whether or not the ray hit the entry. public bool RayCast(Ray ray, float maximumLength, Func filter, out RayCastResult result) { var outputOverlappedElements = PhysicsResources.GetCollidableList(); CollidableTree.GetOverlaps(ray, maximumLength, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = float.MaxValue; for (int i = 0; i < outputOverlappedElements.Count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].RayCast(ray, maximumLength, filter, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } PhysicsResources.GiveBack(outputOverlappedElements); return result.HitData.T < float.MaxValue; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayCastResult result) { var outputOverlappedElements = PhysicsResources.GetCollidableList(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); CollidableTree.GetOverlaps(boundingBox, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = float.MaxValue; for (int i = 0; i < outputOverlappedElements.Count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } PhysicsResources.GiveBack(outputOverlappedElements); return result.HitData.T < float.MaxValue; } /// /// Casts a convex shape against the collidable. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. /// Test to apply to the entry. If it returns true, the entry is processed, otherwise the entry is ignored. If a collidable hierarchy is present /// in the entry, this filter will be passed into inner ray casts. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShapes.ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayCastResult result) { var outputOverlappedElements = PhysicsResources.GetCollidableList(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); CollidableTree.GetOverlaps(boundingBox, outputOverlappedElements); result = new RayCastResult(); result.HitData.T = float.MaxValue; for (int i = 0; i < outputOverlappedElements.Count; ++i) { RayHit hit; if (outputOverlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, filter, out hit)) { if (hit.T < result.HitData.T) { result.HitData = hit; result.HitObject = outputOverlappedElements.Elements[i]; } } } PhysicsResources.GiveBack(outputOverlappedElements); return result.HitData.T < float.MaxValue; } } } ================================================ FILE: BEPUphysics/CollisionShapes/StaticMeshShape.cs ================================================ using BEPUphysics.DataStructures; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionShapes { /// /// The local space information needed by a StaticMesh. /// Since the hierarchy is in world space and owned by the StaticMesh collidable, /// this is a pretty lightweight object. /// public class StaticMeshShape : CollisionShape { TransformableMeshData triangleMeshData; /// /// Gets the triangle mesh data composing the StaticMeshShape. /// public TransformableMeshData TriangleMeshData { get { return triangleMeshData; } } /// /// Constructs a new StaticMeshShape. /// ///Vertices of the mesh. ///Indices of the mesh. ///World transform to use in the local space data. public StaticMeshShape(Vector3[] vertices, uint[] indices, int indexCount, AffineTransform worldTransform) { triangleMeshData = new TransformableMeshData(vertices, indices, indexCount, worldTransform); } /// /// Constructs a new StaticMeshShape. /// ///Vertices of the mesh. ///Indices of the mesh. public StaticMeshShape(Vector3[] vertices, uint[] indices, int indexCount) { triangleMeshData = new TransformableMeshData(vertices, indices, indexCount); } } } ================================================ FILE: BEPUphysics/CollisionShapes/TerrainShape.cs ================================================ using System; using BEPUphysics.CollisionTests.Manifolds; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.CollisionShapes { /// /// The local space data needed by a Terrain collidable. /// Contains the Heightmap and other information. /// public class TerrainShape : CollisionShape { private float[,] heights; //note: changing heights in array does not fire OnShapeChanged automatically. //Need to notify parent manually if you do it. /// /// Gets or sets the height field of the terrain shape. /// public float[,] Heights { get { return heights; } set { heights = value; OnShapeChanged(); } } QuadTriangleOrganization quadTriangleOrganization; /// /// Gets or sets the quad triangle organization. /// public QuadTriangleOrganization QuadTriangleOrganization { get { return quadTriangleOrganization; } set { quadTriangleOrganization = value; OnShapeChanged(); } } /// /// Constructs a TerrainShape. /// ///Heights array used for the shape. ///Triangle organization of each quad. ///Thrown if the heights array has less than 2x2 vertices. public TerrainShape(float[,] heights, QuadTriangleOrganization triangleOrganization) { if (heights.GetLength(0) <= 1 || heights.GetLength(1) <= 1) { throw new ArgumentException("Terrains must have a least 2x2 vertices (one quad)."); } this.heights = heights; quadTriangleOrganization = triangleOrganization; } /// /// Constructs a TerrainShape. /// ///Heights array used for the shape. public TerrainShape(float[,] heights) : this(heights, QuadTriangleOrganization.BottomLeftUpperRight) { } /// /// Constructs the bounding box of the terrain given a transform. /// ///Transform to apply to the terrain during the bounding box calculation. ///Bounding box of the terrain shape when transformed. public void GetBoundingBox(ref AffineTransform transform, out BoundingBox boundingBox) { #if !WINDOWS boundingBox = new BoundingBox(); #endif float minX = float.MaxValue, maxX = -float.MaxValue, minY = float.MaxValue, maxY = -float.MaxValue, minZ = float.MaxValue, maxZ = -float.MaxValue; Vector3 minXvertex = new Vector3(), maxXvertex = new Vector3(), minYvertex = new Vector3(), maxYvertex = new Vector3(), minZvertex = new Vector3(), maxZvertex = new Vector3(); //Find the extreme locations. for (int i = 0; i < heights.GetLength(0); i++) { for (int j = 0; j < heights.GetLength(1); j++) { var vertex = new Vector3(i, heights[i, j], j); Matrix3x3.Transform(ref vertex, ref transform.LinearTransform, out vertex); if (vertex.X < minX) { minX = vertex.X; minXvertex = vertex; } else if (vertex.X > maxX) { maxX = vertex.X; maxXvertex = vertex; } if (vertex.Y < minY) { minY = vertex.Y; minYvertex = vertex; } else if (vertex.Y > maxY) { maxY = vertex.Y; maxYvertex = vertex; } if (vertex.Z < minZ) { minZ = vertex.Z; minZvertex = vertex; } else if (vertex.Z > maxZ) { maxZ = vertex.Z; maxZvertex = vertex; } } } //Shift the bounding box. boundingBox.Min.X = minXvertex.X + transform.Translation.X; boundingBox.Min.Y = minYvertex.Y + transform.Translation.Y; boundingBox.Min.Z = minZvertex.Z + transform.Translation.Z; boundingBox.Max.X = maxXvertex.X + transform.Translation.X; boundingBox.Max.Y = maxYvertex.Y + transform.Translation.Y; boundingBox.Max.Z = maxZvertex.Z + transform.Translation.Z; } /// /// Tests a ray against the terrain shape. /// ///Ray to test against the shape. ///Maximum length of the ray in units of the ray direction's length. ///Transform to apply to the terrain shape during the test. ///Hit data of the ray cast, if any. ///Whether or not the ray hit the transformed terrain shape. public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, out RayHit hit) { return RayCast(ref ray, maximumLength, ref transform, TriangleSidedness.Counterclockwise, out hit); } /// /// Tests a ray against the terrain shape. /// ///Ray to test against the shape. ///Maximum length of the ray in units of the ray direction's length. ///Transform to apply to the terrain shape during the test. ///Sidedness of the triangles to use when raycasting. ///Hit data of the ray cast, if any. ///Whether or not the ray hit the transformed terrain shape. public bool RayCast(ref Ray ray, float maximumLength, ref AffineTransform transform, TriangleSidedness sidedness, out RayHit hit) { hit = new RayHit(); //Put the ray into local space. Ray localRay; AffineTransform inverse; AffineTransform.Invert(ref transform, out inverse); Matrix3x3.Transform(ref ray.Direction, ref inverse.LinearTransform, out localRay.Direction); AffineTransform.Transform(ref ray.Position, ref inverse, out localRay.Position); //Use rasterizey traversal. //The origin is at 0,0,0 and the map goes +X, +Y, +Z. //if it's before the origin and facing away, or outside the max and facing out, early out. float maxX = heights.GetLength(0) - 1; float maxZ = heights.GetLength(1) - 1; Vector3 progressingOrigin = localRay.Position; float distance = 0; //Check the outside cases first. if (progressingOrigin.X < 0) { if (localRay.Direction.X > 0) { //Off the left side. float timeToMinX = -progressingOrigin.X / localRay.Direction.X; distance += timeToMinX; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else return false; //Outside and pointing away from the terrain. } else if (progressingOrigin.X > maxX) { if (localRay.Direction.X < 0) { //Off the left side. float timeToMinX = -(progressingOrigin.X - maxX) / localRay.Direction.X; distance += timeToMinX; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else return false; //Outside and pointing away from the terrain. } if (progressingOrigin.Z < 0) { if (localRay.Direction.Z > 0) { float timeToMinZ = -progressingOrigin.Z / localRay.Direction.Z; distance += timeToMinZ; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else return false; } else if (progressingOrigin.Z > maxZ) { if (localRay.Direction.Z < 0) { float timeToMinZ = -(progressingOrigin.Z - maxZ) / localRay.Direction.Z; distance += timeToMinZ; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToMinZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else return false; } if (distance > maximumLength) return false; //By now, we should be entering the main body of the terrain. int xCell = (int)progressingOrigin.X; int zCell = (int)progressingOrigin.Z; //If it's hitting the border and going in, then correct the index //so that it will initially target a valid quad. //Without this, a quad beyond the border would be tried and failed. if (xCell == heights.GetLength(0) - 1 && localRay.Direction.X < 0) xCell = heights.GetLength(0) - 2; if (zCell == heights.GetLength(1) - 1 && localRay.Direction.Z < 0) zCell = heights.GetLength(1) - 2; while (true) { //Check for a miss. if (xCell < 0 || zCell < 0 || xCell >= heights.GetLength(0) - 1 || zCell >= heights.GetLength(1) - 1) return false; //Test the triangles of this cell. Vector3 v1, v2, v3, v4; // v3 v4 // v1 v2 GetLocalPosition(xCell, zCell, out v1); GetLocalPosition(xCell + 1, zCell, out v2); GetLocalPosition(xCell, zCell + 1, out v3); GetLocalPosition(xCell + 1, zCell + 1, out v4); RayHit hit1, hit2; bool didHit1; bool didHit2; //Don't bother doing ray intersection tests if the ray can't intersect it. float highest = v1.Y; float lowest = v1.Y; if (v2.Y > highest) highest = v2.Y; else if (v2.Y < lowest) lowest = v2.Y; if (v3.Y > highest) highest = v3.Y; else if (v3.Y < lowest) lowest = v3.Y; if (v4.Y > highest) highest = v4.Y; else if (v4.Y < lowest) lowest = v4.Y; if (!(progressingOrigin.Y > highest && localRay.Direction.Y > 0 || progressingOrigin.Y < lowest && localRay.Direction.Y < 0)) { if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight) { //Always perform the raycast as if Y+ in local space is the way the triangles are facing. didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit1); didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v2, ref v4, ref v3, out hit2); } else //if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomRightUpperLeft) { didHit1 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v2, ref v4, out hit1); didHit2 = Toolbox.FindRayTriangleIntersection(ref localRay, maximumLength, sidedness, ref v1, ref v4, ref v3, out hit2); } if (didHit1 && didHit2) { if (hit1.T < hit2.T) { Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit1.T; return true; } Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit2.T; return true; } else if (didHit1) { Vector3.Multiply(ref ray.Direction, hit1.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit1.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit1.T; return true; } else if (didHit2) { Vector3.Multiply(ref ray.Direction, hit2.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Matrix3x3.TransformTranspose(ref hit2.Normal, ref inverse.LinearTransform, out hit.Normal); hit.T = hit2.T; return true; } } //Move to the next cell. float timeToX; if (localRay.Direction.X < 0) timeToX = -(progressingOrigin.X - xCell) / localRay.Direction.X; else if (ray.Direction.X > 0) timeToX = (xCell + 1 - progressingOrigin.X) / localRay.Direction.X; else timeToX = float.MaxValue; float timeToZ; if (localRay.Direction.Z < 0) timeToZ = -(progressingOrigin.Z - zCell) / localRay.Direction.Z; else if (localRay.Direction.Z > 0) timeToZ = (zCell + 1 - progressingOrigin.Z) / localRay.Direction.Z; else timeToZ = float.MaxValue; //Move to the next cell. if (timeToX < timeToZ) { if (localRay.Direction.X < 0) xCell--; else xCell++; distance += timeToX; if (distance > maximumLength) return false; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToX, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } else { if (localRay.Direction.Z < 0) zCell--; else zCell++; distance += timeToZ; if (distance > maximumLength) return false; Vector3 increment; Vector3.Multiply(ref localRay.Direction, timeToZ, out increment); Vector3.Add(ref increment, ref progressingOrigin, out progressingOrigin); } } } /// /// Gets the position of a vertex at the given indices in local space. /// ///Index in the first dimension. ///Index in the second dimension. ///Local space position at the given vertice.s public void GetLocalPosition(int i, int j, out Vector3 v) { #if !WINDOWS v = new Vector3(); #endif v.X = i; v.Y = heights[i, j]; v.Z = j; } /// /// Gets the world space position of a vertex in the terrain at the given indices. /// ///Index in the first dimension. ///Index in the second dimension. /// Transform to apply to the vertex. /// Transformed position of the vertex at the given indices. public void GetPosition(int i, int j, ref AffineTransform transform, out Vector3 position) { if (i <= 0) i = 0; else if (i >= heights.GetLength(0)) i = heights.GetLength(0) - 1; if (j <= 0) j = 0; else if (j >= heights.GetLength(1)) j = heights.GetLength(1) - 1; #if !WINDOWS position = new Vector3(); #endif position.X = i; position.Y = heights[i, j]; position.Z = j; AffineTransform.Transform(ref position, ref transform, out position); } /// /// Gets the world space normal at the given indices. /// ///Index in the first dimension. ///Index in the second dimension. /// Transform to apply to the terrain while computing the normal. /// World space normal at the given indices. public void GetNormal(int i, int j, ref AffineTransform transform, out Vector3 normal) { Vector3 top; Vector3 bottom; Vector3 right; Vector3 left; if (i <= 0) i = 0; else if (i >= heights.GetLength(0)) i = heights.GetLength(0) - 1; if (j <= 0) j = 0; else if (j >= heights.GetLength(1)) j = heights.GetLength(1) - 1; GetPosition(i, Math.Min(j + 1, heights.GetLength(1) - 1), ref transform, out top); GetPosition(i, Math.Max(j - 1, 0), ref transform, out bottom); GetPosition(Math.Min(i + 1, heights.GetLength(0) - 1), j, ref transform, out right); GetPosition(Math.Max(i - 1, 0), j, ref transform, out left); Vector3 temp; Vector3.Subtract(ref top, ref bottom, out temp); Vector3.Subtract(ref right, ref left, out normal); Vector3.Cross(ref temp, ref normal, out normal); normal.Normalize(); } /// /// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape. /// ///Bounding box in the local space of the terrain shape. ///Triangles whose bounding boxes overlap the input bounding box. public bool GetOverlaps(BoundingBox localSpaceBoundingBox, RawList overlappedTriangles) { uint width = (uint)heights.GetLength(0); uint minX = (uint)Math.Max((int)localSpaceBoundingBox.Min.X, 0); uint minY = (uint)Math.Max((int)localSpaceBoundingBox.Min.Z, 0); uint maxX = (uint)Math.Min((int)localSpaceBoundingBox.Max.X, width - 2); uint maxY = (uint)Math.Min((int)localSpaceBoundingBox.Max.Z, heights.GetLength(1) - 2); for (uint i = minX; i <= maxX; i++) { for (uint j = minY; j <= maxY; j++) { //Before adding a triangle to the list, make sure the object isn't too high or low from the quad. float highest, lowest; float y1 = heights[i, j]; float y2 = heights[i + 1, j]; float y3 = heights[i, j + 1]; float y4 = heights[i + 1, j + 1]; highest = y1; lowest = y1; if (y2 > highest) highest = y2; else if (y2 < lowest) lowest = y2; if (y3 > highest) highest = y3; else if (y3 < lowest) lowest = y3; if (y4 > highest) highest = y4; else if (y4 < lowest) lowest = y4; if (localSpaceBoundingBox.Max.Y < lowest || localSpaceBoundingBox.Min.Y > highest) continue; //Now the local bounding box is very likely intersecting those of the triangles. //Add the triangles to the list. var indices = new TriangleMeshConvexContactManifold.TriangleIndices(); //v3 v4 //v1 v2 if (quadTriangleOrganization == QuadTriangleOrganization.BottomLeftUpperRight) { //v1 v2 v3 indices.A = i + j * width; indices.B = i + 1 + j * width; indices.C = i + (j + 1) * width; overlappedTriangles.Add(indices); //v2 v4 v3 indices.A = i + 1 + j * width; indices.B = i + 1 + (j + 1) * width; indices.C = i + (j + 1) * width; overlappedTriangles.Add(indices); } else //Bottom right, Upper left { //v1 v2 v4 indices.A = i + j * width; indices.B = i + 1 + j * width; indices.C = i + 1 + (j + 1) * width; overlappedTriangles.Add(indices); //v1 v4 v3 indices.A = i + j * width; indices.B = i + 1 + (j + 1) * width; indices.C = i + (j + 1) * width; overlappedTriangles.Add(indices); } } } return overlappedTriangles.Count > 0; } /// /// Gets overlapped triangles with the terrain shape with a bounding box in the local space of the shape. /// ///Bounding box in the local space of the terrain shape. ///Indices of elements whose bounding boxes overlap the input bounding box. public bool GetOverlaps(BoundingBox localBoundingBox, RawList overlappedElements) { int width = heights.GetLength(0); int minX = Math.Max((int)localBoundingBox.Min.X, 0); int minY = Math.Max((int)localBoundingBox.Min.Z, 0); int maxX = Math.Min((int)localBoundingBox.Max.X, width - 2); int maxY = Math.Min((int)localBoundingBox.Max.Z, heights.GetLength(1) - 2); for (int i = minX; i <= maxX; i++) { for (int j = minY; j <= maxY; j++) { //Before adding a triangle to the list, make sure the object isn't too high or low from the quad. float highest, lowest; float y1 = heights[i, j]; float y2 = heights[i + 1, j]; float y3 = heights[i, j + 1]; float y4 = heights[i + 1, j + 1]; highest = y1; lowest = y1; if (y2 > highest) highest = y2; else if (y2 < lowest) lowest = y2; if (y3 > highest) highest = y3; else if (y3 < lowest) lowest = y3; if (y4 > highest) highest = y4; else if (y4 < lowest) lowest = y4; if (localBoundingBox.Max.Y < lowest || localBoundingBox.Min.Y > highest) continue; //Now the local bounding box is very likely intersecting those of the triangles. //Add the triangles to the list. int quadIndex = (i + j * width) * 2; overlappedElements.Add(quadIndex); overlappedElements.Add(quadIndex + 1); } } return overlappedElements.Count > 0; } /// /// Gets a world space triangle in the terrain at the given indices (as if it were a mesh). /// ///Indices of the triangle. ///Transform to apply to the triangle vertices. ///First vertex of the triangle. ///Second vertex of the triangle. ///Third vertex of the triangle. public void GetTriangle(ref TriangleMeshConvexContactManifold.TriangleIndices indices, ref AffineTransform transform, out Vector3 a, out Vector3 b, out Vector3 c) { //Reverse the encoded index: //index = i + width * j int width = heights.GetLength(0); int columnA = (int)indices.A / width; int rowA = (int)indices.A - columnA * width; int columnB = (int)indices.B / width; int rowB = (int)indices.B - columnB * width; int columnC = (int)indices.C / width; int rowC = (int)indices.C - columnC * width; GetPosition(rowA, columnA, ref transform, out a); GetPosition(rowB, columnB, ref transform, out b); GetPosition(rowC, columnC, ref transform, out c); } /// /// Gets a world space triangle in the terrain at the given triangle index. /// ///Index of the triangle. ///Transform to apply to the triangle vertices. ///First vertex of the triangle. ///Second vertex of the triangle. ///Third vertex of the triangle. public void GetTriangle(int index, ref AffineTransform transform, out Vector3 a, out Vector3 b, out Vector3 c) { //Find the quad. int quadIndex = index / 2; bool isFirstTriangle = quadIndex * 2 == index; int column = quadIndex / heights.GetLength(0); int row = quadIndex - column * heights.GetLength(0); if (quadTriangleOrganization == CollisionShapes.QuadTriangleOrganization.BottomLeftUpperRight) { if (isFirstTriangle) { GetPosition(row, column, ref transform, out a); GetPosition(row + 1, column, ref transform, out b); GetPosition(row, column + 1, ref transform, out c); } else { GetPosition(row, column + 1, ref transform, out a); GetPosition(row + 1, column + 1, ref transform, out b); GetPosition(row + 1, column, ref transform, out c); } } else { //The quad is BottomRightUpperLeft. if (isFirstTriangle) { GetPosition(row, column, ref transform, out a); GetPosition(row + 1, column, ref transform, out b); GetPosition(row + 1, column + 1, ref transform, out c); } else { GetPosition(row, column, ref transform, out a); GetPosition(row, column + 1, ref transform, out b); GetPosition(row + 1, column + 1, ref transform, out c); } } } } /// /// Defines how a Terrain organizes triangles in its quads. /// public enum QuadTriangleOrganization { /// /// Triangle with a right angle at the (-i,-j) position and another at the (+i,+j) position. /// BottomLeftUpperRight, /// /// Triangle with a right angle at the (+i,-j) position and another at the high (-i,+j) position. /// BottomRightUpperLeft } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/BoxBoxCollider.cs ================================================ using System; using System.Runtime.InteropServices; using BEPUutilities.DataStructures; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using System.Diagnostics; using BEPUphysics.Settings; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Stores basic data used by some collision systems. /// [StructLayout(LayoutKind.Sequential)] public struct BoxContactData : IEquatable { /// /// Position of the candidate contact. /// public Vector3 Position; /// /// Depth of the candidate contact. /// public float Depth; /// /// Id of the candidate contact. /// public int Id; #region IEquatable Members /// /// Returns true if the other data has the same id. /// /// Data to compare. /// True if the other data has the same id, false otherwise. public bool Equals(BoxContactData other) { return Id == other.Id; } #endregion } #if WINDOWS [StructLayout(LayoutKind.Sequential, Pack = 1)] #else #if XBOX360 [StructLayout(LayoutKind.Sequential)] #endif #endif /// /// Basic storage structure for contact data. /// Designed for performance critical code and pointer access. /// public struct BoxContactDataCache { public BoxContactData D1; public BoxContactData D2; public BoxContactData D3; public BoxContactData D4; public BoxContactData D5; public BoxContactData D6; public BoxContactData D7; public BoxContactData D8; //internal BoxContactData d9; //internal BoxContactData d10; //internal BoxContactData d11; //internal BoxContactData d12; //internal BoxContactData d13; //internal BoxContactData d14; //internal BoxContactData d15; //internal BoxContactData d16; /// /// Number of elements in the cache. /// public byte Count; #if ALLOWUNSAFE /// /// Removes an item at the given index. /// /// Index to remove. public unsafe void RemoveAt(int index) { BoxContactDataCache copy = this; BoxContactData* pointer = ©.D1; pointer[index] = pointer[Count - 1]; this = copy; Count--; } #endif } /// /// Contains helper methods for testing collisions between boxes. /// public static class BoxBoxCollider { /// /// Determines if the two boxes are colliding. /// /// First box to collide. /// Second box to collide. /// Transform to apply to shape a. /// Transform to apply to shape b. /// Whether or not the boxes collide. public static bool AreBoxesColliding(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB) { float aX = a.HalfWidth; float aY = a.HalfHeight; float aZ = a.HalfLength; float bX = b.HalfWidth; float bY = b.HalfHeight; float bZ = b.HalfLength; //Relative rotation from A to B. Matrix3x3 bR; Matrix3x3 aO; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out aO); Matrix3x3 bO; Matrix3x3.CreateFromQuaternion(ref transformB.Orientation, out bO); //Relative translation rotated into A's configuration space. Vector3 t; Vector3.Subtract(ref transformB.Position, ref transformA.Position, out t); bR.M11 = aO.M11 * bO.M11 + aO.M12 * bO.M12 + aO.M13 * bO.M13; bR.M12 = aO.M11 * bO.M21 + aO.M12 * bO.M22 + aO.M13 * bO.M23; bR.M13 = aO.M11 * bO.M31 + aO.M12 * bO.M32 + aO.M13 * bO.M33; Matrix3x3 absBR; //Epsilons are added to deal with near-parallel edges. absBR.M11 = Math.Abs(bR.M11) + Toolbox.Epsilon; absBR.M12 = Math.Abs(bR.M12) + Toolbox.Epsilon; absBR.M13 = Math.Abs(bR.M13) + Toolbox.Epsilon; float tX = t.X; t.X = t.X * aO.M11 + t.Y * aO.M12 + t.Z * aO.M13; //Test the axes defines by entity A's rotation matrix. //A.X float rb = bX * absBR.M11 + bY * absBR.M12 + bZ * absBR.M13; if (Math.Abs(t.X) > aX + rb) return false; bR.M21 = aO.M21 * bO.M11 + aO.M22 * bO.M12 + aO.M23 * bO.M13; bR.M22 = aO.M21 * bO.M21 + aO.M22 * bO.M22 + aO.M23 * bO.M23; bR.M23 = aO.M21 * bO.M31 + aO.M22 * bO.M32 + aO.M23 * bO.M33; absBR.M21 = Math.Abs(bR.M21) + Toolbox.Epsilon; absBR.M22 = Math.Abs(bR.M22) + Toolbox.Epsilon; absBR.M23 = Math.Abs(bR.M23) + Toolbox.Epsilon; float tY = t.Y; t.Y = tX * aO.M21 + t.Y * aO.M22 + t.Z * aO.M23; //A.Y rb = bX * absBR.M21 + bY * absBR.M22 + bZ * absBR.M23; if (Math.Abs(t.Y) > aY + rb) return false; bR.M31 = aO.M31 * bO.M11 + aO.M32 * bO.M12 + aO.M33 * bO.M13; bR.M32 = aO.M31 * bO.M21 + aO.M32 * bO.M22 + aO.M33 * bO.M23; bR.M33 = aO.M31 * bO.M31 + aO.M32 * bO.M32 + aO.M33 * bO.M33; absBR.M31 = Math.Abs(bR.M31) + Toolbox.Epsilon; absBR.M32 = Math.Abs(bR.M32) + Toolbox.Epsilon; absBR.M33 = Math.Abs(bR.M33) + Toolbox.Epsilon; t.Z = tX * aO.M31 + tY * aO.M32 + t.Z * aO.M33; //A.Z rb = bX * absBR.M31 + bY * absBR.M32 + bZ * absBR.M33; if (Math.Abs(t.Z) > aZ + rb) return false; //Test the axes defines by entity B's rotation matrix. //B.X float ra = aX * absBR.M11 + aY * absBR.M21 + aZ * absBR.M31; if (Math.Abs(t.X * bR.M11 + t.Y * bR.M21 + t.Z * bR.M31) > ra + bX) return false; //B.Y ra = aX * absBR.M12 + aY * absBR.M22 + aZ * absBR.M32; if (Math.Abs(t.X * bR.M12 + t.Y * bR.M22 + t.Z * bR.M32) > ra + bY) return false; //B.Z ra = aX * absBR.M13 + aY * absBR.M23 + aZ * absBR.M33; if (Math.Abs(t.X * bR.M13 + t.Y * bR.M23 + t.Z * bR.M33) > ra + bZ) return false; //Now for the edge-edge cases. //A.X x B.X ra = aY * absBR.M31 + aZ * absBR.M21; rb = bY * absBR.M13 + bZ * absBR.M12; if (Math.Abs(t.Z * bR.M21 - t.Y * bR.M31) > ra + rb) return false; //A.X x B.Y ra = aY * absBR.M32 + aZ * absBR.M22; rb = bX * absBR.M13 + bZ * absBR.M11; if (Math.Abs(t.Z * bR.M22 - t.Y * bR.M32) > ra + rb) return false; //A.X x B.Z ra = aY * absBR.M33 + aZ * absBR.M23; rb = bX * absBR.M12 + bY * absBR.M11; if (Math.Abs(t.Z * bR.M23 - t.Y * bR.M33) > ra + rb) return false; //A.Y x B.X ra = aX * absBR.M31 + aZ * absBR.M11; rb = bY * absBR.M23 + bZ * absBR.M22; if (Math.Abs(t.X * bR.M31 - t.Z * bR.M11) > ra + rb) return false; //A.Y x B.Y ra = aX * absBR.M32 + aZ * absBR.M12; rb = bX * absBR.M23 + bZ * absBR.M21; if (Math.Abs(t.X * bR.M32 - t.Z * bR.M12) > ra + rb) return false; //A.Y x B.Z ra = aX * absBR.M33 + aZ * absBR.M13; rb = bX * absBR.M22 + bY * absBR.M21; if (Math.Abs(t.X * bR.M33 - t.Z * bR.M13) > ra + rb) return false; //A.Z x B.X ra = aX * absBR.M21 + aY * absBR.M11; rb = bY * absBR.M33 + bZ * absBR.M32; if (Math.Abs(t.Y * bR.M11 - t.X * bR.M21) > ra + rb) return false; //A.Z x B.Y ra = aX * absBR.M22 + aY * absBR.M12; rb = bX * absBR.M33 + bZ * absBR.M31; if (Math.Abs(t.Y * bR.M12 - t.X * bR.M22) > ra + rb) return false; //A.Z x B.Z ra = aX * absBR.M23 + aY * absBR.M13; rb = bX * absBR.M32 + bY * absBR.M31; if (Math.Abs(t.Y * bR.M13 - t.X * bR.M23) > ra + rb) return false; return true; } /// /// Determines if the two boxes are colliding. /// /// First box to collide. /// Second box to collide. /// Distance of separation. /// Axis of separation. /// Transform to apply to shape A. /// Transform to apply to shape B. /// Whether or not the boxes collide. public static bool AreBoxesColliding(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB, out float separationDistance, out Vector3 separatingAxis) { float aX = a.HalfWidth; float aY = a.HalfHeight; float aZ = a.HalfLength; float bX = b.HalfWidth; float bY = b.HalfHeight; float bZ = b.HalfLength; //Relative rotation from A to B. Matrix3x3 bR; Matrix3x3 aO; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out aO); Matrix3x3 bO; Matrix3x3.CreateFromQuaternion(ref transformB.Orientation, out bO); //Relative translation rotated into A's configuration space. Vector3 t; Vector3.Subtract(ref transformB.Position, ref transformA.Position, out t); #region A Face Normals bR.M11 = aO.M11 * bO.M11 + aO.M12 * bO.M12 + aO.M13 * bO.M13; bR.M12 = aO.M11 * bO.M21 + aO.M12 * bO.M22 + aO.M13 * bO.M23; bR.M13 = aO.M11 * bO.M31 + aO.M12 * bO.M32 + aO.M13 * bO.M33; Matrix3x3 absBR; //Epsilons are added to deal with near-parallel edges. absBR.M11 = Math.Abs(bR.M11) + Toolbox.Epsilon; absBR.M12 = Math.Abs(bR.M12) + Toolbox.Epsilon; absBR.M13 = Math.Abs(bR.M13) + Toolbox.Epsilon; float tX = t.X; t.X = t.X * aO.M11 + t.Y * aO.M12 + t.Z * aO.M13; //Test the axes defines by entity A's rotation matrix. //A.X float rarb = aX + bX * absBR.M11 + bY * absBR.M12 + bZ * absBR.M13; if (t.X > rarb) { separationDistance = t.X - rarb; separatingAxis = new Vector3(aO.M11, aO.M12, aO.M13); return false; } if (t.X < -rarb) { separationDistance = -t.X - rarb; separatingAxis = new Vector3(-aO.M11, -aO.M12, -aO.M13); return false; } bR.M21 = aO.M21 * bO.M11 + aO.M22 * bO.M12 + aO.M23 * bO.M13; bR.M22 = aO.M21 * bO.M21 + aO.M22 * bO.M22 + aO.M23 * bO.M23; bR.M23 = aO.M21 * bO.M31 + aO.M22 * bO.M32 + aO.M23 * bO.M33; absBR.M21 = Math.Abs(bR.M21) + Toolbox.Epsilon; absBR.M22 = Math.Abs(bR.M22) + Toolbox.Epsilon; absBR.M23 = Math.Abs(bR.M23) + Toolbox.Epsilon; float tY = t.Y; t.Y = tX * aO.M21 + t.Y * aO.M22 + t.Z * aO.M23; //A.Y rarb = aY + bX * absBR.M21 + bY * absBR.M22 + bZ * absBR.M23; if (t.Y > rarb) { separationDistance = t.Y - rarb; separatingAxis = new Vector3(aO.M21, aO.M22, aO.M23); return false; } if (t.Y < -rarb) { separationDistance = -t.Y - rarb; separatingAxis = new Vector3(-aO.M21, -aO.M22, -aO.M23); return false; } bR.M31 = aO.M31 * bO.M11 + aO.M32 * bO.M12 + aO.M33 * bO.M13; bR.M32 = aO.M31 * bO.M21 + aO.M32 * bO.M22 + aO.M33 * bO.M23; bR.M33 = aO.M31 * bO.M31 + aO.M32 * bO.M32 + aO.M33 * bO.M33; absBR.M31 = Math.Abs(bR.M31) + Toolbox.Epsilon; absBR.M32 = Math.Abs(bR.M32) + Toolbox.Epsilon; absBR.M33 = Math.Abs(bR.M33) + Toolbox.Epsilon; t.Z = tX * aO.M31 + tY * aO.M32 + t.Z * aO.M33; //A.Z rarb = aZ + bX * absBR.M31 + bY * absBR.M32 + bZ * absBR.M33; if (t.Z > rarb) { separationDistance = t.Z - rarb; separatingAxis = new Vector3(aO.M31, aO.M32, aO.M33); return false; } if (t.Z < -rarb) { separationDistance = -t.Z - rarb; separatingAxis = new Vector3(-aO.M31, -aO.M32, -aO.M33); return false; } #endregion #region B Face Normals //Test the axes defines by entity B's rotation matrix. //B.X rarb = bX + aX * absBR.M11 + aY * absBR.M21 + aZ * absBR.M31; float tl = t.X * bR.M11 + t.Y * bR.M21 + t.Z * bR.M31; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(bO.M11, bO.M12, bO.M13); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(-bO.M11, -bO.M12, -bO.M13); return false; } //B.Y rarb = bY + aX * absBR.M12 + aY * absBR.M22 + aZ * absBR.M32; tl = t.X * bR.M12 + t.Y * bR.M22 + t.Z * bR.M32; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(bO.M21, bO.M22, bO.M23); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(-bO.M21, -bO.M22, -bO.M23); return false; } //B.Z rarb = bZ + aX * absBR.M13 + aY * absBR.M23 + aZ * absBR.M33; tl = t.X * bR.M13 + t.Y * bR.M23 + t.Z * bR.M33; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(bO.M31, bO.M32, bO.M33); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(-bO.M31, -bO.M32, -bO.M33); return false; } #endregion #region A.X x B.() //Now for the edge-edge cases. //A.X x B.X rarb = aY * absBR.M31 + aZ * absBR.M21 + bY * absBR.M13 + bZ * absBR.M12; tl = t.Z * bR.M21 - t.Y * bR.M31; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M12 * bO.M13 - aO.M13 * bO.M12, aO.M13 * bO.M11 - aO.M11 * bO.M13, aO.M11 * bO.M12 - aO.M12 * bO.M11); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M12 * aO.M13 - bO.M13 * aO.M12, bO.M13 * aO.M11 - bO.M11 * aO.M13, bO.M11 * aO.M12 - bO.M12 * aO.M11); return false; } //A.X x B.Y rarb = aY * absBR.M32 + aZ * absBR.M22 + bX * absBR.M13 + bZ * absBR.M11; tl = t.Z * bR.M22 - t.Y * bR.M32; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M12 * bO.M23 - aO.M13 * bO.M22, aO.M13 * bO.M21 - aO.M11 * bO.M23, aO.M11 * bO.M22 - aO.M12 * bO.M21); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M22 * aO.M13 - bO.M23 * aO.M12, bO.M23 * aO.M11 - bO.M21 * aO.M13, bO.M21 * aO.M12 - bO.M22 * aO.M11); return false; } //A.X x B.Z rarb = aY * absBR.M33 + aZ * absBR.M23 + bX * absBR.M12 + bY * absBR.M11; tl = t.Z * bR.M23 - t.Y * bR.M33; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M12 * bO.M33 - aO.M13 * bO.M32, aO.M13 * bO.M31 - aO.M11 * bO.M33, aO.M11 * bO.M32 - aO.M12 * bO.M31); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M32 * aO.M13 - bO.M33 * aO.M12, bO.M33 * aO.M11 - bO.M31 * aO.M13, bO.M31 * aO.M12 - bO.M32 * aO.M11); return false; } #endregion #region A.Y x B.() //A.Y x B.X rarb = aX * absBR.M31 + aZ * absBR.M11 + bY * absBR.M23 + bZ * absBR.M22; tl = t.X * bR.M31 - t.Z * bR.M11; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M22 * bO.M13 - aO.M23 * bO.M12, aO.M23 * bO.M11 - aO.M21 * bO.M13, aO.M21 * bO.M12 - aO.M22 * bO.M11); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M12 * aO.M23 - bO.M13 * aO.M22, bO.M13 * aO.M21 - bO.M11 * aO.M23, bO.M11 * aO.M22 - bO.M12 * aO.M21); return false; } //A.Y x B.Y rarb = aX * absBR.M32 + aZ * absBR.M12 + bX * absBR.M23 + bZ * absBR.M21; tl = t.X * bR.M32 - t.Z * bR.M12; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M22 * bO.M23 - aO.M23 * bO.M22, aO.M23 * bO.M21 - aO.M21 * bO.M23, aO.M21 * bO.M22 - aO.M22 * bO.M21); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M22 * aO.M23 - bO.M23 * aO.M22, bO.M23 * aO.M21 - bO.M21 * aO.M23, bO.M21 * aO.M22 - bO.M22 * aO.M21); return false; } //A.Y x B.Z rarb = aX * absBR.M33 + aZ * absBR.M13 + bX * absBR.M22 + bY * absBR.M21; tl = t.X * bR.M33 - t.Z * bR.M13; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M22 * bO.M33 - aO.M23 * bO.M32, aO.M23 * bO.M31 - aO.M21 * bO.M33, aO.M21 * bO.M32 - aO.M22 * bO.M31); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M32 * aO.M23 - bO.M33 * aO.M22, bO.M33 * aO.M21 - bO.M31 * aO.M23, bO.M31 * aO.M22 - bO.M32 * aO.M21); return false; } #endregion #region A.Z x B.() //A.Z x B.X rarb = aX * absBR.M21 + aY * absBR.M11 + bY * absBR.M33 + bZ * absBR.M32; tl = t.Y * bR.M11 - t.X * bR.M21; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M32 * bO.M13 - aO.M33 * bO.M12, aO.M33 * bO.M11 - aO.M31 * bO.M13, aO.M31 * bO.M12 - aO.M32 * bO.M11); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M12 * aO.M33 - bO.M13 * aO.M32, bO.M13 * aO.M31 - bO.M11 * aO.M33, bO.M11 * aO.M32 - bO.M12 * aO.M31); return false; } //A.Z x B.Y rarb = aX * absBR.M22 + aY * absBR.M12 + bX * absBR.M33 + bZ * absBR.M31; tl = t.Y * bR.M12 - t.X * bR.M22; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M32 * bO.M23 - aO.M33 * bO.M22, aO.M33 * bO.M21 - aO.M31 * bO.M23, aO.M31 * bO.M22 - aO.M32 * bO.M21); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M22 * aO.M33 - bO.M23 * aO.M32, bO.M23 * aO.M31 - bO.M21 * aO.M33, bO.M21 * aO.M32 - bO.M22 * aO.M31); return false; } //A.Z x B.Z rarb = aX * absBR.M23 + aY * absBR.M13 + bX * absBR.M32 + bY * absBR.M31; tl = t.Y * bR.M13 - t.X * bR.M23; if (tl > rarb) { separationDistance = tl - rarb; separatingAxis = new Vector3(aO.M32 * bO.M33 - aO.M33 * bO.M32, aO.M33 * bO.M31 - aO.M31 * bO.M33, aO.M31 * bO.M32 - aO.M32 * bO.M31); return false; } if (tl < -rarb) { separationDistance = -tl - rarb; separatingAxis = new Vector3(bO.M32 * aO.M33 - bO.M33 * aO.M32, bO.M33 * aO.M31 - bO.M31 * aO.M33, bO.M31 * aO.M32 - bO.M32 * aO.M31); return false; } #endregion separationDistance = 0; separatingAxis = Vector3.Zero; return true; } /// /// Determines if the two boxes are colliding, including penetration depth data. /// /// First box to collide. /// Second box to collide. /// Distance of separation or penetration. /// Axis of separation or penetration. /// Transform to apply to shape A. /// Transform to apply to shape B. /// Whether or not the boxes collide. public static bool AreBoxesCollidingWithPenetration(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB, out float distance, out Vector3 axis) { float aX = a.HalfWidth; float aY = a.HalfHeight; float aZ = a.HalfLength; float bX = b.HalfWidth; float bY = b.HalfHeight; float bZ = b.HalfLength; //Relative rotation from A to B. Matrix3x3 bR; Matrix3x3 aO; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out aO); Matrix3x3 bO; Matrix3x3.CreateFromQuaternion(ref transformB.Orientation, out bO); //Relative translation rotated into A's configuration space. Vector3 t; Vector3.Subtract(ref transformB.Position, ref transformA.Position, out t); float tempDistance; float minimumDistance = -float.MaxValue; var minimumAxis = new Vector3(); #region A Face Normals bR.M11 = aO.M11 * bO.M11 + aO.M12 * bO.M12 + aO.M13 * bO.M13; bR.M12 = aO.M11 * bO.M21 + aO.M12 * bO.M22 + aO.M13 * bO.M23; bR.M13 = aO.M11 * bO.M31 + aO.M12 * bO.M32 + aO.M13 * bO.M33; Matrix3x3 absBR; //Epsilons are added to deal with near-parallel edges. absBR.M11 = Math.Abs(bR.M11) + Toolbox.Epsilon; absBR.M12 = Math.Abs(bR.M12) + Toolbox.Epsilon; absBR.M13 = Math.Abs(bR.M13) + Toolbox.Epsilon; float tX = t.X; t.X = t.X * aO.M11 + t.Y * aO.M12 + t.Z * aO.M13; //Test the axes defines by entity A's rotation matrix. //A.X float rarb = aX + bX * absBR.M11 + bY * absBR.M12 + bZ * absBR.M13; if (t.X > rarb) { distance = t.X - rarb; axis = new Vector3(aO.M11, aO.M12, aO.M13); return false; } if (t.X < -rarb) { distance = -t.X - rarb; axis = new Vector3(-aO.M11, -aO.M12, -aO.M13); return false; } //Inside if (t.X > 0) { tempDistance = t.X - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M11, aO.M12, aO.M13); } } else { tempDistance = -t.X - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M11, -aO.M12, -aO.M13); } } bR.M21 = aO.M21 * bO.M11 + aO.M22 * bO.M12 + aO.M23 * bO.M13; bR.M22 = aO.M21 * bO.M21 + aO.M22 * bO.M22 + aO.M23 * bO.M23; bR.M23 = aO.M21 * bO.M31 + aO.M22 * bO.M32 + aO.M23 * bO.M33; absBR.M21 = Math.Abs(bR.M21) + Toolbox.Epsilon; absBR.M22 = Math.Abs(bR.M22) + Toolbox.Epsilon; absBR.M23 = Math.Abs(bR.M23) + Toolbox.Epsilon; float tY = t.Y; t.Y = tX * aO.M21 + t.Y * aO.M22 + t.Z * aO.M23; //A.Y rarb = aY + bX * absBR.M21 + bY * absBR.M22 + bZ * absBR.M23; if (t.Y > rarb) { distance = t.Y - rarb; axis = new Vector3(aO.M21, aO.M22, aO.M23); return false; } if (t.Y < -rarb) { distance = -t.Y - rarb; axis = new Vector3(-aO.M21, -aO.M22, -aO.M23); return false; } //Inside if (t.Y > 0) { tempDistance = t.Y - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M21, aO.M22, aO.M23); } } else { tempDistance = -t.Y - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M21, -aO.M22, -aO.M23); } } bR.M31 = aO.M31 * bO.M11 + aO.M32 * bO.M12 + aO.M33 * bO.M13; bR.M32 = aO.M31 * bO.M21 + aO.M32 * bO.M22 + aO.M33 * bO.M23; bR.M33 = aO.M31 * bO.M31 + aO.M32 * bO.M32 + aO.M33 * bO.M33; absBR.M31 = Math.Abs(bR.M31) + Toolbox.Epsilon; absBR.M32 = Math.Abs(bR.M32) + Toolbox.Epsilon; absBR.M33 = Math.Abs(bR.M33) + Toolbox.Epsilon; t.Z = tX * aO.M31 + tY * aO.M32 + t.Z * aO.M33; //A.Z rarb = aZ + bX * absBR.M31 + bY * absBR.M32 + bZ * absBR.M33; if (t.Z > rarb) { distance = t.Z - rarb; axis = new Vector3(aO.M31, aO.M32, aO.M33); return false; } if (t.Z < -rarb) { distance = -t.Z - rarb; axis = new Vector3(-aO.M31, -aO.M32, -aO.M33); return false; } //Inside if (t.Z > 0) { tempDistance = t.Z - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M31, aO.M32, aO.M33); } } else { tempDistance = -t.Z - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M31, -aO.M32, -aO.M33); } } #endregion #region B Face Normals //Test the axes defines by entity B's rotation matrix. //B.X rarb = bX + aX * absBR.M11 + aY * absBR.M21 + aZ * absBR.M31; float tl = t.X * bR.M11 + t.Y * bR.M21 + t.Z * bR.M31; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M11, bO.M12, bO.M13); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(-bO.M11, -bO.M12, -bO.M13); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M11, bO.M12, bO.M13); } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M11, -bO.M12, -bO.M13); } } //B.Y rarb = bY + aX * absBR.M12 + aY * absBR.M22 + aZ * absBR.M32; tl = t.X * bR.M12 + t.Y * bR.M22 + t.Z * bR.M32; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M21, bO.M22, bO.M23); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(-bO.M21, -bO.M22, -bO.M23); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M21, bO.M22, bO.M23); } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M21, -bO.M22, -bO.M23); } } //B.Z rarb = bZ + aX * absBR.M13 + aY * absBR.M23 + aZ * absBR.M33; tl = t.X * bR.M13 + t.Y * bR.M23 + t.Z * bR.M33; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M31, bO.M32, bO.M33); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(-bO.M31, -bO.M32, -bO.M33); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M31, bO.M32, bO.M33); } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M31, -bO.M32, -bO.M33); } } #endregion float axisLengthInverse; Vector3 tempAxis; #region A.X x B.() //Now for the edge-edge cases. //A.X x B.X rarb = aY * absBR.M31 + aZ * absBR.M21 + bY * absBR.M13 + bZ * absBR.M12; tl = t.Z * bR.M21 - t.Y * bR.M31; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M12 * bO.M13 - aO.M13 * bO.M12, aO.M13 * bO.M11 - aO.M11 * bO.M13, aO.M11 * bO.M12 - aO.M12 * bO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M12 * aO.M13 - bO.M13 * aO.M12, bO.M13 * aO.M11 - bO.M11 * aO.M13, bO.M11 * aO.M12 - bO.M12 * aO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M12 * bO.M13 - aO.M13 * bO.M12, aO.M13 * bO.M11 - aO.M11 * bO.M13, aO.M11 * bO.M12 - aO.M12 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M12 * aO.M13 - bO.M13 * aO.M12, bO.M13 * aO.M11 - bO.M11 * aO.M13, bO.M11 * aO.M12 - bO.M12 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.X x B.Y rarb = aY * absBR.M32 + aZ * absBR.M22 + bX * absBR.M13 + bZ * absBR.M11; tl = t.Z * bR.M22 - t.Y * bR.M32; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M12 * bO.M23 - aO.M13 * bO.M22, aO.M13 * bO.M21 - aO.M11 * bO.M23, aO.M11 * bO.M22 - aO.M12 * bO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M22 * aO.M13 - bO.M23 * aO.M12, bO.M23 * aO.M11 - bO.M21 * aO.M13, bO.M21 * aO.M12 - bO.M22 * aO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M12 * bO.M23 - aO.M13 * bO.M22, aO.M13 * bO.M21 - aO.M11 * bO.M23, aO.M11 * bO.M22 - aO.M12 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M22 * aO.M13 - bO.M23 * aO.M12, bO.M23 * aO.M11 - bO.M21 * aO.M13, bO.M21 * aO.M12 - bO.M22 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.X x B.Z rarb = aY * absBR.M33 + aZ * absBR.M23 + bX * absBR.M12 + bY * absBR.M11; tl = t.Z * bR.M23 - t.Y * bR.M33; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M12 * bO.M33 - aO.M13 * bO.M32, aO.M13 * bO.M31 - aO.M11 * bO.M33, aO.M11 * bO.M32 - aO.M12 * bO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M32 * aO.M13 - bO.M33 * aO.M12, bO.M33 * aO.M11 - bO.M31 * aO.M13, bO.M31 * aO.M12 - bO.M32 * aO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M12 * bO.M33 - aO.M13 * bO.M32, aO.M13 * bO.M31 - aO.M11 * bO.M33, aO.M11 * bO.M32 - aO.M12 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M32 * aO.M13 - bO.M33 * aO.M12, bO.M33 * aO.M11 - bO.M31 * aO.M13, bO.M31 * aO.M12 - bO.M32 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion #region A.Y x B.() //A.Y x B.X rarb = aX * absBR.M31 + aZ * absBR.M11 + bY * absBR.M23 + bZ * absBR.M22; tl = t.X * bR.M31 - t.Z * bR.M11; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M22 * bO.M13 - aO.M23 * bO.M12, aO.M23 * bO.M11 - aO.M21 * bO.M13, aO.M21 * bO.M12 - aO.M22 * bO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M12 * aO.M23 - bO.M13 * aO.M22, bO.M13 * aO.M21 - bO.M11 * aO.M23, bO.M11 * aO.M22 - bO.M12 * aO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M22 * bO.M13 - aO.M23 * bO.M12, aO.M23 * bO.M11 - aO.M21 * bO.M13, aO.M21 * bO.M12 - aO.M22 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M12 * aO.M23 - bO.M13 * aO.M22, bO.M13 * aO.M21 - bO.M11 * aO.M23, bO.M11 * aO.M22 - bO.M12 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Y x B.Y rarb = aX * absBR.M32 + aZ * absBR.M12 + bX * absBR.M23 + bZ * absBR.M21; tl = t.X * bR.M32 - t.Z * bR.M12; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M22 * bO.M23 - aO.M23 * bO.M22, aO.M23 * bO.M21 - aO.M21 * bO.M23, aO.M21 * bO.M22 - aO.M22 * bO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M22 * aO.M23 - bO.M23 * aO.M22, bO.M23 * aO.M21 - bO.M21 * aO.M23, bO.M21 * aO.M22 - bO.M22 * aO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M22 * bO.M23 - aO.M23 * bO.M22, aO.M23 * bO.M21 - aO.M21 * bO.M23, aO.M21 * bO.M22 - aO.M22 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M22 * aO.M23 - bO.M23 * aO.M22, bO.M23 * aO.M21 - bO.M21 * aO.M23, bO.M21 * aO.M22 - bO.M22 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Y x B.Z rarb = aX * absBR.M33 + aZ * absBR.M13 + bX * absBR.M22 + bY * absBR.M21; tl = t.X * bR.M33 - t.Z * bR.M13; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M22 * bO.M33 - aO.M23 * bO.M32, aO.M23 * bO.M31 - aO.M21 * bO.M33, aO.M21 * bO.M32 - aO.M22 * bO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M32 * aO.M23 - bO.M33 * aO.M22, bO.M33 * aO.M21 - bO.M31 * aO.M23, bO.M31 * aO.M22 - bO.M32 * aO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M22 * bO.M33 - aO.M23 * bO.M32, aO.M23 * bO.M31 - aO.M21 * bO.M33, aO.M21 * bO.M32 - aO.M22 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M32 * aO.M23 - bO.M33 * aO.M22, bO.M33 * aO.M21 - bO.M31 * aO.M23, bO.M31 * aO.M22 - bO.M32 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion #region A.Z x B.() //A.Z x B.X rarb = aX * absBR.M21 + aY * absBR.M11 + bY * absBR.M33 + bZ * absBR.M32; tl = t.Y * bR.M11 - t.X * bR.M21; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M32 * bO.M13 - aO.M33 * bO.M12, aO.M33 * bO.M11 - aO.M31 * bO.M13, aO.M31 * bO.M12 - aO.M32 * bO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M12 * aO.M33 - bO.M13 * aO.M32, bO.M13 * aO.M31 - bO.M11 * aO.M33, bO.M11 * aO.M32 - bO.M12 * aO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M32 * bO.M13 - aO.M33 * bO.M12, aO.M33 * bO.M11 - aO.M31 * bO.M13, aO.M31 * bO.M12 - aO.M32 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M12 * aO.M33 - bO.M13 * aO.M32, bO.M13 * aO.M31 - bO.M11 * aO.M33, bO.M11 * aO.M32 - bO.M12 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Z x B.Y rarb = aX * absBR.M22 + aY * absBR.M12 + bX * absBR.M33 + bZ * absBR.M31; tl = t.Y * bR.M12 - t.X * bR.M22; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M32 * bO.M23 - aO.M33 * bO.M22, aO.M33 * bO.M21 - aO.M31 * bO.M23, aO.M31 * bO.M22 - aO.M32 * bO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M22 * aO.M33 - bO.M23 * aO.M32, bO.M23 * aO.M31 - bO.M21 * aO.M33, bO.M21 * aO.M32 - bO.M22 * aO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M32 * bO.M23 - aO.M33 * bO.M22, aO.M33 * bO.M21 - aO.M31 * bO.M23, aO.M31 * bO.M22 - aO.M32 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M22 * aO.M33 - bO.M23 * aO.M32, bO.M23 * aO.M31 - bO.M21 * aO.M33, bO.M21 * aO.M32 - bO.M22 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Z x B.Z rarb = aX * absBR.M23 + aY * absBR.M13 + bX * absBR.M32 + bY * absBR.M31; tl = t.Y * bR.M13 - t.X * bR.M23; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(aO.M32 * bO.M33 - aO.M33 * bO.M32, aO.M33 * bO.M31 - aO.M31 * bO.M33, aO.M31 * bO.M32 - aO.M32 * bO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M32 * aO.M33 - bO.M33 * aO.M32, bO.M33 * aO.M31 - bO.M31 * aO.M33, bO.M31 * aO.M32 - bO.M32 * aO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(aO.M32 * bO.M33 - aO.M33 * bO.M32, aO.M33 * bO.M31 - aO.M31 * bO.M33, aO.M31 * bO.M32 - aO.M32 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(bO.M32 * aO.M33 - bO.M33 * aO.M32, bO.M33 * aO.M31 - bO.M31 * aO.M33, bO.M31 * aO.M32 - bO.M32 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion distance = minimumDistance; axis = minimumAxis; return true; } #if ALLOWUNSAFE /// /// Determines if the two boxes are colliding and computes contact data. /// /// First box to collide. /// Second box to collide. /// Distance of separation or penetration. /// Axis of separation or penetration. /// Computed contact data. /// Transform to apply to shape A. /// Transform to apply to shape B. /// Whether or not the boxes collide. public static unsafe bool AreBoxesColliding(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB, out float distance, out Vector3 axis, out TinyStructList contactData) { BoxContactDataCache tempData; bool toReturn = AreBoxesColliding(a, b, ref transformA, ref transformB, out distance, out axis, out tempData); BoxContactData* dataPointer = &tempData.D1; contactData = new TinyStructList(); for (int i = 0; i < tempData.Count; i++) { contactData.Add(ref dataPointer[i]); } return toReturn; } #endif /// /// Determines if the two boxes are colliding and computes contact data. /// /// First box to collide. /// Second box to collide. /// Distance of separation or penetration. /// Axis of separation or penetration. /// Contact positions, depths, and ids. /// Transform to apply to shape A. /// Transform to apply to shape B. /// Whether or not the boxes collide. #if ALLOWUNSAFE public static bool AreBoxesColliding(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB, out float distance, out Vector3 axis, out BoxContactDataCache contactData) #else public static bool AreBoxesColliding(BoxShape a, BoxShape b, ref RigidTransform transformA, ref RigidTransform transformB, out float distance, out Vector3 axis, out TinyStructList contactData) #endif { float aX = a.HalfWidth; float aY = a.HalfHeight; float aZ = a.HalfLength; float bX = b.HalfWidth; float bY = b.HalfHeight; float bZ = b.HalfLength; #if ALLOWUNSAFE contactData = new BoxContactDataCache(); #else contactData = new TinyStructList(); #endif //Relative rotation from A to B. Matrix3x3 bR; Matrix3x3 aO; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out aO); Matrix3x3 bO; Matrix3x3.CreateFromQuaternion(ref transformB.Orientation, out bO); //Relative translation rotated into A's configuration space. Vector3 t; Vector3.Subtract(ref transformB.Position, ref transformA.Position, out t); float tempDistance; float minimumDistance = -float.MaxValue; var minimumAxis = new Vector3(); byte minimumFeature = 2; //2 means edge. 0-> A face, 1 -> B face. #region A Face Normals bR.M11 = aO.M11 * bO.M11 + aO.M12 * bO.M12 + aO.M13 * bO.M13; bR.M12 = aO.M11 * bO.M21 + aO.M12 * bO.M22 + aO.M13 * bO.M23; bR.M13 = aO.M11 * bO.M31 + aO.M12 * bO.M32 + aO.M13 * bO.M33; Matrix3x3 absBR; //Epsilons are added to deal with near-parallel edges. absBR.M11 = Math.Abs(bR.M11) + Toolbox.Epsilon; absBR.M12 = Math.Abs(bR.M12) + Toolbox.Epsilon; absBR.M13 = Math.Abs(bR.M13) + Toolbox.Epsilon; float tX = t.X; t.X = t.X * aO.M11 + t.Y * aO.M12 + t.Z * aO.M13; //Test the axes defines by entity A's rotation matrix. //A.X float rarb = aX + bX * absBR.M11 + bY * absBR.M12 + bZ * absBR.M13; if (t.X > rarb) { distance = t.X - rarb; axis = new Vector3(-aO.M11, -aO.M12, -aO.M13); return false; } if (t.X < -rarb) { distance = -t.X - rarb; axis = new Vector3(aO.M11, aO.M12, aO.M13); return false; } //Inside if (t.X > 0) { tempDistance = t.X - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M11, -aO.M12, -aO.M13); minimumFeature = 0; } } else { tempDistance = -t.X - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M11, aO.M12, aO.M13); minimumFeature = 0; } } bR.M21 = aO.M21 * bO.M11 + aO.M22 * bO.M12 + aO.M23 * bO.M13; bR.M22 = aO.M21 * bO.M21 + aO.M22 * bO.M22 + aO.M23 * bO.M23; bR.M23 = aO.M21 * bO.M31 + aO.M22 * bO.M32 + aO.M23 * bO.M33; absBR.M21 = Math.Abs(bR.M21) + Toolbox.Epsilon; absBR.M22 = Math.Abs(bR.M22) + Toolbox.Epsilon; absBR.M23 = Math.Abs(bR.M23) + Toolbox.Epsilon; float tY = t.Y; t.Y = tX * aO.M21 + t.Y * aO.M22 + t.Z * aO.M23; //A.Y rarb = aY + bX * absBR.M21 + bY * absBR.M22 + bZ * absBR.M23; if (t.Y > rarb) { distance = t.Y - rarb; axis = new Vector3(-aO.M21, -aO.M22, -aO.M23); return false; } if (t.Y < -rarb) { distance = -t.Y - rarb; axis = new Vector3(aO.M21, aO.M22, aO.M23); return false; } //Inside if (t.Y > 0) { tempDistance = t.Y - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M21, -aO.M22, -aO.M23); minimumFeature = 0; } } else { tempDistance = -t.Y - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M21, aO.M22, aO.M23); minimumFeature = 0; } } bR.M31 = aO.M31 * bO.M11 + aO.M32 * bO.M12 + aO.M33 * bO.M13; bR.M32 = aO.M31 * bO.M21 + aO.M32 * bO.M22 + aO.M33 * bO.M23; bR.M33 = aO.M31 * bO.M31 + aO.M32 * bO.M32 + aO.M33 * bO.M33; absBR.M31 = Math.Abs(bR.M31) + Toolbox.Epsilon; absBR.M32 = Math.Abs(bR.M32) + Toolbox.Epsilon; absBR.M33 = Math.Abs(bR.M33) + Toolbox.Epsilon; t.Z = tX * aO.M31 + tY * aO.M32 + t.Z * aO.M33; //A.Z rarb = aZ + bX * absBR.M31 + bY * absBR.M32 + bZ * absBR.M33; if (t.Z > rarb) { distance = t.Z - rarb; axis = new Vector3(-aO.M31, -aO.M32, -aO.M33); return false; } if (t.Z < -rarb) { distance = -t.Z - rarb; axis = new Vector3(aO.M31, aO.M32, aO.M33); return false; } //Inside if (t.Z > 0) { tempDistance = t.Z - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-aO.M31, -aO.M32, -aO.M33); minimumFeature = 0; } } else { tempDistance = -t.Z - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(aO.M31, aO.M32, aO.M33); minimumFeature = 0; } } #endregion const float antiBBias = .01f; minimumDistance += antiBBias; #region B Face Normals //Test the axes defines by entity B's rotation matrix. //B.X rarb = bX + aX * absBR.M11 + aY * absBR.M21 + aZ * absBR.M31; float tl = t.X * bR.M11 + t.Y * bR.M21 + t.Z * bR.M31; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(-bO.M11, -bO.M12, -bO.M13); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M11, bO.M12, bO.M13); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M11, -bO.M12, -bO.M13); minimumFeature = 1; } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M11, bO.M12, bO.M13); minimumFeature = 1; } } //B.Y rarb = bY + aX * absBR.M12 + aY * absBR.M22 + aZ * absBR.M32; tl = t.X * bR.M12 + t.Y * bR.M22 + t.Z * bR.M32; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(-bO.M21, -bO.M22, -bO.M23); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M21, bO.M22, bO.M23); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M21, -bO.M22, -bO.M23); minimumFeature = 1; } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M21, bO.M22, bO.M23); minimumFeature = 1; } } //B.Z rarb = bZ + aX * absBR.M13 + aY * absBR.M23 + aZ * absBR.M33; tl = t.X * bR.M13 + t.Y * bR.M23 + t.Z * bR.M33; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(-bO.M31, -bO.M32, -bO.M33); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(bO.M31, bO.M32, bO.M33); return false; } //Inside if (tl > 0) { tempDistance = tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(-bO.M31, -bO.M32, -bO.M33); minimumFeature = 1; } } else { tempDistance = -tl - rarb; if (tempDistance > minimumDistance) { minimumDistance = tempDistance; minimumAxis = new Vector3(bO.M31, bO.M32, bO.M33); minimumFeature = 1; } } #endregion if (minimumFeature != 1) minimumDistance -= antiBBias; float antiEdgeBias = .01f; minimumDistance += antiEdgeBias; float axisLengthInverse; Vector3 tempAxis; #region A.X x B.() //Now for the edge-edge cases. //A.X x B.X rarb = aY * absBR.M31 + aZ * absBR.M21 + bY * absBR.M13 + bZ * absBR.M12; tl = t.Z * bR.M21 - t.Y * bR.M31; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M12 * aO.M13 - bO.M13 * aO.M12, bO.M13 * aO.M11 - bO.M11 * aO.M13, bO.M11 * aO.M12 - bO.M12 * aO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M12 * bO.M13 - aO.M13 * bO.M12, aO.M13 * bO.M11 - aO.M11 * bO.M13, aO.M11 * bO.M12 - aO.M12 * bO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M12 * aO.M13 - bO.M13 * aO.M12, bO.M13 * aO.M11 - bO.M11 * aO.M13, bO.M11 * aO.M12 - bO.M12 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M12 * bO.M13 - aO.M13 * bO.M12, aO.M13 * bO.M11 - aO.M11 * bO.M13, aO.M11 * bO.M12 - aO.M12 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.X x B.Y rarb = aY * absBR.M32 + aZ * absBR.M22 + bX * absBR.M13 + bZ * absBR.M11; tl = t.Z * bR.M22 - t.Y * bR.M32; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M22 * aO.M13 - bO.M23 * aO.M12, bO.M23 * aO.M11 - bO.M21 * aO.M13, bO.M21 * aO.M12 - bO.M22 * aO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M12 * bO.M23 - aO.M13 * bO.M22, aO.M13 * bO.M21 - aO.M11 * bO.M23, aO.M11 * bO.M22 - aO.M12 * bO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M22 * aO.M13 - bO.M23 * aO.M12, bO.M23 * aO.M11 - bO.M21 * aO.M13, bO.M21 * aO.M12 - bO.M22 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M12 * bO.M23 - aO.M13 * bO.M22, aO.M13 * bO.M21 - aO.M11 * bO.M23, aO.M11 * bO.M22 - aO.M12 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.X x B.Z rarb = aY * absBR.M33 + aZ * absBR.M23 + bX * absBR.M12 + bY * absBR.M11; tl = t.Z * bR.M23 - t.Y * bR.M33; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M32 * aO.M13 - bO.M33 * aO.M12, bO.M33 * aO.M11 - bO.M31 * aO.M13, bO.M31 * aO.M12 - bO.M32 * aO.M11); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M12 * bO.M33 - aO.M13 * bO.M32, aO.M13 * bO.M31 - aO.M11 * bO.M33, aO.M11 * bO.M32 - aO.M12 * bO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M32 * aO.M13 - bO.M33 * aO.M12, bO.M33 * aO.M11 - bO.M31 * aO.M13, bO.M31 * aO.M12 - bO.M32 * aO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M12 * bO.M33 - aO.M13 * bO.M32, aO.M13 * bO.M31 - aO.M11 * bO.M33, aO.M11 * bO.M32 - aO.M12 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion #region A.Y x B.() //A.Y x B.X rarb = aX * absBR.M31 + aZ * absBR.M11 + bY * absBR.M23 + bZ * absBR.M22; tl = t.X * bR.M31 - t.Z * bR.M11; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M12 * aO.M23 - bO.M13 * aO.M22, bO.M13 * aO.M21 - bO.M11 * aO.M23, bO.M11 * aO.M22 - bO.M12 * aO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M22 * bO.M13 - aO.M23 * bO.M12, aO.M23 * bO.M11 - aO.M21 * bO.M13, aO.M21 * bO.M12 - aO.M22 * bO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M12 * aO.M23 - bO.M13 * aO.M22, bO.M13 * aO.M21 - bO.M11 * aO.M23, bO.M11 * aO.M22 - bO.M12 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M22 * bO.M13 - aO.M23 * bO.M12, aO.M23 * bO.M11 - aO.M21 * bO.M13, aO.M21 * bO.M12 - aO.M22 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Y x B.Y rarb = aX * absBR.M32 + aZ * absBR.M12 + bX * absBR.M23 + bZ * absBR.M21; tl = t.X * bR.M32 - t.Z * bR.M12; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M22 * aO.M23 - bO.M23 * aO.M22, bO.M23 * aO.M21 - bO.M21 * aO.M23, bO.M21 * aO.M22 - bO.M22 * aO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M22 * bO.M23 - aO.M23 * bO.M22, aO.M23 * bO.M21 - aO.M21 * bO.M23, aO.M21 * bO.M22 - aO.M22 * bO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M22 * aO.M23 - bO.M23 * aO.M22, bO.M23 * aO.M21 - bO.M21 * aO.M23, bO.M21 * aO.M22 - bO.M22 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M22 * bO.M23 - aO.M23 * bO.M22, aO.M23 * bO.M21 - aO.M21 * bO.M23, aO.M21 * bO.M22 - aO.M22 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Y x B.Z rarb = aX * absBR.M33 + aZ * absBR.M13 + bX * absBR.M22 + bY * absBR.M21; tl = t.X * bR.M33 - t.Z * bR.M13; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M32 * aO.M23 - bO.M33 * aO.M22, bO.M33 * aO.M21 - bO.M31 * aO.M23, bO.M31 * aO.M22 - bO.M32 * aO.M21); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M22 * bO.M33 - aO.M23 * bO.M32, aO.M23 * bO.M31 - aO.M21 * bO.M33, aO.M21 * bO.M32 - aO.M22 * bO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M32 * aO.M23 - bO.M33 * aO.M22, bO.M33 * aO.M21 - bO.M31 * aO.M23, bO.M31 * aO.M22 - bO.M32 * aO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M22 * bO.M33 - aO.M23 * bO.M32, aO.M23 * bO.M31 - aO.M21 * bO.M33, aO.M21 * bO.M32 - aO.M22 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion #region A.Z x B.() //A.Z x B.X rarb = aX * absBR.M21 + aY * absBR.M11 + bY * absBR.M33 + bZ * absBR.M32; tl = t.Y * bR.M11 - t.X * bR.M21; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M12 * aO.M33 - bO.M13 * aO.M32, bO.M13 * aO.M31 - bO.M11 * aO.M33, bO.M11 * aO.M32 - bO.M12 * aO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M32 * bO.M13 - aO.M33 * bO.M12, aO.M33 * bO.M11 - aO.M31 * bO.M13, aO.M31 * bO.M12 - aO.M32 * bO.M11); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M12 * aO.M33 - bO.M13 * aO.M32, bO.M13 * aO.M31 - bO.M11 * aO.M33, bO.M11 * aO.M32 - bO.M12 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M32 * bO.M13 - aO.M33 * bO.M12, aO.M33 * bO.M11 - aO.M31 * bO.M13, aO.M31 * bO.M12 - aO.M32 * bO.M11); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Z x B.Y rarb = aX * absBR.M22 + aY * absBR.M12 + bX * absBR.M33 + bZ * absBR.M31; tl = t.Y * bR.M12 - t.X * bR.M22; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M22 * aO.M33 - bO.M23 * aO.M32, bO.M23 * aO.M31 - bO.M21 * aO.M33, bO.M21 * aO.M32 - bO.M22 * aO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M32 * bO.M23 - aO.M33 * bO.M22, aO.M33 * bO.M21 - aO.M31 * bO.M23, aO.M31 * bO.M22 - aO.M32 * bO.M21); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M22 * aO.M33 - bO.M23 * aO.M32, bO.M23 * aO.M31 - bO.M21 * aO.M33, bO.M21 * aO.M32 - bO.M22 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M32 * bO.M23 - aO.M33 * bO.M22, aO.M33 * bO.M21 - aO.M31 * bO.M23, aO.M31 * bO.M22 - aO.M32 * bO.M21); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } //A.Z x B.Z rarb = aX * absBR.M23 + aY * absBR.M13 + bX * absBR.M32 + bY * absBR.M31; tl = t.Y * bR.M13 - t.X * bR.M23; if (tl > rarb) { distance = tl - rarb; axis = new Vector3(bO.M32 * aO.M33 - bO.M33 * aO.M32, bO.M33 * aO.M31 - bO.M31 * aO.M33, bO.M31 * aO.M32 - bO.M32 * aO.M31); return false; } if (tl < -rarb) { distance = -tl - rarb; axis = new Vector3(aO.M32 * bO.M33 - aO.M33 * bO.M32, aO.M33 * bO.M31 - aO.M31 * bO.M33, aO.M31 * bO.M32 - aO.M32 * bO.M31); return false; } //Inside if (tl > 0) { tempAxis = new Vector3(bO.M32 * aO.M33 - bO.M33 * aO.M32, bO.M33 * aO.M31 - bO.M31 * aO.M33, bO.M31 * aO.M32 - bO.M32 * aO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } else { tempAxis = new Vector3(aO.M32 * bO.M33 - aO.M33 * bO.M32, aO.M33 * bO.M31 - aO.M31 * bO.M33, aO.M31 * bO.M32 - aO.M32 * bO.M31); axisLengthInverse = 1 / tempAxis.Length(); tempDistance = (-tl - rarb) * axisLengthInverse; if (tempDistance > minimumDistance) { minimumFeature = 2; minimumDistance = tempDistance; tempAxis.X *= axisLengthInverse; tempAxis.Y *= axisLengthInverse; tempAxis.Z *= axisLengthInverse; minimumAxis = tempAxis; } } #endregion if (minimumFeature == 2) { //Edge-edge contact conceptually only has one contact, but allowing it to create multiple due to penetration is more robust. GetEdgeEdgeContact(a, b, ref transformA.Position, ref aO, ref transformB.Position, ref bO, minimumDistance, ref minimumAxis, out contactData); //Vector3 position; //float depth; //int id; // GetEdgeEdgeContact(a, b, ref transformA.Position, ref aO, ref transformB.Position, ref bO, ref minimumAxis, out position, out id); //#if ALLOWUNSAFE // contactData.D1.Position = position; // contactData.D1.Depth = minimumDistance; // contactData.D1.Id = id; // contactData.Count = 1; //#else // var toAdd = new BoxContactData(); // toAdd.Position = position; // toAdd.Depth = minimumDistance; // toAdd.Id = id; // contactData.Add(ref toAdd); //#endif } else { minimumDistance -= antiEdgeBias; GetFaceContacts(a, b, ref transformA.Position, ref aO, ref transformB.Position, ref bO, minimumFeature == 0, ref minimumAxis, out contactData); } distance = minimumDistance; axis = minimumAxis; return true; } #if ALLOWUNSAFE internal static void GetEdgeEdgeContact(BoxShape a, BoxShape b, ref Vector3 positionA, ref Matrix3x3 orientationA, ref Vector3 positionB, ref Matrix3x3 orientationB, float depth, ref Vector3 mtd, out BoxContactDataCache contactData) #else internal static void GetEdgeEdgeContact(BoxShape a, BoxShape b, ref Vector3 positionA, ref Matrix3X3 orientationA, ref Vector3 positionB, ref Matrix3X3 orientationB, float depth, ref Vector3 mtd, out TinyStructList contactData) #endif { //Edge-edge contacts conceptually can only create one contact in perfectly rigid collisions. //However, this is a discrete approximation of rigidity; things can penetrate each other. //If edge-edge only returns a single contact, there's a good chance that the box will get into //an oscillating state when under pressure. //To avoid the oscillation, we may sometimes need two edge contacts. //To determine which edges to use, compute 8 dot products. //One for each edge parallel to the contributing axis on each of the two shapes. //The resulting cases are: //One edge on A touching one edge on B. //Two edges on A touching one edge on B. //One edge on A touching two edges on B. //Two edges on A touching two edges on B. //The three latter cases SHOULD be covered by the face-contact system, but in practice, //they are not sufficiently covered because the system decides that the single edge-edge pair //should be used and drops the other contacts, producting the aforementioned oscillation. //All edge cross products result in the MTD, so no recalculation is necessary. //Of the four edges which are aligned with the local edge axis, pick the two //who have vertices which, when dotted with the local mtd, are greatest. //Compute the closest points between each edge pair. For two edges each, //this comes out to four total closest point tests. //This is not a traditional closest point between segments test. //Completely ignore the pair if the closest points turn out to be beyond the intervals of the segments. //Use the offsets found from each test. //Test the A to B offset against the MTD, which is also known to be oriented in a certain way. //That known directionality allows easy computation of depth using MTD dot offset. //Do not use any contacts which have negative depth/positive distance. //Put the minimum translation direction into the local space of each object. Vector3 mtdA, mtdB; Vector3 negatedMtd; Vector3.Negate(ref mtd, out negatedMtd); Matrix3x3.TransformTranspose(ref negatedMtd, ref orientationA, out mtdA); Matrix3x3.TransformTranspose(ref mtd, ref orientationB, out mtdB); #if !WINDOWS Vector3 edgeAStart1 = new Vector3(), edgeAEnd1 = new Vector3(), edgeAStart2 = new Vector3(), edgeAEnd2 = new Vector3(); Vector3 edgeBStart1 = new Vector3(), edgeBEnd1 = new Vector3(), edgeBStart2 = new Vector3(), edgeBEnd2 = new Vector3(); #else Vector3 edgeAStart1, edgeAEnd1, edgeAStart2, edgeAEnd2; Vector3 edgeBStart1, edgeBEnd1, edgeBStart2, edgeBEnd2; #endif float aHalfWidth = a.halfWidth; float aHalfHeight = a.halfHeight; float aHalfLength = a.halfLength; float bHalfWidth = b.halfWidth; float bHalfHeight = b.halfHeight; float bHalfLength = b.halfLength; //Letter stands for owner. Number stands for edge (1 or 2). int edgeAStart1Id, edgeAEnd1Id, edgeAStart2Id, edgeAEnd2Id; int edgeBStart1Id, edgeBEnd1Id, edgeBStart2Id, edgeBEnd2Id; //This is an edge-edge collision, so one (AND ONLY ONE) of the components in the //local direction must be very close to zero. We can use an arbitrary fixed //epsilon because the mtd is always unit length. #region Edge A if (Math.Abs(mtdA.X) < Toolbox.Epsilon) { //mtd is in the Y-Z plane. //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdA. var dots = new TinyList(); dots.Add(-aHalfHeight * mtdA.Y - aHalfLength * mtdA.Z); dots.Add(-aHalfHeight * mtdA.Y + aHalfLength * mtdA.Z); dots.Add(aHalfHeight * mtdA.Y - aHalfLength * mtdA.Z); dots.Add(aHalfHeight * mtdA.Y + aHalfLength * mtdA.Z); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 0, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart1, out edgeAEnd1, out edgeAStart1Id, out edgeAEnd1Id); GetEdgeData(secondHighestIndex, 0, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart2, out edgeAEnd2, out edgeAStart2Id, out edgeAEnd2Id); } else if (Math.Abs(mtdA.Y) < Toolbox.Epsilon) { //mtd is in the X-Z plane //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdA. var dots = new TinyList(); dots.Add(-aHalfWidth * mtdA.X - aHalfLength * mtdA.Z); dots.Add(-aHalfWidth * mtdA.X + aHalfLength * mtdA.Z); dots.Add(aHalfWidth * mtdA.X - aHalfLength * mtdA.Z); dots.Add(aHalfWidth * mtdA.X + aHalfLength * mtdA.Z); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 1, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart1, out edgeAEnd1, out edgeAStart1Id, out edgeAEnd1Id); GetEdgeData(secondHighestIndex, 1, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart2, out edgeAEnd2, out edgeAStart2Id, out edgeAEnd2Id); } else { //mtd is in the X-Y plane //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdA. var dots = new TinyList(); dots.Add(-aHalfWidth * mtdA.X - aHalfHeight * mtdA.Y); dots.Add(-aHalfWidth * mtdA.X + aHalfHeight * mtdA.Y); dots.Add(aHalfWidth * mtdA.X - aHalfHeight * mtdA.Y); dots.Add(aHalfWidth * mtdA.X + aHalfHeight * mtdA.Y); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 2, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart1, out edgeAEnd1, out edgeAStart1Id, out edgeAEnd1Id); GetEdgeData(secondHighestIndex, 2, aHalfWidth, aHalfHeight, aHalfLength, out edgeAStart2, out edgeAEnd2, out edgeAStart2Id, out edgeAEnd2Id); } #endregion #region Edge B if (Math.Abs(mtdB.X) < Toolbox.Epsilon) { //mtd is in the Y-Z plane. //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdB. var dots = new TinyList(); dots.Add(-bHalfHeight * mtdB.Y - bHalfLength * mtdB.Z); dots.Add(-bHalfHeight * mtdB.Y + bHalfLength * mtdB.Z); dots.Add(bHalfHeight * mtdB.Y - bHalfLength * mtdB.Z); dots.Add(bHalfHeight * mtdB.Y + bHalfLength * mtdB.Z); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 0, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart1, out edgeBEnd1, out edgeBStart1Id, out edgeBEnd1Id); GetEdgeData(secondHighestIndex, 0, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart2, out edgeBEnd2, out edgeBStart2Id, out edgeBEnd2Id); } else if (Math.Abs(mtdB.Y) < Toolbox.Epsilon) { //mtd is in the X-Z plane //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdB. var dots = new TinyList(); dots.Add(-bHalfWidth * mtdB.X - bHalfLength * mtdB.Z); dots.Add(-bHalfWidth * mtdB.X + bHalfLength * mtdB.Z); dots.Add(bHalfWidth * mtdB.X - bHalfLength * mtdB.Z); dots.Add(bHalfWidth * mtdB.X + bHalfLength * mtdB.Z); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 1, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart1, out edgeBEnd1, out edgeBStart1Id, out edgeBEnd1Id); GetEdgeData(secondHighestIndex, 1, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart2, out edgeBEnd2, out edgeBStart2Id, out edgeBEnd2Id); } else { //mtd is in the X-Y plane //Perform an implicit dot with the edge location relative to the center. //Find the two edges furthest in the direction of the mtdB. var dots = new TinyList(); dots.Add(-bHalfWidth * mtdB.X - bHalfHeight * mtdB.Y); dots.Add(-bHalfWidth * mtdB.X + bHalfHeight * mtdB.Y); dots.Add(bHalfWidth * mtdB.X - bHalfHeight * mtdB.Y); dots.Add(bHalfWidth * mtdB.X + bHalfHeight * mtdB.Y); //Find the first and second highest indices. int highestIndex, secondHighestIndex; FindHighestIndices(ref dots, out highestIndex, out secondHighestIndex); //Use the indices to compute the edges. GetEdgeData(highestIndex, 2, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart1, out edgeBEnd1, out edgeBStart1Id, out edgeBEnd1Id); GetEdgeData(secondHighestIndex, 2, bHalfWidth, bHalfHeight, bHalfLength, out edgeBStart2, out edgeBEnd2, out edgeBStart2Id, out edgeBEnd2Id); } #endregion Matrix3x3.Transform(ref edgeAStart1, ref orientationA, out edgeAStart1); Matrix3x3.Transform(ref edgeAEnd1, ref orientationA, out edgeAEnd1); Matrix3x3.Transform(ref edgeBStart1, ref orientationB, out edgeBStart1); Matrix3x3.Transform(ref edgeBEnd1, ref orientationB, out edgeBEnd1); Matrix3x3.Transform(ref edgeAStart2, ref orientationA, out edgeAStart2); Matrix3x3.Transform(ref edgeAEnd2, ref orientationA, out edgeAEnd2); Matrix3x3.Transform(ref edgeBStart2, ref orientationB, out edgeBStart2); Matrix3x3.Transform(ref edgeBEnd2, ref orientationB, out edgeBEnd2); Vector3.Add(ref edgeAStart1, ref positionA, out edgeAStart1); Vector3.Add(ref edgeAEnd1, ref positionA, out edgeAEnd1); Vector3.Add(ref edgeBStart1, ref positionB, out edgeBStart1); Vector3.Add(ref edgeBEnd1, ref positionB, out edgeBEnd1); Vector3.Add(ref edgeAStart2, ref positionA, out edgeAStart2); Vector3.Add(ref edgeAEnd2, ref positionA, out edgeAEnd2); Vector3.Add(ref edgeBStart2, ref positionB, out edgeBStart2); Vector3.Add(ref edgeBEnd2, ref positionB, out edgeBEnd2); Vector3 onA, onB; Vector3 offset; float dot; #if ALLOWUNSAFE var tempContactData = new BoxContactDataCache(); unsafe { var contactDataPointer = &tempContactData.D1; #else contactData = new TinyStructList(); #endif //Go through the pairs and add any contacts with positive depth that are within the segments' intervals. if (GetClosestPointsBetweenSegments(ref edgeAStart1, ref edgeAEnd1, ref edgeBStart1, ref edgeBEnd1, out onA, out onB)) { Vector3.Subtract(ref onA, ref onB, out offset); Vector3.Dot(ref offset, ref mtd, out dot); if (dot < 0) //Distance must be negative. { BoxContactData data; data.Position = onA; data.Depth = dot; data.Id = GetContactId(edgeAStart1Id, edgeAEnd1Id, edgeBStart1Id, edgeBEnd1Id); #if ALLOWUNSAFE contactDataPointer[tempContactData.Count] = data; tempContactData.Count++; #else contactData.Add(ref data); #endif } } if (GetClosestPointsBetweenSegments(ref edgeAStart1, ref edgeAEnd1, ref edgeBStart2, ref edgeBEnd2, out onA, out onB)) { Vector3.Subtract(ref onA, ref onB, out offset); Vector3.Dot(ref offset, ref mtd, out dot); if (dot < 0) //Distance must be negative. { BoxContactData data; data.Position = onA; data.Depth = dot; data.Id = GetContactId(edgeAStart1Id, edgeAEnd1Id, edgeBStart2Id, edgeBEnd2Id); #if ALLOWUNSAFE contactDataPointer[tempContactData.Count] = data; tempContactData.Count++; #else contactData.Add(ref data); #endif } } if (GetClosestPointsBetweenSegments(ref edgeAStart2, ref edgeAEnd2, ref edgeBStart1, ref edgeBEnd1, out onA, out onB)) { Vector3.Subtract(ref onA, ref onB, out offset); Vector3.Dot(ref offset, ref mtd, out dot); if (dot < 0) //Distance must be negative. { BoxContactData data; data.Position = onA; data.Depth = dot; data.Id = GetContactId(edgeAStart2Id, edgeAEnd2Id, edgeBStart1Id, edgeBEnd1Id); #if ALLOWUNSAFE contactDataPointer[tempContactData.Count] = data; tempContactData.Count++; #else contactData.Add(ref data); #endif } } if (GetClosestPointsBetweenSegments(ref edgeAStart2, ref edgeAEnd2, ref edgeBStart2, ref edgeBEnd2, out onA, out onB)) { Vector3.Subtract(ref onA, ref onB, out offset); Vector3.Dot(ref offset, ref mtd, out dot); if (dot < 0) //Distance must be negative. { BoxContactData data; data.Position = onA; data.Depth = dot; data.Id = GetContactId(edgeAStart2Id, edgeAEnd2Id, edgeBStart2Id, edgeBEnd2Id); #if ALLOWUNSAFE contactDataPointer[tempContactData.Count] = data; tempContactData.Count++; #else contactData.Add(ref data); #endif } } #if ALLOWUNSAFE } contactData = tempContactData; #endif } private static void GetEdgeData(int index, int axis, float x, float y, float z, out Vector3 edgeStart, out Vector3 edgeEnd, out int edgeStartId, out int edgeEndId) { //Index defines which edge to use. //They follow this pattern: //0: -- //1: -+ //2: +- //3: ++ //The axis index determines the dimensions to use. //0: plane with normal X //1: plane with normal Y //2: plane with normal Z #if !WINDOWS edgeStart = new Vector3(); edgeEnd = new Vector3(); #endif switch (index + axis * 4) { case 0: //X-- edgeStart.X = -x; edgeStart.Y = -y; edgeStart.Z = -z; edgeStartId = 0; //000 edgeEnd.X = x; edgeEnd.Y = -y; edgeEnd.Z = -z; edgeEndId = 4; //100 break; case 1: //X-+ edgeStart.X = -x; edgeStart.Y = -y; edgeStart.Z = z; edgeStartId = 1; //001 edgeEnd.X = x; edgeEnd.Y = -y; edgeEnd.Z = z; edgeEndId = 5; //101 break; case 2: //X+- edgeStart.X = -x; edgeStart.Y = y; edgeStart.Z = -z; edgeStartId = 2; //010 edgeEnd.X = x; edgeEnd.Y = y; edgeEnd.Z = -z; edgeEndId = 6; //110 break; case 3: //X++ edgeStart.X = -x; edgeStart.Y = y; edgeStart.Z = z; edgeStartId = 3; //011 edgeEnd.X = x; edgeEnd.Y = y; edgeEnd.Z = z; edgeEndId = 7; //111 break; case 4: //-Y- edgeStart.X = -x; edgeStart.Y = -y; edgeStart.Z = -z; edgeStartId = 0; //000 edgeEnd.X = -x; edgeEnd.Y = y; edgeEnd.Z = -z; edgeEndId = 2; //010 break; case 5: //-Y+ edgeStart.X = -x; edgeStart.Y = -y; edgeStart.Z = z; edgeStartId = 1; //001 edgeEnd.X = -x; edgeEnd.Y = y; edgeEnd.Z = z; edgeEndId = 3; //011 break; case 6: //+Y- edgeStart.X = x; edgeStart.Y = -y; edgeStart.Z = -z; edgeStartId = 4; //100 edgeEnd.X = x; edgeEnd.Y = y; edgeEnd.Z = -z; edgeEndId = 6; //110 break; case 7: //+Y+ edgeStart.X = x; edgeStart.Y = -y; edgeStart.Z = z; edgeStartId = 5; //101 edgeEnd.X = x; edgeEnd.Y = y; edgeEnd.Z = z; edgeEndId = 7; //111 break; case 8: //--Z edgeStart.X = -x; edgeStart.Y = -y; edgeStart.Z = -z; edgeStartId = 0; //000 edgeEnd.X = -x; edgeEnd.Y = -y; edgeEnd.Z = z; edgeEndId = 1; //001 break; case 9: //-+Z edgeStart.X = -x; edgeStart.Y = y; edgeStart.Z = -z; edgeStartId = 2; //010 edgeEnd.X = -x; edgeEnd.Y = y; edgeEnd.Z = z; edgeEndId = 3; //011 break; case 10: //+-Z edgeStart.X = x; edgeStart.Y = -y; edgeStart.Z = -z; edgeStartId = 4; //100 edgeEnd.X = x; edgeEnd.Y = -y; edgeEnd.Z = z; edgeEndId = 5; //101 break; case 11: //++Z edgeStart.X = x; edgeStart.Y = y; edgeStart.Z = -z; edgeStartId = 6; //110 edgeEnd.X = x; edgeEnd.Y = y; edgeEnd.Z = z; edgeEndId = 7; //111 break; default: throw new ArgumentException("Invalid index or axis."); } } static void FindHighestIndices(ref TinyList dots, out int highestIndex, out int secondHighestIndex) { highestIndex = 0; float highestValue = dots[0]; for (int i = 1; i < 4; i++) { float dot = dots[i]; if (dot > highestValue) { highestIndex = i; highestValue = dot; } } secondHighestIndex = 0; float secondHighestValue = -float.MaxValue; for (int i = 0; i < 4; i++) { float dot = dots[i]; if (i != highestIndex && dot > secondHighestValue) { secondHighestIndex = i; secondHighestValue = dot; } } } /// /// Computes closest points c1 and c2 betwen segments p1q1 and p2q2. /// /// First point of first segment. /// Second point of first segment. /// First point of second segment. /// Second point of second segment. /// Closest point on first segment. /// Closest point on second segment. static bool GetClosestPointsBetweenSegments(ref Vector3 p1, ref Vector3 q1, ref Vector3 p2, ref Vector3 q2, out Vector3 c1, out Vector3 c2) { //Segment direction vectors Vector3 d1; Vector3.Subtract(ref q1, ref p1, out d1); Vector3 d2; Vector3.Subtract(ref q2, ref p2, out d2); Vector3 r; Vector3.Subtract(ref p1, ref p2, out r); //distance float a = d1.LengthSquared(); float e = d2.LengthSquared(); float f; Vector3.Dot(ref d2, ref r, out f); float s, t; if (a <= Toolbox.Epsilon && e <= Toolbox.Epsilon) { //These segments are more like points. c1 = p1; c2 = p2; return false; } if (a <= Toolbox.Epsilon) { // First segment is basically a point. s = 0.0f; t = f / e; if (t < 0 || t > 1) { c1 = new Vector3(); c2 = new Vector3(); return false; } } else { float c = Vector3.Dot(d1, r); if (e <= Toolbox.Epsilon) { // Second segment is basically a point. t = 0.0f; s = MathHelper.Clamp(-c / a, 0.0f, 1.0f); } else { float b = Vector3.Dot(d1, d2); float denom = a * e - b * b; // If segments not parallel, compute closest point on L1 to L2, and // clamp to segment S1. Else pick some s (here .5f) if (denom != 0.0f) { s = (b * f - c * e) / denom; if (s < 0 || s > 1) { //Closest point would be outside of the segment. c1 = new Vector3(); c2 = new Vector3(); return false; } } else //Parallel, just use .5f s = .5f; t = (b * s + f) / e; if (t < 0 || t > 1) { //Closest point would be outside of the segment. c1 = new Vector3(); c2 = new Vector3(); return false; } } } Vector3.Multiply(ref d1, s, out c1); Vector3.Add(ref c1, ref p1, out c1); Vector3.Multiply(ref d2, t, out c2); Vector3.Add(ref c2, ref p2, out c2); return true; } // internal static void GetEdgeEdgeContact(BoxShape a, BoxShape b, ref Vector3 positionA, ref Matrix3X3 orientationA, ref Vector3 positionB, ref Matrix3X3 orientationB, float depth, ref Vector3 mtd, out TinyStructList contactData) // { // //Put the minimum translation direction into the local space of each object. // Vector3 mtdA, mtdB; // Vector3 negatedMtd; // Vector3.Negate(ref mtd, out negatedMtd); // Matrix3X3.TransformTranspose(ref negatedMtd, ref orientationA, out mtdA); // Matrix3X3.TransformTranspose(ref mtd, ref orientationB, out mtdB); //#if !WINDOWS // Vector3 edgeA1 = new Vector3(), edgeA2 = new Vector3(); // Vector3 edgeB1 = new Vector3(), edgeB2 = new Vector3(); //#else // Vector3 edgeA1, edgeA2; // Vector3 edgeB1, edgeB2; //#endif // float aHalfWidth = a.halfWidth; // float aHalfHeight = a.halfHeight; // float aHalfLength = a.halfLength; // float bHalfWidth = b.halfWidth; // float bHalfHeight = b.halfHeight; // float bHalfLength = b.halfLength; // int edgeA1Id, edgeA2Id; // int edgeB1Id, edgeB2Id; // //This is an edge-edge collision, so one (AND ONLY ONE) of the components in the // //local direction must be very close to zero. We can use an arbitrary fixed // //epsilon because the mtd is always unit length. // #region Edge A // if (Math.Abs(mtdA.X) < Toolbox.Epsilon) // { // //mtd is in the Y-Z plane. // if (mtdA.Y > 0) // { // if (mtdA.Z > 0) // { // //++ // edgeA1.X = -aHalfWidth; // edgeA1.Y = aHalfHeight; // edgeA1.Z = aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 6; // edgeA2Id = 7; // } // else // { // //+- // edgeA1.X = -aHalfWidth; // edgeA1.Y = aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = -aHalfLength; // edgeA1Id = 2; // edgeA2Id = 3; // } // } // else // { // if (mtdA.Z > 0) // { // //-+ // edgeA1.X = -aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = -aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 4; // edgeA2Id = 5; // } // else // { // //-- // edgeA1.X = -aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = -aHalfHeight; // edgeA2.Z = -aHalfLength; // edgeA1Id = 0; // edgeA2Id = 1; // } // } // } // else if (Math.Abs(mtdA.Y) < Toolbox.Epsilon) // { // //mtd is in the X-Z plane // if (mtdA.X > 0) // { // if (mtdA.Z > 0) // { // //++ // edgeA1.X = aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 5; // edgeA2Id = 7; // } // else // { // //+- // edgeA1.X = aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = -aHalfLength; // edgeA1Id = 1; // edgeA2Id = 3; // } // } // else // { // if (mtdA.Z > 0) // { // //-+ // edgeA1.X = -aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = aHalfLength; // edgeA2.X = -aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 4; // edgeA2Id = 6; // } // else // { // //-- // edgeA1.X = -aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = -aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = -aHalfLength; // edgeA1Id = 0; // edgeA2Id = 2; // } // } // } // else // { // //mtd is in the X-Y plane // if (mtdA.X > 0) // { // if (mtdA.Y > 0) // { // //++ // edgeA1.X = aHalfWidth; // edgeA1.Y = aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 3; // edgeA2Id = 7; // } // else // { // //+- // edgeA1.X = aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = aHalfWidth; // edgeA2.Y = -aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 1; // edgeA2Id = 5; // } // } // else // { // if (mtdA.Y > 0) // { // //-+ // edgeA1.X = -aHalfWidth; // edgeA1.Y = aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = -aHalfWidth; // edgeA2.Y = aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 2; // edgeA2Id = 6; // } // else // { // //-- // edgeA1.X = -aHalfWidth; // edgeA1.Y = -aHalfHeight; // edgeA1.Z = -aHalfLength; // edgeA2.X = -aHalfWidth; // edgeA2.Y = -aHalfHeight; // edgeA2.Z = aHalfLength; // edgeA1Id = 0; // edgeA2Id = 4; // } // } // } // #endregion // #region Edge B // if (Math.Abs(mtdB.X) < Toolbox.Epsilon) // { // //mtd is in the Y-Z plane. // if (mtdB.Y > 0) // { // if (mtdB.Z > 0) // { // //++ // edgeB1.X = -bHalfWidth; // edgeB1.Y = bHalfHeight; // edgeB1.Z = bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 6; // edgeB2Id = 7; // } // else // { // //+- // edgeB1.X = -bHalfWidth; // edgeB1.Y = bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = -bHalfLength; // edgeB1Id = 2; // edgeB2Id = 3; // } // } // else // { // if (mtdB.Z > 0) // { // //-+ // edgeB1.X = -bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = -bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 4; // edgeB2Id = 5; // } // else // { // //-- // edgeB1.X = -bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = -bHalfHeight; // edgeB2.Z = -bHalfLength; // edgeB1Id = 0; // edgeB2Id = 1; // } // } // } // else if (Math.Abs(mtdB.Y) < Toolbox.Epsilon) // { // //mtd is in the X-Z plane // if (mtdB.X > 0) // { // if (mtdB.Z > 0) // { // //++ // edgeB1.X = bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 5; // edgeB2Id = 7; // } // else // { // //+- // edgeB1.X = bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = -bHalfLength; // edgeB1Id = 1; // edgeB2Id = 3; // } // } // else // { // if (mtdB.Z > 0) // { // //-+ // edgeB1.X = -bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = bHalfLength; // edgeB2.X = -bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 4; // edgeB2Id = 6; // } // else // { // //-- // edgeB1.X = -bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = -bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = -bHalfLength; // edgeB1Id = 0; // edgeB2Id = 2; // } // } // } // else // { // //mtd is in the X-Y plane // if (mtdB.X > 0) // { // if (mtdB.Y > 0) // { // //++ // edgeB1.X = bHalfWidth; // edgeB1.Y = bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 3; // edgeB2Id = 7; // } // else // { // //+- // edgeB1.X = bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = bHalfWidth; // edgeB2.Y = -bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 1; // edgeB2Id = 5; // } // } // else // { // if (mtdB.Y > 0) // { // //-+ // edgeB1.X = -bHalfWidth; // edgeB1.Y = bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = -bHalfWidth; // edgeB2.Y = bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 2; // edgeB2Id = 6; // } // else // { // //-- // edgeB1.X = -bHalfWidth; // edgeB1.Y = -bHalfHeight; // edgeB1.Z = -bHalfLength; // edgeB2.X = -bHalfWidth; // edgeB2.Y = -bHalfHeight; // edgeB2.Z = bHalfLength; // edgeB1Id = 0; // edgeB2Id = 4; // } // } // } // #endregion // //TODO: Since the above uniquely identifies the edge from each box based on two vertices, // //get the edge feature id from vertexA id combined with vertexB id. // //Vertex id's are 3 bit binary 'numbers' because ---, --+, -+-, etc. // Matrix3X3.Transform(ref edgeA1, ref orientationA, out edgeA1); // Matrix3X3.Transform(ref edgeA2, ref orientationA, out edgeA2); // Matrix3X3.Transform(ref edgeB1, ref orientationB, out edgeB1); // Matrix3X3.Transform(ref edgeB2, ref orientationB, out edgeB2); // Vector3.Add(ref edgeA1, ref positionA, out edgeA1); // Vector3.Add(ref edgeA2, ref positionA, out edgeA2); // Vector3.Add(ref edgeB1, ref positionB, out edgeB1); // Vector3.Add(ref edgeB2, ref positionB, out edgeB2); // float s, t; // Vector3 onA, onB; // Toolbox.GetClosestPointsBetweenSegments(ref edgeA1, ref edgeA2, ref edgeB1, ref edgeB2, out s, out t, out onA, out onB); // //Vector3.Add(ref onA, ref onB, out point); // //Vector3.Multiply(ref point, .5f, out point); // point = onA; // //depth = (onB.X - onA.X) * mtd.X + (onB.Y - onA.Y) * mtd.Y + (onB.Z - onA.Z) * mtd.Z; // id = GetContactId(edgeA1Id, edgeA2Id, edgeB1Id, edgeB2Id); // } #if ALLOWUNSAFE internal static void GetFaceContacts(BoxShape a, BoxShape b, ref Vector3 positionA, ref Matrix3x3 orientationA, ref Vector3 positionB, ref Matrix3x3 orientationB, bool aIsFaceOwner, ref Vector3 mtd, out BoxContactDataCache contactData) #else internal static void GetFaceContacts(BoxShape a, BoxShape b, ref Vector3 positionA, ref Matrix3X3 orientationA, ref Vector3 positionB, ref Matrix3X3 orientationB, bool aIsFaceOwner, ref Vector3 mtd, out TinyStructList contactData) #endif { float aHalfWidth = a.halfWidth; float aHalfHeight = a.halfHeight; float aHalfLength = a.halfLength; float bHalfWidth = b.halfWidth; float bHalfHeight = b.halfHeight; float bHalfLength = b.halfLength; BoxFace aBoxFace, bBoxFace; Vector3 negatedMtd; Vector3.Negate(ref mtd, out negatedMtd); GetNearestFace(ref positionA, ref orientationA, ref negatedMtd, aHalfWidth, aHalfHeight, aHalfLength, out aBoxFace); GetNearestFace(ref positionB, ref orientationB, ref mtd, bHalfWidth, bHalfHeight, bHalfLength, out bBoxFace); if (aIsFaceOwner) ClipFacesDirect(ref aBoxFace, ref bBoxFace, ref negatedMtd, out contactData); else ClipFacesDirect(ref bBoxFace, ref aBoxFace, ref mtd, out contactData); if (contactData.Count > 4) PruneContactsMaxDistance(ref mtd, contactData, out contactData); } #if ALLOWUNSAFE private static unsafe void PruneContactsMaxDistance(ref Vector3 mtd, BoxContactDataCache input, out BoxContactDataCache output) { BoxContactData* data = &input.D1; int count = input.Count; //TODO: THE FOLLOWING has a small issue in release mode. //Find the deepest point. float deepestDepth = -1; int deepestIndex = 0; for (int i = 0; i < count; i++) { if (data[i].Depth > deepestDepth) { deepestDepth = data[i].Depth; deepestIndex = i; } } //Identify the furthest point away from the deepest index. float furthestDistance = -1; int furthestIndex = 0; for (int i = 0; i < count; i++) { float distance; Vector3.DistanceSquared(ref data[deepestIndex].Position, ref data[i].Position, out distance); if (distance > furthestDistance) { furthestDistance = distance; furthestIndex = i; } } Vector3 xAxis; Vector3.Subtract(ref data[furthestIndex].Position, ref data[deepestIndex].Position, out xAxis); Vector3 yAxis; Vector3.Cross(ref mtd, ref xAxis, out yAxis); float minY; float maxY; int minYindex = 0; int maxYindex = 0; Vector3.Dot(ref data[0].Position, ref yAxis, out minY); maxY = minY; for (int i = 1; i < count; i++) { float dot; Vector3.Dot(ref yAxis, ref data[i].Position, out dot); if (dot < minY) { minY = dot; minYindex = i; } else if (dot > maxY) { maxY = dot; maxYindex = i; } } output = new BoxContactDataCache { Count = 4, D1 = data[deepestIndex], D2 = data[furthestIndex], D3 = data[minYindex], D4 = data[maxYindex] }; //Vector3 v; //var maximumOffset = new Vector3(); //int maxIndexA = -1, maxIndexB = -1; //float temp; //float maximumDistanceSquared = -float.MaxValue; //for (int i = 0; i < count; i++) //{ // for (int j = i + 1; j < count; j++) // { // Vector3.Subtract(ref data[j].Position, ref data[i].Position, out v); // temp = v.LengthSquared(); // if (temp > maximumDistanceSquared) // { // maximumDistanceSquared = temp; // maxIndexA = i; // maxIndexB = j; // maximumOffset = v; // } // } //} //Vector3 otherDirection; //Vector3.Cross(ref mtd, ref maximumOffset, out otherDirection); //int minimumIndex = -1, maximumIndex = -1; //float minimumDistance = float.MaxValue, maximumDistance = -float.MaxValue; //for (int i = 0; i < count; i++) //{ // if (i != maxIndexA && i != maxIndexB) // { // Vector3.Dot(ref data[i].Position, ref otherDirection, out temp); // if (temp > maximumDistance) // { // maximumDistance = temp; // maximumIndex = i; // } // if (temp < minimumDistance) // { // minimumDistance = temp; // minimumIndex = i; // } // } //} //output = new BoxContactDataCache(); //output.Count = 4; //output.D1 = data[maxIndexA]; //output.D2 = data[maxIndexB]; //output.D3 = data[minimumIndex]; //output.D4 = data[maximumIndex]; } #else private static void PruneContactsMaxDistance(ref Vector3 mtd, TinyStructList input, out TinyStructList output) { int count = input.Count; //Find the deepest point. BoxContactData data, deepestData; input.Get(0, out deepestData); for (int i = 1; i < count; i++) { input.Get(i, out data); if (data.Depth > deepestData.Depth) { deepestData = data; } } //Identify the furthest point away from the deepest index. BoxContactData furthestData; input.Get(0, out furthestData); float furthestDistance; Vector3.DistanceSquared(ref deepestData.Position, ref furthestData.Position, out furthestDistance); for (int i = 1; i < count; i++) { input.Get(i, out data); float distance; Vector3.DistanceSquared(ref deepestData.Position, ref data.Position, out distance); if (distance > furthestDistance) { furthestDistance = distance; furthestData = data; } } Vector3 xAxis; Vector3.Subtract(ref furthestData.Position, ref deepestData.Position, out xAxis); Vector3 yAxis; Vector3.Cross(ref mtd, ref xAxis, out yAxis); float minY; float maxY; BoxContactData minData, maxData; input.Get(0, out minData); maxData = minData; Vector3.Dot(ref minData.Position, ref yAxis, out minY); maxY = minY; for (int i = 1; i < count; i++) { input.Get(i, out data); float dot; Vector3.Dot(ref yAxis, ref data.Position, out dot); if (dot < minY) { minY = dot; minData = data; } else if (dot > maxY) { maxY = dot; maxData = data; } } output = new TinyStructList(); output.Add(ref deepestData); output.Add(ref furthestData); output.Add(ref minData); output.Add(ref maxData); //int count = input.Count; //Vector3 v; //var maximumOffset = new Vector3(); //int maxIndexA = -1, maxIndexB = -1; //float temp; //float maximumDistanceSquared = -float.MaxValue; //BoxContactData itemA, itemB; //for (int i = 0; i < count; i++) //{ // for (int j = i + 1; j < count; j++) // { // input.Get(j, out itemB); // input.Get(i, out itemA); // Vector3.Subtract(ref itemB.Position, ref itemA.Position, out v); // temp = v.LengthSquared(); // if (temp > maximumDistanceSquared) // { // maximumDistanceSquared = temp; // maxIndexA = i; // maxIndexB = j; // maximumOffset = v; // } // } //} //Vector3 otherDirection; //Vector3.Cross(ref mtd, ref maximumOffset, out otherDirection); //int minimumIndex = -1, maximumIndex = -1; //float minimumDistance = float.MaxValue, maximumDistance = -float.MaxValue; //for (int i = 0; i < count; i++) //{ // if (i != maxIndexA && i != maxIndexB) // { // input.Get(i, out itemA); // Vector3.Dot(ref itemA.Position, ref otherDirection, out temp); // if (temp > maximumDistance) // { // maximumDistance = temp; // maximumIndex = i; // } // if (temp < minimumDistance) // { // minimumDistance = temp; // minimumIndex = i; // } // } //} //output = new TinyStructList(); //input.Get(maxIndexA, out itemA); //output.Add(ref itemA); //input.Get(maxIndexB, out itemA); //output.Add(ref itemA); //input.Get(minimumIndex, out itemA); //output.Add(ref itemA); //input.Get(maximumIndex, out itemA); //output.Add(ref itemA); } #endif #if EXCLUDED private static unsafe void clipFacesSH(ref BoxFace clipFace, ref BoxFace face, ref Vector3 mtd, out BoxContactDataCache outputData) { BoxContactDataCache contactDataCache = new BoxContactDataCache(); BoxContactData* data = &contactDataCache.d1; //Set up the initial face list. data[0].position = face.v1; data[0].id = face.id1; data[1].position = face.v2; data[1].id = face.id2; data[2].position = face.v3; data[2].id = face.id3; data[3].position = face.v4; data[3].id = face.id4; contactDataCache.count = 4; BoxContactDataCache temporaryCache; BoxContactData* temp = &temporaryCache.d1; FaceEdge clippingEdge; Vector3 intersection; for (int i = 0; i < 4; i++) {//For each clipping edge (edges of face a) clipFace.GetEdge(i, ref mtd, out clippingEdge); temporaryCache = contactDataCache; contactDataCache.count = 0; Vector3 start = temp[temporaryCache.count - 1].position; int startId = temp[temporaryCache.count - 1].id; for (int j = 0; j < temporaryCache.count; j++) {//For each point in the input list Vector3 end = temp[j].position; int endId = temp[j].id; if (clippingEdge.isPointInside(ref end)) { if (!clippingEdge.isPointInside(ref start)) { ComputeIntersection(ref start, ref end, ref mtd, ref clippingEdge, out intersection); if (contactDataCache.count < 8) { data[contactDataCache.count].position = intersection; data[contactDataCache.count].id = GetContactId(startId, endId, ref clippingEdge); contactDataCache.count++; } else { data[contactDataCache.count - 1].position = intersection; data[contactDataCache.count - 1].id = GetContactId(startId, endId, ref clippingEdge); } } if (contactDataCache.count < 8) { data[contactDataCache.count].position = end; data[contactDataCache.count].id = endId; contactDataCache.count++; } else { data[contactDataCache.count - 1].position = end; data[contactDataCache.count - 1].id = endId; } } else if (clippingEdge.isPointInside(ref start)) { ComputeIntersection(ref start, ref end, ref mtd, ref clippingEdge, out intersection); if (contactDataCache.count < 8) { data[contactDataCache.count].position = intersection; data[contactDataCache.count].id = GetContactId(startId, endId, ref clippingEdge); contactDataCache.count++; } else { data[contactDataCache.count - 1].position = intersection; data[contactDataCache.count - 1].id = GetContactId(startId, endId, ref clippingEdge); } } start = end; startId = endId; } } temporaryCache = contactDataCache; contactDataCache.count = 0; float depth; float a, b; Vector3.Dot(ref clipFace.v1, ref mtd, out a); for (int i = 0; i < temporaryCache.count; i++) { Vector3.Dot(ref temp[i].position, ref mtd, out b); depth = b - a; if (depth <= 0) { data[contactDataCache.count].position = temp[i].position; data[contactDataCache.count].id = temp[i].id; contactDataCache.count++; } } outputData = contactDataCache; /* * List outputList = subjectPolygon; for (Edge clipEdge in clipPolygon) do List inputList = outputList; outputList.clear(); Point S = inputList.last; for (Point E in inputList) do if (E inside clipEdge) then if (S not inside clipEdge) then outputList.add(ComputeIntersection(S,E,clipEdge)); end if outputList.add(E); else if (S inside clipEdge) then outputList.add(ComputeIntersection(S,E,clipEdge)); end if S = E; done done */ } #endif #if ALLOWUNSAFE private static unsafe void ClipFacesDirect(ref BoxFace clipFace, ref BoxFace face, ref Vector3 mtd, out BoxContactDataCache outputData) { var contactData = new BoxContactDataCache(); BoxContactDataCache tempData; //Local version. BoxContactData* data = &contactData.D1; BoxContactData* temp = &tempData.D1; //Local directions on the clip face. Their length is equal to the length of an edge. Vector3 clipX, clipY; Vector3.Subtract(ref clipFace.V4, ref clipFace.V3, out clipX); Vector3.Subtract(ref clipFace.V2, ref clipFace.V3, out clipY); float inverseClipWidth = 1 / clipFace.Width; float inverseClipHeight = 1 / clipFace.Height; float inverseClipWidthSquared = inverseClipWidth * inverseClipWidth; clipX.X *= inverseClipWidthSquared; clipX.Y *= inverseClipWidthSquared; clipX.Z *= inverseClipWidthSquared; float inverseClipHeightSquared = inverseClipHeight * inverseClipHeight; clipY.X *= inverseClipHeightSquared; clipY.Y *= inverseClipHeightSquared; clipY.Z *= inverseClipHeightSquared; //Local directions on the opposing face. Their length is equal to the length of an edge. Vector3 faceX, faceY; Vector3.Subtract(ref face.V4, ref face.V3, out faceX); Vector3.Subtract(ref face.V2, ref face.V3, out faceY); float inverseFaceWidth = 1 / face.Width; float inverseFaceHeight = 1 / face.Height; float inverseFaceWidthSquared = inverseFaceWidth * inverseFaceWidth; faceX.X *= inverseFaceWidthSquared; faceX.Y *= inverseFaceWidthSquared; faceX.Z *= inverseFaceWidthSquared; float inverseFaceHeightSquared = inverseFaceHeight * inverseFaceHeight; faceY.X *= inverseFaceHeightSquared; faceY.Y *= inverseFaceHeightSquared; faceY.Z *= inverseFaceHeightSquared; Vector3 clipCenter; Vector3.Add(ref clipFace.V1, ref clipFace.V3, out clipCenter); //Defer division until after dot product (2 multiplies instead of 3) float clipCenterX, clipCenterY; Vector3.Dot(ref clipCenter, ref clipX, out clipCenterX); Vector3.Dot(ref clipCenter, ref clipY, out clipCenterY); clipCenterX *= .5f; clipCenterY *= .5f; Vector3 faceCenter; Vector3.Add(ref face.V1, ref face.V3, out faceCenter); //Defer division until after dot product (2 multiplies instead of 3) float faceCenterX, faceCenterY; Vector3.Dot(ref faceCenter, ref faceX, out faceCenterX); Vector3.Dot(ref faceCenter, ref faceY, out faceCenterY); faceCenterX *= .5f; faceCenterY *= .5f; //To test bounds, recall that clipX is the length of the X edge. //Going from the center to the max or min goes half of the length of X edge, or +/- 0.5. //Bias could be added here. //const float extent = .5f; //.5f is the default, extra could be added for robustness or speed. float extentX = .5f + .01f * inverseClipWidth; float extentY = .5f + .01f * inverseClipHeight; //float extentX = .5f + .01f * inverseClipXLength; //float extentY = .5f + .01f * inverseClipYLength; float clipCenterMaxX = clipCenterX + extentX; float clipCenterMaxY = clipCenterY + extentY; float clipCenterMinX = clipCenterX - extentX; float clipCenterMinY = clipCenterY - extentY; extentX = .5f + .01f * inverseFaceWidth; extentY = .5f + .01f * inverseFaceHeight; //extentX = .5f + .01f * inverseFaceXLength; //extentY = .5f + .01f * inverseFaceYLength; float faceCenterMaxX = faceCenterX + extentX; float faceCenterMaxY = faceCenterY + extentY; float faceCenterMinX = faceCenterX - extentX; float faceCenterMinY = faceCenterY - extentY; //Find out where the opposing face is. float dotX, dotY; //The four edges can be thought of as minX, maxX, minY and maxY. //Face v1 Vector3.Dot(ref clipX, ref face.V1, out dotX); bool v1MaxXInside = dotX < clipCenterMaxX; bool v1MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V1, out dotY); bool v1MaxYInside = dotY < clipCenterMaxY; bool v1MinYInside = dotY > clipCenterMinY; //Face v2 Vector3.Dot(ref clipX, ref face.V2, out dotX); bool v2MaxXInside = dotX < clipCenterMaxX; bool v2MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V2, out dotY); bool v2MaxYInside = dotY < clipCenterMaxY; bool v2MinYInside = dotY > clipCenterMinY; //Face v3 Vector3.Dot(ref clipX, ref face.V3, out dotX); bool v3MaxXInside = dotX < clipCenterMaxX; bool v3MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V3, out dotY); bool v3MaxYInside = dotY < clipCenterMaxY; bool v3MinYInside = dotY > clipCenterMinY; //Face v4 Vector3.Dot(ref clipX, ref face.V4, out dotX); bool v4MaxXInside = dotX < clipCenterMaxX; bool v4MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V4, out dotY); bool v4MaxYInside = dotY < clipCenterMaxY; bool v4MinYInside = dotY > clipCenterMinY; //Find out where the clip face is. //Clip v1 Vector3.Dot(ref faceX, ref clipFace.V1, out dotX); bool clipv1MaxXInside = dotX < faceCenterMaxX; bool clipv1MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V1, out dotY); bool clipv1MaxYInside = dotY < faceCenterMaxY; bool clipv1MinYInside = dotY > faceCenterMinY; //Clip v2 Vector3.Dot(ref faceX, ref clipFace.V2, out dotX); bool clipv2MaxXInside = dotX < faceCenterMaxX; bool clipv2MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V2, out dotY); bool clipv2MaxYInside = dotY < faceCenterMaxY; bool clipv2MinYInside = dotY > faceCenterMinY; //Clip v3 Vector3.Dot(ref faceX, ref clipFace.V3, out dotX); bool clipv3MaxXInside = dotX < faceCenterMaxX; bool clipv3MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V3, out dotY); bool clipv3MaxYInside = dotY < faceCenterMaxY; bool clipv3MinYInside = dotY > faceCenterMinY; //Clip v4 Vector3.Dot(ref faceX, ref clipFace.V4, out dotX); bool clipv4MaxXInside = dotX < faceCenterMaxX; bool clipv4MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V4, out dotY); bool clipv4MaxYInside = dotY < faceCenterMaxY; bool clipv4MinYInside = dotY > faceCenterMinY; #region Face Vertices if (v1MinXInside && v1MaxXInside && v1MinYInside && v1MaxYInside) { data[contactData.Count].Position = face.V1; data[contactData.Count].Id = face.Id1; contactData.Count++; } if (v2MinXInside && v2MaxXInside && v2MinYInside && v2MaxYInside) { data[contactData.Count].Position = face.V2; data[contactData.Count].Id = face.Id2; contactData.Count++; } if (v3MinXInside && v3MaxXInside && v3MinYInside && v3MaxYInside) { data[contactData.Count].Position = face.V3; data[contactData.Count].Id = face.Id3; contactData.Count++; } if (v4MinXInside && v4MaxXInside && v4MinYInside && v4MaxYInside) { data[contactData.Count].Position = face.V4; data[contactData.Count].Id = face.Id4; contactData.Count++; } #endregion //Compute depths. tempData = contactData; contactData.Count = 0; float depth; float clipFaceDot, faceDot; Vector3.Dot(ref clipFace.V1, ref mtd, out clipFaceDot); for (int i = 0; i < tempData.Count; i++) { Vector3.Dot(ref temp[i].Position, ref mtd, out faceDot); depth = faceDot - clipFaceDot; if (depth <= 0) { data[contactData.Count].Position = temp[i].Position; data[contactData.Count].Depth = depth; data[contactData.Count].Id = temp[i].Id; contactData.Count++; } } byte previousCount = contactData.Count; if (previousCount >= 4) //Early finish :) { outputData = contactData; return; } #region Clip face vertices Vector3 v; float a, b; Vector3.Dot(ref face.V1, ref face.Normal, out b); //CLIP FACE if (clipv1MinXInside && clipv1MaxXInside && clipv1MinYInside && clipv1MaxYInside) { Vector3.Dot(ref clipFace.V1, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V1, ref v, out v); data[contactData.Count].Position = v; data[contactData.Count].Id = clipFace.Id1 + 8; contactData.Count++; } if (clipv2MinXInside && clipv2MaxXInside && clipv2MinYInside && clipv2MaxYInside) { Vector3.Dot(ref clipFace.V2, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V2, ref v, out v); data[contactData.Count].Position = v; data[contactData.Count].Id = clipFace.Id2 + 8; contactData.Count++; } if (clipv3MinXInside && clipv3MaxXInside && clipv3MinYInside && clipv3MaxYInside) { Vector3.Dot(ref clipFace.V3, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V3, ref v, out v); data[contactData.Count].Position = v; data[contactData.Count].Id = clipFace.Id3 + 8; contactData.Count++; } if (clipv4MinXInside && clipv4MaxXInside && clipv4MinYInside && clipv4MaxYInside) { Vector3.Dot(ref clipFace.V4, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V4, ref v, out v); data[contactData.Count].Position = v; data[contactData.Count].Id = clipFace.Id4 + 8; contactData.Count++; } #endregion //Compute depths. tempData = contactData; contactData.Count = previousCount; for (int i = previousCount; i < tempData.Count; i++) { Vector3.Dot(ref temp[i].Position, ref mtd, out faceDot); depth = faceDot - clipFaceDot; if (depth <= 0) { data[contactData.Count].Position = temp[i].Position; data[contactData.Count].Depth = depth; data[contactData.Count].Id = temp[i].Id; contactData.Count++; } } previousCount = contactData.Count; if (previousCount >= 4) //Early finish :) { outputData = contactData; return; } //Intersect edges. //maxX maxY -> v1 //minX maxY -> v2 //minX minY -> v3 //maxX minY -> v4 //Once we get here there can only be 3 contacts or less. //Once 4 possible contacts have been added, switch to using safe increments. //float dot; #region CLIP EDGE: v1 v2 FaceEdge clipEdge; clipFace.GetEdge(0, out clipEdge); if (!v1MaxYInside) { if (v2MaxYInside) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v4MaxYInside) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } } if (!v2MaxYInside) { if (v1MaxYInside) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v3MaxYInside) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } } if (!v3MaxYInside) { if (v2MaxYInside) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } if (v4MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } if (!v4MaxYInside) { if (v1MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } if (v3MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } #endregion #region CLIP EDGE: v2 v3 clipFace.GetEdge(1, out clipEdge); if (!v1MinXInside) { if (v2MinXInside && contactData.Count < 8) { //test v1-v2 against minXminY-minXmaxY //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v4MinXInside && contactData.Count < 8) { //test v1-v3 against minXminY-minXmaxY //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } } if (!v2MinXInside) { if (v1MinXInside && contactData.Count < 8) { //test v1-v2 against minXminY-minXmaxY //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v3MinXInside && contactData.Count < 8) { //test v2-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } } if (!v3MinXInside) { if (v2MinXInside && contactData.Count < 8) { //test v1-v3 against minXminY-minXmaxY //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } if (v4MinXInside && contactData.Count < 8) { //test v3-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } if (!v4MinXInside) { if (v1MinXInside && contactData.Count < 8) { //test v2-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } if (v3MinXInside && contactData.Count < 8) { //test v3-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } #endregion #region CLIP EDGE: v3 v4 clipFace.GetEdge(2, out clipEdge); if (!v1MinYInside) { if (v2MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v4MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } } if (!v2MinYInside) { if (v1MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v3MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } } if (!v3MinYInside) { if (v2MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } if (v4MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } if (!v4MinYInside) { if (v3MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } if (v1MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } } #endregion #region CLIP EDGE: v4 v1 clipFace.GetEdge(3, out clipEdge); if (!v1MaxXInside) { if (v2MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v4MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } } if (!v2MaxXInside) { if (v1MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Count++; } } if (v3MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } } if (!v3MaxXInside) { if (v2MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Count++; } } if (v4MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } if (!v4MaxXInside) { if (v1MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Count++; } } if (v3MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { data[contactData.Count].Position = v; data[contactData.Count].Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Count++; } } } #endregion //Compute depths. tempData = contactData; contactData.Count = previousCount; for (int i = previousCount; i < tempData.Count; i++) { Vector3.Dot(ref temp[i].Position, ref mtd, out faceDot); depth = faceDot - clipFaceDot; if (depth <= 0) { data[contactData.Count].Position = temp[i].Position; data[contactData.Count].Depth = depth; data[contactData.Count].Id = temp[i].Id; contactData.Count++; } } outputData = contactData; } #else private static void ClipFacesDirect(ref BoxFace clipFace, ref BoxFace face, ref Vector3 mtd, out TinyStructList contactData) { contactData = new TinyStructList(); //Local directions on the clip face. Their length is equal to the length of an edge. Vector3 clipX, clipY; Vector3.Subtract(ref clipFace.V4, ref clipFace.V3, out clipX); Vector3.Subtract(ref clipFace.V2, ref clipFace.V3, out clipY); float inverseClipWidth = 1 / clipFace.Width; float inverseClipHeight = 1 / clipFace.Height; float inverseClipWidthSquared = inverseClipWidth * inverseClipWidth; clipX.X *= inverseClipWidthSquared; clipX.Y *= inverseClipWidthSquared; clipX.Z *= inverseClipWidthSquared; float inverseClipHeightSquared = inverseClipHeight * inverseClipHeight; clipY.X *= inverseClipHeightSquared; clipY.Y *= inverseClipHeightSquared; clipY.Z *= inverseClipHeightSquared; //Local directions on the opposing face. Their length is equal to the length of an edge. Vector3 faceX, faceY; Vector3.Subtract(ref face.V4, ref face.V3, out faceX); Vector3.Subtract(ref face.V2, ref face.V3, out faceY); float inverseFaceWidth = 1 / face.Width; float inverseFaceHeight = 1 / face.Height; float inverseFaceWidthSquared = inverseFaceWidth * inverseFaceWidth; faceX.X *= inverseFaceWidthSquared; faceX.Y *= inverseFaceWidthSquared; faceX.Z *= inverseFaceWidthSquared; float inverseFaceHeightSquared = inverseFaceHeight * inverseFaceHeight; faceY.X *= inverseFaceHeightSquared; faceY.Y *= inverseFaceHeightSquared; faceY.Z *= inverseFaceHeightSquared; Vector3 clipCenter; Vector3.Add(ref clipFace.V1, ref clipFace.V3, out clipCenter); //Defer division until after dot product (2 multiplies instead of 3) float clipCenterX, clipCenterY; Vector3.Dot(ref clipCenter, ref clipX, out clipCenterX); Vector3.Dot(ref clipCenter, ref clipY, out clipCenterY); clipCenterX *= .5f; clipCenterY *= .5f; Vector3 faceCenter; Vector3.Add(ref face.V1, ref face.V3, out faceCenter); //Defer division until after dot product (2 multiplies instead of 3) float faceCenterX, faceCenterY; Vector3.Dot(ref faceCenter, ref faceX, out faceCenterX); Vector3.Dot(ref faceCenter, ref faceY, out faceCenterY); faceCenterX *= .5f; faceCenterY *= .5f; //To test bounds, recall that clipX is the length of the X edge. //Going from the center to the max or min goes half of the length of X edge, or +/- 0.5. //Bias could be added here. //const float extent = .5f; //.5f is the default, extra could be added for robustness or speed. float extentX = .5f + .01f * inverseClipWidth; float extentY = .5f + .01f * inverseClipHeight; //float extentX = .5f + .01f * inverseClipXLength; //float extentY = .5f + .01f * inverseClipYLength; float clipCenterMaxX = clipCenterX + extentX; float clipCenterMaxY = clipCenterY + extentY; float clipCenterMinX = clipCenterX - extentX; float clipCenterMinY = clipCenterY - extentY; extentX = .5f + .01f * inverseFaceWidth; extentY = .5f + .01f * inverseFaceHeight; //extentX = .5f + .01f * inverseFaceXLength; //extentY = .5f + .01f * inverseFaceYLength; float faceCenterMaxX = faceCenterX + extentX; float faceCenterMaxY = faceCenterY + extentY; float faceCenterMinX = faceCenterX - extentX; float faceCenterMinY = faceCenterY - extentY; //Find out where the opposing face is. float dotX, dotY; //The four edges can be thought of as minX, maxX, minY and maxY. //Face v1 Vector3.Dot(ref clipX, ref face.V1, out dotX); bool v1MaxXInside = dotX < clipCenterMaxX; bool v1MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V1, out dotY); bool v1MaxYInside = dotY < clipCenterMaxY; bool v1MinYInside = dotY > clipCenterMinY; //Face v2 Vector3.Dot(ref clipX, ref face.V2, out dotX); bool v2MaxXInside = dotX < clipCenterMaxX; bool v2MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V2, out dotY); bool v2MaxYInside = dotY < clipCenterMaxY; bool v2MinYInside = dotY > clipCenterMinY; //Face v3 Vector3.Dot(ref clipX, ref face.V3, out dotX); bool v3MaxXInside = dotX < clipCenterMaxX; bool v3MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V3, out dotY); bool v3MaxYInside = dotY < clipCenterMaxY; bool v3MinYInside = dotY > clipCenterMinY; //Face v4 Vector3.Dot(ref clipX, ref face.V4, out dotX); bool v4MaxXInside = dotX < clipCenterMaxX; bool v4MinXInside = dotX > clipCenterMinX; Vector3.Dot(ref clipY, ref face.V4, out dotY); bool v4MaxYInside = dotY < clipCenterMaxY; bool v4MinYInside = dotY > clipCenterMinY; //Find out where the clip face is. //Clip v1 Vector3.Dot(ref faceX, ref clipFace.V1, out dotX); bool clipv1MaxXInside = dotX < faceCenterMaxX; bool clipv1MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V1, out dotY); bool clipv1MaxYInside = dotY < faceCenterMaxY; bool clipv1MinYInside = dotY > faceCenterMinY; //Clip v2 Vector3.Dot(ref faceX, ref clipFace.V2, out dotX); bool clipv2MaxXInside = dotX < faceCenterMaxX; bool clipv2MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V2, out dotY); bool clipv2MaxYInside = dotY < faceCenterMaxY; bool clipv2MinYInside = dotY > faceCenterMinY; //Clip v3 Vector3.Dot(ref faceX, ref clipFace.V3, out dotX); bool clipv3MaxXInside = dotX < faceCenterMaxX; bool clipv3MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V3, out dotY); bool clipv3MaxYInside = dotY < faceCenterMaxY; bool clipv3MinYInside = dotY > faceCenterMinY; //Clip v4 Vector3.Dot(ref faceX, ref clipFace.V4, out dotX); bool clipv4MaxXInside = dotX < faceCenterMaxX; bool clipv4MinXInside = dotX > faceCenterMinX; Vector3.Dot(ref faceY, ref clipFace.V4, out dotY); bool clipv4MaxYInside = dotY < faceCenterMaxY; bool clipv4MinYInside = dotY > faceCenterMinY; #region Face Vertices BoxContactData item = new BoxContactData(); if (v1MinXInside && v1MaxXInside && v1MinYInside && v1MaxYInside) { item.Position = face.V1; item.Id = face.Id1; contactData.Add(ref item); } if (v2MinXInside && v2MaxXInside && v2MinYInside && v2MaxYInside) { item.Position = face.V2; item.Id = face.Id2; contactData.Add(ref item); } if (v3MinXInside && v3MaxXInside && v3MinYInside && v3MaxYInside) { item.Position = face.V3; item.Id = face.Id3; contactData.Add(ref item); } if (v4MinXInside && v4MaxXInside && v4MinYInside && v4MaxYInside) { item.Position = face.V4; item.Id = face.Id4; contactData.Add(ref item); } #endregion //Compute depths. TinyStructList tempData = contactData; contactData.Clear(); float clipFaceDot, faceDot; Vector3.Dot(ref clipFace.V1, ref mtd, out clipFaceDot); for (int i = 0; i < tempData.Count; i++) { tempData.Get(i, out item); Vector3.Dot(ref item.Position, ref mtd, out faceDot); item.Depth = faceDot - clipFaceDot; if (item.Depth <= 0) { contactData.Add(ref item); } } int previousCount = contactData.Count; if (previousCount >= 4) //Early finish :) { return; } #region Clip face vertices Vector3 v; float a, b; Vector3.Dot(ref face.V1, ref face.Normal, out b); //CLIP FACE if (clipv1MinXInside && clipv1MaxXInside && clipv1MinYInside && clipv1MaxYInside) { Vector3.Dot(ref clipFace.V1, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V1, ref v, out v); item.Position = v; item.Id = clipFace.Id1 + 8; contactData.Add(ref item); } if (clipv2MinXInside && clipv2MaxXInside && clipv2MinYInside && clipv2MaxYInside) { Vector3.Dot(ref clipFace.V2, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V2, ref v, out v); item.Position = v; item.Id = clipFace.Id2 + 8; contactData.Add(ref item); } if (clipv3MinXInside && clipv3MaxXInside && clipv3MinYInside && clipv3MaxYInside) { Vector3.Dot(ref clipFace.V3, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V3, ref v, out v); item.Position = v; item.Id = clipFace.Id3 + 8; contactData.Add(ref item); } if (clipv4MinXInside && clipv4MaxXInside && clipv4MinYInside && clipv4MaxYInside) { Vector3.Dot(ref clipFace.V4, ref face.Normal, out a); Vector3.Multiply(ref face.Normal, a - b, out v); Vector3.Subtract(ref clipFace.V4, ref v, out v); item.Position = v; item.Id = clipFace.Id4 + 8; contactData.Add(ref item); } #endregion //Compute depths. int postClipCount = contactData.Count; tempData = contactData; for (int i = postClipCount - 1; i >= previousCount; i--) //TODO: >=? contactData.RemoveAt(i); for (int i = previousCount; i < tempData.Count; i++) { tempData.Get(i, out item); Vector3.Dot(ref item.Position, ref mtd, out faceDot); item.Depth = faceDot - clipFaceDot; if (item.Depth <= 0) { contactData.Add(ref item); } } previousCount = contactData.Count; if (previousCount >= 4) //Early finish :) { return; } //Intersect edges. //maxX maxY -> v1 //minX maxY -> v2 //minX minY -> v3 //maxX minY -> v4 //Once we get here there can only be 3 contacts or less. //Once 4 possible contacts have been added, switch to using safe increments. //float dot; #region CLIP EDGE: v1 v2 FaceEdge clipEdge; clipFace.GetEdge(0, out clipEdge); if (!v1MaxYInside) { if (v2MaxYInside) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v4MaxYInside) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } } if (!v2MaxYInside) { if (v1MaxYInside) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v3MaxYInside) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } } if (!v3MaxYInside) { if (v2MaxYInside) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } if (v4MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } if (!v4MaxYInside) { if (v1MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } if (v3MaxYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } #endregion #region CLIP EDGE: v2 v3 clipFace.GetEdge(1, out clipEdge); if (!v1MinXInside) { if (v2MinXInside && contactData.Count < 8) { //test v1-v2 against minXminY-minXmaxY //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v4MinXInside && contactData.Count < 8) { //test v1-v3 against minXminY-minXmaxY //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } } if (!v2MinXInside) { if (v1MinXInside && contactData.Count < 8) { //test v1-v2 against minXminY-minXmaxY //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v3MinXInside && contactData.Count < 8) { //test v2-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } } if (!v3MinXInside) { if (v2MinXInside && contactData.Count < 8) { //test v1-v3 against minXminY-minXmaxY //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } if (v4MinXInside && contactData.Count < 8) { //test v3-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } if (!v4MinXInside) { if (v1MinXInside && contactData.Count < 8) { //test v2-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } if (v3MinXInside && contactData.Count < 8) { //test v3-v4 against minXminY-minXmaxY //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } #endregion #region CLIP EDGE: v3 v4 clipFace.GetEdge(2, out clipEdge); if (!v1MinYInside) { if (v2MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v4MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } } if (!v2MinYInside) { if (v1MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v3MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } } if (!v3MinYInside) { if (v2MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } if (v4MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } if (!v4MinYInside) { if (v3MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } if (v1MinYInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipX, ref v, out dot); //if (dot > clipCenterMinX && dot < clipCenterMaxX) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } } #endregion #region CLIP EDGE: v4 v1 clipFace.GetEdge(3, out clipEdge); if (!v1MaxXInside) { if (v2MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v4MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } } if (!v2MaxXInside) { if (v1MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); contactData.Add(ref item); } } if (v3MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } } if (!v3MaxXInside) { if (v2MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); contactData.Add(ref item); } } if (v4MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } if (!v4MaxXInside) { if (v1MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); contactData.Add(ref item); } } if (v3MaxXInside && contactData.Count < 8) { //ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); //Vector3.Dot(ref clipY, ref v, out dot); //if (dot > clipCenterMinY && dot < clipCenterMaxY) if (ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v)) { item.Position = v; item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); contactData.Add(ref item); } } } #endregion //Compute depths. postClipCount = contactData.Count; tempData = contactData; for (int i = postClipCount - 1; i >= previousCount; i--) contactData.RemoveAt(i); for (int i = previousCount; i < tempData.Count; i++) { tempData.Get(i, out item); Vector3.Dot(ref item.Position, ref mtd, out faceDot); item.Depth = faceDot - clipFaceDot; if (item.Depth <= 0) { contactData.Add(ref item); } } } //private static void ClipFacesDirect(ref BoxFace clipFace, ref BoxFace face, ref Vector3 mtd, out TinyStructList contactData) //{ // contactData = new TinyStructList(); // //BoxContactData* data = &contactData.d1; // //BoxContactData* temp = &tempData.d1; // //Local directions on the clip face. Their length is equal to the length of an edge. // Vector3 clipX, clipY; // Vector3.Subtract(ref clipFace.V4, ref clipFace.V3, out clipX); // Vector3.Subtract(ref clipFace.V2, ref clipFace.V3, out clipY); // float inverse = 1 / clipX.LengthSquared(); // clipX.X *= inverse; // clipX.Y *= inverse; // clipX.Z *= inverse; // inverse = 1 / clipY.LengthSquared(); // clipY.X *= inverse; // clipY.Y *= inverse; // clipY.Z *= inverse; // //Local directions on the opposing face. Their length is equal to the length of an edge. // Vector3 faceX, faceY; // Vector3.Subtract(ref face.V4, ref face.V3, out faceX); // Vector3.Subtract(ref face.V2, ref face.V3, out faceY); // inverse = 1 / faceX.LengthSquared(); // faceX.X *= inverse; // faceX.Y *= inverse; // faceX.Z *= inverse; // inverse = 1 / faceY.LengthSquared(); // faceY.X *= inverse; // faceY.Y *= inverse; // faceY.Z *= inverse; // Vector3 clipCenter; // Vector3.Add(ref clipFace.V1, ref clipFace.V3, out clipCenter); // //Defer division until after dot product (2 multiplies instead of 3) // float clipCenterX, clipCenterY; // Vector3.Dot(ref clipCenter, ref clipX, out clipCenterX); // Vector3.Dot(ref clipCenter, ref clipY, out clipCenterY); // clipCenterX *= .5f; // clipCenterY *= .5f; // Vector3 faceCenter; // Vector3.Add(ref face.V1, ref face.V3, out faceCenter); // //Defer division until after dot product (2 multiplies instead of 3) // float faceCenterX, faceCenterY; // Vector3.Dot(ref faceCenter, ref faceX, out faceCenterX); // Vector3.Dot(ref faceCenter, ref faceY, out faceCenterY); // faceCenterX *= .5f; // faceCenterY *= .5f; // //To test bounds, recall that clipX is the length of the X edge. // //Going from the center to the max or min goes half of the length of X edge, or +/- 0.5. // //Bias could be added here. // float extent = .5f; //.5f is the default, extra could be added for robustness or speed. // float clipCenterMaxX = clipCenterX + extent; // float clipCenterMaxY = clipCenterY + extent; // float clipCenterMinX = clipCenterX - extent; // float clipCenterMinY = clipCenterY - extent; // float faceCenterMaxX = faceCenterX + extent; // float faceCenterMaxY = faceCenterY + extent; // float faceCenterMinX = faceCenterX - extent; // float faceCenterMinY = faceCenterY - extent; // //Find out where the opposing face is. // float dotX, dotY; // //The four edges can be thought of as minX, maxX, minY and maxY. // //Face v1 // Vector3.Dot(ref clipX, ref face.V1, out dotX); // bool v1MaxXInside = dotX < clipCenterMaxX; // bool v1MinXInside = dotX > clipCenterMinX; // Vector3.Dot(ref clipY, ref face.V1, out dotY); // bool v1MaxYInside = dotY < clipCenterMaxY; // bool v1MinYInside = dotY > clipCenterMinY; // //Face v2 // Vector3.Dot(ref clipX, ref face.V2, out dotX); // bool v2MaxXInside = dotX < clipCenterMaxX; // bool v2MinXInside = dotX > clipCenterMinX; // Vector3.Dot(ref clipY, ref face.V2, out dotY); // bool v2MaxYInside = dotY < clipCenterMaxY; // bool v2MinYInside = dotY > clipCenterMinY; // //Face v3 // Vector3.Dot(ref clipX, ref face.V3, out dotX); // bool v3MaxXInside = dotX < clipCenterMaxX; // bool v3MinXInside = dotX > clipCenterMinX; // Vector3.Dot(ref clipY, ref face.V3, out dotY); // bool v3MaxYInside = dotY < clipCenterMaxY; // bool v3MinYInside = dotY > clipCenterMinY; // //Face v4 // Vector3.Dot(ref clipX, ref face.V4, out dotX); // bool v4MaxXInside = dotX < clipCenterMaxX; // bool v4MinXInside = dotX > clipCenterMinX; // Vector3.Dot(ref clipY, ref face.V4, out dotY); // bool v4MaxYInside = dotY < clipCenterMaxY; // bool v4MinYInside = dotY > clipCenterMinY; // //Find out where the clip face is. // //Clip v1 // Vector3.Dot(ref faceX, ref clipFace.V1, out dotX); // bool clipv1MaxXInside = dotX < faceCenterMaxX; // bool clipv1MinXInside = dotX > faceCenterMinX; // Vector3.Dot(ref faceY, ref clipFace.V1, out dotY); // bool clipv1MaxYInside = dotY < faceCenterMaxY; // bool clipv1MinYInside = dotY > faceCenterMinY; // //Clip v2 // Vector3.Dot(ref faceX, ref clipFace.V2, out dotX); // bool clipv2MaxXInside = dotX < faceCenterMaxX; // bool clipv2MinXInside = dotX > faceCenterMinX; // Vector3.Dot(ref faceY, ref clipFace.V2, out dotY); // bool clipv2MaxYInside = dotY < faceCenterMaxY; // bool clipv2MinYInside = dotY > faceCenterMinY; // //Clip v3 // Vector3.Dot(ref faceX, ref clipFace.V3, out dotX); // bool clipv3MaxXInside = dotX < faceCenterMaxX; // bool clipv3MinXInside = dotX > faceCenterMinX; // Vector3.Dot(ref faceY, ref clipFace.V3, out dotY); // bool clipv3MaxYInside = dotY < faceCenterMaxY; // bool clipv3MinYInside = dotY > faceCenterMinY; // //Clip v4 // Vector3.Dot(ref faceX, ref clipFace.V4, out dotX); // bool clipv4MaxXInside = dotX < faceCenterMaxX; // bool clipv4MinXInside = dotX > faceCenterMinX; // Vector3.Dot(ref faceY, ref clipFace.V4, out dotY); // bool clipv4MaxYInside = dotY < faceCenterMaxY; // bool clipv4MinYInside = dotY > faceCenterMinY; // var item = new BoxContactData(); // #region Face Vertices // if (v1MinXInside && v1MaxXInside && v1MinYInside && v1MaxYInside) // { // item.Position = face.V1; // item.Id = face.Id1; // contactData.Add(ref item); // } // if (v2MinXInside && v2MaxXInside && v2MinYInside && v2MaxYInside) // { // item.Position = face.V2; // item.Id = face.Id2; // contactData.Add(ref item); // } // if (v3MinXInside && v3MaxXInside && v3MinYInside && v3MaxYInside) // { // item.Position = face.V3; // item.Id = face.Id3; // contactData.Add(ref item); // } // if (v4MinXInside && v4MaxXInside && v4MinYInside && v4MaxYInside) // { // item.Position = face.V4; // item.Id = face.Id4; // contactData.Add(ref item); // } // #endregion // //Compute depths. // TinyStructList tempData = contactData; // contactData.Clear(); // float clipFaceDot, faceDot; // Vector3.Dot(ref clipFace.V1, ref mtd, out clipFaceDot); // for (int i = 0; i < tempData.Count; i++) // { // tempData.Get(i, out item); // Vector3.Dot(ref item.Position, ref mtd, out faceDot); // item.Depth = faceDot - clipFaceDot; // if (item.Depth <= 0) // { // contactData.Add(ref item); // } // } // int previousCount = contactData.Count; // if (previousCount >= 4) //Early finish :) // { // return; // } // #region Clip face vertices // Vector3 faceNormal; // Vector3.Cross(ref faceY, ref faceX, out faceNormal); // //inverse = 1 / faceNormal.LengthSquared(); // //faceNormal.X *= inverse; // //faceNormal.Y *= inverse; // //faceNormal.Z *= inverse; // faceNormal.Normalize(); // Vector3 v; // float a, b; // Vector3.Dot(ref face.V1, ref faceNormal, out b); // //CLIP FACE // if (clipv1MinXInside && clipv1MaxXInside && clipv1MinYInside && clipv1MaxYInside) // { // Vector3.Dot(ref clipFace.V1, ref faceNormal, out a); // Vector3.Multiply(ref faceNormal, a - b, out v); // Vector3.Subtract(ref clipFace.V1, ref v, out v); // item.Position = v; // item.Id = clipFace.Id1 + 8; // contactData.Add(ref item); // } // if (clipv2MinXInside && clipv2MaxXInside && clipv2MinYInside && clipv2MaxYInside) // { // Vector3.Dot(ref clipFace.V2, ref faceNormal, out a); // Vector3.Multiply(ref faceNormal, a - b, out v); // Vector3.Subtract(ref clipFace.V2, ref v, out v); // item.Position = v; // item.Id = clipFace.Id2 + 8; // contactData.Add(ref item); // } // if (clipv3MinXInside && clipv3MaxXInside && clipv3MinYInside && clipv3MaxYInside) // { // Vector3.Dot(ref clipFace.V3, ref faceNormal, out a); // Vector3.Multiply(ref faceNormal, a - b, out v); // Vector3.Subtract(ref clipFace.V3, ref v, out v); // item.Position = v; // item.Id = clipFace.Id3 + 8; // contactData.Add(ref item); // } // if (clipv4MinXInside && clipv4MaxXInside && clipv4MinYInside && clipv4MaxYInside) // { // Vector3.Dot(ref clipFace.V4, ref faceNormal, out a); // Vector3.Multiply(ref faceNormal, a - b, out v); // Vector3.Subtract(ref clipFace.V4, ref v, out v); // item.Position = v; // item.Id = clipFace.Id4 + 8; // contactData.Add(ref item); // } // #endregion // //Compute depths. // int postClipCount = contactData.Count; // tempData = contactData; // for (int i = postClipCount - 1; i >= previousCount; i--) //TODO: >=? // contactData.RemoveAt(i); // for (int i = previousCount; i < tempData.Count; i++) // { // tempData.Get(i, out item); // Vector3.Dot(ref item.Position, ref mtd, out faceDot); // item.Depth = faceDot - clipFaceDot; // if (item.Depth <= 0) // { // contactData.Add(ref item); // } // } // previousCount = contactData.Count; // if (previousCount >= 4) //Early finish :) // { // return; // } // //Intersect edges. // //maxX maxY -> v1 // //minX maxY -> v2 // //minX minY -> v3 // //maxX minY -> v4 // //Once we get here there can only be 3 contacts or less. // //Once 4 possible contacts have been added, switch to using safe increments. // float dot; // #region CLIP EDGE: v1 v2 // FaceEdge clipEdge; // clipFace.GetEdge(0, ref mtd, out clipEdge); // if (!v1MaxYInside) // { // if (v2MaxYInside) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MaxYInside) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v2MaxYInside) // { // if (v1MaxYInside) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MaxYInside) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v3MaxYInside) // { // if (v2MaxYInside) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MaxYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v4MaxYInside) // { // if (v1MaxYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MaxYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // #endregion // #region CLIP EDGE: v2 v3 // clipFace.GetEdge(1, ref mtd, out clipEdge); // if (!v1MinXInside) // { // if (v2MinXInside && contactData.Count < 8) // { // //test v1-v2 against minXminY-minXmaxY // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MinXInside && contactData.Count < 8) // { // //test v1-v3 against minXminY-minXmaxY // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v2MinXInside) // { // if (v1MinXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MinXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v3MinXInside) // { // if (v2MinXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MinXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v4MinXInside) // { // if (v1MinXInside && contactData.Count < 8) // { // //test v2-v4 against minXminY-minXmaxY // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MinXInside && contactData.Count < 8) // { // //test v3-v4 against minXminY-minXmaxY // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // #endregion // #region CLIP EDGE: v3 v4 // clipFace.GetEdge(2, ref mtd, out clipEdge); // if (!v1MinYInside) // { // if (v2MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v2MinYInside) // { // if (v1MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v3MinYInside) // { // if (v2MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v4MinYInside) // { // if (v3MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // if (v1MinYInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipX, ref v, out dot); // if (dot > clipCenterMinX && dot < clipCenterMaxX) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // } // #endregion // #region CLIP EDGE: v4 v1 // clipFace.GetEdge(3, ref mtd, out clipEdge); // if (!v1MaxXInside) // { // if (v2MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v2MaxXInside) // { // if (v1MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V1, ref face.V2, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id1, face.Id2, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v3MaxXInside) // { // if (v2MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V2, ref face.V3, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id2, face.Id3, ref clipEdge); // contactData.Add(ref item); // } // } // if (v4MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // if (!v4MaxXInside) // { // if (v1MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V4, ref face.V1, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id4, face.Id1, ref clipEdge); // contactData.Add(ref item); // } // } // if (v3MaxXInside && contactData.Count < 8) // { // ComputeIntersection(ref face.V3, ref face.V4, ref clipEdge, out v); // Vector3.Dot(ref clipY, ref v, out dot); // if (dot > clipCenterMinY && dot < clipCenterMaxY) // { // item.Position = v; // item.Id = GetContactId(face.Id3, face.Id4, ref clipEdge); // contactData.Add(ref item); // } // } // } // #endregion // //Compute depths. // postClipCount = contactData.Count; // tempData = contactData; // for (int i = postClipCount - 1; i >= previousCount; i--) // contactData.RemoveAt(i); // for (int i = previousCount; i < tempData.Count; i++) // { // tempData.Get(i, out item); // Vector3.Dot(ref item.Position, ref mtd, out faceDot); // item.Depth = faceDot - clipFaceDot; // if (item.Depth <= 0) // { // contactData.Add(ref item); // } // } //} #endif private static bool ComputeIntersection(ref Vector3 edgeA1, ref Vector3 edgeA2, ref FaceEdge clippingEdge, out Vector3 intersection) { //Intersect the incoming edge (edgeA1, edgeA2) with the clipping edge's PLANE. Nicely given by one of its positions and its 'perpendicular,' //which is its normal. Vector3 offset; Vector3.Subtract(ref clippingEdge.A, ref edgeA1, out offset); Vector3 edgeDirection; Vector3.Subtract(ref edgeA2, ref edgeA1, out edgeDirection); float distanceToPlane; Vector3.Dot(ref offset, ref clippingEdge.Perpendicular, out distanceToPlane); float edgeDirectionLength; Vector3.Dot(ref edgeDirection, ref clippingEdge.Perpendicular, out edgeDirectionLength); float t = distanceToPlane / edgeDirectionLength; if (t < 0 || t > 1) { //It's outside of the incoming edge! intersection = new Vector3(); return false; } Vector3.Multiply(ref edgeDirection, t, out offset); Vector3.Add(ref offset, ref edgeA1, out intersection); Vector3.Subtract(ref intersection, ref clippingEdge.A, out offset); Vector3.Subtract(ref clippingEdge.B, ref clippingEdge.A, out edgeDirection); Vector3.Dot(ref edgeDirection, ref offset, out t); if (t < 0 || t > edgeDirection.LengthSquared()) { //It's outside of the clipping edge! return false; } return true; } private static void GetNearestFace(ref Vector3 position, ref Matrix3x3 orientation, ref Vector3 mtd, float halfWidth, float halfHeight, float halfLength, out BoxFace boxFace) { boxFace = new BoxFace(); float xDot = orientation.M11 * mtd.X + orientation.M12 * mtd.Y + orientation.M13 * mtd.Z; float yDot = orientation.M21 * mtd.X + orientation.M22 * mtd.Y + orientation.M23 * mtd.Z; float zDot = orientation.M31 * mtd.X + orientation.M32 * mtd.Y + orientation.M33 * mtd.Z; float absX = Math.Abs(xDot); float absY = Math.Abs(yDot); float absZ = Math.Abs(zDot); Matrix worldTransform; Matrix3x3.ToMatrix4X4(ref orientation, out worldTransform); worldTransform.M41 = position.X; worldTransform.M42 = position.Y; worldTransform.M43 = position.Z; worldTransform.M44 = 1; Vector3 candidate; int bit; if (absX > absY && absX > absZ) { //"X" faces are candidates if (xDot < 0) { halfWidth = -halfWidth; bit = 0; } else bit = 1; candidate = new Vector3(halfWidth, halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V1 = candidate; candidate = new Vector3(halfWidth, -halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V2 = candidate; candidate = new Vector3(halfWidth, -halfHeight, -halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V3 = candidate; candidate = new Vector3(halfWidth, halfHeight, -halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V4 = candidate; if (xDot < 0) boxFace.Normal = orientation.Left; else boxFace.Normal = orientation.Right; boxFace.Width = halfHeight * 2; boxFace.Height = halfLength * 2; boxFace.Id1 = bit + 2 + 4; boxFace.Id2 = bit + 4; boxFace.Id3 = bit + 2; boxFace.Id4 = bit; } else if (absY > absX && absY > absZ) { //"Y" faces are candidates if (yDot < 0) { halfHeight = -halfHeight; bit = 0; } else bit = 2; candidate = new Vector3(halfWidth, halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V1 = candidate; candidate = new Vector3(-halfWidth, halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V2 = candidate; candidate = new Vector3(-halfWidth, halfHeight, -halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V3 = candidate; candidate = new Vector3(halfWidth, halfHeight, -halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V4 = candidate; if (yDot < 0) boxFace.Normal = orientation.Down; else boxFace.Normal = orientation.Up; boxFace.Width = halfWidth * 2; boxFace.Height = halfLength * 2; boxFace.Id1 = 1 + bit + 4; boxFace.Id2 = bit + 4; boxFace.Id3 = 1 + bit; boxFace.Id4 = bit; } else if (absZ > absX && absZ > absY) { //"Z" faces are candidates if (zDot < 0) { halfLength = -halfLength; bit = 0; } else bit = 4; candidate = new Vector3(halfWidth, halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V1 = candidate; candidate = new Vector3(-halfWidth, halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V2 = candidate; candidate = new Vector3(-halfWidth, -halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V3 = candidate; candidate = new Vector3(halfWidth, -halfHeight, halfLength); Vector3.Transform(ref candidate, ref worldTransform, out candidate); boxFace.V4 = candidate; if (zDot < 0) boxFace.Normal = orientation.Forward; else boxFace.Normal = orientation.Backward; boxFace.Width = halfWidth * 2; boxFace.Height = halfHeight * 2; boxFace.Id1 = 1 + 2 + bit; boxFace.Id2 = 2 + bit; boxFace.Id3 = 1 + bit; boxFace.Id4 = bit; } } private struct BoxFace { public int Id1, Id2, Id3, Id4; public Vector3 V1, V2, V3, V4; public Vector3 Normal; public float Width, Height; public int GetId(int i) { switch (i) { case 0: return Id1; case 1: return Id2; case 2: return Id3; case 3: return Id4; } return -1; } public void GetVertex(int i, out Vector3 v) { switch (i) { case 0: v = V1; return; case 1: v = V2; return; case 2: v = V3; return; case 3: v = V4; return; } v = Toolbox.NoVector; } internal void GetEdge(int i, out FaceEdge clippingEdge) { Vector3 insidePoint; switch (i) { case 0: clippingEdge.A = V1; clippingEdge.B = V2; insidePoint = V3; clippingEdge.Id = GetEdgeId(Id1, Id2); break; case 1: clippingEdge.A = V2; clippingEdge.B = V3; insidePoint = V4; clippingEdge.Id = GetEdgeId(Id2, Id3); break; case 2: clippingEdge.A = V3; clippingEdge.B = V4; insidePoint = V1; clippingEdge.Id = GetEdgeId(Id3, Id4); break; case 3: clippingEdge.A = V4; clippingEdge.B = V1; insidePoint = V2; clippingEdge.Id = GetEdgeId(Id4, Id1); break; default: throw new IndexOutOfRangeException(); } //TODO: Edge direction and perpendicular not normalized. Vector3 edgeDirection; Vector3.Subtract(ref clippingEdge.B, ref clippingEdge.A, out edgeDirection); edgeDirection.Normalize(); Vector3.Cross(ref edgeDirection, ref Normal, out clippingEdge.Perpendicular); float dot; Vector3 offset; Vector3.Subtract(ref insidePoint, ref clippingEdge.A, out offset); Vector3.Dot(ref clippingEdge.Perpendicular, ref offset, out dot); if (dot > 0) { clippingEdge.Perpendicular.X = -clippingEdge.Perpendicular.X; clippingEdge.Perpendicular.Y = -clippingEdge.Perpendicular.Y; clippingEdge.Perpendicular.Z = -clippingEdge.Perpendicular.Z; } Vector3.Dot(ref clippingEdge.A, ref clippingEdge.Perpendicular, out clippingEdge.EdgeDistance); } } private static int GetContactId(int vertexAEdgeA, int vertexBEdgeA, int vertexAEdgeB, int vertexBEdgeB) { return GetEdgeId(vertexAEdgeA, vertexBEdgeA) * 2549 + GetEdgeId(vertexAEdgeB, vertexBEdgeB) * 2857; } private static int GetContactId(int vertexAEdgeA, int vertexBEdgeA, ref FaceEdge clippingEdge) { return GetEdgeId(vertexAEdgeA, vertexBEdgeA) * 2549 + clippingEdge.Id * 2857; } private static int GetEdgeId(int id1, int id2) { return (id1 + 1) * 571 + (id2 + 1) * 577; } private struct FaceEdge : IEquatable { public Vector3 A, B; public float EdgeDistance; public int Id; public Vector3 Perpendicular; #region IEquatable Members public bool Equals(FaceEdge other) { return other.Id == Id; } #endregion public bool IsPointInside(ref Vector3 point) { float distance; Vector3.Dot(ref point, ref Perpendicular, out distance); return distance < EdgeDistance; // +1; //TODO: Bias this a little? } } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/BoxSphereTester.cs ================================================ using System; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.Settings; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Static class with methods to help with testing box shapes against sphere shapes. /// public static class BoxSphereTester { /// /// Tests if a box and sphere are colliding. /// ///Box to test. ///Sphere to test. ///Transform to apply to the box. ///Transform to apply to the sphere. ///Contact point between the shapes, if any. ///Whether or not the shapes were colliding. public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact) { contact = new ContactData(); Vector3 localPosition; RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition); #if !WINDOWS Vector3 localClosestPoint = new Vector3(); #else Vector3 localClosestPoint; #endif localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth); localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight); localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength); RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position); Vector3 offset; Vector3.Subtract(ref spherePosition, ref contact.Position, out offset); float offsetLength = offset.LengthSquared(); if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance)) { return false; } //Colliding. if (offsetLength > Toolbox.Epsilon) { offsetLength = (float)Math.Sqrt(offsetLength); //Outside of the box. Vector3.Divide(ref offset, offsetLength, out contact.Normal); contact.PenetrationDepth = sphere.collisionMargin - offsetLength; } else { //Inside of the box. Vector3 penetrationDepths; penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X; penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y; penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z; if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z) { contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector; contact.PenetrationDepth = penetrationDepths.X; } else if (penetrationDepths.Y < penetrationDepths.Z) { contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector; contact.PenetrationDepth = penetrationDepths.Y; } else { contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector; contact.PenetrationDepth = penetrationDepths.X; } contact.PenetrationDepth += sphere.collisionMargin; Vector3.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal); } return true; } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/GJK/GJKToolbox.cs ================================================ using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.Settings; using RigidTransform = BEPUutilities.RigidTransform; namespace BEPUphysics.CollisionTests.CollisionAlgorithms.GJK { /// /// Helper class containing various tests based on GJK. /// public static class GJKToolbox { /// /// Maximum number of iterations the GJK algorithm will do. If the iterations exceed this number, the system will immediately quit and return whatever information it has at the time. /// public static int MaximumGJKIterations = 15; /// /// Defines how many iterations are required to consider a GJK attempt to be 'probably stuck' and proceed with protective measures. /// public static int HighGJKIterations = 8; /// /// Tests if the pair is intersecting. /// ///First shape of the pair. ///Second shape of the pair. ///Transform to apply to the first shape. ///Transform to apply to the second shape. ///Whether or not the shapes are intersecting. public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB) { //Zero isn't a very good guess! But it's a cheap guess. Vector3 separatingAxis = Toolbox.ZeroVector; return AreShapesIntersecting(shapeA, shapeB, ref transformA, ref transformB, ref separatingAxis); } /// /// Tests if the pair is intersecting. /// ///First shape of the pair. ///Second shape of the pair. ///Transform to apply to the first shape. ///Transform to apply to the second shape. ///Warmstartable separating axis used by the method to quickly early-out if possible. Updated to the latest separating axis after each run. ///Whether or not the objects were intersecting. public static bool AreShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref Vector3 localSeparatingAxis) { RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); //Warm start the simplex. var simplex = new SimpleSimplex(); Vector3 extremePoint; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref localSeparatingAxis, ref localtransformB, out extremePoint); simplex.AddNewSimplexPoint(ref extremePoint); Vector3 closestPoint; int count = 0; while (count++ < MaximumGJKIterations) { if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex. closestPoint.LengthSquared() <= simplex.GetErrorTolerance() * Toolbox.BigEpsilon) { //Intersecting, or so close to it that it will be difficult/expensive to figure out the separation. return true; } //Use the closest point as a direction. Vector3 direction; Vector3.Negate(ref closestPoint, out direction); MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref direction, ref localtransformB, out extremePoint); //Since this is a boolean test, we don't need to refine the simplex if it becomes apparent that we cannot reach the origin. //If the most extreme point at any given time does not go past the origin, then we can quit immediately. float dot; Vector3.Dot(ref extremePoint, ref closestPoint, out dot); //extreme point dotted against the direction pointing backwards towards the CSO. if (dot > 0) { // If it's positive, that means that the direction pointing towards the origin produced an extreme point 'in front of' the origin, eliminating the possibility of any intersection. localSeparatingAxis = direction; return false; } simplex.AddNewSimplexPoint(ref extremePoint); } return false; } /// /// Gets the closest points between the shapes. /// ///First shape of the pair. ///Second shape of the pair. ///Transform to apply to the first shape. ///Transform to apply to the second shape. ///Closest point on the first shape to the second shape. ///Closest point on the second shape to the first shape. ///Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified. public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, out Vector3 closestPointA, out Vector3 closestPointB) { //The cached simplex stores locations that are local to the shapes. A fairly decent initial state is between the centroids of the objects. //In local space, the centroids are at the origins. RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); var simplex = new CachedSimplex {State = SimplexState.Point}; // new CachedSimplex(shapeA, shapeB, ref localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref simplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return toReturn; } /// /// Gets the closest points between the shapes. /// ///First shape of the pair. ///Second shape of the pair. ///Transform to apply to the first shape. ///Transform to apply to the second shape. /// Simplex from a previous updated used to warmstart the current attempt. Updated after each run. ///Closest point on the first shape to the second shape. ///Closest point on the second shape to the first shape. ///Whether or not the objects were intersecting. If they are intersecting, then the closest points cannot be identified. public static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref CachedSimplex cachedSimplex, out Vector3 closestPointA, out Vector3 closestPointB) { RigidTransform localtransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localtransformB); bool toReturn = GetClosestPoints(shapeA, shapeB, ref localtransformB, ref cachedSimplex, out closestPointA, out closestPointB); RigidTransform.Transform(ref closestPointA, ref transformA, out closestPointA); RigidTransform.Transform(ref closestPointB, ref transformA, out closestPointB); return toReturn; } private static bool GetClosestPoints(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref CachedSimplex cachedSimplex, out Vector3 localClosestPointA, out Vector3 localClosestPointB) { var simplex = new PairSimplex(ref cachedSimplex, ref localTransformB); Vector3 closestPoint; int count = 0; while (true) { if (simplex.GetPointClosestToOrigin(out closestPoint) || //Also reduces the simplex and computes barycentric coordinates if necessary. closestPoint.LengthSquared() <= Toolbox.Epsilon * simplex.errorTolerance) { //Intersecting. localClosestPointA = Toolbox.ZeroVector; localClosestPointB = Toolbox.ZeroVector; simplex.UpdateCachedSimplex(ref cachedSimplex); return true; } if (++count > MaximumGJKIterations) break; //Must break BEFORE a new vertex is added if we're over the iteration limit. This guarantees final simplex is not a tetrahedron. if (simplex.GetNewSimplexPoint(shapeA, shapeB, count, ref closestPoint)) { //No progress towards origin, not intersecting. break; } } //Compute closest points from the contributing simplexes and barycentric coordinates simplex.GetClosestPoints(out localClosestPointA, out localClosestPointB); //simplex.VerifyContributions(); //if (Vector3.Distance(localClosestPointA - localClosestPointB, closestPoint) > .00001f) // Debug.WriteLine("break."); simplex.UpdateCachedSimplex(ref cachedSimplex); return false; } //TODO: Consider changing the termination epsilons on these casts. Epsilon * Modifier is okay, but there might be better options. /// /// Tests a ray against a convex shape. /// ///Ray to test against the shape. ///Shape to test. ///Transform to apply to the shape for the test. ///Maximum length of the ray in units of the ray direction's length. ///Hit data of the ray cast, if any. ///Whether or not the ray hit the shape. public static bool RayCast(Ray ray, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 extremePointToRayOrigin, extremePoint; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 closestOffset = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, closestPointDotDirection; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (closestOffset.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } shape.GetLocalExtremePoint(closestOffset, out extremePoint); Vector3.Subtract(ref hit.Location, ref extremePoint, out extremePointToRayOrigin); Vector3.Dot(ref closestOffset, ref extremePointToRayOrigin, out vw); //If the closest offset and the extreme point->ray origin direction point the same way, //then we might be able to conservatively advance the point towards the surface. if (vw > 0) { Vector3.Dot(ref closestOffset, ref ray.Direction, out closestPointDotDirection); if (closestPointDotDirection >= 0) { hit = new RayHit(); return false; } hit.T = hit.T - vw / closestPointDotDirection; if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return false; } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = closestOffset; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref extremePoint, ref hit.Location, out shiftedSimplex); //Compute the offset from the simplex surface to the origin. shiftedSimplex.GetPointClosestToOrigin(ref simplex, out closestOffset); } //Transform the hit data into world space. Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return true; } /// /// Sweeps a shape against another shape using a given sweep vector. /// ///Shape to sweep. ///Shape being swept against. ///Sweep vector for the sweptShape. ///Starting transform of the sweptShape. ///Transform to apply to the target shape. ///Hit data of the sweep test, if any. ///Whether or not the swept shape hit the other shape. public static bool ConvexCast(ConvexShape sweptShape, ConvexShape target, ref Vector3 sweep, ref RigidTransform startingSweptTransform, ref RigidTransform targetTransform, out RayHit hit) { return ConvexCast(sweptShape, target, ref sweep, ref Toolbox.ZeroVector, ref startingSweptTransform, ref targetTransform, out hit); } /// /// Sweeps two shapes against another. /// ///First shape being swept. ///Second shape being swept. ///Sweep vector for the first shape. ///Sweep vector for the second shape. ///Transform to apply to the first shape. ///Transform to apply to the second shape. ///Hit data of the sweep test, if any. ///Whether or not the swept shapes hit each other.. public static bool ConvexCast(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB, out RayHit hit) { //Put the velocity into shapeA's local space. Vector3 velocityWorld; Vector3.Subtract(ref sweepB, ref sweepA, out velocityWorld); Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Vector3 rayDirection; Vector3.Transform(ref velocityWorld, ref conjugateOrientationA, out rayDirection); //Transform b into a's local space. RigidTransform localTransformB; Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation); Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position); Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position); Vector3 w, p; hit.T = 0; hit.Location = Vector3.Zero; //The ray starts at the origin. hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; do { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref v, ref localTransformB, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref rayDirection, out vdir); if (vdir >= 0) { hit = new RayHit(); return false; } hit.T = hit.T - vw / vdir; if (hit.T > 1) { //If we've gone beyond where the ray can reach, there's obviously no hit. hit = new RayHit(); return false; } //Shift the ray up. Vector3.Multiply(ref rayDirection, hit.T, out hit.Location); //The ray origin is the origin! Don't need to add any ray position. hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); //Could measure the progress of the ray. If it's too little, could early out. //Not used by default since it's biased towards precision over performance. } while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref Toolbox.ZeroVector)); //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. //Transform the hit data into world space. Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal); Vector3.Multiply(ref velocityWorld, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location); return true; } /// /// Casts a fat (sphere expanded) ray against the shape. /// ///Ray to test against the shape. ///Radius of the ray. ///Shape to test against. ///Transform to apply to the shape for the test. ///Maximum length of the ray in units of the ray direction's length. ///Hit data of the sphere cast, if any. ///Whether or not the sphere cast hit the shape. public static bool SphereCast(Ray ray, float radius, ConvexShape shape, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { //Transform the ray into the object's local space. Vector3.Subtract(ref ray.Position, ref shapeTransform.Position, out ray.Position); Quaternion conjugate; Quaternion.Conjugate(ref shapeTransform.Orientation, out conjugate); Vector3.Transform(ref ray.Position, ref conjugate, out ray.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out ray.Direction); Vector3 w, p; hit.T = 0; hit.Location = ray.Position; hit.Normal = Toolbox.ZeroVector; Vector3 v = hit.Location; RaySimplex simplex = new RaySimplex(); float vw, vdir; int count = 0; //This epsilon has a significant impact on performance and accuracy. Changing it to use BigEpsilon instead increases speed by around 30-40% usually, but jigging is more evident. while (v.LengthSquared() >= Toolbox.Epsilon * simplex.GetErrorTolerance(ref ray.Position)) { if (++count > MaximumGJKIterations) { //It's taken too long to find a hit. Numerical problems are probable; quit. hit = new RayHit(); return false; } shape.GetLocalExtremePointWithoutMargin(ref v, out p); Vector3 contribution; MinkowskiToolbox.ExpandMinkowskiSum(shape.collisionMargin, radius, ref v, out contribution); Vector3.Add(ref p, ref contribution, out p); Vector3.Subtract(ref hit.Location, ref p, out w); Vector3.Dot(ref v, ref w, out vw); if (vw > 0) { Vector3.Dot(ref v, ref ray.Direction, out vdir); hit.T = hit.T - vw / vdir; if (vdir >= 0) { //We would have to back up! return false; } if (hit.T > maximumLength) { //If we've gone beyond where the ray can reach, there's obviously no hit. return false; } //Shift the ray up. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); hit.Normal = v; } RaySimplex shiftedSimplex; simplex.AddNewSimplexPoint(ref p, ref hit.Location, out shiftedSimplex); shiftedSimplex.GetPointClosestToOrigin(ref simplex, out v); } //Transform the hit data into world space. Vector3.Transform(ref hit.Normal, ref shapeTransform.Orientation, out hit.Normal); Vector3.Transform(ref hit.Location, ref shapeTransform.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref shapeTransform.Position, out hit.Location); return true; } /// /// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted /// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time). /// ///Ray to test against the shape. ///Radius of the ray. ///Shape to test against. ///Transform to apply to the shape for the test. ///Maximum length of the ray in units of the ray direction's length. ///Hit data of the sphere cast, if any. ///Whether or not the sphere cast hit the shape. public static bool CCDSphereCast(Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength, out RayHit hit) { int iterations = 0; while (true) { if (GJKToolbox.SphereCast(ray, radius, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0) { //The ray cast isn't embedded in the shape, and it's less than maximum length away! return true; } if (hit.T > maximumLength || hit.T < 0) return false; //Failure showed it was too far, or behind. radius *= MotionSettings.CoreShapeScaling; iterations++; if (iterations > 3) //Limit could be configurable. { //It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help. return GJKToolbox.RayCast(ray, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0; } } } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/GJK/PairSimplex.cs ================================================ using BEPUphysics.CollisionShapes.ConvexShapes; using Microsoft.Xna.Framework; using BEPUutilities; using System.Diagnostics; namespace BEPUphysics.CollisionTests.CollisionAlgorithms.GJK { /// /// Defines the state of a simplex. /// public enum SimplexState : byte { Empty, Point, Segment, Triangle, Tetrahedron } /// /// Stored simplex used to warmstart closest point GJK runs. /// public struct CachedSimplex { //public CachedSimplex(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB) //{ // RigidTransform localTransformB; // MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); // LocalSimplexA = new ContributingShapeSimplex(); // LocalSimplexB = new ContributingShapeSimplex(); // State = SimplexState.Point; // return; // shapeA.GetLocalExtremePointWithoutMargin(ref localTransformB.Position, out LocalSimplexA.A); // Vector3 direction; // Vector3.Negate(ref localTransformB.Position, out direction); // Quaternion conjugate; // Quaternion.Conjugate(ref localTransformB.Orientation, out conjugate); // Vector3.Transform(ref direction, ref conjugate, out direction); // shapeB.GetLocalExtremePointWithoutMargin(ref direction, out LocalSimplexB.A); //} //public CachedSimplex(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB) //{ // LocalSimplexA = new ContributingShapeSimplex(); // LocalSimplexB = new ContributingShapeSimplex(); // State = SimplexState.Point; // return; // shapeA.GetLocalExtremePointWithoutMargin(ref localTransformB.Position, out LocalSimplexA.A); // Vector3 direction; // Vector3.Negate(ref localTransformB.Position, out direction); // Quaternion conjugate; // Quaternion.Conjugate(ref localTransformB.Orientation, out conjugate); // Vector3.Transform(ref direction, ref conjugate, out direction); // shapeB.GetLocalExtremePointWithoutMargin(ref direction, out LocalSimplexB.A); //} /// /// Simplex in the local space of shape A. /// public ContributingShapeSimplex LocalSimplexA; /// /// Simplex in the local space of shape B. /// public ContributingShapeSimplex LocalSimplexB; /// /// State of the simplex at the termination of the last GJK run. /// public SimplexState State; } /// /// List of points composing a shape's contributions to a simplex. /// public struct ContributingShapeSimplex { public Vector3 A; public Vector3 B; public Vector3 C; public Vector3 D; } /// /// GJK simplex used to support closest point tests with warmstarting. /// public struct PairSimplex { /// /// The baseline amount that a GJK iteration must progress through to avoid exiting. /// Defaults to 1e-8f. /// public static float ProgressionEpsilon = 1e-8f; /// /// The baseline amount that an iteration must converge with its distance to avoid exiting. /// Defaults to 1e-7f. /// public static float DistanceConvergenceEpsilon = 1e-7f; /// /// Simplex as viewed from the local space of A. /// public ContributingShapeSimplex SimplexA; /// /// Simplex as viewed from the local space of B. /// public ContributingShapeSimplex SimplexB; public Vector3 A; public Vector3 B; public Vector3 C; public Vector3 D; public SimplexState State; /// /// Weight of vertex A. /// public float U; /// /// Weight of vertex B. /// public float V; /// /// Weight of vertex C. /// public float W; /// /// Transform of the second shape in the first shape's local space. /// public RigidTransform LocalTransformB; private PairSimplex(ref RigidTransform localTransformB) { //This isn't a very good approach since the transform position is not guaranteed to be within the object. Would have to use the GetNewSimplexPoint to make it valid. previousDistanceToClosest = float.MaxValue; errorTolerance = 0; LocalTransformB = localTransformB; //Warm up the simplex using the centroids. //Could also use the GetNewSimplexPoint if it had a Empty case, but test before choosing. State = SimplexState.Point; SimplexA = new ContributingShapeSimplex(); SimplexB = new ContributingShapeSimplex {A = localTransformB.Position}; //minkowski space support = shapeA-shapeB = 0,0,0 - positionB Vector3.Negate(ref localTransformB.Position, out A); B = new Vector3(); C = new Vector3(); D = new Vector3(); U = 0; V = 0; W = 0; } /// /// Constructs a new pair simplex. /// ///Cached simplex to use to warmstart the simplex. ///Transform of shape B in the local space of A. public PairSimplex(ref CachedSimplex cachedSimplex, ref RigidTransform localTransformB) { //NOTE: //USING A CACHED SIMPLEX INVALIDATES ASSUMPTIONS THAT ALLOW SIMPLEX CASES TO BE IGNORED! //To get those assumptions back, either DO NOT USE CACHED SIMPLEXES, or //VERIFY THE SIMPLEXES. //-A point requires no verification. //-A segment needs verification that the origin is in front of A in the direction of B. //-A triangle needs verification that the origin is within the edge planes and in the direction of C. //-A tetrahedron needs verification that the origin is within the edge planes of triangle ABC and is in the direction of D. //This simplex implementation will not ignore any cases, so we can warm start safely with one problem. //Due to relative movement, the simplex may become degenerate. Edges could become points, etc. //Some protections are built into the simplex cases, but keep an eye out for issues. //Most dangerous degeneracy seen so far is tetrahedron. It fails to find any points on opposing sides due to numerical problems and returns intersection. previousDistanceToClosest = float.MaxValue; errorTolerance = 0; LocalTransformB = localTransformB; //Transform the SimplexB into the working space of the simplex and compute the working space simplex. State = cachedSimplex.State; SimplexA = cachedSimplex.LocalSimplexA; SimplexB = new ContributingShapeSimplex(); U = 0; V = 0; W = 0; switch (State) { case SimplexState.Point: Vector3.Transform(ref cachedSimplex.LocalSimplexB.A, ref LocalTransformB.Orientation, out SimplexB.A); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; case SimplexState.Segment: Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); C = new Vector3(); D = new Vector3(); ////Test for degeneracy. //float edgeLengthAB; //Vector3.DistanceSquared(ref A, ref B, out edgeLengthAB); //if (edgeLengthAB < Toolbox.Epsilon) // State = SimplexState.Point; break; case SimplexState.Triangle: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); D = new Vector3(); ////Test for degeneracy. //Vector3 AB, AC; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3 cross; //Vector3.Cross(ref AB, ref AC, out cross); ////If the area is small compared to a tolerance (adjusted by the partial perimeter), it's degenerate. //if (cross.LengthSquared() < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared())) // State = SimplexState.Point; break; case SimplexState.Tetrahedron: Matrix3x3.CreateFromQuaternion(ref localTransformB.Orientation, out transform); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.A, ref transform, out SimplexB.A); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.B, ref transform, out SimplexB.B); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.C, ref transform, out SimplexB.C); Matrix3x3.Transform(ref cachedSimplex.LocalSimplexB.D, ref transform, out SimplexB.D); Vector3.Add(ref SimplexB.A, ref LocalTransformB.Position, out SimplexB.A); Vector3.Add(ref SimplexB.B, ref LocalTransformB.Position, out SimplexB.B); Vector3.Add(ref SimplexB.C, ref LocalTransformB.Position, out SimplexB.C); Vector3.Add(ref SimplexB.D, ref LocalTransformB.Position, out SimplexB.D); Vector3.Subtract(ref SimplexA.A, ref SimplexB.A, out A); Vector3.Subtract(ref SimplexA.B, ref SimplexB.B, out B); Vector3.Subtract(ref SimplexA.C, ref SimplexB.C, out C); Vector3.Subtract(ref SimplexA.D, ref SimplexB.D, out D); ////Test for degeneracy. //Vector3 AD; //Vector3.Subtract(ref B, ref A, out AB); //Vector3.Subtract(ref C, ref A, out AC); //Vector3.Subtract(ref D, ref A, out AD); //Vector3.Cross(ref AB, ref AC, out cross); //float volume; //Vector3.Dot(ref cross, ref AD, out volume); ////Volume is small compared to partial 'perimeter.' //if (volume < Toolbox.BigEpsilon * (AB.LengthSquared() + AC.LengthSquared() + AD.LengthSquared())) // State = SimplexState.Point; break; default: A = new Vector3(); B = new Vector3(); C = new Vector3(); D = new Vector3(); break; } } /// /// Updates the cached simplex with the latest run's results. /// ///Simplex to update. public void UpdateCachedSimplex(ref CachedSimplex simplex) { simplex.LocalSimplexA = SimplexA; switch (State) { case SimplexState.Point: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Quaternion conjugate; Quaternion.Conjugate(ref LocalTransformB.Orientation, out conjugate); Vector3.Transform(ref simplex.LocalSimplexB.A, ref conjugate, out simplex.LocalSimplexB.A); break; case SimplexState.Segment: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Matrix3x3 transform; Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); break; case SimplexState.Triangle: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); break; case SimplexState.Tetrahedron: Vector3.Subtract(ref SimplexB.A, ref LocalTransformB.Position, out simplex.LocalSimplexB.A); Vector3.Subtract(ref SimplexB.B, ref LocalTransformB.Position, out simplex.LocalSimplexB.B); Vector3.Subtract(ref SimplexB.C, ref LocalTransformB.Position, out simplex.LocalSimplexB.C); Vector3.Subtract(ref SimplexB.D, ref LocalTransformB.Position, out simplex.LocalSimplexB.D); Matrix3x3.CreateFromQuaternion(ref LocalTransformB.Orientation, out transform); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.A, ref transform, out simplex.LocalSimplexB.A); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.B, ref transform, out simplex.LocalSimplexB.B); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.C, ref transform, out simplex.LocalSimplexB.C); Matrix3x3.TransformTranspose(ref simplex.LocalSimplexB.D, ref transform, out simplex.LocalSimplexB.D); break; } simplex.State = State; } /// /// Gets the point on the simplex closest to the origin. /// ///Point closest to the origin. ///Whether or not the simplex encloses the origin. public bool GetPointClosestToOrigin(out Vector3 point) { //This method finds the closest point on the simplex to the origin. //Barycentric coordinates are assigned to the MinimumNormCoordinates as necessary to perform the inclusion calculation. //If the simplex is a tetrahedron and found to be overlapping the origin, the function returns true to tell the caller to terminate. //Elements of the simplex that are not used to determine the point of minimum norm are removed from the simplex. switch (State) { case SimplexState.Point: point = A; U = 1; break; case SimplexState.Segment: GetPointOnSegmentClosestToOrigin(out point); break; case SimplexState.Triangle: GetPointOnTriangleClosestToOrigin(out point); break; case SimplexState.Tetrahedron: return GetPointOnTetrahedronClosestToOrigin(out point); default: point = Toolbox.ZeroVector; break; } return false; } /// /// Gets the point on the segment closest to the origin. /// ///Point closest to origin. public void GetPointOnSegmentClosestToOrigin(out Vector3 point) { Vector3 segmentDisplacement; Vector3.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3.Dot(ref segmentDisplacement, ref A, out dotA); if (dotA > 0) { //'Behind' segment. This can't happen in a boolean version, //but with closest points warmstarting or raycasts, it will. State = SimplexState.Point; U = 1; point = A; return; } float dotB; Vector3.Dot(ref segmentDisplacement, ref B, out dotB); if (dotB > 0) { //Inside segment. U = dotB / segmentDisplacement.LengthSquared(); V = 1 - U; Vector3.Multiply(ref segmentDisplacement, V, out point); Vector3.Add(ref point, ref A, out point); return; } //It should be possible in the warmstarted closest point calculation/raycasting to be outside B. //It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found. //Outside B. //Remove current A; we're becoming a point. A = B; SimplexA.A = SimplexA.B; SimplexB.A = SimplexB.B; State = SimplexState.Point; U = 1; point = A; } /// /// Gets the point on the triangle closest to the origin. /// ///Point closest to origin. public void GetPointOnTriangleClosestToOrigin(out Vector3 point) { Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->P, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! State = SimplexState.Point; U = 1; point = A; return; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! State = SimplexState.Point; A = B; U = 1; SimplexA.A = SimplexA.B; SimplexB.A = SimplexB.B; point = B; return; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero { State = SimplexState.Segment; V = AdotAB / (AdotAB - BdotAB); U = 1 - V; Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! State = SimplexState.Point; A = C; SimplexA.A = SimplexA.C; SimplexB.A = SimplexB.C; U = 1; point = A; return; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Get rid of B. Compress C into B. State = SimplexState.Segment; B = C; SimplexA.B = SimplexA.C; SimplexB.B = SimplexB.C; V = AdotAC / (AdotAC - CdotAC); U = 1 - V; Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Throw away A. C->A. //TODO: Does B->A, C->B work better? State = SimplexState.Segment; A = C; SimplexA.A = SimplexA.C; SimplexB.A = SimplexB.C; U = d3d4 / (d3d4 + d6d5); V = 1 - U; Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, U, out point); Vector3.Add(ref point, ref B, out point); return; } //On the face of the triangle. float denom = 1f / (va + vb + vc); V = vb * denom; W = vc * denom; U = 1 - V - W; Vector3.Multiply(ref ab, V, out point); Vector3 acw; Vector3.Multiply(ref ac, W, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); } /// /// Gets the point on the tetrahedron closest to the origin. /// ///Closest point to the origin. ///Whether or not the tetrahedron encloses the origin. public bool GetPointOnTetrahedronClosestToOrigin(out Vector3 point) { //Thanks to the fact that D is new and that we know that the origin is within the extruded //triangular prism of ABC (and on the "D" side of ABC), //we can immediately ignore voronoi regions: //A, B, C, AC, AB, BC, ABC //and only consider: //D, DA, DB, DC, DAC, DCB, DBA //There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully. PairSimplex minimumSimplex = new PairSimplex(); point = new Vector3(); float minimumDistance = float.MaxValue; PairSimplex candidate; float candidateDistance; Vector3 candidatePoint; if (TryTetrahedronTriangle(ref A, ref C, ref D, ref SimplexA.A, ref SimplexA.C, ref SimplexA.D, ref SimplexB.A, ref SimplexB.C, ref SimplexB.D, errorTolerance, ref B, out candidate, out candidatePoint)) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidatePoint.LengthSquared(); } //Try BDC instead of CBD if (TryTetrahedronTriangle(ref B, ref D, ref C, ref SimplexA.B, ref SimplexA.D, ref SimplexA.C, ref SimplexB.B, ref SimplexB.D, ref SimplexB.C, errorTolerance, ref A, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } //Try ADB instead of BAD if (TryTetrahedronTriangle(ref A, ref D, ref B, ref SimplexA.A, ref SimplexA.D, ref SimplexA.B, ref SimplexB.A, ref SimplexB.D, ref SimplexB.B, errorTolerance, ref C, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref A, ref B, ref C, ref SimplexA.A, ref SimplexA.B, ref SimplexA.C, ref SimplexB.A, ref SimplexB.B, ref SimplexB.C, errorTolerance, ref D, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (minimumDistance < float.MaxValue) { minimumSimplex.LocalTransformB = LocalTransformB; minimumSimplex.previousDistanceToClosest = previousDistanceToClosest; minimumSimplex.errorTolerance = errorTolerance; this = minimumSimplex; return false; } return true; } private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 A1, ref Vector3 B1, ref Vector3 C1, ref Vector3 A2, ref Vector3 B2, ref Vector3 C2, float errorTolerance, ref Vector3 otherPoint, out PairSimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new PairSimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN >= -Toolbox.Epsilon * errorTolerance) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; simplex.A = A; simplex.U = 1; simplex.SimplexA.A = A1; simplex.SimplexB.A = A2; point = A; return true; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = B; simplex.U = 1; simplex.SimplexA.A = B1; simplex.SimplexB.A = B2; point = B; return true; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; simplex.V = AdotAB / (AdotAB - BdotAB); simplex.U = 1 - simplex.V; simplex.A = A; simplex.B = B; simplex.SimplexA.A = A1; simplex.SimplexB.A = A2; simplex.SimplexA.B = B1; simplex.SimplexB.B = B2; Vector3.Multiply(ref ab, simplex.V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = C; simplex.U = 1; simplex.SimplexA.A = C1; simplex.SimplexB.A = C2; point = C; return true; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = A; simplex.B = C; simplex.SimplexA.A = A1; simplex.SimplexA.B = C1; simplex.SimplexB.A = A2; simplex.SimplexB.B = C2; simplex.V = AdotAC / (AdotAC - CdotAC); simplex.U = 1 - simplex.V; Vector3.Multiply(ref ac, simplex.V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = B; simplex.B = C; simplex.SimplexA.A = B1; simplex.SimplexA.B = C1; simplex.SimplexB.A = B2; simplex.SimplexB.B = C2; simplex.V = d3d4 / (d3d4 + d6d5); simplex.U = 1 - simplex.V; Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, simplex.V, out point); Vector3.Add(ref point, ref B, out point); return true; } //On the face of the triangle. simplex.A = A; simplex.B = B; simplex.C = C; simplex.SimplexA.A = A1; simplex.SimplexA.B = B1; simplex.SimplexA.C = C1; simplex.SimplexB.A = A2; simplex.SimplexB.B = B2; simplex.SimplexB.C = C2; simplex.State = SimplexState.Triangle; float denom = 1f / (va + vb + vc); simplex.W = vc * denom; simplex.V = vb * denom; simplex.U = 1 - simplex.V - simplex.W; Vector3.Multiply(ref ab, simplex.V, out point); Vector3 acw; Vector3.Multiply(ref ac, simplex.W, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return true; } return false; } internal float errorTolerance; /// /// Gets the error tolerance of the simplex. /// public float ErrorTolerance { get { return errorTolerance; } } float previousDistanceToClosest; /// /// Adds a new point to the simplex. /// ///First shape in the pair. ///Second shape in the pair. ///Current iteration count. ///Current point on simplex closest to origin. ///Whether or not GJK should exit due to a lack of progression. public bool GetNewSimplexPoint(ConvexShape shapeA, ConvexShape shapeB, int iterationCount, ref Vector3 closestPoint) { Vector3 negativeDirection; Vector3.Negate(ref closestPoint, out negativeDirection); Vector3 sa, sb; shapeA.GetLocalExtremePointWithoutMargin(ref negativeDirection, out sa); shapeB.GetExtremePointWithoutMargin(closestPoint, ref LocalTransformB, out sb); Vector3 S; Vector3.Subtract(ref sa, ref sb, out S); //If S is not further towards the origin along negativeDirection than closestPoint, then we're done. float dotS; Vector3.Dot(ref S, ref negativeDirection, out dotS); //-P * S float distanceToClosest = closestPoint.LengthSquared(); float progression = dotS + distanceToClosest; //It's likely that the system is oscillating between two or more states, usually because of a degenerate simplex. //Rather than detect specific problem cases, this approach just lets it run and catches whatever falls through. //During oscillation, one of the states is usually just BARELY outside of the numerical tolerance. //After a bunch of iterations, the system lets it pick the 'better' one. if (iterationCount > GJKToolbox.HighGJKIterations && distanceToClosest - previousDistanceToClosest < DistanceConvergenceEpsilon * errorTolerance) return true; if (distanceToClosest < previousDistanceToClosest) previousDistanceToClosest = distanceToClosest; //If "A" is the new point always, then the switch statement can be removed //in favor of just pushing three points up. switch (State) { case SimplexState.Point: if (progression <= (errorTolerance = MathHelper.Max(A.LengthSquared(), S.LengthSquared())) * ProgressionEpsilon) return true; State = SimplexState.Segment; B = S; SimplexA.B = sa; SimplexB.B = sb; return false; case SimplexState.Segment: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), S.LengthSquared())) * ProgressionEpsilon) return true; State = SimplexState.Triangle; C = S; SimplexA.C = sa; SimplexB.C = sb; return false; case SimplexState.Triangle: if (progression <= (errorTolerance = MathHelper.Max(MathHelper.Max(A.LengthSquared(), B.LengthSquared()), MathHelper.Max(C.LengthSquared(), S.LengthSquared()))) * ProgressionEpsilon) return true; State = SimplexState.Tetrahedron; D = S; SimplexA.D = sa; SimplexB.D = sb; return false; } return false; } /// /// Gets the closest points by using the barycentric coordinates and shape simplex contributions. /// ///Closest point on shape A. ///Closest point on shape B. public void GetClosestPoints(out Vector3 closestPointA, out Vector3 closestPointB) { //A * U + B * V + C * W switch (State) { case SimplexState.Point: closestPointA = SimplexA.A; closestPointB = SimplexB.A; return; case SimplexState.Segment: Vector3 temp; Vector3.Multiply(ref SimplexA.A, U, out closestPointA); Vector3.Multiply(ref SimplexA.B, V, out temp); Vector3.Add(ref closestPointA, ref temp, out closestPointA); Vector3.Multiply(ref SimplexB.A, U, out closestPointB); Vector3.Multiply(ref SimplexB.B, V, out temp); Vector3.Add(ref closestPointB, ref temp, out closestPointB); return; case SimplexState.Triangle: Vector3.Multiply(ref SimplexA.A, U, out closestPointA); Vector3.Multiply(ref SimplexA.B, V, out temp); Vector3.Add(ref closestPointA, ref temp, out closestPointA); Vector3.Multiply(ref SimplexA.C, W, out temp); Vector3.Add(ref closestPointA, ref temp, out closestPointA); Vector3.Multiply(ref SimplexB.A, U, out closestPointB); Vector3.Multiply(ref SimplexB.B, V, out temp); Vector3.Add(ref closestPointB, ref temp, out closestPointB); Vector3.Multiply(ref SimplexB.C, W, out temp); Vector3.Add(ref closestPointB, ref temp, out closestPointB); return; } closestPointA = Toolbox.ZeroVector; closestPointB = Toolbox.ZeroVector; } internal void VerifyContributions() { switch (State) { case SimplexState.Point: if (Vector3.Distance(SimplexA.A - SimplexB.A, A) > .0001f) Debug.WriteLine("break."); break; case SimplexState.Segment: if (Vector3.Distance(SimplexA.A - SimplexB.A, A) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.B - SimplexB.B, B) > .0001f) Debug.WriteLine("break."); break; case SimplexState.Triangle: if (Vector3.Distance(SimplexA.A - SimplexB.A, A) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.B - SimplexB.B, B) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.C - SimplexB.C, C) > .0001f) Debug.WriteLine("break."); break; case SimplexState.Tetrahedron: if (Vector3.Distance(SimplexA.A - SimplexB.A, A) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.B - SimplexB.B, B) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.C - SimplexB.C, C) > .0001f) Debug.WriteLine("break."); if (Vector3.Distance(SimplexA.D - SimplexB.D, D) > .0001f) Debug.WriteLine("break."); break; } } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/GJK/RaySimplex.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests.CollisionAlgorithms.GJK { /// /// GJK simplex supporting ray-based tests. /// public struct RaySimplex { /// /// First vertex in the simplex. /// public Vector3 A; /// /// Second vertex in the simplex. /// public Vector3 B; /// /// Third vertex in the simplex. /// public Vector3 C; /// /// Fourth vertex in the simplex. /// public Vector3 D; /// /// Current state of the simplex. /// public SimplexState State; /// /// Gets the point on the simplex that is closest to the origin. /// ///Simplex to test. ///Closest point on the simplex. ///Whether or not the simplex contains the origin. public bool GetPointClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { //This method finds the closest point on the simplex to the origin. //Barycentric coordinates are assigned to the MinimumNormCoordinates as necessary to perform the inclusion calculation. //If the simplex is a tetrahedron and found to be overlapping the origin, the function returns true to tell the caller to terminate. //Elements of the simplex that are not used to determine the point of minimum norm are removed from the simplex. switch (State) { case SimplexState.Point: point = A; break; case SimplexState.Segment: GetPointOnSegmentClosestToOrigin(ref simplex, out point); break; case SimplexState.Triangle: GetPointOnTriangleClosestToOrigin(ref simplex, out point); break; case SimplexState.Tetrahedron: return GetPointOnTetrahedronClosestToOrigin(ref simplex, out point); default: point = Toolbox.ZeroVector; break; } return false; } /// /// Finds the point on the segment to the origin. /// ///Simplex to test. ///Closest point. public void GetPointOnSegmentClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { Vector3 segmentDisplacement; Vector3.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3.Dot(ref segmentDisplacement, ref A, out dotA); if (dotA > 0) { //'Behind' segment. This can't happen in a boolean version, //but with closest points warmstarting or raycasts, it will. simplex.State = SimplexState.Point; point = A; return; } float dotB; Vector3.Dot(ref segmentDisplacement, ref B, out dotB); if (dotB > 0) { //Inside segment. float V = -dotA / segmentDisplacement.LengthSquared(); Vector3.Multiply(ref segmentDisplacement, V, out point); Vector3.Add(ref point, ref A, out point); return; } //It should be possible in the warmstarted closest point calculation/raycasting to be outside B. //It is not possible in a 'boolean' GJK, where it early outs as soon as a separating axis is found. //Outside B. //Remove current A; we're becoming a point. simplex.A = simplex.B; simplex.State = SimplexState.Point; point = A; } /// /// Gets the point on the triangle that is closest to the origin. /// ///Simplex to test. ///Closest point to origin. public void GetPointOnTriangleClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->P, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; point = A; return; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = simplex.B; point = B; return; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0)//Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; float V = AdotAB / (AdotAB - BdotAB); Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = simplex.C; point = A; return; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Get rid of B. Compress C into B. simplex.State = SimplexState.Segment; simplex.B = simplex.C; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Throw away A. C->A. //TODO: Does B->A, C->B work better? simplex.State = SimplexState.Segment; simplex.A = simplex.C; float U = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, U, out point); Vector3.Add(ref point, ref B, out point); return; } //On the face of the triangle. float denom = 1f / (va + vb + vc); float v = vb * denom; float w = vc * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); } /// /// Gets the point closest to the origin on the tetrahedron. /// ///Simplex to test. ///Closest point. ///Whether or not the tetrahedron encloses the origin. public bool GetPointOnTetrahedronClosestToOrigin(ref RaySimplex simplex, out Vector3 point) { //Thanks to the fact that D is new and that we know that the origin is within the extruded //triangular prism of ABC (and on the "D" side of ABC), //we can immediately ignore voronoi regions: //A, B, C, AC, AB, BC, ABC //and only consider: //D, DA, DB, DC, DAC, DCB, DBA //There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully. //When this method is being called, we don't care about the state of 'this' simplex. It's just a temporary shifted simplex. //The one that needs to be updated is the simplex being passed in. var minimumSimplex = new RaySimplex(); point = new Vector3(); float minimumDistance = float.MaxValue; RaySimplex candidate; float candidateDistance; Vector3 candidatePoint; if (TryTetrahedronTriangle(ref A, ref C, ref D, ref simplex.A, ref simplex.C, ref simplex.D, ref B, out candidate, out candidatePoint)) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidatePoint.LengthSquared(); } if (TryTetrahedronTriangle(ref C, ref B, ref D, ref simplex.C, ref simplex.B, ref simplex.D, ref A, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref B, ref A, ref D, ref simplex.B, ref simplex.A, ref simplex.D, ref C, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref A, ref B, ref C, ref simplex.A, ref simplex.B, ref simplex.C, ref D, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (minimumDistance < float.MaxValue) { simplex = minimumSimplex; return false; } return true; } private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 simplexA, ref Vector3 simplexB, ref Vector3 simplexC, ref Vector3 otherPoint, out RaySimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new RaySimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN >= 0) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //Check to see if it's outside A. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside A. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; if (AdotAC <= 0f && AdotAB <= 0) { //It is A! simplex.State = SimplexState.Point; simplex.A = simplexA; point = A; return true; } //Check to see if it's outside B. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside B. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; if (BdotAB >= 0f && BdotAC <= BdotAB) { //It is B! simplex.State = SimplexState.Point; simplex.A = simplexB; point = B; return true; } //Check to see if it's outside AB. float vc = AdotAB * BdotAC - BdotAB * AdotAC; if (vc <= 0 && AdotAB > 0 && BdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexB; float V = AdotAB / (AdotAB - BdotAB); Vector3.Multiply(ref ab, V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = simplexC; point = C; return true; } //Check if it's outside AC. //float AdotAB, AdotAC; //Vector3.Dot(ref ab, ref A, out AdotAB); //Vector3.Dot(ref ac, ref A, out AdotAC); //AdotAB = -AdotAB; //AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexA; simplex.B = simplexC; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check if it's outside BC. //float BdotAB, BdotAC; //Vector3.Dot(ref ab, ref B, out BdotAB); //Vector3.Dot(ref ac, ref B, out BdotAC); //BdotAB = -BdotAB; //BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = simplexB; simplex.B = simplexC; float V = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, V, out point); Vector3.Add(ref point, ref B, out point); return true; } //On the face of the triangle. simplex.State = SimplexState.Triangle; simplex.A = simplexA; simplex.B = simplexB; simplex.C = simplexC; float denom = 1f / (va + vb + vc); float w = vc * denom; float v = vb * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return true; } return false; } /// /// Adds a new point to the simplex. /// ///Point to add. ///Current ray hit location. ///Simplex shifted with the hit location. public void AddNewSimplexPoint(ref Vector3 point, ref Vector3 hitLocation, out RaySimplex shiftedSimplex) { shiftedSimplex = new RaySimplex(); switch (State) { case SimplexState.Empty: State = SimplexState.Point; A = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); break; case SimplexState.Point: State = SimplexState.Segment; B = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); break; case SimplexState.Segment: State = SimplexState.Triangle; C = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C); break; case SimplexState.Triangle: State = SimplexState.Tetrahedron; D = point; Vector3.Subtract(ref hitLocation, ref A, out shiftedSimplex.A); Vector3.Subtract(ref hitLocation, ref B, out shiftedSimplex.B); Vector3.Subtract(ref hitLocation, ref C, out shiftedSimplex.C); Vector3.Subtract(ref hitLocation, ref D, out shiftedSimplex.D); break; } shiftedSimplex.State = State; } /// /// Gets the error tolerance for the simplex. /// /// Origin of the ray. /// Error tolerance of the simplex. public float GetErrorTolerance(ref Vector3 rayOrigin) { switch (State) { case SimplexState.Point: float distanceA; Vector3.DistanceSquared(ref A, ref rayOrigin, out distanceA); return distanceA; case SimplexState.Segment: float distanceB; Vector3.DistanceSquared(ref A, ref rayOrigin, out distanceA); Vector3.DistanceSquared(ref B, ref rayOrigin, out distanceB); return MathHelper.Max(distanceA, distanceB); case SimplexState.Triangle: float distanceC; Vector3.DistanceSquared(ref A, ref rayOrigin, out distanceA); Vector3.DistanceSquared(ref B, ref rayOrigin, out distanceB); Vector3.DistanceSquared(ref C, ref rayOrigin, out distanceC); return MathHelper.Max(distanceA, MathHelper.Max(distanceB, distanceC)); case SimplexState.Tetrahedron: float distanceD; Vector3.DistanceSquared(ref A, ref rayOrigin, out distanceA); Vector3.DistanceSquared(ref B, ref rayOrigin, out distanceB); Vector3.DistanceSquared(ref C, ref rayOrigin, out distanceC); Vector3.DistanceSquared(ref D, ref rayOrigin, out distanceD); return MathHelper.Max(distanceA, MathHelper.Max(distanceB, MathHelper.Max(distanceC, distanceD))); } return 0; } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/GJK/SimpleSimplex.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests.CollisionAlgorithms.GJK { /// /// GJK simplex supporting boolean intersection tests. /// public struct SimpleSimplex { /// /// First vertex of the simplex. /// public Vector3 A; /// /// Second vertex of the simplex. /// public Vector3 B; /// /// Third vertex of the simplex. /// public Vector3 C; /// /// Fourth vertex of the simplex. /// public Vector3 D; /// /// Current state of the simplex. /// public SimplexState State; /// /// Gets the point on the simplex closest to the origin. /// ///Closest point to the origin. ///Whether or not the simplex encloses the origin. public bool GetPointClosestToOrigin(out Vector3 point) { //This method finds the closest point on the simplex to the origin. //Barycentric coordinates are assigned to the MinimumNormCoordinates as necessary to perform the inclusion calculation. //If the simplex is a tetrahedron and found to be overlapping the origin, the function returns true to tell the caller to terminate. //Elements of the simplex that are not used to determine the point of minimum norm are removed from the simplex. switch (State) { case SimplexState.Point: point = A; break; case SimplexState.Segment: GetPointOnSegmentClosestToOrigin(out point); break; case SimplexState.Triangle: GetPointOnTriangleClosestToOrigin(out point); break; case SimplexState.Tetrahedron: return GetPointOnTetrahedronClosestToOrigin(out point); default: point = Toolbox.ZeroVector; break; } return false; } /// /// Gets the closest point on the segment to the origin. /// ///Closest point. public void GetPointOnSegmentClosestToOrigin(out Vector3 point) { Vector3 segmentDisplacement; Vector3.Subtract(ref B, ref A, out segmentDisplacement); float dotA; Vector3.Dot(ref segmentDisplacement, ref A, out dotA); //Inside segment. float V = -dotA / segmentDisplacement.LengthSquared(); Vector3.Multiply(ref segmentDisplacement, V, out point); Vector3.Add(ref point, ref A, out point); //if (dotB > 0) //{ //} //else //{ // //It is not possible to be anywhere but within the segment in a 'boolean' GJK, where it early outs as soon as a separating axis is found. // //Outside B. // //Remove current A; we're becoming a point. // A = B; // State = SimplexState.Point; // point = A; //} //It can never be outside A! //That would mean that the origin is LESS extreme along the search direction than our extreme point--- our search direction would not have picked that direction. } /// /// Gets the closest point on the triangle to the origin. /// ///Closest point. public void GetPointOnTriangleClosestToOrigin(out Vector3 point) { Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //CAN'T BE IN A'S REGION. //CAN'T BE IN B'S REGION. //CAN'T BE IN AB'S REGION. //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float d5, d6; Vector3.Dot(ref ab, ref C, out d5); Vector3.Dot(ref ac, ref C, out d6); d5 = -d5; d6 = -d6; if (d6 >= 0f && d5 <= d6) { //It is C! State = SimplexState.Point; A = C; point = A; return; } //Check if it's outside AC. float d1, d2; Vector3.Dot(ref ab, ref A, out d1); Vector3.Dot(ref ac, ref A, out d2); d1 = -d1; d2 = -d2; float vb = d5 * d2 - d1 * d6; if (vb <= 0f && d2 > 0f && d6 < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { //Get rid of B. Compress C into B. State = SimplexState.Segment; B = C; float V = d2 / (d2 - d6); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return; } //Check if it's outside BC. float d3, d4; Vector3.Dot(ref ab, ref B, out d3); Vector3.Dot(ref ac, ref B, out d4); d3 = -d3; d4 = -d4; float va = d3 * d6 - d5 * d4; float d3d4; float d6d5; if (va <= 0f && (d3d4 = d4 - d3) > 0f && (d6d5 = d5 - d6) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { //Throw away A. C->A. //TODO: Does B->A, C->B work better? State = SimplexState.Segment; A = C; float U = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, U, out point); Vector3.Add(ref point, ref B, out point); return; } //On the face of the triangle. float vc = d1 * d4 - d3 * d2; float denom = 1f / (va + vb + vc); float v = vb * denom; float w = vc * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); } /// /// Gets the closest point on the tetrahedron to the origin. /// ///Closest point. ///Whether or not the simplex encloses the origin. public bool GetPointOnTetrahedronClosestToOrigin(out Vector3 point) { //Thanks to the fact that D is new and that we know that the origin is within the extruded //triangular prism of ABC (and on the "D" side of ABC), //we can immediately ignore voronoi regions: //A, B, C, AC, AB, BC, ABC //and only consider: //D, DA, DB, DC, DAC, DCB, DBA //There is some overlap of calculations in this method, since DAC, DCB, and DBA are tested fully. SimpleSimplex minimumSimplex = new SimpleSimplex(); point = new Vector3(); float minimumDistance = float.MaxValue; SimpleSimplex candidate; float candidateDistance; Vector3 candidatePoint; if (TryTetrahedronTriangle(ref A, ref C, ref D, ref B, out candidate, out candidatePoint)) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidatePoint.LengthSquared(); } if (TryTetrahedronTriangle(ref C, ref B, ref D, ref A, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (TryTetrahedronTriangle(ref B, ref A, ref D, ref C, out candidate, out candidatePoint) && (candidateDistance = candidatePoint.LengthSquared()) < minimumDistance) { point = candidatePoint; minimumSimplex = candidate; minimumDistance = candidateDistance; } if (minimumDistance < float.MaxValue) { this = minimumSimplex; return false; } return true; } private static bool TryTetrahedronTriangle(ref Vector3 A, ref Vector3 B, ref Vector3 C, ref Vector3 otherPoint, out SimpleSimplex simplex, out Vector3 point) { //Note that there may be some extra terms that can be removed from this process. //Some conditions could use less parameters, since it is known that the origin //is not 'behind' BC or AC. simplex = new SimpleSimplex(); point = new Vector3(); Vector3 ab, ac; Vector3.Subtract(ref B, ref A, out ab); Vector3.Subtract(ref C, ref A, out ac); Vector3 normal; Vector3.Cross(ref ab, ref ac, out normal); float AdotN, ADdotN; Vector3 AD; Vector3.Subtract(ref otherPoint, ref A, out AD); Vector3.Dot(ref A, ref normal, out AdotN); Vector3.Dot(ref AD, ref normal, out ADdotN); //If (-A * N) * (AD * N) < 0, D and O are on opposite sides of the triangle. if (AdotN * ADdotN > 0) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... //CAN'T BE IN A'S REGION. //CAN'T BE IN B'S REGION. //CAN'T BE IN AB'S REGION. //Check to see if it's outside C. //TODO: Note that in a boolean-style GJK, it shouldn't be possible to be outside C. float CdotAB, CdotAC; Vector3.Dot(ref ab, ref C, out CdotAB); Vector3.Dot(ref ac, ref C, out CdotAC); CdotAB = -CdotAB; CdotAC = -CdotAC; if (CdotAC >= 0f && CdotAB <= CdotAC) { //It is C! simplex.State = SimplexState.Point; simplex.A = C; point = C; return true; } //Check if it's outside AC. float AdotAB, AdotAC; Vector3.Dot(ref ab, ref A, out AdotAB); Vector3.Dot(ref ac, ref A, out AdotAC); AdotAB = -AdotAB; AdotAC = -AdotAC; float vb = CdotAB * AdotAC - AdotAB * CdotAC; if (vb <= 0f && AdotAC > 0f && CdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = A; simplex.B = C; float V = AdotAC / (AdotAC - CdotAC); Vector3.Multiply(ref ac, V, out point); Vector3.Add(ref point, ref A, out point); return true; } //Check if it's outside BC. float BdotAB, BdotAC; Vector3.Dot(ref ab, ref B, out BdotAB); Vector3.Dot(ref ac, ref B, out BdotAC); BdotAB = -BdotAB; BdotAC = -BdotAC; float va = BdotAB * CdotAC - CdotAB * BdotAC; float d3d4; float d6d5; if (va <= 0f && (d3d4 = BdotAC - BdotAB) > 0f && (d6d5 = CdotAB - CdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { simplex.State = SimplexState.Segment; simplex.A = B; simplex.B = C; float V = d3d4 / (d3d4 + d6d5); Vector3 bc; Vector3.Subtract(ref C, ref B, out bc); Vector3.Multiply(ref bc, V, out point); Vector3.Add(ref point, ref B, out point); return true; } //On the face of the triangle. float vc = AdotAB * BdotAC - BdotAB * AdotAC; simplex.A = A; simplex.B = B; simplex.C = C; simplex.State = SimplexState.Triangle; float denom = 1f / (va + vb + vc); float w = vc * denom; float v = vb * denom; Vector3.Multiply(ref ab, v, out point); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref A, ref point, out point); Vector3.Add(ref point, ref acw, out point); return true; } return false; } /// /// Adds a new point to the simplex. /// ///Point to add. public void AddNewSimplexPoint(ref Vector3 point) { switch (State) { case SimplexState.Empty: State = SimplexState.Point; A = point; break; case SimplexState.Point: State = SimplexState.Segment; B = point; break; case SimplexState.Segment: State = SimplexState.Triangle; C = point; break; case SimplexState.Triangle: State = SimplexState.Tetrahedron; D = point; break; } } /// /// Gets the error tolerance of the simplex. /// ///Error tolerance of the simplex. public float GetErrorTolerance() { switch (State) { case SimplexState.Point: return A.LengthSquared(); case SimplexState.Segment: return MathHelper.Max(A.LengthSquared(), B.LengthSquared()); case SimplexState.Triangle: return MathHelper.Max(A.LengthSquared(), MathHelper.Max(B.LengthSquared(), C.LengthSquared())); case SimplexState.Tetrahedron: return MathHelper.Max(A.LengthSquared(), MathHelper.Max(B.LengthSquared(), MathHelper.Max(C.LengthSquared(), D.LengthSquared()))); } return 1; } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/GeneralConvexPairTester.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Tests convex shapes against other convex shapes for contact generation. /// public class GeneralConvexPairTester { //TODO: warmstarted calculations like those within this tester will carry over bad information if the shape of an object is changed. //Need to notify the system to take appropriate action when a shape changes... /// /// Whether or not to use simplex caching in general case convex-convex collisions. /// This will improve performance in simulations relying on the general case system, /// but may decrease quality of behavior for curved shapes. /// public static bool UseSimplexCaching; private CollisionState state = CollisionState.Separated; private CollisionState previousState = CollisionState.Separated; Vector3 localSeparatingAxis; CachedSimplex cachedSimplex; protected internal ConvexCollidable collidableA; protected internal ConvexCollidable collidableB; /// /// Gets the first collidable in the pair. /// public ConvexCollidable CollidableA { get { return collidableA; } } /// /// Gets the second collidable in the pair. /// public ConvexCollidable CollidableB { get { return collidableB; } } /// /// Generates a contact between the objects, if possible. /// ///Contact created between the pair, if possible. ///Whether or not the objects were colliding. public bool GenerateContactCandidate(out ContactData contact) { //Generate contacts. This will just find one closest point using general supportmapping based systems like MPR and GJK. //The collision system moves through a state machine depending on the latest collision generation result. //At first, assume that the pair is completely separating. This is almost always the correct guess for new pairs. //An extremely fast, warm-startable boolean GJK test can be performed. If it returns with nonintersection, we can quit and do nothing. //If the initial boolean GJK test finds intersection, move onto a shallow contact test. //The shallow contact test is a different kind of GJK test that finds the closest points between the shape pair. It's not as speedy as the boolean version. //The algorithm is run between the marginless versions of the shapes, so that the closest points will form a contact somewhere in the space separating the cores. //If the closest point system finds no intersection and returns the closest points, the state is changed to ShallowContact. //If the closest point system finds intersection of the core shapes, then the state is changed to DeepContact, and MPR is run to determine contact information. //The system tries to escape from deep contact to shallow contact, and from shallow contact to separated whenever possible. //Here's the state flow: //On Separated: BooleanGJK // -Intersecting -> Go to ShallowContact. // -Nonintersecting -> Do nothing. //On ShallowContact: ClosestPointsGJK // -Intersecting -> Go to DeepContact. // -Nonintersecting: Go to Separated (without test) if squared distance > margin squared, otherwise use closest points to make contact. //On DeepContact: MPR // -Intersecting -> Go to ShallowContact if penetration depth < margin // -Nonintersecting -> This case is rare, but not impossible. Go to Separated (without test). previousState = state; switch (state) { case CollisionState.Separated: if (GJKToolbox.AreShapesIntersecting(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localSeparatingAxis)) { state = CollisionState.ShallowContact; return DoShallowContact(out contact); } contact = new ContactData(); return false; case CollisionState.ShallowContact: return DoShallowContact(out contact); case CollisionState.DeepContact: return DoDeepContact(out contact); } contact = new ContactData(); return false; } private bool DoShallowContact(out ContactData contact) { Vector3 closestA, closestB; //RigidTransform transform = RigidTransform.Identity; //Vector3 closestAnew, closestBnew; //CachedSimplex cachedTest = cachedSimplex; //bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew); ////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB); //bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB); //Vector3 closestAold, closestBold; //bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold); //if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting && // Vector3.DistanceSquared(closestAnew, closestBnew) - Vector3.DistanceSquared(closestA, closestB) > .0001f && // (Vector3.DistanceSquared(closestA, closestAnew) > .0001f || // Vector3.DistanceSquared(closestB, closestBnew) > .0001f)))// || // //Math.Abs(Vector3.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon))) // Debug.WriteLine("Break."); //Vector3 sub; //Vector3.Subtract(ref closestA, ref closestB, out sub); //if (sub.LengthSquared() < Toolbox.Epsilon) if (UseSimplexCaching) GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB); else { //The initialization of the pair creates a pretty decent simplex to start from. //Just don't try to update it. CachedSimplex preInitializedSimplex = cachedSimplex; GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB); } Vector3 displacement; Vector3.Subtract(ref closestB, ref closestA, out displacement); float distanceSquared = displacement.LengthSquared(); if (distanceSquared < Toolbox.Epsilon) { state = CollisionState.DeepContact; return DoDeepContact(out contact); } localDirection = displacement; //Use this as the direction for future deep contacts. float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin; if (distanceSquared < margin * margin) { //Generate a contact. contact = new ContactData(); //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (margin > Toolbox.Epsilon) //Avoid a NaN! Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB else contact.Position = new Vector3(); Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB. contact.Normal = displacement; float distance = (float)Math.Sqrt(distanceSquared); Vector3.Divide(ref contact.Normal, distance, out contact.Normal); contact.PenetrationDepth = margin - distance; return true; } //Too shallow to make a contact- move back to separation. state = CollisionState.Separated; contact = new ContactData(); return false; } Vector3 localDirection; private bool DoDeepContact(out ContactData contact) { #region Informed search if (previousState == CollisionState.Separated) //If it was shallow before, then its closest points will be used to find the normal. { //It's overlapping! Find the relative velocity at the point relative to the two objects. The point is still in local space! //Vector3 velocityA; //Vector3.Cross(ref contact.Position, ref collidableA.entity.angularVelocity, out velocityA); //Vector3.Add(ref velocityA, ref collidableA.entity.linearVelocity, out velocityA); //Vector3 velocityB; //Vector3.Subtract(ref contact.Position, ref localTransformB.Position, out velocityB); //Vector3.Cross(ref velocityB, ref collidableB.entity.angularVelocity, out velocityB); //Vector3.Add(ref velocityB, ref collidableB.entity.linearVelocity, out velocityB); ////The velocity is negated because the direction so point backwards along the velocity. //Vector3.Subtract(ref velocityA, ref velocityB, out localDirection); //The above takes into account angular velocity, but linear velocity alone is a lot more stable and does the job just fine. if (collidableA.entity != null && collidableB.entity != null) Vector3.Subtract(ref collidableA.entity.linearVelocity, ref collidableB.entity.linearVelocity, out localDirection); else localDirection = localSeparatingAxis; if (localDirection.LengthSquared() < Toolbox.Epsilon) { localDirection = Vector3.Up; } } if (MPRToolbox.GetContact(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref localDirection, out contact)) { if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) state = CollisionState.ShallowContact; return true; } //This is rare, but could happen. state = CollisionState.Separated; return false; //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position)) //{ // //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity. // Vector3 rayCastDirection; // float lengthSquared = localDirection.LengthSquared(); // if (lengthSquared > Toolbox.Epsilon) // { // Vector3.Divide(ref localDirection, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2; // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); // } // else // { // contact.PenetrationDepth = float.MaxValue; // contact.Normal = Toolbox.UpVector; // } // //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity. // //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization). // Vector3 normalCandidate; // float depthCandidate; // lengthSquared = localTransformB.Position.LengthSquared(); // if (lengthSquared > Toolbox.Epsilon) // { // Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection); // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate); // if (depthCandidate < contact.PenetrationDepth) // { // contact.Normal = normalCandidate; // } // } // //Correct the penetration depth. // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); // ////The local casting can optionally continue. Eventually, it will converge to the local minimum. // //while (true) // //{ // // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out depthCandidate, out normalCandidate); // // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon) // // break; // // contact.PenetrationDepth = depthCandidate; // // contact.Normal = normalCandidate; // //} // contact.Id = -1; // //we're still in local space! transform it all back. // Matrix3X3 orientation; // Matrix3X3.CreateFromQuaternion(ref collidableA.worldTransform.Orientation, out orientation); // Matrix3X3.Transform(ref contact.Normal, ref orientation, out contact.Normal); // //Vector3.Negate(ref contact.Normal, out contact.Normal); // Matrix3X3.Transform(ref contact.Position, ref orientation, out contact.Position); // Vector3.Add(ref contact.Position, ref collidableA.worldTransform.Position, out contact.Position); // if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) // state = CollisionState.ShallowContact; // return true; //} ////This is rare, but could happen. //state = CollisionState.Separated; //contact = new ContactData(); //return false; #endregion #region Testing //RigidTransform localTransformB; //MinkowskiToolbox.GetLocalTransform(ref collidableA.worldTransform, ref collidableB.worldTransform, out localTransformB); //contact.Id = -1; //if (MPRTesting.GetLocalOverlapPosition(collidableA.Shape, collidableB.Shape, ref localTransformB, out contact.Position)) //{ // Vector3 rayCastDirection = localTransformB.Position; // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localTransformB, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); // RigidTransform.Transform(ref contact.Position, ref collidableA.worldTransform, out contact.Position); // Vector3.Transform(ref contact.Normal, ref collidableA.worldTransform.Orientation, out contact.Normal); // return true; //} //contact.Normal = new Vector3(); //contact.PenetrationDepth = 0; //return false; #endregion #region v0.15.2 and before //if (MPRToolbox.AreObjectsColliding(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, out contact)) //{ // if (contact.PenetrationDepth < collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin) // state = CollisionState.ShallowContact; //If it's emerged from the deep contact, we can go back to using the preferred GJK method. // return true; //} ////This is rare, but could happen. //state = CollisionState.Separated; //return false; #endregion } /// /// Initializes the pair tester. /// ///First shape in the pair. ///Second shape in the pair. public void Initialize(Collidable shapeA, Collidable shapeB) { collidableA = (ConvexCollidable)shapeA; collidableB = (ConvexCollidable)shapeB; cachedSimplex = new CachedSimplex { State = SimplexState.Point };// new CachedSimplex(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform); } /// /// Cleans up the pair tester. /// public void CleanUp() { state = CollisionState.Separated; previousState = CollisionState.Separated; cachedSimplex = new CachedSimplex(); localSeparatingAxis = new Vector3(); collidableA = null; collidableB = null; } enum CollisionState { Separated, ShallowContact, DeepContact } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/MPRToolbox.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; using System.Diagnostics; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Contains a variety of queries and computation methods that make use of minkowski portal refinement. /// public static class MPRToolbox { //TODO: Lots of nasty code repeat. //The common phases of portal construction, correction, and refinement are implemented with subtle differences. //With some effort, they could be shoved together. /// /// Number of iterations that the MPR system will run in its inner loop before giving up and returning with failure. /// public static int InnerIterationLimit = 15; /// /// Number of iterations that the MPR system will run in its outer loop before giving up and moving on to its inner loop. /// public static int OuterIterationLimit = 15; private static float surfaceEpsilon = 1e-7f; /// /// Gets or sets how close surface-finding based MPR methods have to get before exiting. /// Defaults to 1e-7. /// public static float SurfaceEpsilon { get { return surfaceEpsilon; } set { if (value > 0) surfaceEpsilon = value; else throw new ArgumentException("Epsilon must be positive."); } } private static float depthRefinementEpsilon = 1e-4f; /// /// Gets or sets how close the penetration depth refinement system should converge before quitting. /// Making this smaller can help more precisely find a local minimum at the cost of performance. /// The change will likely only be visible on curved shapes, since polytopes will converge extremely rapidly to a precise local minimum. /// Defaults to 1e-4. /// public static float DepthRefinementEpsilon { get { return depthRefinementEpsilon; } set { if (value > 0) depthRefinementEpsilon = value; else throw new ArgumentException("Epsilon must be positive."); } } private static float rayCastSurfaceEpsilon = 1e-9f; /// /// Gets or sets how close surface-finding ray casts have to get before exiting. /// Defaults to 1e-9. /// public static float RayCastSurfaceEpsilon { get { return rayCastSurfaceEpsilon; } set { if (value > 0) rayCastSurfaceEpsilon = value; else throw new ArgumentException("Epsilon must be positive."); } } private static int maximumDepthRefinementIterations = 3; /// /// Gets or sets the maximum number of iterations to use to reach the local penetration depth minimum when using the RefinePenetration function. /// Increasing this allows the system to work longer to find local penetration minima. /// The change will likely only be visible on curved shapes, since polytopes will converge extremely rapidly to a precise local minimum. /// Defaults to 3. /// public static int MaximumDepthRefinementIterations { get { return maximumDepthRefinementIterations; } set { if (value > 0) maximumDepthRefinementIterations = value; else throw new ArgumentException("Iteration count must be positive."); } } /// /// Gets a world space point in the overlapped volume between two shapes. /// /// First shape in the pair. /// Second shape in the pair. /// Transformation to apply to the first shape. /// Transformation to apply to the second shape. /// Position within the overlapped volume of the two shapes, if any. /// Whether or not the two shapes overlap. public static bool GetOverlapPosition(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, out Vector3 position) { RigidTransform localTransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); bool toReturn = GetLocalOverlapPosition(shapeA, shapeB, ref localTransformB, out position); RigidTransform.Transform(ref position, ref transformA, out position); return toReturn; } /// /// Gets a point in the overlapped volume between two shapes in shape A's local space. /// /// First shape in the pair. /// Second shape in the pair. /// Position within the overlapped volume of the two shapes in shape A's local space, if any. /// Whether or not the two shapes overlap. public static bool GetLocalOverlapPosition(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, out Vector3 position) { return GetLocalOverlapPosition(shapeA, shapeB, ref localTransformB.Position, ref localTransformB, out position); } internal static bool GetLocalOverlapPosition(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 originRay, ref RigidTransform localTransformB, out Vector3 position) { //Compute the origin ray. This points from a point known to be inside the minkowski sum to the origin. //The centers of the shapes are used to create the interior point. //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (originRay.LengthSquared() < Toolbox.Epsilon) { position = new Vector3(); //DEBUGlastPosition = position; return true; } Vector3 v0; Vector3.Negate(ref originRay, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = originRay; Vector3 v1; Vector3 v1A, v1B; //extreme point contributions from each shape. Used later to compute contact position; could be used to cache simplex too. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1A, out v1B, out v1); //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3 v2A, v2B; Vector3.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3.Dot(ref v1, ref originRay, out dot); if (dot < 0) { //Origin is outside. position = new Vector3(); return false; } //Origin is inside. //Compute barycentric coordinates along simplex (segment). float dotv0; //Dot > 0, so dotv0 starts out negative. Vector3.Dot(ref v0, ref originRay, out dotv0); float barycentricCoordinate = -dotv0 / (dot - dotv0); //Vector3.Subtract(ref v1A, ref v0A, out offset); //'v0a' is just the zero vector, so there's no need to calculate the offset. Vector3.Multiply(ref v1A, barycentricCoordinate, out position); return true; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2A, out v2B, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); Vector3 v3A, v3B, v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3A, out v3B, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); float dot; Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; v2B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; v1B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v2, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3.Subtract(ref v3, ref v2, out temp1); Vector3.Subtract(ref v1, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); float dot; Vector3.Dot(ref n, ref v1, out dot); if (dot >= 0) { Vector3 temp3; //Compute the barycentric coordinates of the origin. //This is done by computing the scaled volume (parallelepiped) of the tetrahedra //formed by each triangle of the v0v1v2v3 tetrahedron and the origin. //TODO: consider a different approach using T parameter or something. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Subtract(ref v3, ref v0, out temp3); Vector3 cross; Vector3.Cross(ref temp1, ref temp2, out cross); float v0v1v2v3volume; Vector3.Dot(ref cross, ref temp3, out v0v1v2v3volume); Vector3.Cross(ref v1, ref v2, out cross); float ov1v2v3volume; Vector3.Dot(ref cross, ref v3, out ov1v2v3volume); Vector3.Cross(ref originRay, ref temp2, out cross); float v0ov2v3volume; Vector3.Dot(ref cross, ref temp3, out v0ov2v3volume); Vector3.Cross(ref temp1, ref originRay, out cross); float v0v1ov3volume; Vector3.Dot(ref cross, ref temp3, out v0v1ov3volume); if (v0v1v2v3volume > Toolbox.Epsilon * .01f) { float inverseTotalVolume = 1 / v0v1v2v3volume; float v0Weight = ov1v2v3volume * inverseTotalVolume; float v1Weight = v0ov2v3volume * inverseTotalVolume; float v2Weight = v0v1ov3volume * inverseTotalVolume; float v3Weight = 1 - v0Weight - v1Weight - v2Weight; position = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A; } else { position = new Vector3(); } //DEBUGlastPosition = position; return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. Vector3 v4, v4A, v4B; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4A, out v4B, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! position = new Vector3(); return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { position = new Vector3(); //DEBUGlastPosition = position; return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3.Cross(ref v4, ref v0, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; v1B = v4B; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; v3B = v4B; } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; v2B = v4B; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; v1B = v4B; } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } } /// /// Determines if two shapes are colliding. /// /// First shape in the pair. /// Second shape of the pair. /// Transformation to apply to shape A. /// Transformation to apply to shape B. /// Whether or not the shapes are overlapping. public static bool AreShapesOverlapping(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB) { RigidTransform localTransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); return AreLocalShapesOverlapping(shapeA, shapeB, ref localTransformB); } /// /// Determines if two shapes are colliding. Shape B is positioned relative to shape A. /// /// First shape in the pair. /// Second shape of the pair. /// Relative transform of shape B to shape A. /// Whether or not the shapes are overlapping. public static bool AreLocalShapesOverlapping(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB) { return AreLocalShapesOverlapping(shapeA, shapeB, ref localTransformB.Position, ref localTransformB); } /// /// Determines if two shapes are colliding. Shape B is positioned relative to shape A. /// /// First shape in the pair. /// Second shape of the pair. /// Direction in which to cast the overlap ray. Necessary when an object's origin is not contained in its geometry. /// Relative transform of shape B to shape A. /// Whether or not the shapes are overlapping. internal static bool AreLocalShapesOverlapping(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 originRay, ref RigidTransform localTransformB) { //Compute the origin ray. This points from a point known to be inside the minkowski sum to the origin. //The centers of the shapes are used to create the interior point. //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (originRay.LengthSquared() < Toolbox.Epsilon) { return true; } Vector3 v0; Vector3.Negate(ref originRay, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = originRay; Vector3 v1; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1); //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3.Dot(ref v1, ref originRay, out dot); if (dot < 0) { //Origin is outside. return false; } //Origin is inside. return true; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); Vector3 v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); float dot; Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v2, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3.Subtract(ref v3, ref v2, out temp1); Vector3.Subtract(ref v1, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); float dot; Vector3.Dot(ref n, ref v1, out dot); if (dot >= 0) { return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. Vector3 v4; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3.Cross(ref v4, ref v0, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } } /// /// Casts a ray from the origin in the given direction at the surface of the minkowski difference. /// Assumes that the origin is within the minkowski difference. /// /// First shape in the pair. /// Second shape in the pair. /// Transformation of shape B relative to shape A. /// Direction to cast the ray. /// Length along the direction vector that the impact was found. /// Normal of the impact at the surface of the convex. public static void LocalSurfaceCast(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref Vector3 direction, out float t, out Vector3 normal) { // Local surface cast is very similar to regular MPR. However, instead of starting at an interior point and targeting the origin, // the ray starts at the origin (a point known to be in both shape and shapeB), and just goes towards the direction until the surface // is found. The portal (v1, v2, v3) at termination defines the surface normal, and the distance from the origin to the portal along the direction is used as the 't' result. //'v0' is no longer explicitly tracked since it is simply the origin. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = direction; Vector3 v1; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1); //v1 could be zero in some degenerate cases. //if (v1.LengthSquared() < Toolbox.Epsilon) //{ // t = 0; // normal = n; // return; //} //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3.Cross(ref direction, ref v1, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! float rayLengthSquared = direction.LengthSquared(); if (rayLengthSquared > Toolbox.Epsilon * .01f) Vector3.Divide(ref direction, (float)Math.Sqrt(rayLengthSquared), out normal); else normal = new Vector3(); float rate; Vector3.Dot(ref normal, ref direction, out rate); float distance; Vector3.Dot(ref normal, ref v1, out distance); if (rate > 0) t = distance / rate; else t = 0; return; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Cross(ref v1, ref v2, out n); //It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated //relative to the direction vector. float dot; Vector3.Dot(ref n, ref direction, out dot); if (dot > 0) { //It's not properly calibrated. Flip the winding (and the previously calculated normal). Vector3.Negate(ref n, out n); temp1 = v1; v1 = v2; v2 = temp1; } Vector3 v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3); if (count > MPRToolbox.OuterIterationLimit) { //Can't enclose the origin! That's a bit odd; something is wrong. t = float.MaxValue; normal = Toolbox.UpVector; return; } count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal //defined by v1, v2, v3. // If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); Vector3.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v1, ref v3, out n); continue; } // If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v2, ref v3, out n); continue; } break; } //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); // Refine the portal. count = 0; while (true) { //Compute the outward facing normal. Vector3.Subtract(ref v1, ref v2, out temp1); Vector3.Subtract(ref v3, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); //Keep working towards the surface. Find the next extreme point. Vector3 v4; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4); //If the plane which generated the normal is very close to the extreme point, then we're at the surface. Vector3.Dot(ref n, ref v1, out dot); float supportDot; Vector3.Dot(ref v4, ref n, out supportDot); if (supportDot - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //normal = n; //float normalLengthInverse = 1 / normal.Length(); //Vector3.Multiply(ref normal, normalLengthInverse, out normal); ////Find the distance from the origin to the plane. //t = dot * normalLengthInverse; float lengthSquared = n.LengthSquared(); if (lengthSquared > Toolbox.Epsilon * .01f) { Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out normal); //The plane is very close to the surface, and the ray is known to pass through it. //dot is the rate. Vector3.Dot(ref normal, ref direction, out dot); //supportDot is the distance to the plane. Vector3.Dot(ref normal, ref v1, out supportDot); if (dot > 0) t = supportDot / dot; else t = 0; } else { normal = Vector3.Up; t = 0; } ////DEBUG STUFF: //DEBUGlastRayT = t; //DEBUGlastRayDirection = direction; //DEBUGlastDepth = t; //DEBUGlastNormal = normal; //DEBUGlastV1 = v1; //DEBUGlastV2 = v2; //DEBUGlastV3 = v3; return; } //Still haven't exited, so refine the portal. //Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) //This may look a little weird at first. //'inside' here means 'on the positive side of the plane.' //There are three total planes being tested, one for each of v1, v2, and v3. //The planes are created from consistently wound vertices, so it's possible to determine //where the ray passes through the portal based upon its relationship to two of the three planes. //The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point. //This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction. //It eliminates the need for extra cross products for the inner if. Vector3.Cross(ref v4, ref direction, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 } } count++; //Here's an unoptimized equivalent without the scalar triple product reorder. #region Equivalent refinement //Vector3.Cross(ref v1, ref v4, out temp1); //Vector3.Dot(ref temp1, ref direction, out dot); //if (dot > 0) //{ // Vector3.Cross(ref v2, ref v4, out temp2); // Vector3.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Inside v1, v4, v0 and inside v2, v4, v0 // v1 = v4; // } // else // { // //Inside v1, v4, v0 and outside v2, v4, v0 // v3 = v4; // } //} //else //{ // Vector3.Cross(ref v3, ref v4, out temp2); // Vector3.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Outside v1, v4, v0 and inside v3, v4, v0 // v2 = v4; // } // else // { // //Outside v1, v4, v0 and outside v3, v4, v0 // v1 = v4; // } //} #endregion //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); } } /// /// Casts a ray from the origin in the given direction at the surface of the minkowski difference. /// Assumes that the origin is within the minkowski difference. /// /// First shape in the pair. /// Second shape in the pair. /// Transformation of shape B relative to shape A. /// Direction to cast the ray. /// Length along the direction vector that the impact was found. /// Normal of the impact at the surface of the convex. /// Location of the ray cast hit on the surface of A. public static void LocalSurfaceCast(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref Vector3 direction, out float t, out Vector3 normal, out Vector3 position) { // Local surface cast is very similar to regular MPR. However, instead of starting at an interior point and targeting the origin, // the ray starts at the origin (a point known to be in both shape and shapeB), and just goes towards the direction until the surface // is found. The portal (v1, v2, v3) at termination defines the surface normal, and the distance from the origin to the portal along the direction is used as the 't' result. //'v0' is no longer explicitly tracked since it is simply the origin. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = direction; Vector3 v1, v1A; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1A, out v1); //v1 could be zero in some degenerate cases. //if (v1.LengthSquared() < Toolbox.Epsilon) //{ // t = 0; // normal = n; // return; //} //Find another extreme point in a direction perpendicular to the previous. Vector3 v2, v2A; Vector3.Cross(ref direction, ref v1, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! float rayLengthSquared = direction.LengthSquared(); if (rayLengthSquared > Toolbox.Epsilon * .01f) Vector3.Divide(ref direction, (float)Math.Sqrt(rayLengthSquared), out normal); else normal = new Vector3(); float rate; Vector3.Dot(ref normal, ref direction, out rate); float distance; Vector3.Dot(ref normal, ref v1, out distance); if (rate > 0) t = distance / rate; else t = 0; position = v1A; return; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2A, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Cross(ref v1, ref v2, out n); //It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated //relative to the direction vector. float dot; Vector3.Dot(ref n, ref direction, out dot); if (dot > 0) { //It's not properly calibrated. Flip the winding (and the previously calculated normal). Vector3.Negate(ref n, out n); temp1 = v1; v1 = v2; v2 = temp1; temp1 = v1A; v1A = v2A; v2A = temp1; } Vector3 v3, v3A; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3A, out v3); if (count > MPRToolbox.OuterIterationLimit) { //Can't enclose the origin! That's a bit odd; something is wrong. t = float.MaxValue; normal = Toolbox.UpVector; position = new Vector3(); return; } count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal //defined by v1, v2, v3. // If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); Vector3.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v1, ref v3, out n); continue; } // If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref direction, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v2, ref v3, out n); continue; } break; } //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); // Refine the portal. count = 0; while (true) { //Compute the outward facing normal. Vector3.Subtract(ref v1, ref v2, out temp1); Vector3.Subtract(ref v3, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); //Keep working towards the surface. Find the next extreme point. Vector3 v4, v4A; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4A, out v4); //If the plane which generated the normal is very close to the extreme point, then we're at the surface. Vector3.Dot(ref n, ref v1, out dot); float supportDot; Vector3.Dot(ref v4, ref n, out supportDot); if (supportDot - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //normal = n; //float normalLengthInverse = 1 / normal.Length(); //Vector3.Multiply(ref normal, normalLengthInverse, out normal); ////Find the distance from the origin to the plane. //t = dot * normalLengthInverse; float lengthSquared = n.LengthSquared(); if (lengthSquared > Toolbox.Epsilon * .01f) { Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out normal); //The plane is very close to the surface, and the ray is known to pass through it. //dot is the rate. Vector3.Dot(ref normal, ref direction, out dot); //supportDot is the distance to the plane. Vector3.Dot(ref normal, ref v1, out supportDot); if (dot > 0) t = supportDot / dot; else t = 0; } else { normal = Vector3.Up; t = 0; } float v1Weight, v2Weight, v3Weight; Vector3.Multiply(ref direction, t, out position); Toolbox.GetBarycentricCoordinates(ref position, ref v1, ref v2, ref v3, out v1Weight, out v2Weight, out v3Weight); Vector3.Multiply(ref v1A, v1Weight, out position); Vector3 temp; Vector3.Multiply(ref v2A, v2Weight, out temp); Vector3.Add(ref temp, ref position, out position); Vector3.Multiply(ref v3A, v3Weight, out temp); Vector3.Add(ref temp, ref position, out position); ////DEBUG STUFF: //DEBUGlastRayT = t; //DEBUGlastRayDirection = direction; //DEBUGlastDepth = t; //DEBUGlastNormal = normal; //DEBUGlastV1 = v1; //DEBUGlastV2 = v2; //DEBUGlastV3 = v3; return; } //Still haven't exited, so refine the portal. //Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) //This may look a little weird at first. //'inside' here means 'on the positive side of the plane.' //There are three total planes being tested, one for each of v1, v2, and v3. //The planes are created from consistently wound vertices, so it's possible to determine //where the ray passes through the portal based upon its relationship to two of the three planes. //The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point. //This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction. //It eliminates the need for extra cross products for the inner if. Vector3.Cross(ref v4, ref direction, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; } } count++; //Here's an unoptimized equivalent without the scalar triple product reorder. #region Equivalent refinement //Vector3.Cross(ref v1, ref v4, out temp1); //Vector3.Dot(ref temp1, ref direction, out dot); //if (dot > 0) //{ // Vector3.Cross(ref v2, ref v4, out temp2); // Vector3.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Inside v1, v4, v0 and inside v2, v4, v0 // v1 = v4; // } // else // { // //Inside v1, v4, v0 and outside v2, v4, v0 // v3 = v4; // } //} //else //{ // Vector3.Cross(ref v3, ref v4, out temp2); // Vector3.Dot(ref temp2, ref direction, out dot); // if (dot > 0) // { // //Outside v1, v4, v0 and inside v3, v4, v0 // v2 = v4; // } // else // { // //Outside v1, v4, v0 and outside v3, v4, v0 // v1 = v4; // } //} #endregion //if (!VerifySimplex(ref Toolbox.ZeroVector, ref v1, ref v2, ref v3, ref direction)) // Debug.WriteLine("Break."); } } static bool VerifySimplex(ref Vector3 v0, ref Vector3 v1, ref Vector3 v2, ref Vector3 v3, ref Vector3 direction) { //v1, v0, v3 Vector3 cross = Vector3.Cross(v0 - v1, v3 - v1); float planeProduct1 = Vector3.Dot(cross, direction); //v3, v0, v2 cross = Vector3.Cross(v0 - v3, v2 - v3); float planeProduct2 = Vector3.Dot(cross, direction); //v2, v0, v1 cross = Vector3.Cross(v0 - v2, v1 - v2); float planeProduct3 = Vector3.Dot(cross, direction); return (planeProduct1 <= 0 && planeProduct2 <= 0 && planeProduct3 <= 0) || (planeProduct1 >= 0 && planeProduct2 >= 0 && planeProduct3 >= 0); } /// /// Gets a contact point between two convex shapes. /// /// First shape in the pair. /// Second shape in the pair. /// Transformation to apply to the first shape. /// Transformation to apply to the second shape. /// Axis along which to first test the penetration depth. /// Contact data between the two shapes, if any. /// Whether or not the shapes overlap. public static bool GetContact(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform transformA, ref RigidTransform transformB, ref Vector3 penetrationAxis, out ContactData contact) { RigidTransform localTransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); if (MPRToolbox.AreLocalShapesOverlapping(shapeA, shapeB, ref localTransformB)) { //First, try to use the heuristically found direction. This comes from either the GJK shallow contact separating axis or from the relative velocity. Vector3 rayCastDirection; float lengthSquared = penetrationAxis.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3.Divide(ref penetrationAxis, (float)Math.Sqrt(lengthSquared), out rayCastDirection);// (Vector3.Normalize(localDirection) + Vector3.Normalize(collidableB.worldTransform.Position - collidableA.worldTransform.Position)) / 2; MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out contact.PenetrationDepth, out contact.Normal); } else { contact.PenetrationDepth = float.MaxValue; contact.Normal = Toolbox.UpVector; } //Try the offset between the origins as a second option. Sometimes this is a better choice than the relative velocity. //TODO: Could use the position-finding MPR iteration to find the A-B direction hit by continuing even after the origin has been found (optimization). Vector3 normalCandidate; float depthCandidate; lengthSquared = localTransformB.Position.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { Vector3.Divide(ref localTransformB.Position, (float)Math.Sqrt(lengthSquared), out rayCastDirection); MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref rayCastDirection, out depthCandidate, out normalCandidate); if (depthCandidate < contact.PenetrationDepth) { contact.Normal = normalCandidate; contact.PenetrationDepth = depthCandidate; } } //if (contact.PenetrationDepth > 1) // Debug.WriteLine("Break."); //Correct the penetration depth. RefinePenetration(shapeA, shapeB, ref localTransformB, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position); ////Correct the penetration depth. //MPRTesting.LocalSurfaceCast(shape, shapeB, ref localPoint, ref contact.Normal, out contact.PenetrationDepth, out rayCastDirection); ////The local casting can optionally continue. Eventually, it will converge to the local minimum. //while (true) //{ // MPRTesting.LocalSurfaceCast(collidableA.Shape, collidableB.Shape, ref localPoint, ref contact.Normal, out depthCandidate, out normalCandidate); // if (contact.PenetrationDepth - depthCandidate <= Toolbox.BigEpsilon) // break; // contact.PenetrationDepth = depthCandidate; // contact.Normal = normalCandidate; //} contact.Id = -1; //we're still in local space! transform it all back. Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref transformA.Orientation, out orientation); Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Vector3.Negate(ref contact.Normal, out contact.Normal); Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3.Add(ref contact.Position, ref transformA.Position, out contact.Position); return true; } contact = new ContactData(); return false; } /// /// Incrementally refines the penetration depth and normal towards the local minimum. /// /// First shape in the pair. /// Second shape in the pair. /// Transformation of shape B relative to shape A. /// Initial depth estimate. /// Initial normal estimate. /// Refined penetration depth. /// Refined normal. /// Refined position. public static void RefinePenetration(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, float initialDepth, ref Vector3 initialNormal, out float penetrationDepth, out Vector3 refinedNormal, out Vector3 position) { //The local casting can optionally continue. Eventually, it will converge to the local minimum. int optimizingCount = 0; refinedNormal = initialNormal; penetrationDepth = initialDepth; float candidateDepth; Vector3 candidateNormal; while (true) { MPRToolbox.LocalSurfaceCast(shapeA, shapeB, ref localTransformB, ref refinedNormal, out candidateDepth, out candidateNormal, out position); if (penetrationDepth - candidateDepth <= depthRefinementEpsilon || ++optimizingCount >= maximumDepthRefinementIterations) { //If we've reached the end due to convergence, the normal will be extremely close to correct (if not 100% correct). //The candidateDepth computed is the previous contact normal's depth. //The reason why the previous normal is kept is that the last raycast computed the depth for that normal, not the new normal. penetrationDepth = candidateDepth; break; } penetrationDepth = candidateDepth; refinedNormal = candidateNormal; } } //SWEEPS /// /// Sweeps the shapes against each other and finds a point, time, and normal of impact. /// /// First shape in the pair. /// Second shape in the pair. /// Sweep direction and amount to apply to the first shape. /// Sweep direction and amount to apply to the second shape. /// Initial transform to apply to the first shape. /// Initial transform to apply to the second shape. /// Hit data between the two shapes, if any. /// Whether or not the swept shapes hit each other. public static bool Sweep(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweepA, ref Vector3 sweepB, ref RigidTransform transformA, ref RigidTransform transformB, out RayHit hit) { //Put the relative velocity into shape's local space. Vector3 velocityWorld; //note the order of subtraction. It 'should' be B-A, but the ray direction the algorithm works with is actually OPPOSITE. Vector3.Subtract(ref sweepA, ref sweepB, out velocityWorld); Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Vector3 localDirection; Vector3.Transform(ref velocityWorld, ref conjugateOrientationA, out localDirection); //Sweeping two objects against each other is very similar to the local surface cast. //The ray starts at the origin and goes in the sweep direction. //However, unlike the local surface cast, the origin may start outside of the minkowski difference. //Additionally, the method can early out if the length traversed by the ray is found to be longer than the maximum length, or if the origin is found to be outside the minkowski difference. //The support points of the minkowski difference are also modified. By default, the minkowski difference should very rarely contain the origin. //Sweep tests aren't very useful if the objects are intersecting! //However, in order for the local surface cast to actually find a proper result (assuming there is a hit at all), the origin must be inside the minkowski difference. //So, expand the minkowski difference using the sweep direction with magnitude sufficient to fully include the plane defined by the origin and the sweep direction. //If there's going to be a hit, then the origin will be within this expanded shape. //If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape. //However, the minkowski difference may contain the shape. In this case, the time of impact is zero. //If the swept (with sweep = 0 in case of incorrect direction) minkowski difference does not contain the shape, then the raycast cannot begin and we also know that the shapes will not intersect. //If the sweep amount is nonnegative and the minkowski difference contains the shape, then the normal raycasting process can continue. //Perform the usual local raycast, but use the swept minkowski difference. //Once the surface is found, the final t parameter of the ray is equal to the sweep distance minus the local raycast computed t parameter. RigidTransform localTransformB; MinkowskiToolbox.GetLocalTransform(ref transformA, ref transformB, out localTransformB); //First: Compute the sweep amount along the sweep direction. //This sweep amount needs to expand the minkowski difference to fully intersect the plane defined by the sweep direction and origin. float rayLengthSquared = localDirection.LengthSquared(); float sweepLength; if (rayLengthSquared > Toolbox.Epsilon * .01f) { Vector3.Dot(ref localTransformB.Position, ref localDirection, out sweepLength); sweepLength /= rayLengthSquared; //Scale the sweep length by the margins. Divide by the length to pull the margin into terms of the length of the ray. sweepLength += (shapeA.maximumRadius + shapeB.maximumRadius) / (float)Math.Sqrt(rayLengthSquared); } else { rayLengthSquared = 0; sweepLength = 0; } //If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape. //Do not sweep backward. bool negativeLength; if (negativeLength = sweepLength < 0) sweepLength = 0; Vector3 sweep; Vector3.Multiply(ref localDirection, sweepLength, out sweep); //Check to see if the origin is contained within the swept shape. if (!AreSweptShapesIntersecting(shapeA, shapeB, ref sweep, ref localTransformB, out hit.Location)) //Computes a hit location to be used if the early-outs due to being in contact. { //The origin is not contained within the sweep volume. The raycast definitely misses. hit.T = float.MaxValue; hit.Normal = new Vector3(); hit.Location = new Vector3(); return false; } if (negativeLength) { //The origin is contained, but we shouldn't continue. //The ray is facing backwards. The time of impact would be 0. hit.T = 0; Vector3.Normalize(ref localDirection, out hit.Normal); Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal); //hit.Location = hit.T * localDirection; Vector3.Transform(ref hit.Location, ref transformA.Orientation, out hit.Location); Vector3.Add(ref hit.Location, ref transformA.Position, out hit.Location); hit.Location += sweepA * hit.T; return true; } //OKAY! We've finally finished all the pre-testing. Cast the ray! if (LocalSweepCast(shapeA, shapeB, sweepLength, rayLengthSquared, ref localDirection, ref sweep, ref localTransformB, out hit)) { //Compute the actual hit location on the minkowski surface. Vector3 minkowskiRayHit = -hit.T * localDirection; //TODO: This uses MPR to identify a witness point on shape A. //It's a very roundabout way to do it. There should be a much simpler/faster way to compute the witness point directly, or with a little sampling. GetLocalPosition(shapeA, shapeB, ref localTransformB, ref minkowskiRayHit, out hit.Location); //The hit location is still in local space, so transform it into world space using A's transform. RigidTransform.Transform(ref hit.Location, ref transformA, out hit.Location); Vector3.Transform(ref hit.Normal, ref transformA.Orientation, out hit.Normal); //Push the world space hit location relative to object A along A's sweep direction. Vector3 temp; Vector3.Multiply(ref sweepA, hit.T, out temp); Vector3.Add(ref temp, ref hit.Location, out hit.Location); return true; } return false; } private static bool LocalSweepCast(ConvexShape shape, ConvexShape shapeB, float sweepLength, float rayLengthSquared, ref Vector3 localDirection, ref Vector3 sweep, ref RigidTransform localTransformB, out RayHit hit) { //By now, the ray is known to be within the swept shape and facing the right direction for a normal raycast. //First guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = localDirection; Vector3 v1, v1A; GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v1A, out v1); //v1 could be zero in some degenerate cases. //if (v1.LengthSquared() < Toolbox.Epsilon) //{ // hit.T = 0; // Vector3.Normalize(ref n, out hit.Normal); // Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); // //hit.Location = hit.T * localDirection; // Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); // Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); // hit.Location += sweepA * hit.T; // return true; //} //Find another extreme point in a direction perpendicular to the previous. Vector3 v2, v2A; Vector3.Cross(ref localDirection, ref v1, out n); hit.Location = new Vector3(); if (n.LengthSquared() < Toolbox.Epsilon * .01f) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! if (rayLengthSquared > Toolbox.Epsilon * .01f) Vector3.Divide(ref localDirection, (float)Math.Sqrt(rayLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); float rate; Vector3.Dot(ref hit.Normal, ref localDirection, out rate); float distance; Vector3.Dot(ref hit.Normal, ref v1, out distance); if (rate > 0) hit.T = sweepLength - distance / rate; else hit.T = sweepLength; if (hit.T < 0) hit.T = 0; //Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); ////hit.Location = hit.T * localDirection; //Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); //Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); //hit.Location += sweepA * hit.T; return hit.T <= 1; } GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v2A, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Cross(ref v1, ref v2, out n); //It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated //relative to the direction vector. float dot; Vector3.Dot(ref n, ref localDirection, out dot); if (dot > 0) { //It's not properly calibrated. Flip the winding (and the previously calculated normal). Vector3.Negate(ref n, out n); temp1 = v1; v1 = v2; v2 = temp1; temp1 = v1A; v1A = v2A; v2A = temp1; } Vector3 v3, v3A; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v3A, out v3); if (count > MPRToolbox.OuterIterationLimit) { //Can't enclose the origin! That's a bit odd. Something is wrong; the preparation for this raycast //guarantees that the origin is enclosed. Could be a numerical problem. hit.T = float.MaxValue; hit.Normal = new Vector3(); hit.Location = new Vector3(); return false; } count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal //defined by v1, v2, v3. // If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); Vector3.Dot(ref temp1, ref localDirection, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v1, ref v3, out n); continue; } // If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref localDirection, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v2, ref v3, out n); continue; } break; } // Refine the portal. count = 0; while (true) { //Compute the outward facing normal. Vector3.Subtract(ref v1, ref v2, out temp1); Vector3.Subtract(ref v3, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); //Keep working towards the surface. Find the next extreme point. Vector3 v4, v4A; GetSweptExtremePoint(shape, shapeB, ref localTransformB, ref sweep, ref n, out v4A, out v4); //If the plane which generated the normal is very close to the extreme point, then we're at the surface. Vector3.Dot(ref n, ref v1, out dot); float supportDot; Vector3.Dot(ref v4, ref n, out supportDot); if (supportDot - dot < rayCastSurfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //The portal is now on the surface. The algorithm can now compute the TOI and exit. float lengthSquared = n.LengthSquared(); if (lengthSquared > Toolbox.Epsilon * .00001f) { Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out hit.Normal); //The plane is very close to the surface, and the ray is known to pass through it. //dot is the rate. Vector3.Dot(ref hit.Normal, ref localDirection, out dot); //supportDot is the distance to the plane. Vector3.Dot(ref hit.Normal, ref v1, out supportDot); hit.T = sweepLength - supportDot / dot; } else { Vector3.Normalize(ref localDirection, out hit.Normal); hit.T = sweepLength; } //Sometimes, when the objects are intersecting, the T parameter can be negative. //In this case, just go with t = 0. if (hit.T < 0) hit.T = 0; //Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); //Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); //Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); //hit.Location += sweepA * (hit.T); //Compute the barycentric coordinates of the ray hit location. //Vector3 mdHitLocation = t * localDirection; //float v1Weight, v2Weight, v3Weight; //Toolbox.GetBarycentricCoordinates(ref mdHitLocation, ref v1, ref v2, ref v3, out v1Weight, out v2Weight, out v3Weight); //hit.Location = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A; //hit.Location += sweepA * hit.T; //Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); //Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); return hit.T <= 1; } //Still haven't exited, so refine the portal. //Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) //This may look a little weird at first. //'inside' here means 'on the positive side of the plane.' //There are three total planes being tested, one for each of v1, v2, and v3. //The planes are created from consistently wound vertices, so it's possible to determine //where the ray passes through the portal based upon its relationship to two of the three planes. //The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point. //This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction. //It eliminates the need for extra cross products for the inner if. Vector3.Cross(ref v4, ref localDirection, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; } } count++; } } /// /// Computes the position of the minkowski point in the local space of A. /// This assumes that the minkowski point is contained in A-B. /// /// First shape to test. /// Second shape to test. /// Transform of shape B in the local space of A. /// Position in minkowski space to pull into the local space of A. /// Position of the minkowski space point in the local space of A. internal static void GetLocalPosition(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref Vector3 minkowskiPosition, out Vector3 position) { //Compute the ray. This points from a point known to be inside the minkowski sum to the test minkowski position; //The centers of the shapes are used to create the interior point. Vector3 rayDirection; Vector3.Add(ref minkowskiPosition, ref localTransformB.Position, out rayDirection); //It's possible that the point is extremely close to the A-B center. In this case, early out. if (rayDirection.LengthSquared() < Toolbox.Epsilon) { //0,0,0 is the contributing position from object A if it overlaps with the A-B minkowski center. //A-B center is fromed from the center position of A minus the center position of B. We're in A's local space. position = new Vector3(); //DEBUGlastPosition = position; return; } Vector3 v0; Vector3.Negate(ref localTransformB.Position, out v0); //Since we're in A's local space, A-B is just -B. //Now that the ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = rayDirection; Vector3 v1; Vector3 v1A, v1B; //extreme point contributions from each shape. Used later to compute contact position; could be used to cache simplex too. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v1A, out v1B, out v1); //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3 v2A, v2B; Vector3.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. //For this test, we already guarantee that the point is extremely close to the A-B shape or inside of it, so don't bother //trying to return false. float dot = Vector3.Dot(v1 - minkowskiPosition, rayDirection); //Vector3.Dot(ref v1, ref rayDirection, out dot); //if (dot < 0) //if we were trying to return false here (in a IsPointContained style test), then the '0' should actually be a dot between the minkowskiPoint and rayDirection (simplified by a subtraction and then a dot). //{ // //Origin is outside. // position = new Vector3(); // return false; //} //Origin is inside. //Compute barycentric coordinates along simplex (segment). float dotv0 = Vector3.Dot(v0 - minkowskiPosition, rayDirection); //Dot > 0, so dotv0 starts out negative. //Vector3.Dot(ref v0, ref rayDirection, out dotv0); float barycentricCoordinate = -dotv0 / (dot - dotv0); //Vector3.Subtract(ref v1A, ref v0A, out offset); //'v0a' is just the zero vector, so there's no need to calculate the offset. Vector3.Multiply(ref v1A, barycentricCoordinate, out position); Vector3 offset; Vector3.Subtract(ref v1B, ref localTransformB.Position, out offset); return; } MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v2A, out v2B, out v2); Vector3 temp; Vector3 v0v1, v0v2; //Set n for the first iteration. Vector3.Subtract(ref v1, ref v0, out v0v1); Vector3.Subtract(ref v2, ref v0, out v0v2); Vector3.Cross(ref v0v1, ref v0v2, out n); Vector3 pointToV0; Vector3.Subtract(ref v0, ref minkowskiPosition, out pointToV0); Vector3 v3A, v3B, v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v3A, out v3B, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3 v0v3; Vector3.Subtract(ref v1, ref v0, out v0v1); Vector3.Subtract(ref v3, ref v0, out v0v3); Vector3.Cross(ref v0v1, ref v0v3, out temp); float dot; Vector3.Dot(ref temp, ref pointToV0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; v2B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v0v1, ref v0v3, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Subtract(ref v2, ref v0, out v0v2); Vector3.Cross(ref v0v3, ref v0v2, out temp); Vector3.Dot(ref temp, ref pointToV0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; v1B = v3B; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v0v2, ref v0v3, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref rayDirection)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3 v2v3, v2v1; Vector3.Subtract(ref v3, ref v2, out v2v3); Vector3.Subtract(ref v1, ref v2, out v2v1); Vector3.Cross(ref v2v3, ref v2v1, out n); float dot; Vector3 pointToV1; Vector3.Subtract(ref v1, ref minkowskiPosition, out pointToV1); Vector3.Dot(ref pointToV1, ref n, out dot); //Because this method is intended for use with surface collisions, rely on the surface push case. //This will not significantly harm performance, but will simplify the termination condition. //if (dot >= 0) //{ // Vector3 temp3; // //Compute the barycentric coordinates of the origin. // //This is done by computing the scaled volume (parallelepiped) of the tetrahedra // //formed by each triangle of the v0v1v2v3 tetrahedron and the origin. // //TODO: consider a different approach using T parameter or something. // Vector3.Subtract(ref v1, ref v0, out temp1); // Vector3.Subtract(ref v2, ref v0, out temp2); // Vector3.Subtract(ref v3, ref v0, out temp3); // Vector3 cross; // Vector3.Cross(ref temp1, ref temp2, out cross); // float v0v1v2v3volume; // Vector3.Dot(ref cross, ref temp3, out v0v1v2v3volume); // Vector3.Cross(ref v1, ref v2, out cross); // float ov1v2v3volume; // Vector3.Dot(ref cross, ref v3, out ov1v2v3volume); // Vector3.Cross(ref rayDirection, ref temp2, out cross); // float v0ov2v3volume; // Vector3.Dot(ref cross, ref temp3, out v0ov2v3volume); // Vector3.Cross(ref temp1, ref rayDirection, out cross); // float v0v1ov3volume; // Vector3.Dot(ref cross, ref temp3, out v0v1ov3volume); // if (v0v1v2v3volume > Toolbox.Epsilon * .01f) // { // float inverseTotalVolume = 1 / v0v1v2v3volume; // float v0Weight = ov1v2v3volume * inverseTotalVolume; // float v1Weight = v0ov2v3volume * inverseTotalVolume; // float v2Weight = v0v1ov3volume * inverseTotalVolume; // float v3Weight = 1 - v0Weight - v1Weight - v2Weight; // position = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A; // } // else // { // position = new Vector3(); // } // //DEBUGlastPosition = position; // return; //} //We haven't yet found the origin. Find the support point in the portal's outward facing direction. Vector3 v4, v4A, v4B; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref n, ref localTransformB, out v4A, out v4B, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3 pointToV4; Vector3.Subtract(ref v4, ref minkowskiPosition, out pointToV4); Vector3.Dot(ref pointToV4, ref n, out dot2); //if (dot2 < 0) //We're not concerned with this test! We already have guarantees that the shape contains or very nearly contains the minkowski point. //{ // //The origin is outside! // position = new Vector3(); // return false; //} //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < rayCastSurfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //We found the surface. Technically, we did not find the minkowski point yet, but it must be really close based on the guarantees //required by this method. //The ray intersection with the plane defined by our final portal should be extremely close to the actual minkowski point. //In fact, it is probably close enough such that the barycentric coordinates can be computed using the minkowski point directly! float weight1, weight2, weight3; Toolbox.GetBarycentricCoordinates(ref minkowskiPosition, ref v1, ref v2, ref v3, out weight1, out weight2, out weight3); Vector3.Multiply(ref v1A, weight1, out position); Vector3.Multiply(ref v2A, weight2, out v2A); Vector3.Multiply(ref v3A, weight3, out v3A); Vector3.Add(ref v2A, ref position, out position); Vector3.Add(ref v3A, ref position, out position); return; } count++; //Still haven't exited, so refine the portal. //Test minkowskiPoint against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3.Cross(ref pointToV4, ref pointToV0, out temp); Vector3.Dot(ref pointToV1, ref temp, out dot); if (dot >= 0) //Dot v0 x v4 against v1, and dot it against { Vector3 pointToV2; Vector3.Subtract(ref v2, ref minkowskiPosition, out pointToV2); Vector3.Dot(ref pointToV2, ref temp, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; v1B = v4B; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; v3B = v4B; } } else { Vector3 pointToV3; Vector3.Subtract(ref v3, ref minkowskiPosition, out pointToV3); Vector3.Dot(ref pointToV3, ref temp, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; v2B = v4B; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; v1B = v4B; } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref rayDirection)) // Debug.WriteLine("Break."); } } /// /// Determines if two shapes are intersecting. /// /// First shape in the pair. /// Second shape in the pair. /// Sweep direction and magnitude. /// Transformation of shape B in the local space of A. /// Position of the minkowski difference origin in the local space of A, if the swept volumes intersect. /// Whether the swept shapes intersect. public static bool AreSweptShapesIntersecting(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 sweep, ref RigidTransform localTransformB, out Vector3 position) { //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (localTransformB.Position.LengthSquared() < Toolbox.Epsilon) { position = new Vector3(); return true; } Vector3 v0; Vector3.Negate(ref localTransformB.Position, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = localTransformB.Position; Vector3 v1; Vector3 v1A; //extreme point contributions from each shape. Used later to compute contact position; could be used to cache simplex too. //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v1A, out v1B, out v1); GetSweptExtremePoint(shapeA, shapeB, ref localTransformB, ref sweep, ref n, out v1A, out v1); //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3 v2A; Vector3.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3.Dot(ref v1, ref localTransformB.Position, out dot); if (dot < 0) { //Origin is outside. position = new Vector3(); return false; } //Origin is inside. //Compute barycentric coordinates along simplex (segment). float dotv0; //Dot > 0, so dotv0 starts out negative. Vector3.Dot(ref v0, ref localTransformB.Position, out dotv0); float barycentricCoordinate = -dotv0 / (dot - dotv0); //Vector3.Subtract(ref v1A, ref v0A, out offset); //'v0a' is just the zero vector, so there's no need to calculate the offset. Vector3.Multiply(ref v1A, barycentricCoordinate, out position); return true; } //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v2A, out v2B, out v2); GetSweptExtremePoint(shapeA, shapeB, ref localTransformB, ref sweep, ref n, out v2A, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); Vector3 v3A, v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v3A, out v3B, out v3); GetSweptExtremePoint(shapeA, shapeB, ref localTransformB, ref sweep, ref n, out v3A, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); float dot; Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; v2A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; v1A = v3A; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v2, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3.Subtract(ref v3, ref v2, out temp1); Vector3.Subtract(ref v1, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); float dot; Vector3.Dot(ref n, ref v1, out dot); if (dot >= 0) { Vector3 temp3; //Compute the barycentric coordinates of the origin. //This is done by computing the scaled volume (parallelepiped) of the tetrahedra //formed by each triangle of the v0v1v2v3 tetrahedron and the origin. //TODO: consider a different approach using T parameter or something. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Subtract(ref v3, ref v0, out temp3); Vector3 cross; Vector3.Cross(ref temp1, ref temp2, out cross); float v0v1v2v3volume; Vector3.Dot(ref cross, ref temp3, out v0v1v2v3volume); Vector3.Cross(ref v1, ref v2, out cross); float ov1v2v3volume; Vector3.Dot(ref cross, ref v3, out ov1v2v3volume); Vector3.Cross(ref localTransformB.Position, ref temp2, out cross); float v0ov2v3volume; Vector3.Dot(ref cross, ref temp3, out v0ov2v3volume); Vector3.Cross(ref temp1, ref localTransformB.Position, out cross); float v0v1ov3volume; Vector3.Dot(ref cross, ref temp3, out v0v1ov3volume); float inverseTotalVolume = 1 / v0v1v2v3volume; float v0Weight = ov1v2v3volume * inverseTotalVolume; float v1Weight = v0ov2v3volume * inverseTotalVolume; float v2Weight = v0v1ov3volume * inverseTotalVolume; float v3Weight = 1 - v0Weight - v1Weight - v2Weight; position = v1Weight * v1A + v2Weight * v2A + v3Weight * v3A; //DEBUGlastPosition = position; return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. Vector3 v4, v4A; //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v4A, out v4B, out v4); GetSweptExtremePoint(shapeA, shapeB, ref localTransformB, ref sweep, ref n, out v4A, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! position = new Vector3(); return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { position = new Vector3(); //DEBUGlastPosition = position; return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3.Cross(ref v4, ref v0, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 v1A = v4A; } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 v3A = v4A; } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 v2A = v4A; } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 v1A = v4A; } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } } static void GetSweptExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref RigidTransform localTransformB, ref Vector3 sweep, ref Vector3 extremePointDirection, out Vector3 extremePointA, out Vector3 extremePoint) { Vector3 b; MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shapeA, shapeB, ref extremePointDirection, ref localTransformB, out extremePointA, out b, out extremePoint); float dot; Vector3.Dot(ref extremePointDirection, ref sweep, out dot); if (dot > 0) { Vector3.Add(ref extremePoint, ref sweep, out extremePoint); } } //RAY CAST /// /// Casts a ray against a shape. /// /// Ray to test against the shape. /// Maximum length of the ray. /// Shape to test with a ray. /// Transform to apply to the shape. /// Hit data of the ray on the shape, if any. /// Whether or not the swept shapes hit each other. public static bool RayCast(Ray ray, float maximumLength, ConvexShape shape, ref RigidTransform transform, out RayHit hit) { //Compute the local origin and direction of the ray. Ray localRay; Quaternion conjugate; Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position); Quaternion.Conjugate(ref transform.Orientation, out conjugate); Vector3.Transform(ref localRay.Position, ref conjugate, out localRay.Position); Vector3.Transform(ref ray.Direction, ref conjugate, out localRay.Direction); //Note that we will be casting the ray *backwards* against the sweep-expanded surface of the shape. Vector3.Negate(ref localRay.Direction, out localRay.Direction); //A ray cast is a special case of two-body sweeping. This is implemented separately from the two-body sweep above almost entirely for a tiny performance boost. //It also allows for certain tricks to help with numerical stability. //Sweeping two objects against each other is very similar to the local surface cast. //The ray starts at the origin and goes in the sweep direction. //However, unlike the local surface cast, the origin may start outside of the minkowski difference. //Additionally, the method can early out if the length traversed by the ray is found to be longer than the maximum length, or if the origin is found to be outside the minkowski difference. //The support points of the minkowski difference are also modified. By default, the minkowski difference should very rarely contain the origin. //Sweep tests aren't very useful if the objects are intersecting! //However, in order for the local surface cast to actually find a proper result (assuming there is a hit at all), the origin must be inside the minkowski difference. //So, expand the minkowski difference using the sweep direction with magnitude sufficient to fully include the plane defined by the origin and the sweep direction. //If there's going to be a hit, then the origin will be within this expanded shape. //If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape. //However, the minkowski difference may contain the shape. In this case, the time of impact is zero. //If the swept (with sweep = 0 in case of incorrect direction) minkowski difference does not contain the shape, then the raycast cannot begin and we also know that the shapes will not intersect. //If the sweep amount is nonnegative and the minkowski difference contains the shape, then the normal raycasting process can continue. //Perform the usual local raycast, but use the swept minkowski difference. //Once the surface is found, the final t parameter of the ray is equal to the sweep distance minus the local raycast computed t parameter. //Given that this is a ray cast, we can make some modifications. Rays frequently traverse large distances, but stretching the swept volume //a huge amount to match could manifest numerical issues. Instead, pull the ray up close to the object. RayHit sphereHit; if (Toolbox.RayCastSphere(ref ray, ref transform.Position, shape.maximumRadius, maximumLength, out sphereHit)) { //We can scoot ourselves almost all the way up to the intersection with the outer sphere. //Stop just short to prevent a possible erroneous 'just-barely-contained' result. sphereHit.T = Math.Max(sphereHit.T - .1f, 0); Vector3 offset; Vector3.Multiply(ref localRay.Direction, -sphereHit.T, out offset); Vector3.Add(ref localRay.Position, ref offset, out localRay.Position); } else { //If the ray cast doesn't hit the bounding sphere, it's impossible for the ray to hit the shape itself. hit.T = float.MaxValue; hit.Normal = new Vector3(); hit.Location = new Vector3(); return false; } //First: Compute the sweep amount along the sweep direction. //This sweep amount needs to expand the minkowski difference to fully intersect the plane defined by the sweep direction and origin. float rayLengthSquared = localRay.Direction.LengthSquared(); float sweepLength; if (rayLengthSquared > Toolbox.Epsilon * .01f) { Vector3.Dot(ref localRay.Position, ref localRay.Direction, out sweepLength); //Ray length isn't necessarily normalized... sweepLength /= rayLengthSquared; //Scale the sweep length by the margins. Divide by the length to pull the margin into terms of the length of the ray. sweepLength += shape.maximumRadius / (float)Math.Sqrt(rayLengthSquared); } else { rayLengthSquared = 0; sweepLength = 0; } //If the sweep direction is found to be negative, the ray can be thought of as pointing away from the shape. //Do not sweep backward. bool negativeLength; if (negativeLength = sweepLength < 0) sweepLength = 0; Vector3 sweep; Vector3.Multiply(ref localRay.Direction, sweepLength, out sweep); //Check to see if the origin is contained within the swept shape. if (!SweptShapeContainsPoint(shape, ref sweep, ref localRay.Position)) { //The origin is not contained within the sweep volume. The raycast definitely misses. hit.T = float.MaxValue; hit.Normal = new Vector3(); hit.Location = new Vector3(); return false; } if (negativeLength) { //The origin is contained, but we shouldn't continue. //The ray is facing backwards. The time of impact would be 0 (because we already verified that we are contained in the shape volume). hit.T = 0; Vector3.Normalize(ref ray.Direction, out hit.Normal); hit.Location = ray.Position; return true; } //OKAY! We've finally finished all the pre-testing. Cast the ray! if (LocalSweepCast(shape, sweepLength, rayLengthSquared, ref localRay.Direction, ref sweep, ref localRay.Position, out hit)) { hit.T += sphereHit.T; if (hit.T <= maximumLength) { //Get the world space hit location. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); //Transform the normal. Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); return true; } } return false; } /// /// Determines if a point is contained within the swept volume of a shape. /// /// First shape in the pair. /// Sweep direction and magnitude. /// Point to check for containment in the shape in the local space of the shape. /// Whether the swept shape contains the point. public static bool SweptShapeContainsPoint(ConvexShape shape, ref Vector3 sweep, ref Vector3 localPoint) { //It's possible that the two objects' centers are overlapping, or very very close to it. In this case, //they are obviously colliding and we can immediately exit. if (localPoint.LengthSquared() < Toolbox.Epsilon) { return true; } Vector3 v0; Vector3.Negate(ref localPoint, out v0); //Since we're in A's local space, A-B is just -B. //Now that the origin ray is known, create a portal through which the ray passes. //To do this, first guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = localPoint; Vector3 v1; //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v1A, out v1B, out v1); GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v1); //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3.Cross(ref v1, ref v0, out n); if (n.LengthSquared() < Toolbox.Epsilon) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! //If the origin is further along this direction than the extreme point, then there is no intersection. //If the origin is within this extreme point, then there is an intersection. float dot; Vector3.Dot(ref v1, ref localPoint, out dot); if (dot < 0) { //Origin is outside. return false; } return true; } //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v2A, out v2B, out v2); GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v2, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); Vector3 v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v3A, out v3B, out v3); GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v3); if (count > MPRToolbox.OuterIterationLimit) break; count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the origin ray found earlier actually passes through the portal //defined by v1, v2, v3. // If the origin is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); float dot; Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v1, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } // If the origin is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref v0, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Subtract(ref v2, ref v0, out temp1); Vector3.Subtract(ref v3, ref v0, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); continue; } break; } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); // Refine the portal. while (true) { //Test the origin against the plane defined by v1, v2, v3. If it's inside, we're done. //Compute the outward facing normal. Vector3.Subtract(ref v3, ref v2, out temp1); Vector3.Subtract(ref v1, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); float dot; Vector3.Dot(ref n, ref v1, out dot); if (dot >= 0) { return true; } //We haven't yet found the origin. Find the support point in the portal's outward facing direction. Vector3 v4; //MinkowskiToolbox.GetLocalMinkowskiExtremePoint(shape, shapeB, ref n, ref localPoint, out v4A, out v4B, out v4); GetSweptExtremePoint(shape, ref localPoint, ref sweep, ref n, out v4); //If the origin is further along the direction than the extreme point, it's not inside the shape. float dot2; Vector3.Dot(ref v4, ref n, out dot2); if (dot2 < 0) { //The origin is outside! return false; } //If the plane which generated the normal is very close to the extreme point, then we're at the surface //and we have not found the origin; it's either just BARELY inside, or it is outside. Assume it's outside. if (dot2 - dot < surfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //DEBUGlastPosition = position; return false; } count++; //Still haven't exited, so refine the portal. //Test origin against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) Vector3.Cross(ref v4, ref v0, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 } } //if (!VerifySimplex(ref v0, ref v1, ref v2, ref v3, ref localPoint.Position)) // Debug.WriteLine("Break."); } } private static bool LocalSweepCast(ConvexShape shape, float sweepLength, float rayLengthSquared, ref Vector3 localDirection, ref Vector3 sweep, ref Vector3 rayOrigin, out RayHit hit) { //By now, the ray is known to be within the swept shape and facing the right direction for a normal raycast. //First guess a portal. //This implementation is similar to that of the original XenoCollide. //'n' will be the direction used to find supports throughout the algorithm. Vector3 n = localDirection; Vector3 v1; GetSweptExtremePoint(shape, ref rayOrigin, ref sweep, ref n, out v1); //v1 could be zero in some degenerate cases. //if (v1.LengthSquared() < Toolbox.Epsilon) //{ // hit.T = 0; // Vector3.Normalize(ref n, out hit.Normal); // Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); // //hit.Location = hit.T * localDirection; // Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); // Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); // hit.Location += sweepA * hit.T; // return true; //} //Find another extreme point in a direction perpendicular to the previous. Vector3 v2; Vector3.Cross(ref localDirection, ref v1, out n); hit.Location = new Vector3(); if (n.LengthSquared() < Toolbox.Epsilon * .01f) { //v1 and v0 could be parallel. //This isn't a bad thing- it means the direction is exactly aligned with the extreme point offset. //In other words, if the raycast is followed out to the surface, it will arrive at the extreme point! if (rayLengthSquared > Toolbox.Epsilon * .01f) Vector3.Divide(ref localDirection, (float)Math.Sqrt(rayLengthSquared), out hit.Normal); else hit.Normal = new Vector3(); float rate; Vector3.Dot(ref hit.Normal, ref localDirection, out rate); float distance; Vector3.Dot(ref hit.Normal, ref v1, out distance); if (rate > 0) hit.T = sweepLength - distance / rate; else hit.T = sweepLength; if (hit.T < 0) hit.T = 0; //Vector3.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal); ////hit.Location = hit.T * localDirection; //Vector3.Transform(ref hit.Location, ref transform.Orientation, out hit.Location); //Vector3.Add(ref hit.Location, ref transform.Position, out hit.Location); //hit.Location += sweepA * hit.T; return hit.T <= 1; } GetSweptExtremePoint(shape, ref rayOrigin, ref sweep, ref n, out v2); Vector3 temp1, temp2; //Set n for the first iteration. Vector3.Cross(ref v1, ref v2, out n); //It's possible that v1 and v2 were constructed in such a way that 'n' is not properly calibrated //relative to the direction vector. float dot; Vector3.Dot(ref n, ref localDirection, out dot); if (dot > 0) { //It's not properly calibrated. Flip the winding (and the previously calculated normal). Vector3.Negate(ref n, out n); temp1 = v1; v1 = v2; v2 = temp1; } Vector3 v3; int count = 0; while (true) { //Find a final extreme point using the normal of the plane defined by v0, v1, v2. GetSweptExtremePoint(shape, ref rayOrigin, ref sweep, ref n, out v3); if (count > MPRToolbox.OuterIterationLimit) { //Can't enclose the origin! That's a bit odd. Something is wrong; the preparation for this raycast //guarantees that the origin is enclosed. Could be a numerical problem. hit.T = float.MaxValue; hit.Normal = new Vector3(); hit.Location = new Vector3(); return false; } count++; //By now, the simplex is a tetrahedron, but it is not known whether or not the ray actually passes through the portal //defined by v1, v2, v3. // If the direction is outside the plane defined by v1,v0,v3, then the portal is invalid. Vector3.Cross(ref v1, ref v3, out temp1); Vector3.Dot(ref temp1, ref localDirection, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v2) with the new extreme point. v2 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v1, ref v3, out n); continue; } // If the direction is outside the plane defined by v3,v0,v2, then the portal is invalid. Vector3.Cross(ref v3, ref v2, out temp1); Vector3.Dot(ref temp1, ref localDirection, out dot); if (dot < 0) { //Replace the point that was on the inside of the plane (v1) with the new extreme point. v1 = v3; // Calculate the normal of the plane that will be used to find a new extreme point. Vector3.Cross(ref v2, ref v3, out n); continue; } break; } // Refine the portal. count = 0; while (true) { //Compute the outward facing normal. Vector3.Subtract(ref v1, ref v2, out temp1); Vector3.Subtract(ref v3, ref v2, out temp2); Vector3.Cross(ref temp1, ref temp2, out n); //Keep working towards the surface. Find the next extreme point. Vector3 v4; GetSweptExtremePoint(shape, ref rayOrigin, ref sweep, ref n, out v4); //If the plane which generated the normal is very close to the extreme point, then we're at the surface. Vector3.Dot(ref n, ref v1, out dot); float supportDot; Vector3.Dot(ref v4, ref n, out supportDot); if (supportDot - dot < rayCastSurfaceEpsilon || count > MPRToolbox.InnerIterationLimit) // TODO: Could use a dynamic epsilon for possibly better behavior. { //The portal is now on the surface. The algorithm can now compute the TOI and exit. float lengthSquared = n.LengthSquared(); if (lengthSquared > Toolbox.Epsilon * .00001f) { Vector3.Divide(ref n, (float)Math.Sqrt(lengthSquared), out hit.Normal); //The plane is very close to the surface, and the ray is known to pass through it. //dot is the rate. Vector3.Dot(ref hit.Normal, ref localDirection, out dot); //supportDot is the distance to the plane. Vector3.Dot(ref hit.Normal, ref v1, out supportDot); hit.T = sweepLength - supportDot / dot; } else { Vector3.Normalize(ref localDirection, out hit.Normal); hit.T = sweepLength; } //Sometimes, when the objects are intersecting, the T parameter can be negative. //In this case, just go with t = 0. if (hit.T < 0) hit.T = 0; return true; } //Still haven't exited, so refine the portal. //Test direction against the three planes that separate the new portal candidates: (v1,v4,v0) (v2,v4,v0) (v3,v4,v0) //This may look a little weird at first. //'inside' here means 'on the positive side of the plane.' //There are three total planes being tested, one for each of v1, v2, and v3. //The planes are created from consistently wound vertices, so it's possible to determine //where the ray passes through the portal based upon its relationship to two of the three planes. //The third vertex which is found to be opposite the face which contains the ray is replaced with the extreme point. //This v4 x direction is just a minor reordering of a scalar triple product: (v1 x v4) * direction. //It eliminates the need for extra cross products for the inner if. Vector3.Cross(ref v4, ref localDirection, out temp1); Vector3.Dot(ref v1, ref temp1, out dot); if (dot >= 0) { Vector3.Dot(ref v2, ref temp1, out dot); if (dot >= 0) { v1 = v4; // Inside v1 & inside v2 ==> eliminate v1 } else { v3 = v4; // Inside v1 & outside v2 ==> eliminate v3 } } else { Vector3.Dot(ref v3, ref temp1, out dot); if (dot >= 0) { v2 = v4; // Outside v1 & inside v3 ==> eliminate v2 } else { v1 = v4; // Outside v1 & outside v3 ==> eliminate v1 } } count++; } } static void GetSweptExtremePoint(ConvexShape shape, ref Vector3 point, ref Vector3 sweep, ref Vector3 extremePointDirection, out Vector3 extremePoint) { shape.GetExtremePoint(extremePointDirection, ref Toolbox.RigidIdentity, out extremePoint); Vector3.Subtract(ref extremePoint, ref point, out extremePoint); float dot; Vector3.Dot(ref extremePointDirection, ref sweep, out dot); if (dot > 0) { Vector3.Add(ref extremePoint, ref sweep, out extremePoint); } } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/MinkowskiToolbox.cs ================================================ using System; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Helper class that supports other systems using minkowski space operations. /// public static class MinkowskiToolbox { /// /// Gets the local transform of B in the space of A. /// ///First transform. ///Second transform. ///Transform of B in the local space of A. public static void GetLocalTransform(ref RigidTransform transformA, ref RigidTransform transformB, out RigidTransform localTransformB) { //Put B into A's space. Quaternion conjugateOrientationA; Quaternion.Conjugate(ref transformA.Orientation, out conjugateOrientationA); Quaternion.Concatenate(ref transformB.Orientation, ref conjugateOrientationA, out localTransformB.Orientation); Vector3.Subtract(ref transformB.Position, ref transformA.Position, out localTransformB.Position); Vector3.Transform(ref localTransformB.Position, ref conjugateOrientationA, out localTransformB.Position); } /// /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. /// ///First shape. ///Second shape. ///Extreme point direction in local space. ///Transform of shapeB in the local space of A. ///The extreme point in the local space of A. public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3 v; Vector3 negativeN; Vector3.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out v); Vector3.Subtract(ref extremePoint, ref v, out extremePoint); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, ref direction, out v); Vector3.Add(ref extremePoint, ref v, out extremePoint); } /// /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. /// ///First shape. ///Second shape. ///Extreme point direction in local space. ///Transform of shapeB in the local space of A. /// The extreme point on shapeA. ///The extreme point in the local space of A. public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePointA, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA); Vector3 v; Vector3.Negate(ref direction, out v); Vector3 extremePointB; shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB); Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint); } /// /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA. /// ///First shape. ///Second shape. ///Extreme point direction in local space. ///Transform of shapeB in the local space of A. /// The extreme point on shapeA. /// The extreme point on shapeB. ///The extreme point in the local space of A. public static void GetLocalMinkowskiExtremePoint(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePointA, out Vector3 extremePointB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePointA); Vector3 v; Vector3.Negate(ref direction, out v); shapeB.GetExtremePointWithoutMargin(v, ref localTransformB, out extremePointB); ExpandMinkowskiSum(shapeA.collisionMargin, shapeB.collisionMargin, direction, ref extremePointA, ref extremePointB); Vector3.Subtract(ref extremePointA, ref extremePointB, out extremePoint); } /// /// Gets the extreme point of the minkowski difference of shapeA and shapeB in the local space of shapeA, without a margin. /// ///First shape. ///Second shape. ///Extreme point direction in local space. ///Transform of shapeB in the local space of A. ///The extreme point in the local space of A. public static void GetLocalMinkowskiExtremePointWithoutMargin(ConvexShape shapeA, ConvexShape shapeB, ref Vector3 direction, ref RigidTransform localTransformB, out Vector3 extremePoint) { //Extreme point of A-B along D = (extreme point of A along D) - (extreme point of B along -D) shapeA.GetLocalExtremePointWithoutMargin(ref direction, out extremePoint); Vector3 extremePointB; Vector3 negativeN; Vector3.Negate(ref direction, out negativeN); shapeB.GetExtremePointWithoutMargin(negativeN, ref localTransformB, out extremePointB); Vector3.Subtract(ref extremePoint, ref extremePointB, out extremePoint); } /// /// Computes the expansion of the minkowski sum due to margins in a given direction. /// ///First margin. ///Second margin. ///Extreme point direction. ///Margin contribution to the extreme point. public static void ExpandMinkowskiSum(float marginA, float marginB, ref Vector3 direction, out Vector3 contribution) { float lengthSquared = direction.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { //The contribution to the minkowski sum by the margin is: //direction * marginA - (-direction) * marginB. Vector3.Multiply(ref direction, (marginA + marginB) / (float)Math.Sqrt(lengthSquared), out contribution); } else { contribution = new Vector3(); } } /// /// Computes the expansion of the minkowski sum due to margins in a given direction. /// ///First margin. ///Second margin. ///Extreme point direction. ///Margin contribution to the shapeA. ///Margin contribution to the shapeB. public static void ExpandMinkowskiSum(float marginA, float marginB, Vector3 direction, ref Vector3 toExpandA, ref Vector3 toExpandB) { float lengthSquared = direction.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { lengthSquared = 1 / (float)Math.Sqrt(lengthSquared); //The contribution to the minkowski sum by the margin is: //direction * marginA - (-direction) * marginB. Vector3 contribution; Vector3.Multiply(ref direction, marginA * lengthSquared, out contribution); Vector3.Add(ref toExpandA, ref contribution, out toExpandA); Vector3.Multiply(ref direction, marginB * lengthSquared, out contribution); Vector3.Subtract(ref toExpandB, ref contribution, out toExpandB); } //If the direction is too small, then the expansion values are left unchanged. } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/SphereTester.cs ================================================ using System; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.Settings; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Helper class to test spheres against each other. /// public static class SphereTester { /// /// Computes contact data for two spheres. /// /// First sphere. /// Second sphere. /// Position of the first sphere. /// Position of the second sphere. /// Contact data between the spheres, if any. /// Whether or not the spheres are touching. public static bool AreSpheresColliding(SphereShape a, SphereShape b, ref Vector3 positionA, ref Vector3 positionB, out ContactData contact) { contact = new ContactData(); float radiusSum = a.collisionMargin + b.collisionMargin; Vector3 centerDifference; Vector3.Subtract(ref positionB, ref positionA, out centerDifference); float centerDistance = centerDifference.LengthSquared(); if (centerDistance < (radiusSum + CollisionDetectionSettings.maximumContactDistance) * (radiusSum + CollisionDetectionSettings.maximumContactDistance)) { //In collision! if (radiusSum > Toolbox.Epsilon) //This would be weird, but it is still possible to cause a NaN. Vector3.Multiply(ref centerDifference, a.collisionMargin / (radiusSum), out contact.Position); else contact.Position = new Vector3(); Vector3.Add(ref contact.Position, ref positionA, out contact.Position); centerDistance = (float)Math.Sqrt(centerDistance); if (centerDistance > Toolbox.BigEpsilon) { Vector3.Divide(ref centerDifference, centerDistance, out contact.Normal); } else { contact.Normal = Toolbox.UpVector; } contact.PenetrationDepth = radiusSum - centerDistance; return true; } return false; } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/TriangleConvexPairTester.cs ================================================ using System; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using BEPUphysics.Settings; using BEPUutilities.DataStructures; using System.Diagnostics; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Persistent tester that compares triangles against convex objects. /// public class TriangleConvexPairTester : TrianglePairTester { internal ConvexShape convex; internal CollisionState state = CollisionState.Plane; private const int EscapeAttemptPeriod = 10; int escapeAttempts; Vector3 localSeparatingAxis; //Relies on the triangle being located in the local space of the convex object. The convex transform is used to transform the //contact points back from the convex's local space into world space. /// /// Generates a contact between the triangle and convex. /// ///Contact between the shapes, if any. ///Whether or not the shapes are colliding. public override bool GenerateContactCandidate(out TinyStructList contactList) { switch (state) { case CollisionState.Plane: return DoPlaneTest(out contactList); case CollisionState.ExternalSeparated: return DoExternalSeparated(out contactList); case CollisionState.ExternalNear: return DoExternalNear(out contactList); case CollisionState.Deep: return DoDeepContact(out contactList); default: contactList = new TinyStructList(); return false; } } private bool DoPlaneTest(out TinyStructList contactList) { //Find closest point between object and plane. Vector3 reverseNormal; Vector3 ab, ac; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3.Cross(ref ac, ref ab, out reverseNormal); //Convex position dot normal is ALWAYS zero. The thing to look at is the plane's 'd'. //If the distance along the normal is positive, then the convex is 'behind' that normal. float dotA; Vector3.Dot(ref triangle.vA, ref reverseNormal, out dotA); contactList = new TinyStructList(); switch (triangle.sidedness) { case TriangleSidedness.DoubleSided: if (dotA < 0) { //The reverse normal is pointing towards the convex. //It needs to point away from the convex so that the direction //will get the proper extreme point. Vector3.Negate(ref reverseNormal, out reverseNormal); dotA = -dotA; } break; case TriangleSidedness.Clockwise: //if (dotA < 0) //{ // //The reverse normal is pointing towards the convex. // return false; //} break; case TriangleSidedness.Counterclockwise: //if (dotA > 0) //{ // //The reverse normal is pointing away from the convex. // return false; //} //The reverse normal is pointing towards the convex. //It needs to point away from the convex so that the direction //will get the proper extreme point. Vector3.Negate(ref reverseNormal, out reverseNormal); dotA = -dotA; break; } Vector3 extremePoint; convex.GetLocalExtremePointWithoutMargin(ref reverseNormal, out extremePoint); //See if the extreme point is within the face or not. //It might seem like the easy "depth" test should come first, since a barycentric //calculation takes a bit more time. However, transferring from plane to depth is 'rare' //(like all transitions), and putting this test here is logically closer to its requirements' //computation. if (GetVoronoiRegion(ref extremePoint) != VoronoiRegion.ABC) { state = CollisionState.ExternalSeparated; return DoExternalSeparated(out contactList); } float dotE; Vector3.Dot(ref extremePoint, ref reverseNormal, out dotE); float t = (dotA - dotE) / reverseNormal.LengthSquared(); Vector3 offset; Vector3.Multiply(ref reverseNormal, t, out offset); //Compare the distance from the plane to the convex object. float distanceSquared = offset.LengthSquared(); float marginSum = triangle.collisionMargin + convex.collisionMargin; //TODO: Could just normalize early and avoid computing point plane before it's necessary. //Exposes a sqrt but... if (t <= 0 || distanceSquared < marginSum * marginSum) { //The convex object is in the margin of the plane. //All that's left is to create the contact. var contact = new ContactData(); //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (marginSum > Toolbox.Epsilon) //This can be zero! It would cause a NaN is unprotected. Vector3.Multiply(ref offset, convex.collisionMargin / marginSum, out contact.Position); //t * AB else contact.Position = new Vector3(); Vector3.Add(ref extremePoint, ref contact.Position, out contact.Position); //A + t * AB. float normalLength = reverseNormal.Length(); Vector3.Divide(ref reverseNormal, normalLength, out contact.Normal); float distance = normalLength * t; contact.PenetrationDepth = marginSum - distance; if (contact.PenetrationDepth > marginSum) { //Check to see if the inner sphere is touching the plane. //This does not override other tests; there can be more than one contact from a single triangle. ContactData alternateContact; if (TryInnerSphereContact(out alternateContact))// && alternateContact.PenetrationDepth > contact.PenetrationDepth) { contactList.Add(ref alternateContact); } //The convex object is stuck deep in the plane! //The most problematic case for this is when //an object is right on top of a cliff. //The lower, vertical triangle may occasionally detect //a contact with the object, but would compute an extremely //deep depth if the normal plane test was used. //Verify that the depth is correct by trying another approach. CollisionState previousState = state; state = CollisionState.ExternalNear; TinyStructList alternateContacts; if (DoExternalNear(out alternateContacts)) { alternateContacts.Get(0, out alternateContact); if (alternateContact.PenetrationDepth + .01f < contact.PenetrationDepth) //Bias against the subtest's result, since the plane version will probably have a better position. { //It WAS a bad contact. contactList.Add(ref alternateContact); //DoDeepContact (which can be called from within DoExternalNear) can generate two contacts, but the second contact would just be an inner sphere (which we already generated). //DoExternalNear can only generate one contact. So we only need the first contact! //TODO: This is a fairly fragile connection between the two stages. Consider robustifying. (Also, the TryInnerSphereContact is done twice! This process is very rare for marginful pairs, though) } else { //Well, it really is just that deep. contactList.Add(ref contact); state = previousState; } } else { //If the external near test finds that there was no collision at all, //just return to plane testing. If the point turns up outside the face region //next time, the system will adapt. state = previousState; return false; } } else { contactList.Add(ref contact); } return true; } return false; } private bool DoExternalSeparated(out TinyStructList contactList) { if (GJKToolbox.AreShapesIntersecting(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref localSeparatingAxis)) { state = CollisionState.ExternalNear; return DoExternalNear(out contactList); } TryToEscape(); contactList = new TinyStructList(); return false; } private bool DoExternalNear(out TinyStructList contactList) { Vector3 closestA, closestB; //Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful. //TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle. //If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin. //This causes an instant-out, always. Not good! //By giving the contributing simplex the average centroid, it has a better guess. Vector3 triangleCentroid; Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid); Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid); Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid); var initialSimplex = new CachedSimplex { State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid } }; if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref initialSimplex, out closestA, out closestB)) { state = CollisionState.Deep; return DoDeepContact(out contactList); } Vector3 displacement; Vector3.Subtract(ref closestB, ref closestA, out displacement); float distanceSquared = displacement.LengthSquared(); float margin = convex.collisionMargin + triangle.collisionMargin; contactList = new TinyStructList(); if (distanceSquared < margin * margin) { //Try to generate a contact. var contact = new ContactData(); //Determine if the normal points in the appropriate direction given the sidedness of the triangle. if (triangle.sidedness != TriangleSidedness.DoubleSided) { Vector3 triangleNormal, ab, ac; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3.Cross(ref ab, ref ac, out triangleNormal); float dot; Vector3.Dot(ref triangleNormal, ref displacement, out dot); if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) return false; if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0) return false; } //Displacement is from A to B. point = A + t * AB, where t = marginA / margin. if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected. Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB else contact.Position = new Vector3(); Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB. contact.Normal = displacement; float distance = (float)Math.Sqrt(distanceSquared); Vector3.Divide(ref contact.Normal, distance, out contact.Normal); contact.PenetrationDepth = margin - distance; contactList.Add(ref contact); TryToEscape(ref contact.Position); return true; } //Too far to make a contact- move back to separation. state = CollisionState.ExternalSeparated; return false; } private bool DoDeepContact(out TinyStructList contactList) { //Find the origin to triangle center offset. Vector3 center; Vector3.Add(ref triangle.vA, ref triangle.vB, out center); Vector3.Add(ref center, ref triangle.vC, out center); Vector3.Multiply(ref center, 1f / 3f, out center); ContactData contact; contactList = new TinyStructList(); if (MPRToolbox.AreLocalShapesOverlapping(convex, triangle, ref center, ref Toolbox.RigidIdentity)) { float dot; Vector3 triangleNormal, ab, ac; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3.Cross(ref ab, ref ac, out triangleNormal); float lengthSquared = triangleNormal.LengthSquared(); if (lengthSquared < Toolbox.Epsilon * .01f) { //Degenerate triangle! That's no good. //Just use the direction pointing from A to B, "B" being the triangle. That direction is center - origin, or just center. MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref center, out contact.PenetrationDepth, out contact.Normal, out contact.Position); } else { //Normalize the normal. Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(lengthSquared), out triangleNormal); ////The first direction to check is one of the triangle's edge normals. Choose the one that is most aligned with the offset from A to B. ////Project the direction onto the triangle plane. //Vector3.Dot(ref triangleNormal, ref center, out dot); //Vector3 trianglePlaneDirection; //Vector3.Multiply(ref triangleNormal, dot, out trianglePlaneDirection); //Vector3.Subtract(ref trianglePlaneDirection, ref center, out trianglePlaneDirection); ////To find out which edge to use, compute which region the direction is in. ////This is done by constructing three planes which segment the triangle into three sub-triangles. ////These planes are defined by A, origin, center; B, origin, center; C, origin, center. ////The plane tests against the direction can be reordered to: ////(center x direction) * A ////(center x direction) * B ////(center x direction) * C //Vector3 OxD; //Vector3.Cross(ref trianglePlaneDirection, ref center, out OxD); //Vector3 p; //float dotA, dotB, dotC; //Vector3.Dot(ref triangle.vA, ref OxD, out dotA); //Vector3.Dot(ref triangle.vB, ref OxD, out dotB); //Vector3.Dot(ref triangle.vC, ref OxD, out dotC); //if (dotA >= 0 && dotB <= 0) //{ // //Direction is in the AB edge zone. // //Compute the edge normal using AB x (AO x AB). // Vector3 AB, AO; // Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); // Vector3.Subtract(ref center, ref triangle.vA, out AO); // Vector3.Cross(ref AO, ref AB, out p); // Vector3.Cross(ref AB, ref p, out trianglePlaneDirection); //} //else if (dotB >= 0 && dotC <= 0) //{ // //Direction is in the BC edge zone. // //Compute the edge normal using BC x (BO x BC). // Vector3 BC, BO; // Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC); // Vector3.Subtract(ref center, ref triangle.vB, out BO); // Vector3.Cross(ref BO, ref BC, out p); // Vector3.Cross(ref BC, ref p, out trianglePlaneDirection); //} //else // dotC > 0 && dotA < 0 //{ // //Direction is in the CA edge zone. // //Compute the edge normal using CA x (CO x CA). // Vector3 CA, CO; // Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA); // Vector3.Subtract(ref center, ref triangle.vC, out CO); // Vector3.Cross(ref CO, ref CA, out p); // Vector3.Cross(ref CA, ref p, out trianglePlaneDirection); //} //dot = trianglePlaneDirection.LengthSquared(); //if (dot > Toolbox.Epsilon) //{ // Vector3.Divide(ref trianglePlaneDirection, (float)Math.Sqrt(dot), out trianglePlaneDirection); // MPRTesting.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref trianglePlaneDirection, out contact.PenetrationDepth, out contact.Normal); // //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle. // Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot); // if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)) // { // //Normal was facing the wrong way. // //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal. // Vector3 previousNormal = contact.Normal; // Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot); // Vector3.Multiply(ref contact.Normal, dot, out p); // Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal); // float length = contact.Normal.LengthSquared(); // if (length > Toolbox.Epsilon) // { // //Renormalize the corrected normal. // Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal); // Vector3.Dot(ref contact.Normal, ref previousNormal, out dot); // contact.PenetrationDepth *= dot; // } // else // { // contact.PenetrationDepth = float.MaxValue; // contact.Normal = new Vector3(); // } // } //} //else //{ // contact.PenetrationDepth = float.MaxValue; // contact.Normal = new Vector3(); //} //TODO: This tests all three edge axes with a full MPR raycast. That's not really necessary; the correct edge normal should be discoverable, resulting in a single MPR raycast. //Find the edge directions that will be tested with MPR. Vector3 AO, BO, CO; Vector3 AB, BC, CA; Vector3.Subtract(ref center, ref triangle.vA, out AO); Vector3.Subtract(ref center, ref triangle.vB, out BO); Vector3.Subtract(ref center, ref triangle.vC, out CO); Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vB, out BC); Vector3.Subtract(ref triangle.vA, ref triangle.vC, out CA); //We don't have to worry about degenerate triangles here because we've already handled that possibility above. Vector3 ABnormal, BCnormal, CAnormal; //Project the center onto the edge to find the direction from the center to the edge AB. Vector3.Dot(ref AO, ref AB, out dot); Vector3.Multiply(ref AB, dot / AB.LengthSquared(), out ABnormal); Vector3.Subtract(ref AO, ref ABnormal, out ABnormal); ABnormal.Normalize(); //Project the center onto the edge to find the direction from the center to the edge BC. Vector3.Dot(ref BO, ref BC, out dot); Vector3.Multiply(ref BC, dot / BC.LengthSquared(), out BCnormal); Vector3.Subtract(ref BO, ref BCnormal, out BCnormal); BCnormal.Normalize(); //Project the center onto the edge to find the direction from the center to the edge BC. Vector3.Dot(ref CO, ref CA, out dot); Vector3.Multiply(ref CA, dot / CA.LengthSquared(), out CAnormal); Vector3.Subtract(ref CO, ref CAnormal, out CAnormal); CAnormal.Normalize(); MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref ABnormal, out contact.PenetrationDepth, out contact.Normal); //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle. Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot); if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)) { //Normal was facing the wrong way. //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal. Vector3 previousNormal = contact.Normal; Vector3.Dot(ref contact.Normal, ref triangleNormal, out dot); Vector3 p; Vector3.Multiply(ref contact.Normal, dot, out p); Vector3.Subtract(ref contact.Normal, ref p, out contact.Normal); float length = contact.Normal.LengthSquared(); if (length > Toolbox.Epsilon) { //Renormalize the corrected normal. Vector3.Divide(ref contact.Normal, (float)Math.Sqrt(length), out contact.Normal); Vector3.Dot(ref contact.Normal, ref previousNormal, out dot); contact.PenetrationDepth *= dot; } else { contact.PenetrationDepth = float.MaxValue; contact.Normal = new Vector3(); } } Vector3 candidateNormal; float candidateDepth; MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref BCnormal, out candidateDepth, out candidateNormal); //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle. Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot); if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)) { //Normal was facing the wrong way. //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal. Vector3 previousNormal = candidateNormal; Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot); Vector3 p; Vector3.Multiply(ref candidateNormal, dot, out p); Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal); float length = candidateNormal.LengthSquared(); if (length > Toolbox.Epsilon) { //Renormalize the corrected normal. Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal); Vector3.Dot(ref candidateNormal, ref previousNormal, out dot); candidateDepth *= dot; } else { contact.PenetrationDepth = float.MaxValue; contact.Normal = new Vector3(); } } if (candidateDepth < contact.PenetrationDepth) { contact.Normal = candidateNormal; contact.PenetrationDepth = candidateDepth; } MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref CAnormal, out candidateDepth, out candidateNormal); //Check to see if the normal is facing in the proper direction, considering that this may not be a two-sided triangle. Vector3.Dot(ref triangleNormal, ref candidateNormal, out dot); if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)) { //Normal was facing the wrong way. //Instead of ignoring it entirely, correct the direction to as close as it can get by removing any component parallel to the triangle normal. Vector3 previousNormal = candidateNormal; Vector3.Dot(ref candidateNormal, ref triangleNormal, out dot); Vector3 p; Vector3.Multiply(ref candidateNormal, dot, out p); Vector3.Subtract(ref candidateNormal, ref p, out candidateNormal); float length = candidateNormal.LengthSquared(); if (length > Toolbox.Epsilon) { //Renormalize the corrected normal. Vector3.Divide(ref candidateNormal, (float)Math.Sqrt(length), out candidateNormal); Vector3.Dot(ref candidateNormal, ref previousNormal, out dot); candidateDepth *= dot; } else { contact.PenetrationDepth = float.MaxValue; contact.Normal = new Vector3(); } } if (candidateDepth < contact.PenetrationDepth) { contact.Normal = candidateNormal; contact.PenetrationDepth = candidateDepth; } //Try the depth along the positive triangle normal. //If it's clockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle). if (triangle.sidedness != TriangleSidedness.Clockwise) { MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal); if (candidateDepth < contact.PenetrationDepth) { contact.Normal = candidateNormal; contact.PenetrationDepth = candidateDepth; } } //Try the depth along the negative triangle normal. //If it's counterclockwise, this direction is unnecessary (the resulting normal would be invalidated by the onesidedness of the triangle). if (triangle.sidedness != TriangleSidedness.Counterclockwise) { Vector3.Negate(ref triangleNormal, out triangleNormal); MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref triangleNormal, out candidateDepth, out candidateNormal); if (candidateDepth < contact.PenetrationDepth) { contact.Normal = candidateNormal; contact.PenetrationDepth = candidateDepth; } } } MPRToolbox.RefinePenetration(convex, triangle, ref Toolbox.RigidIdentity, contact.PenetrationDepth, ref contact.Normal, out contact.PenetrationDepth, out contact.Normal, out contact.Position); //It's possible for the normal to still face the 'wrong' direction according to one sided triangles. if (triangle.sidedness != TriangleSidedness.DoubleSided) { Vector3.Dot(ref triangleNormal, ref contact.Normal, out dot); if (dot < 0) { //Skip the add process. goto InnerSphere; } } contact.Id = -1; if (contact.PenetrationDepth < convex.collisionMargin + triangle.collisionMargin) { state = CollisionState.ExternalNear; //If it's emerged from the deep contact, we can go back to using the preferred GJK method. } contactList.Add(ref contact); } InnerSphere: if (TryInnerSphereContact(out contact)) { contactList.Add(ref contact); } if (contactList.Count > 0) return true; state = CollisionState.ExternalSeparated; return false; } void TryToEscape() { if (++escapeAttempts == EscapeAttemptPeriod) { escapeAttempts = 0; state = CollisionState.Plane; } } void TryToEscape(ref Vector3 position) { if (++escapeAttempts == EscapeAttemptPeriod && GetVoronoiRegion(ref position) == VoronoiRegion.ABC) { escapeAttempts = 0; state = CollisionState.Plane; } } private bool TryInnerSphereContact(out ContactData contact) { Vector3 closestPoint; Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint); float length = closestPoint.LengthSquared(); float minimumRadius = convex.minimumRadius * (MotionSettings.CoreShapeScaling + .01f); if (length < minimumRadius * minimumRadius) { Vector3 triangleNormal, ab, ac; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3.Cross(ref ab, ref ac, out triangleNormal); float dot; Vector3.Dot(ref closestPoint, ref triangleNormal, out dot); if ((triangle.sidedness == TriangleSidedness.Clockwise && dot > 0) || (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)) { //Normal was facing the wrong way. contact = new ContactData(); return false; } length = (float)Math.Sqrt(length); contact.Position = closestPoint; if (length > Toolbox.Epsilon) //Watch out for NaN's! { Vector3.Divide(ref closestPoint, length, out contact.Normal); } else { //The direction is undefined. Use the triangle's normal. //One sided triangles can only face in the appropriate direction. float normalLength = triangleNormal.LengthSquared(); if (triangleNormal.LengthSquared() > Toolbox.Epsilon) { Vector3.Divide(ref triangleNormal, (float)Math.Sqrt(normalLength), out triangleNormal); if (triangle.sidedness == TriangleSidedness.Clockwise) contact.Normal = triangleNormal; else Vector3.Negate(ref triangleNormal, out contact.Normal); } else { //Degenerate triangle! contact = new ContactData(); return false; } } //Compute the actual depth of the contact. MPRToolbox.LocalSurfaceCast(convex, triangle, ref Toolbox.RigidIdentity, ref contact.Normal, out contact.PenetrationDepth, out triangleNormal); //Trash the 'corrected' normal. We want to use the spherical normal. contact.Id = -1; return true; } contact = new ContactData(); return false; } /// /// Determines what voronoi region a given point is in. /// ///Point to test. ///Voronoi region containing the point. private VoronoiRegion GetVoronoiRegion(ref Vector3 p) { //The point we are comparing against the triangle is 0,0,0, so instead of storing an "A->P" vector, //just use -A. //Same for B->, C->P... Vector3 ab, ac, ap; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3.Subtract(ref p, ref triangle.vA, out ap); //Check to see if it's outside A. float APdotAB, APdotAC; Vector3.Dot(ref ap, ref ab, out APdotAB); Vector3.Dot(ref ap, ref ac, out APdotAC); if (APdotAC <= 0f && APdotAB <= 0) { //It is A! return VoronoiRegion.A; } //Check to see if it's outside B. float BPdotAB, BPdotAC; Vector3 bp; Vector3.Subtract(ref p, ref triangle.vB, out bp); Vector3.Dot(ref ab, ref bp, out BPdotAB); Vector3.Dot(ref ac, ref bp, out BPdotAC); if (BPdotAB >= 0f && BPdotAC <= BPdotAB) { //It is B! return VoronoiRegion.B; } //Check to see if it's outside AB. float vc = APdotAB * BPdotAC - BPdotAB * APdotAC; if (vc <= 0 && APdotAB > 0 && BPdotAB < 0) //Note > and < instead of => <=; avoids possibly division by zero { return VoronoiRegion.AB; } //Check to see if it's outside C. float CPdotAB, CPdotAC; Vector3 cp; Vector3.Subtract(ref p, ref triangle.vC, out cp); Vector3.Dot(ref ab, ref cp, out CPdotAB); Vector3.Dot(ref ac, ref cp, out CPdotAC); if (CPdotAC >= 0f && CPdotAB <= CPdotAC) { //It is C! return VoronoiRegion.C; } //Check if it's outside AC. float vb = CPdotAB * APdotAC - APdotAB * CPdotAC; if (vb <= 0f && APdotAC > 0f && CPdotAC < 0f) //Note > instead of >= and < instead of <=; prevents bad denominator { return VoronoiRegion.AC; } //Check if it's outside BC. float va = BPdotAB * CPdotAC - CPdotAB * BPdotAC; if (va <= 0f && (BPdotAC - BPdotAB) > 0f && (CPdotAB - CPdotAC) > 0f)//Note > instead of >= and < instead of <=; prevents bad denominator { return VoronoiRegion.BC; } //On the face of the triangle. return VoronoiRegion.ABC; } /// /// Initializes the pair tester. /// ///Convex shape to use. ///Triangle shape to use. public override void Initialize(ConvexShape convex, TriangleShape triangle) { this.convex = convex; this.triangle = triangle; } /// /// Cleans up the pair tester. /// public override void CleanUp() { triangle = null; convex = null; state = CollisionState.Plane; escapeAttempts = 0; localSeparatingAxis = new Vector3(); Updated = false; } internal enum CollisionState { Plane, ExternalSeparated, ExternalNear, Deep } public override VoronoiRegion GetRegion(ref ContactData contact) { //Deep contact can produce non-triangle normals while still being within the triangle. //To solve this problem, find the voronoi region to which the contact belongs using its normal. //The voronoi region will be either the most extreme vertex, or the edge that includes //the first and second most extreme vertices. //If the normal dotted with an extreme edge direction is near 0, then it belongs to the edge. //Otherwise, it belongs to the vertex. //MPR tends to produce 'approximate' normals, though. //Use a fairly forgiving epsilon. float dotA, dotB, dotC; Vector3.Dot(ref triangle.vA, ref contact.Normal, out dotA); Vector3.Dot(ref triangle.vB, ref contact.Normal, out dotB); Vector3.Dot(ref triangle.vC, ref contact.Normal, out dotC); //Since normal points from convex to triangle always, reverse dot signs. dotA = -dotA; dotB = -dotB; dotC = -dotC; float faceEpsilon = .01f; const float edgeEpsilon = .01f; float edgeDot; Vector3 edgeDirection; if (dotA > dotB && dotA > dotC) { //A is extreme. if (dotB > dotC) { //B is second most extreme. if (Math.Abs(dotA - dotC) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vB, ref triangle.vA, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.AB; else return VoronoiRegion.A; } } else { //C is second most extreme. if (Math.Abs(dotA - dotB) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vC, ref triangle.vA, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.AC; else return VoronoiRegion.A; } } } else if (dotB > dotC) { //B is extreme. if (dotC > dotA) { //C is second most extreme. if (Math.Abs(dotB - dotA) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vC, ref triangle.vB, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.BC; else return VoronoiRegion.B; } } else { //A is second most extreme. if (Math.Abs(dotB - dotC) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vA, ref triangle.vB, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.AB; else return VoronoiRegion.B; } } } else { //C is extreme. if (dotA > dotB) { //A is second most extreme. if (Math.Abs(dotC - dotB) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vA, ref triangle.vC, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.AC; else return VoronoiRegion.C; } } else { //B is second most extreme. if (Math.Abs(dotC - dotA) < faceEpsilon) { //The normal is basically a face normal. This can happen at the edges occasionally. return VoronoiRegion.ABC; } else { Vector3.Subtract(ref triangle.vB, ref triangle.vC, out edgeDirection); Vector3.Dot(ref edgeDirection, ref contact.Normal, out edgeDot); if (edgeDot * edgeDot < edgeDirection.LengthSquared() * edgeEpsilon) return VoronoiRegion.BC; else return VoronoiRegion.C; } } } } public override bool ShouldCorrectContactNormal { get { return state == CollisionState.Deep; } } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/TrianglePairTester.cs ================================================ using System; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using BEPUphysics.Settings; using BEPUutilities.DataStructures; using System.Diagnostics; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Persistent tester that compares triangles against convex objects. /// public abstract class TrianglePairTester { internal TriangleShape triangle; /// /// Whether or not the pair tester was updated during the last attempt. /// public bool Updated; //Relies on the triangle being located in the local space of the convex object. The convex transform is used to transform the //contact points back from the convex's local space into world space. /// /// Generates a contact between the triangle and convex. /// ///Contact between the shapes, if any. ///Whether or not the shapes are colliding. public abstract bool GenerateContactCandidate(out TinyStructList contactList); /// /// Gets the triangle region in which the contact resides. /// /// Contact to check. /// Region in which the contact resides. public abstract VoronoiRegion GetRegion(ref ContactData contact); /// /// Whether or not the last found contact should have its normal corrected. /// public abstract bool ShouldCorrectContactNormal { get; } /// /// Initializes the pair tester. /// ///Convex shape to use. ///Triangle shape to use. public abstract void Initialize(ConvexShape convex, TriangleShape triangle); /// /// Cleans up the pair tester. /// public abstract void CleanUp(); } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/TriangleSpherePairTester.cs ================================================ using System; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using BEPUphysics.Settings; using BEPUutilities.DataStructures; using System.Diagnostics; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Persistent tester that compares triangles against convex objects. /// public sealed class TriangleSpherePairTester : TrianglePairTester { internal SphereShape sphere; private VoronoiRegion lastRegion; //Relies on the triangle being located in the local space of the convex object. The convex transform is used to transform the //contact points back from the convex's local space into world space. /// /// Generates a contact between the triangle and convex. /// ///Contact between the shapes, if any. ///Whether or not the shapes are colliding. public override bool GenerateContactCandidate(out TinyStructList contactList) { contactList = new TinyStructList(); Vector3 ab, ac; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac); Vector3 triangleNormal; Vector3.Cross(ref ab, ref ac, out triangleNormal); if (triangleNormal.LengthSquared() < Toolbox.Epsilon * .01f) { //If the triangle is degenerate, use the offset between its center and the sphere. Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleNormal); Vector3.Add(ref triangleNormal, ref triangle.vC, out triangleNormal); Vector3.Multiply(ref triangleNormal, 1 / 3f, out triangleNormal); if (triangleNormal.LengthSquared() < Toolbox.Epsilon * .01f) triangleNormal = Toolbox.UpVector; //Alrighty then! Pick a random direction. } float dot; Vector3.Dot(ref triangleNormal, ref triangle.vA, out dot); switch (triangle.sidedness) { case TriangleSidedness.DoubleSided: if (dot < 0) Vector3.Negate(ref triangleNormal, out triangleNormal); //Normal must face outward. break; case TriangleSidedness.Clockwise: if (dot > 0) return false; //Wrong side, can't have a contact pointing in a reasonable direction. break; case TriangleSidedness.Counterclockwise: if (dot < 0) return false; //Wrong side, can't have a contact pointing in a reasonable direction. break; } Vector3 closestPoint; //Could optimize this process a bit. The 'point' being compared is always zero. Additionally, since the triangle normal is available, //there is a little extra possible optimization. lastRegion = Toolbox.GetClosestPointOnTriangleToPoint(ref triangle.vA, ref triangle.vB, ref triangle.vC, ref Toolbox.ZeroVector, out closestPoint); float lengthSquared = closestPoint.LengthSquared(); float marginSum = triangle.collisionMargin + sphere.collisionMargin; if (lengthSquared <= marginSum * marginSum) { var contact = new ContactData(); if (lengthSquared < Toolbox.Epsilon) { //Super close to the triangle. Normalizing would be dangerous. Vector3.Negate(ref triangleNormal, out contact.Normal); contact.Normal.Normalize(); contact.PenetrationDepth = marginSum; contactList.Add(ref contact); return true; } lengthSquared = (float)Math.Sqrt(lengthSquared); Vector3.Divide(ref closestPoint, lengthSquared, out contact.Normal); contact.PenetrationDepth = marginSum - lengthSquared; contact.Position = closestPoint; contactList.Add(ref contact); return true; } return false; } public override VoronoiRegion GetRegion(ref ContactData contact) { return lastRegion; } public override bool ShouldCorrectContactNormal { get { return false; } } /// /// Initializes the pair tester. /// ///Convex shape to use. ///Triangle shape to use. public override void Initialize(ConvexShape convex, TriangleShape triangle) { this.sphere = (SphereShape)convex; this.triangle = triangle; } /// /// Cleans up the pair tester. /// public override void CleanUp() { triangle = null; sphere = null; Updated = false; } } } ================================================ FILE: BEPUphysics/CollisionTests/CollisionAlgorithms/TriangleTrianglePairTester.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests.CollisionAlgorithms { /// /// Generates candidates between two triangles and manages the persistent state of the pair. /// public class TriangleTrianglePairTester : TriangleConvexPairTester { //TODO: Having a specialized triangle-triangle pair test would be nice. Even if it didn't use an actual triangle-triangle test, certain assumptions could still make it speedier and more elegant. //"Closest points between triangles" + persistent manifolding would probably be the best approach (a lot faster than the triangle-convex general case anyway). public override bool GenerateContactCandidate(out TinyStructList contactList) { if (base.GenerateContactCandidate(out contactList)) { //The triangle-convex pair test has already rejected contacts whose normals would violate the first triangle's sidedness. //However, since it's a vanilla triangle-convex test, it doesn't know about the sidedness of the other triangle! var shape = ((TriangleShape)convex); Vector3 normal; //Lots of recalculating ab-bc! Vector3 ab, ac; Vector3.Subtract(ref shape.vB, ref shape.vA, out ab); Vector3.Subtract(ref shape.vC, ref shape.vA, out ac); Vector3.Cross(ref ab, ref ac, out normal); var sidedness = shape.sidedness; if (sidedness != TriangleSidedness.DoubleSided) { for (int i = contactList.Count - 1; i >= 0; i--) { ContactData item; contactList.Get(i, out item); float dot; Vector3.Dot(ref item.Normal, ref normal, out dot); if (sidedness == TriangleSidedness.Clockwise) { if (dot < 0) { contactList.RemoveAt(i); } } else { if (dot > 0) { contactList.RemoveAt(i); } } } } return contactList.Count > 0; } return false; } } } ================================================ FILE: BEPUphysics/CollisionTests/Contact.cs ================================================ using System; using BEPUphysics.Settings; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests { /// /// Handles information about a contact point during a collision between two bodies. /// public class Contact { /// /// Amount of penetration between the two objects. /// public float PenetrationDepth; /// /// Identifier used to link contact data with existing contacts and categorize members of a manifold. /// public int Id = -1; /// /// Normal direction of the surface at the contact point. /// public Vector3 Normal; /// /// Position of the contact point. /// public Vector3 Position; /// /// Sets up the contact with new information. /// ///Contact data to initialize the contact with. public void Setup(ref ContactData candidate) { candidate.Validate(); Position = candidate.Position; Normal = candidate.Normal; PenetrationDepth = candidate.PenetrationDepth; Id = candidate.Id; } /// /// Outputs the position, normal, and depth information of the contact into a string. /// /// Position, normal, and depth information of the contact in a string. public override string ToString() { return "Position: " + Position + " Normal: " + Normal + " Depth: " + PenetrationDepth; } } } ================================================ FILE: BEPUphysics/CollisionTests/ContactData.cs ================================================ using Microsoft.Xna.Framework; using System; namespace BEPUphysics.CollisionTests { /// /// Contact data created by collision detection. /// public struct ContactData :IEquatable { /// /// Amount of penetration between the two objects. /// public float PenetrationDepth; /// /// Feature-based id used to match contacts from the previous frame to their current versions. /// public int Id; /// /// Normal direction of the surface at the contact point. /// public Vector3 Normal; /// /// Position of the contact point. /// public Vector3 Position; /// /// Returns the fully qualified type name of this instance. /// /// /// A containing a fully qualified type name. /// /// 2 public override string ToString() { return Position + ", " + Normal; } public bool Equals(ContactData other) { return other.PenetrationDepth == PenetrationDepth && other.Id == Id && other.Normal == Normal && other.Position == Position; } } } ================================================ FILE: BEPUphysics/CollisionTests/ContactReducer.cs ================================================ using System; using BEPUutilities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests { /// /// Helper class that reduces contact manifolds to reasonable numbers of contacts. /// public static class ContactReducer { //This works in the general case where there can be any number of contacts and candidates. Could specialize it as an optimization to single-contact added incremental manifolds. /// /// Reduces the contact manifold to a good subset. /// ///Contacts to reduce. ///Contact candidates to include in the reduction process. ///Contacts that need to removed to reach the reduced state. ///Contact candidates that should be added to reach the reduced state. ///Thrown when the set being reduced is empty. public static void ReduceContacts(RawList contacts, RawValueList contactCandidates, RawList contactsToRemove, RawValueList toAdd) { //Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector. float maximumDepth = -float.MaxValue; int deepestIndex = -1; Vector3 normal = Toolbox.ZeroVector; for (int i = 0; i < contacts.Count; i++) { Vector3.Add(ref normal, ref contacts.Elements[i].Normal, out normal); if (contacts.Elements[i].PenetrationDepth > maximumDepth) { deepestIndex = i; maximumDepth = contacts.Elements[i].PenetrationDepth; } } for (int i = 0; i < contactCandidates.Count; i++) { Vector3.Add(ref normal, ref contactCandidates.Elements[i].Normal, out normal); if (contactCandidates.Elements[i].PenetrationDepth > maximumDepth) { deepestIndex = contacts.Count + i; maximumDepth = contactCandidates.Elements[i].PenetrationDepth; } } //If the normals oppose each other, this can happen. It doesn't need to be normalized, but having SOME normal is necessary. if (normal.LengthSquared() < Toolbox.Epsilon) if (contacts.Count > 0) normal = contacts.Elements[0].Normal; else if (contactCandidates.Count > 0) normal = contactCandidates.Elements[0].Normal; //This method is only called when there's too many contacts, so if contacts is empty, the candidates must NOT be empty. else //This method should not have been called at all if it gets here. throw new ArgumentException("Cannot reduce an empty contact set."); //Find the contact (candidate) that is furthest away from the deepest contact (candidate). Vector3 deepestPosition; if (deepestIndex < contacts.Count) deepestPosition = contacts.Elements[deepestIndex].Position; else deepestPosition = contactCandidates.Elements[deepestIndex - contacts.Count].Position; float distanceSquared; float furthestDistance = 0; int furthestIndex = -1; for (int i = 0; i < contacts.Count; i++) { Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared); if (distanceSquared > furthestDistance) { furthestDistance = distanceSquared; furthestIndex = i; } } for (int i = 0; i < contactCandidates.Count; i++) { Vector3.DistanceSquared(ref contactCandidates.Elements[i].Position, ref deepestPosition, out distanceSquared); if (distanceSquared > furthestDistance) { furthestDistance = distanceSquared; furthestIndex = contacts.Count + i; } } if (furthestIndex == -1) { //Either this method was called when it shouldn't have been, or all contacts and contact candidates are at the same location. if (contacts.Count > 0) { for (int i = 1; i < contacts.Count; i++) { contactsToRemove.Add(i); } return; } if (contactCandidates.Count > 0) { toAdd.Add(ref contactCandidates.Elements[0]); return; } throw new ArgumentException("Cannot reduce an empty contact set."); } Vector3 furthestPosition; if (furthestIndex < contacts.Count) furthestPosition = contacts.Elements[furthestIndex].Position; else furthestPosition = contactCandidates.Elements[furthestIndex - contacts.Count].Position; Vector3 xAxis; Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis); //Create the second axis of the 2d 'coordinate system' of the manifold. Vector3 yAxis; Vector3.Cross(ref xAxis, ref normal, out yAxis); //Determine the furthest points along the axis. float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue; int minYAxisIndex = -1, maxYAxisIndex = -1; for (int i = 0; i < contacts.Count; i++) { float dot; Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot); if (dot < minYAxisDot) { minYAxisIndex = i; minYAxisDot = dot; } if (dot > maxYAxisDot) { maxYAxisIndex = i; maxYAxisDot = dot; } } for (int i = 0; i < contactCandidates.Count; i++) { float dot; Vector3.Dot(ref contactCandidates.Elements[i].Position, ref yAxis, out dot); if (dot < minYAxisDot) { minYAxisIndex = i + contacts.Count; minYAxisDot = dot; } if (dot > maxYAxisDot) { maxYAxisIndex = i + contacts.Count; maxYAxisDot = dot; } } //the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points. //Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list. //Cycle through the candidates. If any match, add them to the toAdd list. //Repeated entries in the reduced manifold aren't a problem. //-Contacts list does not include repeats with itself. //-A contact is only removed if it doesn't match anything. //-Contact candidates do not repeat with themselves. //-Contact candidates do not repeat with contacts. //-Contact candidates are added if they match any of the indices. for (int i = 0; i < contactCandidates.Count; i++) { int totalIndex = i + contacts.Count; if (totalIndex == deepestIndex || totalIndex == furthestIndex || totalIndex == minYAxisIndex || totalIndex == maxYAxisIndex) { //This contact is present in the new manifold. Add it. toAdd.Add(ref contactCandidates.Elements[i]); } } for (int i = 0; i < contacts.Count; i++) { if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex)) { //This contact is not present in the new manifold. Remove it. contactsToRemove.Add(i); } } } //This works in the specific case of 4 contacts and 1 contact candidate. /// /// Reduces a 4-contact manifold and contact candidate to 4 total contacts. /// ///Contacts to reduce. ///Contact candidate to include in the reduction process. ///Contacts that need to be removed to reduce the manifold. ///Whether or not to add the contact candidate to reach the reduced manifold. ///Thrown when the contact manifold being reduced doesn't have 4 contacts. public static void ReduceContacts(RawList contacts, ref ContactData contactCandidate, RawList toRemove, out bool addCandidate) { if (contacts.Count != 4) throw new ArgumentException("Can only use this method to reduce contact lists with four contacts and a contact candidate."); //addCandidate = true; //float min = float.MaxValue; //int minIndex = 3; //for (int i = 0; i < 4; i++) //{ // if (contacts.Elements[i].PenetrationDepth < min) // { // min = contacts.Elements[i].PenetrationDepth; // minIndex = i; // } //} //toRemove.Add(minIndex); //return; //Find the deepest point of all contacts/candidates, as well as a compounded 'normal' vector. float maximumDepth = -float.MaxValue; int deepestIndex = -1; for (int i = 0; i < 4; i++) { if (contacts.Elements[i].PenetrationDepth > maximumDepth) { deepestIndex = i; maximumDepth = contacts.Elements[i].PenetrationDepth; } } if (contactCandidate.PenetrationDepth > maximumDepth) { deepestIndex = 4; } //Find the contact (candidate) that is furthest away from the deepest contact (candidate). Vector3 deepestPosition; if (deepestIndex < 4) deepestPosition = contacts.Elements[deepestIndex].Position; else deepestPosition = contactCandidate.Position; float distanceSquared; float furthestDistance = 0; int furthestIndex = -1; for (int i = 0; i < 4; i++) { Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref deepestPosition, out distanceSquared); if (distanceSquared > furthestDistance) { furthestDistance = distanceSquared; furthestIndex = i; } } Vector3.DistanceSquared(ref contactCandidate.Position, ref deepestPosition, out distanceSquared); if (distanceSquared > furthestDistance) { furthestIndex = 4; } Vector3 furthestPosition; if (furthestIndex < contacts.Count) furthestPosition = contacts.Elements[furthestIndex].Position; else furthestPosition = contactCandidate.Position; Vector3 xAxis; Vector3.Subtract(ref deepestPosition, ref furthestPosition, out xAxis); //Create the second axis of the 2d 'coordinate system' of the manifold. Vector3 yAxis; Vector3.Cross(ref xAxis, ref contacts.Elements[0].Normal, out yAxis); //Determine the furthest points along the axis. float minYAxisDot = float.MaxValue, maxYAxisDot = -float.MaxValue; int minYAxisIndex = -1, maxYAxisIndex = -1; float dot; for (int i = 0; i < 4; i++) { Vector3.Dot(ref contacts.Elements[i].Position, ref yAxis, out dot); if (dot < minYAxisDot) { minYAxisIndex = i; minYAxisDot = dot; } if (dot > maxYAxisDot) { maxYAxisIndex = i; maxYAxisDot = dot; } } Vector3.Dot(ref contactCandidate.Position, ref yAxis, out dot); if (dot < minYAxisDot) { minYAxisIndex = 4; } if (dot > maxYAxisDot) { maxYAxisIndex = 4; } //the deepestIndex, furthestIndex, minYAxisIndex, and maxYAxisIndex are the extremal points. //Cycle through the existing contacts. If any DO NOT MATCH the existing candidates, add them to the toRemove list. //Cycle through the candidates. If any match, add them to the toAdd list. //Repeated entries in the reduced manifold aren't a problem. //-Contacts list does not include repeats with itself. //-A contact is only removed if it doesn't match anything. //-Contact candidates do not repeat with themselves. //-Contact candidates do not repeat with contacts. //-Contact candidates are added if they match any of the indices. if (4 == deepestIndex || 4 == furthestIndex || 4 == minYAxisIndex || 4 == maxYAxisIndex) { addCandidate = true; //Only reduce when we are going to add a new contact, and only get rid of one. for (int i = 0; i < 4; i++) { if (!(i == deepestIndex || i == furthestIndex || i == minYAxisIndex || i == maxYAxisIndex)) { //This contact is not present in the new manifold. Remove it. toRemove.Add(i); break; } } } else addCandidate = false; } } } ================================================ FILE: BEPUphysics/CollisionTests/ContactRefresher.cs ================================================ using BEPUutilities; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using System.Diagnostics; using System; namespace BEPUphysics.CollisionTests { /// /// Helper class that refreshes manifolds to keep them recent. /// public class ContactRefresher { /// /// Refreshes the contact manifold, removing any out of date contacts /// and updating others. /// public static void ContactRefresh(RawList contacts, RawValueList supplementData, ref RigidTransform transformA, ref RigidTransform transformB, RawList toRemove) { //TODO: Could also refresh normals with some trickery. //Would also need to refresh depth using new normals, and would require some extra information. for (int k = 0; k < contacts.Count; k++) { contacts.Elements[k].Validate(); ContactSupplementData data = supplementData.Elements[k]; Vector3 newPosA, newPosB; RigidTransform.Transform(ref data.LocalOffsetA, ref transformA, out newPosA); RigidTransform.Transform(ref data.LocalOffsetB, ref transformB, out newPosB); //ab - (ab*n)*n //Compute the horizontal offset. Vector3 ab; Vector3.Subtract(ref newPosB, ref newPosA, out ab); float dot; Vector3.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); Vector3 temp; Vector3.Multiply(ref contacts.Elements[k].Normal, dot, out temp); Vector3.Subtract(ref ab, ref temp, out temp); dot = temp.LengthSquared(); if (dot > CollisionDetectionSettings.ContactInvalidationLengthSquared) { toRemove.Add(k); } else { //Depth refresh: //Find deviation (IE, (Ra-Rb)*N) and add to base depth. Vector3.Dot(ref ab, ref contacts.Elements[k].Normal, out dot); contacts.Elements[k].PenetrationDepth = data.BasePenetrationDepth - dot; if (contacts.Elements[k].PenetrationDepth < -CollisionDetectionSettings.maximumContactDistance) toRemove.Add(k); else { //Refresh position and ra/rb. Vector3 newPos; Vector3.Add(ref newPosB, ref newPosA, out newPos); Vector3.Multiply(ref newPos, .5f, out newPos); contacts.Elements[k].Position = newPos; //This is an interesting idea, but has very little effect one way or the other. //data.BasePenetrationDepth = contacts.Elements[k].PenetrationDepth; //RigidTransform.TransformByInverse(ref newPos, ref transformA, out data.LocalOffsetA); //RigidTransform.TransformByInverse(ref newPos, ref transformB, out data.LocalOffsetB); } contacts.Elements[k].Validate(); } } } } } ================================================ FILE: BEPUphysics/CollisionTests/ContactSupplementData.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.CollisionTests { /// /// Extra data associated with a contact point used to refresh contacts each frame. /// public struct ContactSupplementData { /// /// Offset from the center of the first object to the contact point in the object's local space. /// public Vector3 LocalOffsetA; /// /// Offset from the center of the second object to the contact point in the object's local space. /// public Vector3 LocalOffsetB; /// /// Original penetration depth computed at the associatd contact. /// public float BasePenetrationDepth; } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/BoxContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contact data between two boxes. /// public class BoxContactManifold : ContactManifold { protected ConvexCollidable boxA, boxB; /// /// Gets the first collidable in the pair. /// public ConvexCollidable CollidableA { get { return boxA; } } /// /// Gets the second collidable in the pair. /// public ConvexCollidable CollidableB { get { return boxB; } } /// /// Constructs a new manifold. /// public BoxContactManifold() { contacts = new RawList(4); unusedContacts = new UnsafeResourcePool(4); contactIndicesToRemove = new RawList(4); } #if ALLOWUNSAFE /// /// Updates the manifold. /// ///Timestep duration. public override void Update(float dt) { //Now, generate a contact between the two shapes. float distance; Vector3 axis; BoxContactDataCache manifold; if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold)) { unsafe { BoxContactData* manifoldPointer = &manifold.D1; Vector3.Negate(ref axis, out axis); var toRemove = new TinyList(); for (int i = 0; i < contacts.Count; i++) { bool found = false; for (int j = manifold.Count - 1; j >= 0; j--) { if (contacts.Elements[i].Id == manifoldPointer[j].Id) { found = true; contacts.Elements[i].Validate(); //Update contact... contacts.Elements[i].Position = manifoldPointer[j].Position; contacts.Elements[i].PenetrationDepth = -manifoldPointer[j].Depth; contacts.Elements[i].Normal = axis; //Remove manifold entry contacts.Elements[i].Validate(); manifold.RemoveAt(j); break; } } if (!found) {//No match found toRemove.Add(i); } } //toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through. for (int i = toRemove.Count - 1; i >= 0; i--) Remove(toRemove[i]); //Add new contacts. for (int i = 0; i < manifold.Count; i++) { var newContact = new ContactData { Position = manifoldPointer[i].Position, PenetrationDepth = -manifoldPointer[i].Depth, Normal = axis, Id = manifoldPointer[i].Id }; Add(ref newContact); } } } else { //Not colliding, so get rid of it. for (int i = contacts.Count - 1; i >= 0; i--) { Remove(i); } } } #else public override void Update(float dt) { //Now, generate a contact between the two shapes. float distance; Vector3 axis; var manifold = new TinyStructList(); if (BoxBoxCollider.AreBoxesColliding(boxA.Shape, boxB.Shape, ref boxA.worldTransform, ref boxB.worldTransform, out distance, out axis, out manifold)) { Vector3.Negate(ref axis, out axis); TinyList toRemove = new TinyList(); BoxContactData data; for (int i = 0; i < contacts.Count; i++) { bool found = false; for (int j = manifold.Count - 1; j >= 0; j--) { manifold.Get(j, out data); if (contacts.Elements[i].Id == data.Id) { found = true; //Update contact... contacts.Elements[i].Position = data.Position; contacts.Elements[i].PenetrationDepth = -data.Depth; contacts.Elements[i].Normal = axis; contacts.Elements[i].Validate(); //Remove manifold entry manifold.RemoveAt(j); break; } } if (!found) {//No match found toRemove.Add(i); } } ////Go through the indices to remove. ////For each one, replace the removal index with a contact in the new manifold. //int removalIndex; //for (removalIndex = toRemove.count - 1; removalIndex >= 0 && manifold.count > 0; removalIndex--) //{ // int indexToReplace = toRemove[removalIndex]; // toRemove.RemoveAt(removalIndex); // manifold.Get(manifold.count - 1, out data); // //Update contact... // contacts.Elements[indexToReplace].Position = data.Position; // contacts.Elements[indexToReplace].PenetrationDepth = -data.Depth; // contacts.Elements[indexToReplace].Normal = axis; // contacts.Elements[indexToReplace].Id = data.Id; // //Remove manifold entry // manifold.RemoveAt(manifold.count - 1); //} //Alright, we ran out of contacts to replace (if, in fact, toRemove isn't empty now). Just remove the remainder. //toRemove is sorted by increasing index. Go backwards along it so that the indices are valid all the way through. for (int i = toRemove.Count - 1; i >= 0; i--) Remove(toRemove[i]); //Add new contacts. for (int i = 0; i < manifold.Count; i++) { manifold.Get(i, out data); ContactData newContact = new ContactData(); newContact.Position = data.Position; newContact.PenetrationDepth = -data.Depth; newContact.Normal = axis; newContact.Id = data.Id; Add(ref newContact); } } else { //Not colliding, so get rid of it. for (int i = contacts.Count - 1; i >= 0; i--) { Remove(i); } } } #endif /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. ///Thrown when the collidables being used are not of the proper type. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { boxA = (ConvexCollidable)newCollidableA; boxB = (ConvexCollidable)newCollidableB; if (boxA == null || boxB == null) { throw new ArgumentException("Inappropriate types used to initialize pair tester."); } } /// /// Cleans up the manifold. /// public override void CleanUp() { boxA = null; boxB = null; base.CleanUp(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/BoxSphereContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contact data between two boxes. /// public class BoxSphereContactManifold : ContactManifold { protected ConvexCollidable box; protected ConvexCollidable sphere; /// /// Gets the first collidable in the pair. /// public ConvexCollidable CollidableA { get { return box; } } /// /// Gets the second collidable in the pair. /// public ConvexCollidable CollidableB { get { return sphere; } } /// /// Constructs a new manifold. /// public BoxSphereContactManifold() { contacts = new RawList(1); } Contact contact = new Contact(); bool previouslyColliding; /// /// Updates the manifold. /// ///Timestep duration. public override void Update(float dt) { ContactData contactData; bool colliding = false; if (BoxSphereTester.AreShapesColliding(box.Shape, sphere.Shape, ref box.worldTransform, ref sphere.worldTransform.Position, out contactData)) { if (!previouslyColliding && contactData.PenetrationDepth >= 0)//Don't use the contact if it's an initial contact and the depth is negative. Why not? Bounciness and InitialCollisionDetected. { Add(ref contactData); colliding = true; } else if (previouslyColliding) { contactData.Validate(); contact.Normal = contactData.Normal; contact.PenetrationDepth = contactData.PenetrationDepth; contact.Position = contactData.Position; colliding = true; } } else { if (previouslyColliding) Remove(0); } previouslyColliding = colliding; } protected override void Add(ref ContactData contactCandidate) { contactCandidate.Validate(); contact.Normal = contactCandidate.Normal; contact.PenetrationDepth = contactCandidate.PenetrationDepth; contact.Position = contactCandidate.Position; contacts.Add(contact); OnAdded(contact); } protected override void Remove(int index) { contacts.RemoveAt(index); OnRemoved(contact); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. ///Thrown when the collidables being used are not of the proper type. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { box = newCollidableA as ConvexCollidable; sphere = newCollidableB as ConvexCollidable; if (box == null || sphere == null) { box = newCollidableB as ConvexCollidable; sphere = newCollidableA as ConvexCollidable; if (box == null || sphere == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } } /// /// Cleans up the manifold. /// public override void CleanUp() { box = null; sphere = null; previouslyColliding = false; //We don't have to worry about losing a reference to our contact- we keep it local! contacts.Clear(); } /// /// Clears the contacts associated with this manifold. /// public override void ClearContacts() { previouslyColliding = false; base.ClearContacts(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/ContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Superclass of manifolds which manage persistent contacts over multiple frames. /// public abstract class ContactManifold { protected RawList contactIndicesToRemove; protected internal RawList contacts; /// /// Gets the contacts in the manifold. /// public ReadOnlyList Contacts { get { return new ReadOnlyList(contacts); } } protected UnsafeResourcePool unusedContacts; protected void RemoveQueuedContacts() { //TOREMOVE MUST BE SORTED LEAST TO GREATEST INDEX. for (int i = contactIndicesToRemove.Count - 1; i >= 0; i--) { Remove(contactIndicesToRemove.Elements[i]); } contactIndicesToRemove.Clear(); } protected virtual void Remove(int contactIndex) { Contact removing = contacts.Elements[contactIndex]; contacts.FastRemoveAt(contactIndex); OnRemoved(removing); unusedContacts.GiveBack(removing); } protected virtual void Add(ref ContactData contactCandidate) { Contact adding = unusedContacts.Take(); adding.Setup(ref contactCandidate); contacts.Add(adding); OnAdded(adding); } /// /// Fires when a contact is added. /// public event Action ContactAdded; /// /// Fires when a contact is removed. /// public event Action ContactRemoved; protected void OnAdded(Contact contact) { if (ContactAdded != null) ContactAdded(contact); } protected void OnRemoved(Contact contact) { if (ContactRemoved != null) ContactRemoved(contact); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public abstract void Initialize(Collidable newCollidableA, Collidable newCollidableB); /// /// Cleans up the manifold. /// public virtual void CleanUp() { for (int i = contacts.Count - 1; i >= 0; --i) { unusedContacts.GiveBack(contacts.Elements[i]); contacts.FastRemoveAt(i); } } /// /// Updates the manifold. /// ///Timestep duration. public abstract void Update(float dt); /// /// Clears the contacts associated with this manifold. /// public virtual void ClearContacts() { for (int i = contacts.Count - 1; i >= 0; i--) { Remove(i); } } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/GeneralConvexContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.Settings; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts for two convex collidables. /// public class GeneralConvexContactManifold : ContactManifold { RawValueList supplementData = new RawValueList(4); GeneralConvexPairTester pairTester; /// /// Gets the pair tester used by the manifold to do testing. /// public GeneralConvexPairTester PairTester { get { return pairTester; } } protected ConvexCollidable collidableA, collidableB; /// /// Gets the first collidable in the pair. /// public ConvexCollidable CollidableA { get { return collidableA; } } /// /// Gets the second collidable in the pair. /// public ConvexCollidable CollidableB { get { return collidableB; } } /// /// Constructs a new convex-convex manifold. /// public GeneralConvexContactManifold() { contacts = new RawList(4); unusedContacts = new UnsafeResourcePool(4); contactIndicesToRemove = new RawList(4); pairTester = new GeneralConvexPairTester(); } /// /// Updates the manifold. /// ///Timestep duration. public override void Update(float dt) { //First, refresh all existing contacts. This is an incremental manifold. ContactRefresher.ContactRefresh(contacts, supplementData, ref collidableA.worldTransform, ref collidableB.worldTransform, contactIndicesToRemove); RemoveQueuedContacts(); //Now, generate a contact between the two shapes. ContactData contact; if (pairTester.GenerateContactCandidate(out contact)) { if (IsContactUnique(ref contact)) { //Check if adding the new contact would overflow the manifold. if (contacts.Count == 4) { //Adding that contact would overflow the manifold. Reduce to the best subset. bool addCandidate; ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate); RemoveQueuedContacts(); if (addCandidate) Add(ref contact); } else { //Won't overflow the manifold, so just toss it in. Add(ref contact); } } } else { //No collision, clean out the manifold. for (int i = contacts.Count - 1; i >= 0; i--) { Remove(i); } } } protected override void Add(ref ContactData contactCandidate) { ContactSupplementData supplement; supplement.BasePenetrationDepth = contactCandidate.PenetrationDepth; //The closest point method computes the local space versions before transforming to world... consider cutting out the middle man RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplement.LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplement.LocalOffsetB); supplementData.Add(ref supplement); base.Add(ref contactCandidate); } protected override void Remove(int contactIndex) { supplementData.RemoveAt(contactIndex); base.Remove(contactIndex); } private bool IsContactUnique(ref ContactData contactCandidate) { contactCandidate.Validate(); for (int i = 0; i < contacts.Count; i++) { float distanceSquared; Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //Update the existing 'redundant' contact with the new information. //This works out because the new contact is the deepest contact according to the previous collision detection iteration. contacts.Elements[i].Normal = contactCandidate.Normal; contacts.Elements[i].Position = contactCandidate.Position; contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth; RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplementData.Elements[i].LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplementData.Elements[i].LocalOffsetB); return false; } } return true; } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { collidableA = newCollidableA as ConvexCollidable; collidableB = newCollidableB as ConvexCollidable; pairTester.Initialize(newCollidableA, newCollidableB); if (collidableA == null || collidableB == null) { throw new ArgumentException("Inappropriate types used to initialize pair tester."); } } /// /// Cleans up the manifold. /// public override void CleanUp() { supplementData.Clear(); collidableA = null; collidableB = null; pairTester.CleanUp(); base.CleanUp(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/InstancedMeshContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.DataStructures; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public abstract class InstancedMeshContactManifold : TriangleMeshConvexContactManifold { protected InstancedMesh mesh; internal RawList overlappedTriangles = new RawList(8); /// /// Gets the mesh of the pair. /// public InstancedMesh Mesh { get { return mesh; } } protected internal override int FindOverlappingTriangles(float dt) { BoundingBox boundingBox; convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref mesh.worldTransform, out boundingBox); if (convex.entity != null) { Vector3 transformedVelocity; Matrix3x3 inverse; Matrix3x3.Invert(ref mesh.worldTransform.LinearTransform, out inverse); Matrix3x3.Transform(ref convex.entity.linearVelocity, ref inverse, out transformedVelocity); Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity); if (transformedVelocity.X > 0) boundingBox.Max.X += transformedVelocity.X; else boundingBox.Min.X += transformedVelocity.X; if (transformedVelocity.Y > 0) boundingBox.Max.Y += transformedVelocity.Y; else boundingBox.Min.Y += transformedVelocity.Y; if (transformedVelocity.Z > 0) boundingBox.Max.Z += transformedVelocity.Z; else boundingBox.Min.Z += transformedVelocity.Z; } mesh.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, overlappedTriangles); return overlappedTriangles.Count; } protected override bool ConfigureTriangle(int i, out TriangleIndices indices) { MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data; int triangleIndex = overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC); AffineTransform.Transform(ref localTriangleShape.vA, ref mesh.worldTransform, out localTriangleShape.vA); AffineTransform.Transform(ref localTriangleShape.vB, ref mesh.worldTransform, out localTriangleShape.vB); AffineTransform.Transform(ref localTriangleShape.vC, ref mesh.worldTransform, out localTriangleShape.vC); //In instanced meshes, the bounding box we found in local space could collect more triangles than strictly necessary. //By doing a second pass, we should be able to prune out quite a few of them. BoundingBox triangleAABB; Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleAABB); bool toReturn; triangleAABB.Intersects(ref convex.boundingBox, out toReturn); if (!toReturn) { indices = new TriangleIndices(); return false; } localTriangleShape.sidedness = mesh.sidedness; localTriangleShape.collisionMargin = 0; indices = new TriangleIndices() { A = data.indices[triangleIndex], B = data.indices[triangleIndex + 1], C = data.indices[triangleIndex + 2] }; return true; } protected internal override void CleanUpOverlappingTriangles() { overlappedTriangles.Clear(); } protected override bool UseImprovedBoundaryHandling { get { return mesh.improveBoundaryBehavior; } } /// /// Cleans up the manifold. /// public override void CleanUp() { mesh = null; convex = null; base.CleanUp(); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { convex = newCollidableA as ConvexCollidable; mesh = newCollidableB as InstancedMesh; if (convex == null || mesh == null) { convex = newCollidableB as ConvexCollidable; mesh = newCollidableA as InstancedMesh; if (convex == null || mesh == null) throw new ArgumentException("Inappropriate types used to initialize contact manifold."); } } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/InstancedMeshConvexContactManifold.cs ================================================ using BEPUutilities.ResourceManagement; using BEPUphysics.CollisionTests.CollisionAlgorithms; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public class InstancedMeshConvexContactManifold : InstancedMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(CollisionAlgorithms.TrianglePairTester tester) { testerPool.GiveBack((TriangleConvexPairTester)tester); } protected override CollisionAlgorithms.TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/InstancedMeshSphereContactManifold.cs ================================================ using BEPUutilities.ResourceManagement; using BEPUphysics.CollisionTests.CollisionAlgorithms; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public class InstancedMeshSphereContactManifold : InstancedMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(CollisionAlgorithms.TrianglePairTester tester) { testerPool.GiveBack((TriangleSpherePairTester)tester); } protected override CollisionAlgorithms.TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/MobileMeshContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; using BEPUphysics.CollisionShapes; using BEPUphysics.CollisionTests.CollisionAlgorithms; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public abstract class MobileMeshContactManifold : TriangleMeshConvexContactManifold { protected MobileMeshCollidable mesh; internal int parentContactCount; internal RawList overlappedTriangles = new RawList(8); /// /// Gets the mesh of the pair. /// public MobileMeshCollidable Mesh { get { return mesh; } } protected override RigidTransform MeshTransform { get { return mesh.worldTransform; } } //Expand the convex's bounding box to include the mobile mesh's movement. protected internal override int FindOverlappingTriangles(float dt) { BoundingBox boundingBox; AffineTransform transform = new AffineTransform(mesh.worldTransform.Orientation, mesh.worldTransform.Position); convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref transform, out boundingBox); Vector3 transformedVelocity; //Compute the relative velocity with respect to the mesh. The mesh's bounding tree is NOT expanded with velocity, //so whatever motion there is between the two objects needs to be included in the convex's bounding box. if (convex.entity != null) transformedVelocity = convex.entity.linearVelocity; else transformedVelocity = new Vector3(); if (mesh.entity != null) Vector3.Subtract(ref transformedVelocity, ref mesh.entity.linearVelocity, out transformedVelocity); //The linear transform is known to be orientation only, so using the transpose is allowed. Matrix3x3.TransformTranspose(ref transformedVelocity, ref transform.LinearTransform, out transformedVelocity); Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity); if (transformedVelocity.X > 0) boundingBox.Max.X += transformedVelocity.X; else boundingBox.Min.X += transformedVelocity.X; if (transformedVelocity.Y > 0) boundingBox.Max.Y += transformedVelocity.Y; else boundingBox.Min.Y += transformedVelocity.Y; if (transformedVelocity.Z > 0) boundingBox.Max.Z += transformedVelocity.Z; else boundingBox.Min.Z += transformedVelocity.Z; mesh.Shape.TriangleMesh.Tree.GetOverlaps(boundingBox, overlappedTriangles); return overlappedTriangles.Count; } protected override bool ConfigureTriangle(int i, out TriangleIndices indices) { MeshBoundingBoxTreeData data = mesh.Shape.TriangleMesh.Data; int triangleIndex = overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC); AffineTransform transform; AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out transform); AffineTransform.Transform(ref localTriangleShape.vA, ref transform, out localTriangleShape.vA); AffineTransform.Transform(ref localTriangleShape.vB, ref transform, out localTriangleShape.vB); AffineTransform.Transform(ref localTriangleShape.vC, ref transform, out localTriangleShape.vC); //In instanced meshes, the bounding box we found in local space could collect more triangles than strictly necessary. //By doing a second pass, we should be able to prune out quite a few of them. BoundingBox triangleAABB; Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleAABB); bool toReturn; triangleAABB.Intersects(ref convex.boundingBox, out toReturn); if (!toReturn) { indices = new TriangleIndices(); return false; } TriangleSidedness sidedness; switch (mesh.Shape.solidity) { case MobileMeshSolidity.Clockwise: sidedness = TriangleSidedness.Clockwise; break; case MobileMeshSolidity.Counterclockwise: sidedness = TriangleSidedness.Counterclockwise; break; case MobileMeshSolidity.DoubleSided: sidedness = TriangleSidedness.DoubleSided; break; default: sidedness = mesh.Shape.solidSidedness; break; } localTriangleShape.sidedness = sidedness; localTriangleShape.collisionMargin = 0; indices = new TriangleIndices() { A = data.indices[triangleIndex], B = data.indices[triangleIndex + 1], C = data.indices[triangleIndex + 2] }; return true; } protected internal override void CleanUpOverlappingTriangles() { overlappedTriangles.Clear(); } protected override bool UseImprovedBoundaryHandling { get { return mesh.improveBoundaryBehavior; } } float previousDepth; Vector3 lastValidConvexPosition; protected override void ProcessCandidates(RawValueList candidates) { if (candidates.Count == 0 && parentContactCount == 0 && Mesh.Shape.solidity == MobileMeshSolidity.Solid) { //If there's no new contacts on the mesh and it's supposed to be a solid, //then we must check the convex for containment within the shell. //We already know that it's not on the shell, meaning that the shape is either //far enough away outside the shell that there's no contact (and we're done), //or it's far enough inside the shell that the triangles cannot create contacts. //To find out which it is, raycast against the shell. Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out orientation); Ray ray; Vector3.Subtract(ref convex.worldTransform.Position, ref mesh.worldTransform.Position, out ray.Position); Matrix3x3.TransformTranspose(ref ray.Position, ref orientation, out ray.Position); //Cast from the current position back to the previous position. Vector3.Subtract(ref lastValidConvexPosition, ref ray.Position, out ray.Direction); float rayDirectionLength = ray.Direction.LengthSquared(); if (rayDirectionLength < Toolbox.Epsilon) { //The object may not have moved enough to normalize properly. If so, choose something arbitrary. //Try the direction from the center of the object to the convex's position. ray.Direction = ray.Position; rayDirectionLength = ray.Direction.LengthSquared(); if (rayDirectionLength < Toolbox.Epsilon) { //This is unlikely; just pick something completely arbitrary then. ray.Direction = Vector3.Up; rayDirectionLength = 1; } } Vector3.Divide(ref ray.Direction, (float)Math.Sqrt(rayDirectionLength), out ray.Direction); RayHit hit; if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit)) { ContactData newContact = new ContactData {Id = 2}; //Give it a special id so that we know that it came from the inside. Matrix3x3.Transform(ref ray.Position, ref orientation, out newContact.Position); Vector3.Add(ref newContact.Position, ref mesh.worldTransform.Position, out newContact.Position); newContact.Normal = hit.Normal; newContact.Normal.Normalize(); float factor; Vector3.Dot(ref ray.Direction, ref newContact.Normal, out factor); newContact.PenetrationDepth = -factor * hit.T + convex.Shape.minimumRadius; Matrix3x3.Transform(ref newContact.Normal, ref orientation, out newContact.Normal); newContact.Validate(); //Do not yet create a new contact. Check to see if an 'inner contact' with id == 2 already exists. bool addContact = true; for (int i = 0; i < contacts.Count; i++) { if (contacts.Elements[i].Id == 2) { contacts.Elements[i].Position = newContact.Position; contacts.Elements[i].Normal = newContact.Normal; contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth; supplementData.Elements[i].LocalOffsetA = new Vector3(); supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh. addContact = false; break; } } if (addContact && contacts.Count == 0) Add(ref newContact); previousDepth = newContact.PenetrationDepth; } else { //It's possible that we had a false negative. The previous frame may have been in deep intersection, and this frame just failed to come to the same conclusion. //If we set the target location to the current location, the object will never escape the mesh. Instead, only do that if two frames agree that we are no longer colliding. if (previousDepth > 0) { //We're not touching the mesh. lastValidConvexPosition = ray.Position; } previousDepth = 0; } } } /// /// Cleans up the manifold. /// public override void CleanUp() { mesh = null; convex = null; parentContactCount = 0; base.CleanUp(); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { convex = newCollidableA as ConvexCollidable; mesh = newCollidableB as MobileMeshCollidable; if (convex == null || mesh == null) { convex = newCollidableB as ConvexCollidable; mesh = newCollidableA as MobileMeshCollidable; if (convex == null || mesh == null) throw new ArgumentException("Inappropriate types used to initialize contact manifold."); } } UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleConvexPairTester)tester); } protected override TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/MobileMeshConvexContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public class MobileMeshConvexContactManifold : MobileMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleConvexPairTester)tester); } protected override TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/MobileMeshSphereContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public class MobileMeshSphereContactManifold : MobileMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleSpherePairTester)tester); } protected override TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/MobileMeshTriangleContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a convex and an instanced mesh. /// public class MobileMeshTriangleContactManifold : MobileMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleTrianglePairTester)tester); } protected override TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/SphereContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contact data between two boxes. /// public class SphereContactManifold : ContactManifold { protected ConvexCollidable sphereA; protected ConvexCollidable sphereB; /// /// Gets the first collidable in the pair. /// public ConvexCollidable CollidableA { get { return sphereA; } } /// /// Gets the second collidable in the pair. /// public ConvexCollidable CollidableB { get { return sphereB; } } /// /// Constructs a new manifold. /// public SphereContactManifold() { contacts = new RawList(1); } Contact contact = new Contact(); bool previouslyColliding; /// /// Updates the manifold. /// ///Timestep duration. public override void Update(float dt) { ContactData contactData; bool colliding = false; if (SphereTester.AreSpheresColliding(sphereA.Shape, sphereB.Shape, ref sphereA.worldTransform.Position, ref sphereB.worldTransform.Position, out contactData)) { if (!previouslyColliding && contactData.PenetrationDepth >= 0) //Don't use the contact if it's an initial contact and the depth is negative. Why not? Bounciness and InitialCollisionDetected. { Add(ref contactData); colliding = true; } else if (previouslyColliding) { contactData.Validate(); contact.Normal = contactData.Normal; contact.PenetrationDepth = contactData.PenetrationDepth; contact.Position = contactData.Position; colliding = true; } } else { if (previouslyColliding) Remove(0); } previouslyColliding = colliding; } protected override void Add(ref ContactData contactCandidate) { contactCandidate.Validate(); contact.Normal = contactCandidate.Normal; contact.PenetrationDepth = contactCandidate.PenetrationDepth; contact.Position = contactCandidate.Position; contacts.Add(contact); OnAdded(contact); } protected override void Remove(int index) { contacts.RemoveAt(index); OnRemoved(contact); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. ///Thrown when the collidables being used are not of the proper type. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { sphereA = (ConvexCollidable)newCollidableA; sphereB = (ConvexCollidable)newCollidableB; if (sphereA == null || sphereB == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } /// /// Cleans up the manifold. /// public override void CleanUp() { sphereA = null; sphereB = null; previouslyColliding = false; //We don't have to worry about losing a reference to our contact- we keep it local! contacts.Clear(); } /// /// Clears the contacts associated with this manifold. /// public override void ClearContacts() { previouslyColliding = false; base.ClearContacts(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/StaticMeshContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.DataStructures; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a static mesh and a convex. /// public abstract class StaticMeshContactManifold : TriangleMeshConvexContactManifold { protected StaticMesh mesh; internal RawList overlappedTriangles = new RawList(4); /// /// Gets the static mesh associated with this pair. /// public StaticMesh Mesh { get { return mesh; } } protected internal override int FindOverlappingTriangles(float dt) { mesh.Mesh.Tree.GetOverlaps(convex.boundingBox, overlappedTriangles); return overlappedTriangles.Count; } protected override bool ConfigureTriangle(int i, out TriangleIndices indices) { int triangleIndex = overlappedTriangles.Elements[i]; mesh.Mesh.Data.GetTriangle(triangleIndex, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC); localTriangleShape.sidedness = mesh.sidedness; localTriangleShape.collisionMargin = 0; indices = new TriangleIndices { A = mesh.Mesh.Data.indices[triangleIndex], B = mesh.Mesh.Data.indices[triangleIndex + 1], C = mesh.Mesh.Data.indices[triangleIndex + 2] }; return true; } protected internal override void CleanUpOverlappingTriangles() { overlappedTriangles.Clear(); } protected override bool UseImprovedBoundaryHandling { get { return mesh.improveBoundaryBehavior; } } /// /// Cleans up the manifold. /// public override void CleanUp() { mesh = null; convex = null; base.CleanUp(); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { convex = newCollidableA as ConvexCollidable; mesh = newCollidableB as StaticMesh; if (convex == null || mesh == null) { convex = newCollidableB as ConvexCollidable; mesh = newCollidableA as StaticMesh; if (convex == null || mesh == null) throw new ArgumentException("Inappropriate types used to initialize contact manifold."); } } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/StaticMeshConvexContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a static mesh and a convex. /// public class StaticMeshConvexContactManifold : StaticMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(CollisionAlgorithms.TrianglePairTester tester) { testerPool.GiveBack((TriangleConvexPairTester)tester); } protected override CollisionAlgorithms.TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/StaticMeshSphereContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a static mesh and a convex. /// public class StaticMeshSphereContactManifold : StaticMeshContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override void GiveBackTester(CollisionAlgorithms.TrianglePairTester tester) { testerPool.GiveBack((TriangleSpherePairTester)tester); } protected override CollisionAlgorithms.TrianglePairTester GetTester() { return testerPool.Take(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/TerrainContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; using BEPUphysics.Settings; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a Terrain and a convex. /// public abstract class TerrainContactManifold : TriangleMeshConvexContactManifold { protected Terrain terrain; internal RawList overlappedTriangles = new RawList(4); /// /// Gets the terrain associated with this pair. /// public Terrain Terrain { get { return terrain; } } protected internal override int FindOverlappingTriangles(float dt) { BoundingBox boundingBox; convex.Shape.GetLocalBoundingBox(ref convex.worldTransform, ref terrain.worldTransform, out boundingBox); if (convex.entity != null) { Vector3 transformedVelocity; Matrix3x3 inverse; Matrix3x3.Invert(ref terrain.worldTransform.LinearTransform, out inverse); Matrix3x3.Transform(ref convex.entity.linearVelocity, ref inverse, out transformedVelocity); Vector3.Multiply(ref transformedVelocity, dt, out transformedVelocity); if (transformedVelocity.X > 0) boundingBox.Max.X += transformedVelocity.X; else boundingBox.Min.X += transformedVelocity.X; if (transformedVelocity.Y > 0) boundingBox.Max.Y += transformedVelocity.Y; else boundingBox.Min.Y += transformedVelocity.Y; if (transformedVelocity.Z > 0) boundingBox.Max.Z += transformedVelocity.Z; else boundingBox.Min.Z += transformedVelocity.Z; } terrain.Shape.GetOverlaps(boundingBox, overlappedTriangles); return overlappedTriangles.Count; } protected override bool ConfigureTriangle(int i, out TriangleIndices indices) { indices = overlappedTriangles.Elements[i]; terrain.Shape.GetTriangle(ref indices, ref terrain.worldTransform, out localTriangleShape.vA, out localTriangleShape.vB, out localTriangleShape.vC); localTriangleShape.collisionMargin = 0; //Calibrate the sidedness of the triangle such that it's always facing up. //TODO: There's quite a bit of redundancy in here with other systems. Vector3 AB, AC, normal; Vector3.Subtract(ref localTriangleShape.vB, ref localTriangleShape.vA, out AB); Vector3.Subtract(ref localTriangleShape.vC, ref localTriangleShape.vA, out AC); Vector3.Cross(ref AB, ref AC, out normal); Vector3 terrainUp = new Vector3(terrain.worldTransform.LinearTransform.M21, terrain.worldTransform.LinearTransform.M22, terrain.worldTransform.LinearTransform.M23); float dot; Vector3.Dot(ref terrainUp, ref normal, out dot); if (dot > 0) { localTriangleShape.sidedness = TriangleSidedness.Clockwise; } else { localTriangleShape.sidedness = TriangleSidedness.Counterclockwise; } //Unlike other 'instanced' geometries, terrains are almost always axis aligned in some way and/or have low triangle density relative to what they are colliding with. //Instead of performing additional tests, just assume that it's a fairly regular situation. return true; } protected internal override void CleanUpOverlappingTriangles() { overlappedTriangles.Clear(); } protected override void ProcessCandidates(RawValueList candidates) { //If the candidates list is empty, then let's see if the convex is in the 'thickness' of the terrain. if (candidates.Count == 0 & terrain.thickness > 0) { RayHit rayHit; Ray ray = new Ray { Position = convex.worldTransform.Position, Direction = terrain.worldTransform.LinearTransform.Up }; ray.Direction.Normalize(); //The raycast has to use doublesidedness, since we're casting from the bottom up. if (terrain.Shape.RayCast(ref ray, terrain.thickness, ref terrain.worldTransform, TriangleSidedness.DoubleSided, out rayHit)) { //Found a hit! rayHit.Normal.Normalize(); float dot; Vector3.Dot(ref ray.Direction, ref rayHit.Normal, out dot); var newContact = new ContactData { Normal = rayHit.Normal, Position = convex.worldTransform.Position, Id = 2, PenetrationDepth = -rayHit.T * dot + convex.Shape.minimumRadius }; newContact.Validate(); bool found = false; for (int i = 0; i < contacts.Count; i++) { if (contacts.Elements[i].Id == 2) { //As set above, an id of 2 corresponds to a contact created from this raycast process. contacts.Elements[i].Normal = newContact.Normal; contacts.Elements[i].Position = newContact.Position; contacts.Elements[i].PenetrationDepth = newContact.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = newContact.PenetrationDepth; supplementData.Elements[i].LocalOffsetA = new Vector3(); supplementData.Elements[i].LocalOffsetB = ray.Position; //convex local position in mesh. found = true; break; } } if (!found) candidates.Add(ref newContact); } } } protected override bool UseImprovedBoundaryHandling { get { return terrain.improveBoundaryBehavior; } } /// /// Cleans up the manifold. /// public override void CleanUp() { terrain = null; convex = null; base.CleanUp(); } /// /// Initializes the manifold. /// ///First collidable. ///Second collidable. public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { convex = newCollidableA as ConvexCollidable; terrain = newCollidableB as Terrain; if (convex == null || terrain == null) { convex = newCollidableB as ConvexCollidable; terrain = newCollidableA as Terrain; if (convex == null || terrain == null) throw new ArgumentException("Inappropriate types used to initialize contact manifold."); } } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/TerrainConvexContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { public class TerrainConvexContactManifold : TerrainContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override TrianglePairTester GetTester() { return testerPool.Take(); } protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleConvexPairTester)tester); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/TerrainSphereContactManifold.cs ================================================ using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; namespace BEPUphysics.CollisionTests.Manifolds { public class TerrainSphereContactManifold : TerrainContactManifold { UnsafeResourcePool testerPool = new UnsafeResourcePool(); protected override TrianglePairTester GetTester() { return testerPool.Take(); } protected override void GiveBackTester(TrianglePairTester tester) { testerPool.GiveBack((TriangleSpherePairTester)tester); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/TriangleConvexContactManifold.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.Settings; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contacts between a triangle and convex. /// public class TriangleConvexContactManifold : ContactManifold { RawValueList supplementData = new RawValueList(4); TriangleConvexPairTester pairTester; TriangleShape localTriangleShape = new TriangleShape(); /// /// Gets the pair tester used by the manifold. /// public TriangleConvexPairTester PairTester { get { return pairTester; } } protected ConvexCollidable convex; protected ConvexCollidable triangle; /// /// Gets the convex associated with the pair. /// public ConvexCollidable Convex { get { return convex; } } /// /// Gets the triangle associated with the pair. /// public ConvexCollidable Triangle { get { return triangle; } } /// /// Constructs a new manifold. /// public TriangleConvexContactManifold() { contacts = new RawList(4); unusedContacts = new UnsafeResourcePool(4); contactIndicesToRemove = new RawList(4); pairTester = new TriangleConvexPairTester(); } public override void Update(float dt) { //First, refresh all existing contacts. This is an incremental manifold. ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref triangle.worldTransform, contactIndicesToRemove); RemoveQueuedContacts(); //Compute the local triangle vertices. //TODO: this could be quicker and cleaner. localTriangleShape.collisionMargin = triangle.Shape.collisionMargin; localTriangleShape.sidedness = triangle.Shape.sidedness; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref triangle.worldTransform.Orientation, out orientation); Matrix3x3.Transform(ref triangle.Shape.vA, ref orientation, out localTriangleShape.vA); Matrix3x3.Transform(ref triangle.Shape.vB, ref orientation, out localTriangleShape.vB); Matrix3x3.Transform(ref triangle.Shape.vC, ref orientation, out localTriangleShape.vC); Vector3.Add(ref localTriangleShape.vA, ref triangle.worldTransform.Position, out localTriangleShape.vA); Vector3.Add(ref localTriangleShape.vB, ref triangle.worldTransform.Position, out localTriangleShape.vB); Vector3.Add(ref localTriangleShape.vC, ref triangle.worldTransform.Position, out localTriangleShape.vC); Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA); Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB); Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC); Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation); Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA); Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB); Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC); //Now, generate a contact between the two shapes. ContactData contact; TinyStructList contactList; if (pairTester.GenerateContactCandidate(out contactList)) { for (int i = 0; i < contactList.Count; i++) { contactList.Get(i, out contact); //Put the contact into world space. Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position); Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Check if the contact is unique before proceeding. if (IsContactUnique(ref contact)) { //Check if adding the new contact would overflow the manifold. if (contacts.Count == 4) { //Adding that contact would overflow the manifold. Reduce to the best subset. bool addCandidate; ContactReducer.ReduceContacts(contacts, ref contact, contactIndicesToRemove, out addCandidate); RemoveQueuedContacts(); if (addCandidate) Add(ref contact); } else { //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else. Add(ref contact); } } } } else { //Clear out the contacts, it's separated. for (int i = contacts.Count - 1; i >= 0; i--) Remove(i); } } protected override void Add(ref ContactData contactCandidate) { ContactSupplementData supplement; supplement.BasePenetrationDepth = contactCandidate.PenetrationDepth; //The closest point method computes the local space versions before transforming to world... consider cutting out the middle man RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplement.LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref triangle.worldTransform, out supplement.LocalOffsetB); supplementData.Add(ref supplement); base.Add(ref contactCandidate); } protected override void Remove(int contactIndex) { supplementData.RemoveAt(contactIndex); base.Remove(contactIndex); } private bool IsContactUnique(ref ContactData contactCandidate) { contactCandidate.Validate(); float distanceSquared; for (int i = 0; i < contacts.Count; i++) { Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //Update the existing 'redundant' contact with the new information. //This works out because the new contact is the deepest contact according to the previous collision detection iteration. contacts.Elements[i].Normal = contactCandidate.Normal; contacts.Elements[i].Position = contactCandidate.Position; contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth; RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref triangle.worldTransform, out supplementData.Elements[i].LocalOffsetB); return false; } } return true; } public override void Initialize(Collidable newCollidableA, Collidable newCollidableB) { convex = newCollidableA as ConvexCollidable; triangle = newCollidableB as ConvexCollidable; if (convex == null || triangle == null) { convex = newCollidableB as ConvexCollidable; triangle = newCollidableA as ConvexCollidable; if (convex == null || triangle == null) throw new ArgumentException("Inappropriate types used to initialize contact manifold."); } pairTester.Initialize(convex.Shape, localTriangleShape); } public override void CleanUp() { supplementData.Clear(); convex = null; triangle = null; pairTester.CleanUp(); base.CleanUp(); } } } ================================================ FILE: BEPUphysics/CollisionTests/Manifolds/TriangleMeshConvexContactManifold.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.Settings; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; namespace BEPUphysics.CollisionTests.Manifolds { /// /// Manages persistent contact data between a triangle mesh and a convex. /// public abstract class TriangleMeshConvexContactManifold : ContactManifold { protected RawValueList supplementData = new RawValueList(4); Dictionary activePairTesters = new Dictionary(8); RawValueList candidatesToAdd; RawValueList reducedCandidates = new RawValueList(4); protected TriangleShape localTriangleShape = new TriangleShape(); protected abstract TrianglePairTester GetTester(); protected abstract void GiveBackTester(TrianglePairTester tester); HashSet blockedVertexRegions = new HashSet(); HashSet blockedEdgeRegions = new HashSet(); RawValueList edgeContacts = new RawValueList(8); RawValueList vertexContacts = new RawValueList(8); protected ConvexCollidable convex; /// /// Gets the convex collidable associated with this pair. /// public ConvexCollidable ConvexCollidable { get { return convex; } } /// /// Constructs a new contact manifold. /// protected TriangleMeshConvexContactManifold() { contacts = new RawList(4); unusedContacts = new UnsafeResourcePool(4); contactIndicesToRemove = new RawList(4); candidatesToAdd = new RawValueList(8); } protected virtual RigidTransform MeshTransform { get { return RigidTransform.Identity; } } protected abstract bool UseImprovedBoundaryHandling { get; } protected internal abstract int FindOverlappingTriangles(float dt); protected abstract bool ConfigureTriangle(int i, out TriangleIndices indices); protected internal abstract void CleanUpOverlappingTriangles(); /// /// Updates the manifold. /// ///Timestep duration. public override void Update(float dt) { //First, refresh all existing contacts. This is an incremental manifold. var transform = MeshTransform; ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove); RemoveQueuedContacts(); CleanUpOverlappingTriangles(); //Get all the overlapped triangle indices. int triangleCount = FindOverlappingTriangles(dt); Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation); var guaranteedContacts = 0; for (int i = 0; i < triangleCount; i++) { //Initialize the local triangle. TriangleIndices indices; if (ConfigureTriangle(i, out indices)) { //Find a pairtester for the triangle. TrianglePairTester pairTester; if (!activePairTesters.TryGetValue(indices, out pairTester)) { pairTester = GetTester(); pairTester.Initialize(convex.Shape, localTriangleShape); activePairTesters.Add(indices, pairTester); } pairTester.Updated = true; //Put the triangle into the local space of the convex. Vector3.Subtract(ref localTriangleShape.vA, ref convex.worldTransform.Position, out localTriangleShape.vA); Vector3.Subtract(ref localTriangleShape.vB, ref convex.worldTransform.Position, out localTriangleShape.vB); Vector3.Subtract(ref localTriangleShape.vC, ref convex.worldTransform.Position, out localTriangleShape.vC); Matrix3x3.TransformTranspose(ref localTriangleShape.vA, ref orientation, out localTriangleShape.vA); Matrix3x3.TransformTranspose(ref localTriangleShape.vB, ref orientation, out localTriangleShape.vB); Matrix3x3.TransformTranspose(ref localTriangleShape.vC, ref orientation, out localTriangleShape.vC); //Now, generate a contact between the two shapes. ContactData contact; TinyStructList contactList; if (pairTester.GenerateContactCandidate(out contactList)) { for (int j = 0; j < contactList.Count; j++) { contactList.Get(j, out contact); if (UseImprovedBoundaryHandling) { if (AnalyzeCandidate(ref indices, pairTester, ref contact)) { //This is let through if there's a face contact. Face contacts cannot be blocked. guaranteedContacts++; AddLocalContact(ref contact, ref orientation); } } else { AddLocalContact(ref contact, ref orientation); } } } //Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it. //Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique. //For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices. //For A, B, or C, add the index of the vertex to the blockedVertexRegions set. //If the edge/vertex is already present in the set, then DO NOT add the contact. //When adding a contact, add ALL other voronoi regions to the blocked sets. } } if (UseImprovedBoundaryHandling) { //If there were no face contacts that absolutely must be included, we may get into a very rare situation //where absolutely no contacts get created. For example, a sphere falling directly on top of a vertex in a flat terrain. //It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices). //In some cases, the contacts will be ignored instead of corrected (e.g. spheres). //To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires. //Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts. //TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge, //and an edge only restricts the far vertex and other two edges. This introduces an occasional bump though... //It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles. //For this state to continue beyond a brief instant generally requires the object be orientation locked and slender. //However, some characters fit this description, so it can't be ignored! //Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only). //However, an object stuck between multiple triangles is not in a stable state. In the edge case, the object gets shoved to one side //as one contact 'wins' the solver war. That's not enough to escape, unfortunately. //The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability. //So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :) //This isn't a completely free operation, but it's guarded behind pretty rare conditions. //Essentially, we will check to see if there's just edge contacts fighting against each other. //If they are, then we will correct any stuck-contributing normals to the triangle normal. if (vertexContacts.Count == 0 && guaranteedContacts == 0 && edgeContacts.Count > 1) { //There are only edge contacts, check to see if: //all normals are coplanar, and //at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner). bool allNormalsInSamePlane = true; bool atLeastOneNormalAgainst = false; var firstNormal = edgeContacts.Elements[0].ContactData.Normal; edgeContacts.Elements[0].CorrectedNormal.Normalize(); float dot; Vector3.Dot(ref firstNormal, ref edgeContacts.Elements[0].CorrectedNormal, out dot); if (Math.Abs(dot) > .01f) { //Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity. allNormalsInSamePlane = false; } else { //TODO: Note that we're only checking the new edge contacts, not the existing contacts. //It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity //we'll ignore that possibility for now. for (int i = 1; i < edgeContacts.Count; i++) { Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot); if (dot < 0) { atLeastOneNormalAgainst = true; } //Check to see if the normal is outside the plane. Vector3.Dot(ref edgeContacts.Elements[i].ContactData.Normal, ref edgeContacts.Elements[0].CorrectedNormal, out dot); if (Math.Abs(dot) > .01f) { //We are not stuck! allNormalsInSamePlane = false; break; } } } if (allNormalsInSamePlane && atLeastOneNormalAgainst) { //Uh oh! all the normals are parallel... The object is probably in a weird situation. //Let's correct the normals! //Already normalized the first contact above. //We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already. edgeContacts.Elements[0].ContactData.Normal = edgeContacts.Elements[0].CorrectedNormal; edgeContacts.Elements[0].ShouldCorrect = true; for (int i = 1; i < edgeContacts.Count; i++) { //Must normalize the corrected normal before using it. edgeContacts.Elements[i].CorrectedNormal.Normalize(); Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot); if (dot < .01) { //Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness! //If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal. //Make them the same thing :) edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal; edgeContacts.Elements[i].ShouldCorrect = true; //Note that the penetration depth is NOT corrected. The contact's depth no longer represents the true depth. //However, we only need to have some penetration depth to get the object to escape the rut. //Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction. //If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system //would have picked a different depth, as it finds a reasonable approximation of the minimum penetration! //As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it). } } } } for (int i = 0; i < edgeContacts.Count; i++) { //Only correct if it's allowed AND it's blocked. //If it's not blocked, the contact being created is necessary! //The normal generated by the triangle-convex tester is already known not to //violate the triangle sidedness. if (!blockedEdgeRegions.Contains(edgeContacts.Elements[i].Edge)) { //If it's not blocked, use the contact as-is without correcting it. AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation); } else if (edgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0) { //If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that //it will not interfere (and cause a bump or something). float dot; edgeContacts.Elements[i].CorrectedNormal.Normalize(); Vector3.Dot(ref edgeContacts.Elements[i].CorrectedNormal, ref edgeContacts.Elements[i].ContactData.Normal, out dot); edgeContacts.Elements[i].ContactData.Normal = edgeContacts.Elements[i].CorrectedNormal; edgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth. AddLocalContact(ref edgeContacts.Elements[i].ContactData, ref orientation); } //If it's blocked AND it doesn't allow correction, ignore its existence. } for (int i = 0; i < vertexContacts.Count; i++) { if (!blockedVertexRegions.Contains(vertexContacts.Elements[i].Vertex)) { //If it's not blocked, use the contact as-is without correcting it. AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation); } else if (vertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0) { //If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that //it will not interfere (and cause a bump or something). float dot; vertexContacts.Elements[i].CorrectedNormal.Normalize(); Vector3.Dot(ref vertexContacts.Elements[i].CorrectedNormal, ref vertexContacts.Elements[i].ContactData.Normal, out dot); vertexContacts.Elements[i].ContactData.Normal = vertexContacts.Elements[i].CorrectedNormal; vertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth. AddLocalContact(ref vertexContacts.Elements[i].ContactData, ref orientation); } //If it's blocked AND it doesn't allow correction, ignore its existence. } blockedEdgeRegions.Clear(); blockedVertexRegions.Clear(); vertexContacts.Clear(); edgeContacts.Clear(); } //Remove stale pair testers. //This will only remove 8 stale ones per frame, but it doesn't really matter. //VERY rarely will there be more than 8 in a single frame, and they will be immediately taken care of in the subsequent frame. var toRemove = new TinyList(); foreach (KeyValuePair pair in activePairTesters) { if (!pair.Value.Updated) { if (!toRemove.Add(pair.Key)) break; } else pair.Value.Updated = false; } for (int i = toRemove.Count - 1; i >= 0; i--) { var pairTester = activePairTesters[toRemove[i]]; pairTester.CleanUp(); GiveBackTester(pairTester); activePairTesters.Remove(toRemove[i]); } //Some child types will want to do some extra post processing on the manifold. ProcessCandidates(candidatesToAdd); //Check if adding the new contacts would overflow the manifold. if (contacts.Count + candidatesToAdd.Count > 4) { //Adding all the contacts would overflow the manifold. Reduce to the best subset. ContactReducer.ReduceContacts(contacts, candidatesToAdd, contactIndicesToRemove, reducedCandidates); RemoveQueuedContacts(); for (int i = reducedCandidates.Count - 1; i >= 0; i--) { Add(ref reducedCandidates.Elements[i]); reducedCandidates.RemoveAt(i); } } else if (candidatesToAdd.Count > 0) { //Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else. for (int i = 0; i < candidatesToAdd.Count; i++) { Add(ref candidatesToAdd.Elements[i]); } } candidatesToAdd.Clear(); } void AddLocalContact(ref ContactData contact, ref Matrix3x3 orientation) { //Put the contact into world space. Matrix3x3.Transform(ref contact.Position, ref orientation, out contact.Position); Vector3.Add(ref contact.Position, ref convex.worldTransform.Position, out contact.Position); Matrix3x3.Transform(ref contact.Normal, ref orientation, out contact.Normal); //Check to see if the contact is unique before proceeding. if (IsContactUnique(ref contact)) { candidatesToAdd.Add(ref contact); } } protected void GetNormal(ref Vector3 uncorrectedNormal, out Vector3 normal) { //Compute the normal of the triangle in the current convex's local space. //Note its reliance on the local triangle shape. It must be initialized to the correct values before this is called. Vector3 AB, AC; Vector3.Subtract(ref localTriangleShape.vB, ref localTriangleShape.vA, out AB); Vector3.Subtract(ref localTriangleShape.vC, ref localTriangleShape.vA, out AC); //Compute the normal based on the sidedness. switch (localTriangleShape.sidedness) { case TriangleSidedness.DoubleSided: //If it's double sided, then pick the triangle normal which points in the same direction //as the contact normal that's going to be corrected. float dot; Vector3.Cross(ref AB, ref AC, out normal); Vector3.Dot(ref normal, ref uncorrectedNormal, out dot); if (dot < 0) Vector3.Negate(ref normal, out normal); break; case TriangleSidedness.Clockwise: //If it's clockwise, always use ACxAB. Vector3.Cross(ref AC, ref AB, out normal); break; default: //If it's counterclockwise, always use ABxAC. Vector3.Cross(ref AB, ref AC, out normal); break; } //If the normal is degenerate, just use the uncorrected normal. if (normal.LengthSquared() < Toolbox.Epsilon) normal = uncorrectedNormal; } bool AnalyzeCandidate(ref TriangleIndices indices, TrianglePairTester pairTester, ref ContactData contact) { switch (pairTester.GetRegion(ref contact)) { case VoronoiRegion.A: //Add the contact. VertexContact vertexContact; GetNormal(ref contact.Normal, out vertexContact.CorrectedNormal); vertexContact.ContactData = contact; vertexContact.Vertex = indices.A; vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; vertexContacts.Add(ref vertexContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.B); blockedVertexRegions.Add(indices.C); break; case VoronoiRegion.B: //Add the contact. GetNormal(ref contact.Normal, out vertexContact.CorrectedNormal); vertexContact.ContactData = contact; vertexContact.Vertex = indices.B; vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; vertexContacts.Add(ref vertexContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.C); break; case VoronoiRegion.C: //Add the contact. GetNormal(ref contact.Normal, out vertexContact.CorrectedNormal); vertexContact.ContactData = contact; vertexContact.Vertex = indices.C; vertexContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; vertexContacts.Add(ref vertexContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.B); break; case VoronoiRegion.AB: //Add the contact. EdgeContact edgeContact; GetNormal(ref contact.Normal, out edgeContact.CorrectedNormal); edgeContact.Edge = new Edge(indices.A, indices.B); edgeContact.ContactData = contact; edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; edgeContacts.Add(ref edgeContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.B); blockedVertexRegions.Add(indices.C); break; case VoronoiRegion.AC: //Add the contact. GetNormal(ref contact.Normal, out edgeContact.CorrectedNormal); edgeContact.Edge = new Edge(indices.A, indices.C); edgeContact.ContactData = contact; edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; edgeContacts.Add(ref edgeContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.B); blockedVertexRegions.Add(indices.C); break; case VoronoiRegion.BC: //Add the contact. GetNormal(ref contact.Normal, out edgeContact.CorrectedNormal); edgeContact.Edge = new Edge(indices.B, indices.C); edgeContact.ContactData = contact; edgeContact.ShouldCorrect = pairTester.ShouldCorrectContactNormal; edgeContacts.Add(ref edgeContact); //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.B); blockedVertexRegions.Add(indices.C); break; default: //Block all of the other voronoi regions. blockedEdgeRegions.Add(new Edge(indices.A, indices.B)); blockedEdgeRegions.Add(new Edge(indices.B, indices.C)); blockedEdgeRegions.Add(new Edge(indices.A, indices.C)); blockedVertexRegions.Add(indices.A); blockedVertexRegions.Add(indices.B); blockedVertexRegions.Add(indices.C); //Should add the contact. return true; } return false; } protected override void Add(ref ContactData contactCandidate) { ContactSupplementData supplement; supplement.BasePenetrationDepth = contactCandidate.PenetrationDepth; //The closest point method computes the local space versions before transforming to world... consider cutting out the middle man RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplement.LocalOffsetA); RigidTransform transform = MeshTransform; RigidTransform.TransformByInverse(ref contactCandidate.Position, ref transform, out supplement.LocalOffsetB); supplementData.Add(ref supplement); base.Add(ref contactCandidate); } protected override void Remove(int contactIndex) { supplementData.RemoveAt(contactIndex); base.Remove(contactIndex); } private bool IsContactUnique(ref ContactData contactCandidate) { contactCandidate.Validate(); float distanceSquared; RigidTransform meshTransform = MeshTransform; for (int i = 0; i < contacts.Count; i++) { Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that //a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent. Vector3.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared); if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum) { //Update the existing 'redundant' contact with the new information. //This works out because the new contact is the deepest contact according to the previous collision detection iteration. contacts.Elements[i].Normal = contactCandidate.Normal; contacts.Elements[i].Position = contactCandidate.Position; contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth; supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth; RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA); RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB); return false; } } } for (int i = 0; i < candidatesToAdd.Count; i++) { Vector3.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared); if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) { //This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that //a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent. Vector3.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared); if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum) return false; } } //for (int i = 0; i < edgeContacts.count; i++) //{ // Vector3.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared); // if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) // { // return false; // } //} //for (int i = 0; i < vertexContacts.count; i++) //{ // Vector3.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared); // if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared) // { // return false; // } //} return true; } protected virtual void ProcessCandidates(RawValueList candidates) { } /// /// Cleans up the manifold. /// public override void CleanUp() { supplementData.Clear(); convex = null; foreach (KeyValuePair pair in activePairTesters) { pair.Value.CleanUp(); GiveBackTester(pair.Value); } activePairTesters.Clear(); CleanUpOverlappingTriangles(); base.CleanUp(); } /// /// Edge of a triangle in a mesh in terms of vertex indices. /// public struct Edge : IEquatable { private uint A; private uint B; public Edge(uint a, uint b) { A = a; B = b; } public override int GetHashCode() { return (int)A + (int)B; } public bool Equals(Edge edge) { return (edge.A == A && edge.B == B) || (edge.A == B && edge.B == A); } } /// /// Stores indices of a triangle. /// public struct TriangleIndices : IEquatable { /// /// First index in the triangle. /// public uint A; /// /// Second index in the triangle. /// public uint B; /// /// Third index in the triangle. /// public uint C; /// /// Returns the hash code for this instance. /// /// /// A 32-bit signed integer that is the hash code for this instance. /// /// 2 public override int GetHashCode() { return (int)A + (int)B + (int)C; } /// /// Indicates whether the current object is equal to another object of the same type. /// /// /// true if the current object is equal to the parameter; otherwise, false. /// /// An object to compare with this object. public bool Equals(TriangleIndices other) { return A == other.A && B == other.B && C == other.C; } } struct EdgeContact { public bool ShouldCorrect; public Vector3 CorrectedNormal; public Edge Edge; public ContactData ContactData; } struct VertexContact { public bool ShouldCorrect; public Vector3 CorrectedNormal; public uint Vertex; public ContactData ContactData; } } } ================================================ FILE: BEPUphysics/Constraints/Collision/ContactFrictionConstraint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; using BEPUphysics.Settings; namespace BEPUphysics.Constraints.Collision { /// /// Computes the friction force for a contact when central friction cannot be used. /// public class ContactFrictionConstraint : EntitySolverUpdateable { private ContactManifoldConstraint contactManifoldConstraint; /// /// Gets the manifold constraint associated with this friction constraint. /// public ContactManifoldConstraint ContactManifoldConstraint { get { return contactManifoldConstraint; } } private ContactPenetrationConstraint penetrationConstraint; /// /// Gets the penetration constraint associated with this friction constraint. /// public ContactPenetrationConstraint PenetrationConstraint { get { return penetrationConstraint; } } /// /// Constructs a new friction constraint. /// public ContactFrictionConstraint() { isActive = false; } internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; private float angularAX, angularAY, angularAZ; private float angularBX, angularBY, angularBZ; //Inverse effective mass matrix private float friction; internal float linearAX, linearAY, linearAZ; private Entity entityA, entityB; private bool entityAIsDynamic, entityBIsDynamic; private float velocityToImpulse; /// /// Configures the friction constraint for a new contact. /// ///Manifold to which the constraint belongs. ///Penetration constraint associated with this friction constraint. public void Setup(ContactManifoldConstraint contactManifoldConstraint, ContactPenetrationConstraint penetrationConstraint) { this.contactManifoldConstraint = contactManifoldConstraint; this.penetrationConstraint = penetrationConstraint; IsActive = true; linearAX = 0; linearAY = 0; linearAZ = 0; entityA = contactManifoldConstraint.EntityA; entityB = contactManifoldConstraint.EntityB; } /// /// Cleans upt he friction constraint. /// public void CleanUp() { accumulatedImpulse = 0; contactManifoldConstraint = null; penetrationConstraint = null; entityA = null; entityB = null; IsActive = false; } /// /// Gets the direction in which the friction force acts. /// public Vector3 FrictionDirection { get { return new Vector3(linearAX, linearAY, linearAZ); } } /// /// Gets the total impulse applied by this friction constraint in the last time step. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the relative velocity of the constraint. This is the velocity along the tangent movement direction. /// public float RelativeVelocity { get { float velocity = 0; if (entityA != null) velocity += entityA.linearVelocity.X * linearAX + entityA.linearVelocity.Y * linearAY + entityA.linearVelocity.Z * linearAZ + entityA.angularVelocity.X * angularAX + entityA.angularVelocity.Y * angularAY + entityA.angularVelocity.Z * angularAZ; if (entityB != null) velocity += -entityB.linearVelocity.X * linearAX - entityB.linearVelocity.Y * linearAY - entityB.linearVelocity.Z * linearAZ + entityB.angularVelocity.X * angularBX + entityB.angularVelocity.Y * angularBY + entityB.angularVelocity.Z * angularBZ; return velocity; } } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //Compute relative velocity and convert to impulse float lambda = RelativeVelocity * velocityToImpulse; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; float maxForce = friction * penetrationConstraint.accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForce, maxForce); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (entityAIsDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } return Math.Abs(lambda); } /// /// Initializes the constraint for this frame. /// /// Time since the last frame. public override void Update(float dt) { entityAIsDynamic = entityA != null && entityA.isDynamic; entityBIsDynamic = entityB != null && entityB.isDynamic; //Compute the three dimensional relative velocity at the point. Vector3 velocityA = new Vector3(), velocityB = new Vector3(); Vector3 ra = penetrationConstraint.ra, rb = penetrationConstraint.rb; if (entityA != null) { Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA); Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA); } if (entityB != null) { Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB); Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB); } Vector3 relativeVelocity; Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity); //Get rid of the normal velocity. Vector3 normal = penetrationConstraint.contact.Normal; float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z; relativeVelocity.X -= normalVelocityScalar * normal.X; relativeVelocity.Y -= normalVelocityScalar * normal.Y; relativeVelocity.Z -= normalVelocityScalar * normal.Z; //Create the jacobian entry and decide the friction coefficient. float length = relativeVelocity.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float)Math.Sqrt(length); linearAX = relativeVelocity.X / length; linearAY = relativeVelocity.Y / length; linearAZ = relativeVelocity.Z / length; friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold ? contactManifoldConstraint.materialInteraction.KineticFriction : contactManifoldConstraint.materialInteraction.StaticFriction; } else { //If there's no velocity, there's no jacobian. Give up. //This is 'fast' in that it will early out on essentially resting objects, //but it may introduce instability. //If it doesn't look good, try the next approach. //isActive = false; //return; //if the above doesn't work well, try using the previous frame's jacobian. if (linearAX != 0 || linearAY != 0 || linearAZ != 0) { friction = contactManifoldConstraint.materialInteraction.StaticFriction; } else { //Can't really do anything here, give up. isActiveInSolver = false; return; //Could also cross the up with normal to get a random direction. Questionable value. } } //angular A = Ra x N angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY); angularAY = (ra.Z * linearAX) - (ra.X * linearAZ); angularAZ = (ra.X * linearAY) - (ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y); angularBY = (linearAZ * rb.X) - (linearAX * rb.Z); angularBZ = (linearAX * rb.Y) - (linearAY * rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (entityAIsDynamic) { tX = angularAX * entityA.inertiaTensorInverse.M11 + angularAY * entityA.inertiaTensorInverse.M21 + angularAZ * entityA.inertiaTensorInverse.M31; tY = angularAX * entityA.inertiaTensorInverse.M12 + angularAY * entityA.inertiaTensorInverse.M22 + angularAZ * entityA.inertiaTensorInverse.M32; tZ = angularAX * entityA.inertiaTensorInverse.M13 + angularAY * entityA.inertiaTensorInverse.M23 + angularAZ * entityA.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + entityA.inverseMass; } else entryA = 0; if (entityBIsDynamic) { tX = angularBX * entityB.inertiaTensorInverse.M11 + angularBY * entityB.inertiaTensorInverse.M21 + angularBZ * entityB.inertiaTensorInverse.M31; tY = angularBX * entityB.inertiaTensorInverse.M12 + angularBY * entityB.inertiaTensorInverse.M22 + angularBZ * entityB.inertiaTensorInverse.M32; tZ = angularBX * entityB.inertiaTensorInverse.M13 + angularBY * entityB.inertiaTensorInverse.M23 + angularBZ * entityB.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + entityB.inverseMass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Softness? } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (entityAIsDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //This should never really have to be called. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } } } ================================================ FILE: BEPUphysics/Constraints/Collision/ContactManifoldConstraint.cs ================================================ using BEPUphysics.Constraints.SolverGroups; using BEPUutilities.DataStructures; using BEPUphysics.Entities; using BEPUphysics.CollisionTests; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.Materials; using BEPUphysics.SolverSystems; namespace BEPUphysics.Constraints.Collision { /// /// Superclass of collision constraints that include multiple contact subconstraints. /// public abstract class ContactManifoldConstraint : SolverGroup { internal InteractionProperties materialInteraction; /// /// Gets or sets the material-blended properties used by this constraint. /// public InteractionProperties MaterialInteraction { get { return materialInteraction; } set { materialInteraction = value; } } protected Entity entityA; /// /// Gets the first entity associated with the manifold. /// public Entity EntityA { get { return entityA; } } protected Entity entityB; /// /// Gets the second entity associated with the manifold. /// public Entity EntityB { get { return entityB; } } protected internal CollidablePairHandler pair; /// /// Gets the pair handler owning this constraint. /// public CollidablePairHandler Pair { get { return pair; } } protected internal override void OnInvolvedEntitiesChanged() { //The default implementation of this method is pretty complicated. //This is a special constraint that has certain guarantees that allow a simpler method to be used. //This updates the involved entities and connected members lists, but does not update references. //It does not have to update references because this 'constraint' can never change entities while belonging to a solver. //It doesn't even need to notify the parent solvergroup. CollectInvolvedEntities(); } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //The default implementation for solver groups looks at every single subconstraint. //That's not necessary for these special constraints. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } /// /// Adds a contact to be managed by the constraint. /// ///Contact to add. public abstract void AddContact(Contact contact); /// /// Removes a contact from the constraint. /// ///Contact to remove. public abstract void RemoveContact(Contact contact); /// /// Initializes the constraint. /// ///First entity of the pair. ///Second entity of the pair. ///Pair owning this constraint. public virtual void Initialize(Entity a, Entity b, CollidablePairHandler newPair) { //This should only be called before the constraint has been added to the solver. entityA = a; entityB = b; pair = newPair; OnInvolvedEntitiesChanged(); } /// /// Cleans up the constraint. /// public abstract void CleanUp(); protected internal void CleanUpReferences() { entityA = null; entityB = null; OnInvolvedEntitiesChanged(); } /// /// Called when the updateable is removed from its solver. /// /// Solver from which the updateable was removed. public override void OnRemovalFromSolver(Solver oldSolver) { //This should only be called after the constraint has been removed from the solver. if (pair == null) { CleanUpReferences(); } } /// /// Sets the activity state of the constraint based on the activity state of its connections. /// Called automatically by the space owning a constaint. If a constraint is a sub-constraint that hasn't been directly added to the space, /// this may need to be called alongside the preStep from within the parent constraint. /// public override void UpdateSolverActivity() { if (isActive) { isActiveInSolver = pair.BroadPhaseOverlap.collisionRule < CollisionRule.NoSolver && ((entityA != null && entityA.isDynamic && entityA.activityInformation.IsActive) || //At least one of the objects must be an active dynamic entity. (entityB != null && entityB.isDynamic && entityB.activityInformation.IsActive)); for (int i = 0; i < solverUpdateables.Count; i++) { solverUpdateables.Elements[i].isActiveInSolver = solverUpdateables.Elements[i].isActive && isActiveInSolver; } } else isActiveInSolver = false; } /// /// Updates the material properties associated with the constraint. /// ///Material associated with the first entity of the pair. ///Material associated with the second entity of the pair. public void UpdateMaterialProperties(Material materialA, Material materialB) { if (materialA != null && materialB != null) MaterialManager.GetInteractionProperties(materialA, materialB, out materialInteraction); else if (materialA == null && materialB != null) { materialInteraction.KineticFriction = materialB.kineticFriction; materialInteraction.StaticFriction = materialB.staticFriction; materialInteraction.Bounciness = materialB.bounciness; } else if (materialA != null) { materialInteraction.KineticFriction = materialA.kineticFriction; materialInteraction.StaticFriction = materialA.staticFriction; materialInteraction.Bounciness = materialA.bounciness; } else { materialInteraction.KineticFriction = 0; materialInteraction.StaticFriction = 0; materialInteraction.Bounciness = 0; } } } } ================================================ FILE: BEPUphysics/Constraints/Collision/ContactManifoldConstraintGroup.cs ================================================ using BEPUphysics.Constraints.SolverGroups; using BEPUphysics.Entities; using System; using BEPUutilities.DataStructures; namespace BEPUphysics.Constraints.Collision { /// /// Constraint group containing multiple contact manifold constraints. /// Used by some pairs which manage multiple sub-pairs. /// public class ContactManifoldConstraintGroup : SolverGroup { protected Entity entityA; /// /// Gets the first entity in the pair. /// public Entity EntityA { get { return entityA; } } protected Entity entityB; /// /// Gets the second entity in the pair. /// public Entity EntityB { get { return entityB; } } /// /// Adds a constraint to the group. /// ///Constraint to add. public new void Add(EntitySolverUpdateable manifoldConstraint) { //This is a similar process to a normal solver group. //However, it does not attempt to change involved entities. //This is for two reasons: //-It is unnecessary; a contact manifold is always between the same two entities throughout its lifespan. //-It causes race conditions; this method is called in a multithreaded context and changing involved // entities calls upon sequential-only methods. if (manifoldConstraint.solver == null) { if (manifoldConstraint.SolverGroup == null) { solverUpdateables.Add(manifoldConstraint); manifoldConstraint.SolverGroup = this; manifoldConstraint.Solver = solver; } else { throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup."); } } else { throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver."); } } /// /// Removes a constraint from the group. /// ///Constraint to remove. public new void Remove(EntitySolverUpdateable manifoldConstraint) { //This is a similar process to a normal solver group. //However, it does not attempt to change involved entities. //This is for two reasons: //-It is unnecessary; a contact manifold is always between the same two entities throughout its lifespan. //-It causes race conditions; this method is called in a multithreaded context and changing involved // entities calls upon sequential-only methods. if (manifoldConstraint.SolverGroup == this) { solverUpdateables.Remove(manifoldConstraint); manifoldConstraint.SolverGroup = null; manifoldConstraint.Solver = null; } else { throw new InvalidOperationException("Cannot remove SolverUpdateable from SolverGroup; it doesn't belong to this SolverGroup."); } } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //The default implementation for solver groups looks at every single subconstraint. //That's not necessary for these special constraints. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } protected internal override void OnInvolvedEntitiesChanged() { //The default implementation of this method is pretty complicated. //This is a special constraint that has certain guarantees that allow a simpler method to be used. //This updates the involved entities and connected members lists, but does not update references. //It does not have to update references because this 'constraint' can never change entities while belonging to a solver. //It doesn't even need to notify the parent solvergroup. CollectInvolvedEntities(); } /// /// Initializes the constraint group. /// ///First entity of the pair. ///Second entity of the pair. public virtual void Initialize(Entity a, Entity b) { //This should only be called before the constraint has been added to the solver. entityA = a; entityB = b; OnInvolvedEntitiesChanged(); } /// /// Cleans up the constraint group. /// public virtual void CleanUp() { //This should only be called after the constraint has been removed from the solver. entityA = null; entityB = null; OnInvolvedEntitiesChanged(); } } } ================================================ FILE: BEPUphysics/Constraints/Collision/ContactPenetrationConstraint.cs ================================================ using BEPUphysics.Entities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; using BEPUphysics.CollisionTests; using BEPUphysics.Settings; using BEPUutilities; using System; namespace BEPUphysics.Constraints.Collision { /// /// Computes the forces necessary to keep two entities from going through each other at a contact point. /// public class ContactPenetrationConstraint : EntitySolverUpdateable { internal Contact contact; /// /// Gets the contact associated with this penetration constraint. /// public Contact Contact { get { return contact; } } internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; internal float angularAX, angularAY, angularAZ; internal float angularBX, angularBY, angularBZ; private float softness; private float bias; private float linearAX, linearAY, linearAZ; private Entity entityA, entityB; private bool entityADynamic, entityBDynamic; //Inverse effective mass matrix internal float velocityToImpulse; private ContactManifoldConstraint contactManifoldConstraint; internal Vector3 ra, rb; /// /// Constructs a new penetration constraint. /// public ContactPenetrationConstraint() { isActive = false; } /// /// Configures the penetration constraint. /// ///Owning manifold constraint. ///Contact associated with the penetration constraint. public void Setup(ContactManifoldConstraint contactManifoldConstraint, Contact contact) { this.contactManifoldConstraint = contactManifoldConstraint; this.contact = contact; isActive = true; entityA = contactManifoldConstraint.EntityA; entityB = contactManifoldConstraint.EntityB; } /// /// Cleans up the constraint. /// public void CleanUp() { accumulatedImpulse = 0; contactManifoldConstraint = null; contact = null; entityA = null; entityB = null; isActive = false; } /// /// Gets the total normal impulse applied by this penetration constraint to maintain the separation of the involved entities. /// public float NormalImpulse { get { return accumulatedImpulse; } } /// /// Gets the relative velocity between the associated entities at the contact point along the contact normal. /// public float RelativeVelocity { get { float lambda = 0; if (entityA != null) { lambda = entityA.linearVelocity.X * linearAX + entityA.linearVelocity.Y * linearAY + entityA.linearVelocity.Z * linearAZ + entityA.angularVelocity.X * angularAX + entityA.angularVelocity.Y * angularAY + entityA.angularVelocity.Z * angularAZ; } if (entityB != null) { lambda += -entityB.linearVelocity.X * linearAX - entityB.linearVelocity.Y * linearAY - entityB.linearVelocity.Z * linearAZ + entityB.angularVelocity.X * angularBX + entityB.angularVelocity.Y * angularBY + entityB.angularVelocity.Z * angularBZ; } return lambda; } } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { entityADynamic = entityA != null && entityA.isDynamic; entityBDynamic = entityB != null && entityB.isDynamic; //Set up the jacobians. linearAX = -contact.Normal.X; linearAY = -contact.Normal.Y; linearAZ = -contact.Normal.Z; //linearBX = -linearAX; //linearBY = -linearAY; //linearBZ = -linearAZ; //angular A = Ra x N if (entityA != null) { Vector3.Subtract(ref contact.Position, ref entityA.position, out ra); angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY); angularAY = (ra.Z * linearAX) - (ra.X * linearAZ); angularAZ = (ra.X * linearAY) - (ra.Y * linearAX); } //Angular B = N x Rb if (entityB != null) { Vector3.Subtract(ref contact.Position, ref entityB.position, out rb); angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y); angularBY = (linearAZ * rb.X) - (linearAX * rb.Z); angularBZ = (linearAX * rb.Y) - (linearAY * rb.X); } //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (entityADynamic) { tX = angularAX * entityA.inertiaTensorInverse.M11 + angularAY * entityA.inertiaTensorInverse.M21 + angularAZ * entityA.inertiaTensorInverse.M31; tY = angularAX * entityA.inertiaTensorInverse.M12 + angularAY * entityA.inertiaTensorInverse.M22 + angularAZ * entityA.inertiaTensorInverse.M32; tZ = angularAX * entityA.inertiaTensorInverse.M13 + angularAY * entityA.inertiaTensorInverse.M23 + angularAZ * entityA.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + entityA.inverseMass; } else entryA = 0; if (entityBDynamic) { tX = angularBX * entityB.inertiaTensorInverse.M11 + angularBY * entityB.inertiaTensorInverse.M21 + angularBZ * entityB.inertiaTensorInverse.M31; tY = angularBX * entityB.inertiaTensorInverse.M12 + angularBY * entityB.inertiaTensorInverse.M22 + angularBZ * entityB.inertiaTensorInverse.M32; tZ = angularBX * entityB.inertiaTensorInverse.M13 + angularBY * entityB.inertiaTensorInverse.M23 + angularBZ * entityB.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + entityB.inverseMass; } else entryB = 0; //If we used a single fixed softness value, then heavier objects will tend to 'squish' more than light objects. //In the extreme case, very heavy objects could simply fall through the ground by force of gravity. //To see why this is the case, consider that a given dt, softness, and bias factor correspond to an equivalent spring's damping and stiffness coefficients. //Imagine trying to hang objects of different masses on the fixed-strength spring: obviously, heavier ones will pull it further down. //To counteract this, scale the softness value based on the effective mass felt by the constraint. //Larger effective masses should correspond to smaller softnesses so that the spring has the same positional behavior. //Fortunately, we're already computing the necessary values: the raw, unsoftened effective mass inverse shall be used to compute the softness. float effectiveMassInverse = entryA + entryB; softness = CollisionResponseSettings.Softness * effectiveMassInverse; velocityToImpulse = -1 / (softness + effectiveMassInverse); //Bounciness and bias (penetration correction) if (contact.PenetrationDepth >= 0) { bias = MathHelper.Min( MathHelper.Max(0, contact.PenetrationDepth - CollisionDetectionSettings.AllowedPenetration) * CollisionResponseSettings.PenetrationRecoveryStiffness / dt, CollisionResponseSettings.MaximumPenetrationCorrectionSpeed); if (contactManifoldConstraint.materialInteraction.Bounciness > 0) { //Target a velocity which includes a portion of the incident velocity. float relativeVelocity = -RelativeVelocity; if (relativeVelocity > CollisionResponseSettings.BouncinessVelocityThreshold) bias = MathHelper.Max(relativeVelocity * contactManifoldConstraint.materialInteraction.Bounciness, bias); } } else { //The contact is actually separated right now. Allow the solver to target a position that is just barely in collision. //If the solver finds that an accumulated negative impulse is required to hit this target, then no work will be done. bias = contact.PenetrationDepth / dt; //This implementation is going to ignore bounciness for now. //Since it's not being used for CCD, these negative-depth contacts //only really occur in situations where no bounce should occur. //if (contactManifoldConstraint.materialInteraction.Bounciness > 0) //{ // //Target a velocity which includes a portion of the incident velocity. // //The contact isn't colliding currently, but go ahead and target the post-bounce velocity. // //The bias is added to the bounce velocity to simulate the object continuing to the surface and then bouncing off. // float relativeVelocity = -RelativeVelocity; // if (relativeVelocity > CollisionResponseSettings.BouncinessVelocityThreshold) // bias = relativeVelocity * contactManifoldConstraint.materialInteraction.Bounciness + bias; //} } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (entityADynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } } /// /// Computes and applies an impulse to keep the colliders from penetrating. /// /// Impulse applied. public override float SolveIteration() { //Compute relative velocity float lambda = (RelativeVelocity - bias + softness * accumulatedImpulse) * velocityToImpulse; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(0, accumulatedImpulse + lambda); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (entityADynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } return Math.Abs(lambda); } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //This should never really have to be called. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } } } ================================================ FILE: BEPUphysics/Constraints/Collision/ConvexContactManifoldConstraint.cs ================================================ using BEPUphysics.CollisionTests; using BEPUutilities.DataStructures; using System.Collections.Generic; namespace BEPUphysics.Constraints.Collision { /// /// Contact manifold constraint that is used by manifolds whose normals are assumed to be /// essentially the same. This assumption can only be maintained between two convex objects. /// public class ConvexContactManifoldConstraint : ContactManifoldConstraint { //This contact manifold constraint covers a single, 4-contact pair. //The solver group is composed of multiple constraints. //One pentration constraint for each contact. //One sliding constraint. //One twist constraint. internal TwistFrictionConstraint twistFriction; /// /// Gets the twist friction constraint used by the manifold. /// public TwistFrictionConstraint TwistFriction { get { return twistFriction; } } internal SlidingFrictionTwoAxis slidingFriction; /// /// Gets the sliding friction constraint used by the manifold. /// public SlidingFrictionTwoAxis SlidingFriction { get { return slidingFriction; } } internal RawList penetrationConstraints; /// /// Gets the penetration constraints used by the manifold. /// public ReadOnlyList ContactPenetrationConstraints { get { return new ReadOnlyList(penetrationConstraints); } } Stack penetrationConstraintPool = new Stack(4); /// /// Constructs a new convex contact manifold constraint. /// public ConvexContactManifoldConstraint() { //All of the constraints are always in the solver group. Some of them are just deactivated sometimes. //This reduces some bookkeeping complications. penetrationConstraints = new RawList(4); //Order matters in this adding process. Sliding friction computes some information used by the twist friction, and both use penetration impulses. for (int i = 0; i < 4; i++) { var penetrationConstraint = new ContactPenetrationConstraint(); Add(penetrationConstraint); penetrationConstraintPool.Push(penetrationConstraint); } slidingFriction = new SlidingFrictionTwoAxis(); Add(slidingFriction); twistFriction = new TwistFrictionConstraint(); Add(twistFriction); } /// /// Cleans up the constraint. /// public override void CleanUp() { //Deactivate any remaining constraints. for (int i = penetrationConstraints.Count - 1; i >= 0; i--) { var penetrationConstraint = penetrationConstraints.Elements[i]; penetrationConstraint.CleanUp(); penetrationConstraints.RemoveAt(i); penetrationConstraintPool.Push(penetrationConstraint); } if (twistFriction.isActive) { twistFriction.CleanUp(); slidingFriction.CleanUp(); } } /// /// Adds a contact to be managed by the constraint. /// ///Contact to add. public override void AddContact(Contact contact) { contact.Validate(); var penetrationConstraint = penetrationConstraintPool.Pop(); penetrationConstraint.Setup(this, contact); penetrationConstraints.Add(penetrationConstraint); if (!twistFriction.isActive) { //This is the first real contact. All constraints need to become active. twistFriction.Setup(this); slidingFriction.Setup(this); } } /// /// Removes a contact from the constraint. /// ///Contact to remove. public override void RemoveContact(Contact contact) { for (int i = 0; i < penetrationConstraints.Count; i++) { ContactPenetrationConstraint penetrationConstraint; if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact) { penetrationConstraint.CleanUp(); penetrationConstraints.RemoveAt(i); penetrationConstraintPool.Push(penetrationConstraint); break; } } if (penetrationConstraints.Count == 0) { //No more contacts. Disable everything. //Don't have to worry about speculative contacts here; if there existed a regular manifold contact, there couldn't now exist a speculative contact. twistFriction.CleanUp(); slidingFriction.CleanUp(); } } //NOTE: Even though the order of addition to the solver group ensures penetration constraints come first, the //order of penetration constraints themselves matters in terms of determinism! //Consider what happens when penetration constraints are added and removed. They cycle through a stack, //so the penetration constraints in the solver group's listing have inconsistent ordering. Reloading the simulation //doesn't reset the penetration constraint pools, so suddenly everything is nonrepeatable, even single threaded. //By having the update use the order defined by contact addition/removal, determinism is maintained (so long as contact addition/removal is deterministic!) /// /// Performs the frame's configuration step. /// ///Timestep duration. public sealed override void Update(float dt) { for (int i = 0; i < penetrationConstraints.Count; i++) UpdateUpdateable(penetrationConstraints.Elements[i], dt); UpdateUpdateable(slidingFriction, dt); UpdateUpdateable(twistFriction, dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public sealed override void ExclusiveUpdate() { for (int i = 0; i < penetrationConstraints.Count; i++) ExclusiveUpdateUpdateable(penetrationConstraints.Elements[i]); ExclusiveUpdateUpdateable(slidingFriction); ExclusiveUpdateUpdateable(twistFriction); } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public sealed override float SolveIteration() { int activeConstraints = 0; for (int i = 0; i < penetrationConstraints.Count; i++) SolveUpdateable(penetrationConstraints.Elements[i], ref activeConstraints); SolveUpdateable(slidingFriction, ref activeConstraints); SolveUpdateable(twistFriction, ref activeConstraints); isActiveInSolver = activeConstraints > 0; return solverSettings.minimumImpulse + 1; //Never let the system deactivate due to low impulses; solver group takes care of itself. } } } ================================================ FILE: BEPUphysics/Constraints/Collision/NonConvexContactManifoldConstraint.cs ================================================ using BEPUphysics.CollisionTests; using BEPUutilities.DataStructures; using System.Collections.Generic; namespace BEPUphysics.Constraints.Collision { /// /// Collision constraint for non-convex manifolds. These manifolds are usually used in cases /// where the contacts are coming from multiple objects or from non-convex objects. The normals /// will likely face more than one direction. /// public class NonConvexContactManifoldConstraint : ContactManifoldConstraint { //Unlike the convex manifold constraint, this constraint enforces no requirements //on the contact data. The collisions can form a nonconvex patch. They can have differing normals. //This is required for proper collision handling on large structures //The solver group is composed of multiple constraints. //One pentration constraint for each contact. //One friction constraint for each contact. internal RawList penetrationConstraints; /// /// Gets the penetration constraints in the manifold. /// public ReadOnlyList ContactPenetrationConstraints { get { return new ReadOnlyList(penetrationConstraints); } } Stack penetrationConstraintPool = new Stack(4); internal RawList frictionConstraints; /// /// Gets the friction constraints in the manifold. /// public ReadOnlyList ContactFrictionConstraints { get { return new ReadOnlyList(frictionConstraints); } } Stack frictionConstraintPool = new Stack(4); /// /// Constructs a new nonconvex manifold constraint. /// public NonConvexContactManifoldConstraint() { //All of the constraints are always in the solver group. Some of them are just deactivated sometimes. //This reduces some bookkeeping complications. penetrationConstraints = new RawList(4); frictionConstraints = new RawList(4); for (int i = 0; i < 4; i++) { var penetrationConstraint = new ContactPenetrationConstraint(); penetrationConstraintPool.Push(penetrationConstraint); Add(penetrationConstraint); var frictionConstraint = new ContactFrictionConstraint(); frictionConstraintPool.Push(frictionConstraint); Add(frictionConstraint); } } /// /// Cleans up the constraint. /// public override void CleanUp() { //Deactivate any remaining constraints. for (int i = penetrationConstraints.Count - 1; i >= 0; i--) { var penetrationConstraint = penetrationConstraints.Elements[i]; penetrationConstraint.CleanUp(); penetrationConstraints.RemoveAt(i); penetrationConstraintPool.Push(penetrationConstraint); } for (int i = frictionConstraints.Count - 1; i >= 0; i--) { var frictionConstraint = frictionConstraints.Elements[i]; frictionConstraint.CleanUp(); frictionConstraints.RemoveAt(i); frictionConstraintPool.Push(frictionConstraint); } } //TODO: PROBLEM IS that the add contact/remove contact, when they go from 0 -> !0 or !0 -> 0, the whole constraint is added/removed from the solver. //The Added/Removed contact methods here will run ambiguously before or after they are removed from the solver. //That ambiguous order doesn't really matter though, since everything that these add/remove methods do is local to this solver object and its children. //It doesn't go out and modify any external values on referenced entities. That only happens when it's added or removed from the solver by whatever owns this object! //To avoid ANY ambiguity, some third party is now responsible for adding and removing contacts from this. /// /// Adds a contact to be managed by the constraint. /// ///Contact to add. public override void AddContact(Contact contact) { contact.Validate(); var penetrationConstraint = penetrationConstraintPool.Pop(); penetrationConstraint.Setup(this, contact); penetrationConstraints.Add(penetrationConstraint); var frictionConstraint = frictionConstraintPool.Pop(); frictionConstraint.Setup(this, penetrationConstraint); frictionConstraints.Add(frictionConstraint); } /// /// Removes a contact from the constraint. /// ///Contact to remove. public override void RemoveContact(Contact contact) { ContactPenetrationConstraint penetrationConstraint = null; for (int i = 0; i < penetrationConstraints.Count; i++) { if ((penetrationConstraint = penetrationConstraints.Elements[i]).contact == contact) { penetrationConstraint.CleanUp(); penetrationConstraints.RemoveAt(i); penetrationConstraintPool.Push(penetrationConstraint); break; } } for (int i = frictionConstraints.Count - 1; i >= 0; i--) { ContactFrictionConstraint frictionConstraint = frictionConstraints[i]; if (frictionConstraint.PenetrationConstraint == penetrationConstraint) { frictionConstraint.CleanUp(); frictionConstraints.RemoveAt(i); frictionConstraintPool.Push(frictionConstraint); break; } } } //NOTE: Even though the order of addition to the solver group ensures penetration constraints come first, the //order of penetration constraints themselves matters in terms of determinism! //Consider what happens when penetration constraints are added and removed. They cycle through a stack, //so the penetration constraints in the solver group's listing have inconsistent ordering. Reloading the simulation //doesn't reset the penetration constraint pools, so suddenly everything is nonrepeatable, even single threaded. //By having the update use the order defined by contact addition/removal, determinism is maintained (so long as contact addition/removal is deterministic!) /// /// Performs the frame's configuration step. /// ///Timestep duration. public sealed override void Update(float dt) { for (int i = 0; i < penetrationConstraints.Count; i++) UpdateUpdateable(penetrationConstraints.Elements[i], dt); for (int i = 0; i < frictionConstraints.Count; i++) UpdateUpdateable(frictionConstraints.Elements[i], dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public sealed override void ExclusiveUpdate() { for (int i = 0; i < penetrationConstraints.Count; i++) ExclusiveUpdateUpdateable(penetrationConstraints.Elements[i]); for (int i = 0; i < frictionConstraints.Count; i++) ExclusiveUpdateUpdateable(frictionConstraints.Elements[i]); } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public sealed override float SolveIteration() { int activeConstraints = 0; for (int i = 0; i < penetrationConstraints.Count; i++) SolveUpdateable(penetrationConstraints.Elements[i], ref activeConstraints); for (int i = 0; i < frictionConstraints.Count; i++) SolveUpdateable(frictionConstraints.Elements[i], ref activeConstraints); isActiveInSolver = activeConstraints > 0; return solverSettings.minimumImpulse + 1; //Never let the system deactivate due to low impulses; solver group takes care of itself. } } } ================================================ FILE: BEPUphysics/Constraints/Collision/SlidingFrictionTwoAxis.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; using BEPUphysics.Settings; namespace BEPUphysics.Constraints.Collision { /// /// Computes the forces to slow down and stop sliding motion between two entities when centralized friction is active. /// public class SlidingFrictionTwoAxis : EntitySolverUpdateable { private ConvexContactManifoldConstraint contactManifoldConstraint; /// /// Gets the contact manifold constraint that owns this constraint. /// public ConvexContactManifoldConstraint ContactManifoldConstraint { get { return contactManifoldConstraint; } } internal Vector2 accumulatedImpulse; internal Matrix2x3 angularA, angularB; private int contactCount; private float friction; internal Matrix2x3 linearA; private Entity entityA, entityB; private bool entityADynamic, entityBDynamic; private Vector3 ra, rb; private Matrix2x2 velocityToImpulse; /// /// Gets the first direction in which the friction force acts. /// This is one of two directions that are perpendicular to each other and the normal of a collision between two entities. /// public Vector3 FrictionDirectionX { get { return new Vector3(linearA.M11, linearA.M12, linearA.M13); } } /// /// Gets the second direction in which the friction force acts. /// This is one of two directions that are perpendicular to each other and the normal of a collision between two entities. /// public Vector3 FrictionDirectionY { get { return new Vector3(linearA.M21, linearA.M22, linearA.M23); } } /// /// Gets the total impulse applied by sliding friction in the last time step. /// The X component of this vector is the force applied along the frictionDirectionX, /// while the Y component is the force applied along the frictionDirectionY. /// public Vector2 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the tangential relative velocity between the associated entities at the contact point. /// public Vector2 RelativeVelocity { get { //Compute relative velocity //Explicit version: //Vector2 dot; //Matrix2x3.Transform(ref parentA.myInternalLinearVelocity, ref linearA, out lambda); //Matrix2x3.Transform(ref parentB.myInternalLinearVelocity, ref linearA, out dot); //lambda.X -= dot.X; lambda.Y -= dot.Y; //Matrix2x3.Transform(ref parentA.myInternalAngularVelocity, ref angularA, out dot); //lambda.X += dot.X; lambda.Y += dot.Y; //Matrix2x3.Transform(ref parentB.myInternalAngularVelocity, ref angularB, out dot); //lambda.X += dot.X; lambda.Y += dot.Y; //Inline version: //lambda.X = linearA.M11 * parentA.myInternalLinearVelocity.X + linearA.M12 * parentA.myInternalLinearVelocity.Y + linearA.M13 * parentA.myInternalLinearVelocity.Z - // linearA.M11 * parentB.myInternalLinearVelocity.X - linearA.M12 * parentB.myInternalLinearVelocity.Y - linearA.M13 * parentB.myInternalLinearVelocity.Z + // angularA.M11 * parentA.myInternalAngularVelocity.X + angularA.M12 * parentA.myInternalAngularVelocity.Y + angularA.M13 * parentA.myInternalAngularVelocity.Z + // angularB.M11 * parentB.myInternalAngularVelocity.X + angularB.M12 * parentB.myInternalAngularVelocity.Y + angularB.M13 * parentB.myInternalAngularVelocity.Z; //lambda.Y = linearA.M21 * parentA.myInternalLinearVelocity.X + linearA.M22 * parentA.myInternalLinearVelocity.Y + linearA.M23 * parentA.myInternalLinearVelocity.Z - // linearA.M21 * parentB.myInternalLinearVelocity.X - linearA.M22 * parentB.myInternalLinearVelocity.Y - linearA.M23 * parentB.myInternalLinearVelocity.Z + // angularA.M21 * parentA.myInternalAngularVelocity.X + angularA.M22 * parentA.myInternalAngularVelocity.Y + angularA.M23 * parentA.myInternalAngularVelocity.Z + // angularB.M21 * parentB.myInternalAngularVelocity.X + angularB.M22 * parentB.myInternalAngularVelocity.Y + angularB.M23 * parentB.myInternalAngularVelocity.Z; //Re-using information version: //TODO: va + wa x ra - vb - wb x rb, dotted against each axis, is it faster? float dvx = 0, dvy = 0, dvz = 0; if (entityA != null) { dvx = entityA.linearVelocity.X + (entityA.angularVelocity.Y * ra.Z) - (entityA.angularVelocity.Z * ra.Y); dvy = entityA.linearVelocity.Y + (entityA.angularVelocity.Z * ra.X) - (entityA.angularVelocity.X * ra.Z); dvz = entityA.linearVelocity.Z + (entityA.angularVelocity.X * ra.Y) - (entityA.angularVelocity.Y * ra.X); } if (entityB != null) { dvx += -entityB.linearVelocity.X - (entityB.angularVelocity.Y * rb.Z) + (entityB.angularVelocity.Z * rb.Y); dvy += -entityB.linearVelocity.Y - (entityB.angularVelocity.Z * rb.X) + (entityB.angularVelocity.X * rb.Z); dvz += -entityB.linearVelocity.Z - (entityB.angularVelocity.X * rb.Y) + (entityB.angularVelocity.Y * rb.X); } //float dvx = entityA.linearVelocity.X + (entityA.angularVelocity.Y * ra.Z) - (entityA.angularVelocity.Z * ra.Y) // - entityB.linearVelocity.X - (entityB.angularVelocity.Y * rb.Z) + (entityB.angularVelocity.Z * rb.Y); //float dvy = entityA.linearVelocity.Y + (entityA.angularVelocity.Z * ra.X) - (entityA.angularVelocity.X * ra.Z) // - entityB.linearVelocity.Y - (entityB.angularVelocity.Z * rb.X) + (entityB.angularVelocity.X * rb.Z); //float dvz = entityA.linearVelocity.Z + (entityA.angularVelocity.X * ra.Y) - (entityA.angularVelocity.Y * ra.X) // - entityB.linearVelocity.Z - (entityB.angularVelocity.X * rb.Y) + (entityB.angularVelocity.Y * rb.X); #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif lambda.X = dvx * linearA.M11 + dvy * linearA.M12 + dvz * linearA.M13; lambda.Y = dvx * linearA.M21 + dvy * linearA.M22 + dvz * linearA.M23; return lambda; //Using XNA Cross product instead of inline //Vector3 wara, wbrb; //Vector3.Cross(ref parentA.myInternalAngularVelocity, ref Ra, out wara); //Vector3.Cross(ref parentB.myInternalAngularVelocity, ref Rb, out wbrb); //float dvx, dvy, dvz; //dvx = wara.X + parentA.myInternalLinearVelocity.X - wbrb.X - parentB.myInternalLinearVelocity.X; //dvy = wara.Y + parentA.myInternalLinearVelocity.Y - wbrb.Y - parentB.myInternalLinearVelocity.Y; //dvz = wara.Z + parentA.myInternalLinearVelocity.Z - wbrb.Z - parentB.myInternalLinearVelocity.Z; //lambda.X = dvx * linearA.M11 + dvy * linearA.M12 + dvz * linearA.M13; //lambda.Y = dvx * linearA.M21 + dvy * linearA.M22 + dvz * linearA.M23; } } /// /// Constructs a new sliding friction constraint. /// public SlidingFrictionTwoAxis() { isActive = false; } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { Vector2 lambda = RelativeVelocity; //Convert to impulse //Matrix2x2.Transform(ref lambda, ref velocityToImpulse, out lambda); float x = lambda.X; lambda.X = x * velocityToImpulse.M11 + lambda.Y * velocityToImpulse.M21; lambda.Y = x * velocityToImpulse.M12 + lambda.Y * velocityToImpulse.M22; //Accumulate and clamp Vector2 previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse.X += lambda.X; accumulatedImpulse.Y += lambda.Y; float length = accumulatedImpulse.LengthSquared(); float maximumFrictionForce = 0; for (int i = 0; i < contactCount; i++) { maximumFrictionForce += contactManifoldConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; } maximumFrictionForce *= friction; if (length > maximumFrictionForce * maximumFrictionForce) { length = maximumFrictionForce / (float)Math.Sqrt(length); accumulatedImpulse.X *= length; accumulatedImpulse.Y *= length; } lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; //Single Axis clamp //float maximumFrictionForce = 0; //for (int i = 0; i < contactCount; i++) //{ // maximumFrictionForce += pair.contacts[i].penetrationConstraint.accumulatedImpulse; //} //maximumFrictionForce *= friction; //float previousAccumulatedImpulse = accumulatedImpulse.X; //accumulatedImpulse.X = MathHelper.Clamp(accumulatedImpulse.X + lambda.X, -maximumFrictionForce, maximumFrictionForce); //lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse; //previousAccumulatedImpulse = accumulatedImpulse.Y; //accumulatedImpulse.Y = MathHelper.Clamp(accumulatedImpulse.Y + lambda.Y, -maximumFrictionForce, maximumFrictionForce); //lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse; //Apply impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif //Matrix2x3.Transform(ref lambda, ref linearA, out linear); linear.X = lambda.X * linearA.M11 + lambda.Y * linearA.M21; linear.Y = lambda.X * linearA.M12 + lambda.Y * linearA.M22; linear.Z = lambda.X * linearA.M13 + lambda.Y * linearA.M23; if (entityADynamic) { //Matrix2x3.Transform(ref lambda, ref angularA, out angular); angular.X = lambda.X * angularA.M11 + lambda.Y * angularA.M21; angular.Y = lambda.X * angularA.M12 + lambda.Y * angularA.M22; angular.Z = lambda.X * angularA.M13 + lambda.Y * angularA.M23; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; //Matrix2x3.Transform(ref lambda, ref angularB, out angular); angular.X = lambda.X * angularB.M11 + lambda.Y * angularB.M21; angular.Y = lambda.X * angularB.M12 + lambda.Y * angularB.M22; angular.Z = lambda.X * angularB.M13 + lambda.Y * angularB.M23; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } return Math.Abs(lambda.X) + Math.Abs(lambda.Y); } internal Vector3 manifoldCenter, relativeVelocity; /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { entityADynamic = entityA != null && entityA.isDynamic; entityBDynamic = entityB != null && entityB.isDynamic; contactCount = contactManifoldConstraint.penetrationConstraints.Count; switch (contactCount) { case 1: manifoldCenter = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position; break; case 2: Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); manifoldCenter.X *= .5f; manifoldCenter.Y *= .5f; manifoldCenter.Z *= .5f; break; case 3: Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .333333333f; manifoldCenter.Y *= .333333333f; manifoldCenter.Z *= .333333333f; break; case 4: //This isn't actually the center of the manifold. Is it good enough? Sure seems like it. Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Position, ref contactManifoldConstraint.penetrationConstraints.Elements[1].contact.Position, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[2].contact.Position, ref manifoldCenter, out manifoldCenter); Vector3.Add(ref contactManifoldConstraint.penetrationConstraints.Elements[3].contact.Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .25f; manifoldCenter.Y *= .25f; manifoldCenter.Z *= .25f; break; default: manifoldCenter = Toolbox.NoVector; break; } //Compute the three dimensional relative velocity at the point. Vector3 velocityA, velocityB; if (entityA != null) { Vector3.Subtract(ref manifoldCenter, ref entityA.position, out ra); Vector3.Cross(ref entityA.angularVelocity, ref ra, out velocityA); Vector3.Add(ref velocityA, ref entityA.linearVelocity, out velocityA); } else velocityA = new Vector3(); if (entityB != null) { Vector3.Subtract(ref manifoldCenter, ref entityB.position, out rb); Vector3.Cross(ref entityB.angularVelocity, ref rb, out velocityB); Vector3.Add(ref velocityB, ref entityB.linearVelocity, out velocityB); } else velocityB = new Vector3(); Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity); //Get rid of the normal velocity. Vector3 normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal; float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z; relativeVelocity.X -= normalVelocityScalar * normal.X; relativeVelocity.Y -= normalVelocityScalar * normal.Y; relativeVelocity.Z -= normalVelocityScalar * normal.Z; //Create the jacobian entry and decide the friction coefficient. float length = relativeVelocity.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float)Math.Sqrt(length); float inverseLength = 1 / length; linearA.M11 = relativeVelocity.X * inverseLength; linearA.M12 = relativeVelocity.Y * inverseLength; linearA.M13 = relativeVelocity.Z * inverseLength; friction = length > CollisionResponseSettings.StaticFrictionVelocityThreshold ? contactManifoldConstraint.materialInteraction.KineticFriction : contactManifoldConstraint.materialInteraction.StaticFriction; } else { friction = contactManifoldConstraint.materialInteraction.StaticFriction; //If there was no velocity, try using the previous frame's jacobian... if it exists. //Reusing an old one is okay since jacobians are cleared when a contact is initialized. if (!(linearA.M11 != 0 || linearA.M12 != 0 || linearA.M13 != 0)) { //Otherwise, just redo it all. //Create arbitrary axes. Vector3 axis1; Vector3.Cross(ref normal, ref Toolbox.RightVector, out axis1); length = axis1.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float)Math.Sqrt(length); float inverseLength = 1 / length; linearA.M11 = axis1.X * inverseLength; linearA.M12 = axis1.Y * inverseLength; linearA.M13 = axis1.Z * inverseLength; } else { Vector3.Cross(ref normal, ref Toolbox.UpVector, out axis1); axis1.Normalize(); linearA.M11 = axis1.X; linearA.M12 = axis1.Y; linearA.M13 = axis1.Z; } } } //Second axis is first axis x normal linearA.M21 = (linearA.M12 * normal.Z) - (linearA.M13 * normal.Y); linearA.M22 = (linearA.M13 * normal.X) - (linearA.M11 * normal.Z); linearA.M23 = (linearA.M11 * normal.Y) - (linearA.M12 * normal.X); //Compute angular jacobians if (entityA != null) { //angularA 1 = ra x linear axis 1 angularA.M11 = (ra.Y * linearA.M13) - (ra.Z * linearA.M12); angularA.M12 = (ra.Z * linearA.M11) - (ra.X * linearA.M13); angularA.M13 = (ra.X * linearA.M12) - (ra.Y * linearA.M11); //angularA 2 = ra x linear axis 2 angularA.M21 = (ra.Y * linearA.M23) - (ra.Z * linearA.M22); angularA.M22 = (ra.Z * linearA.M21) - (ra.X * linearA.M23); angularA.M23 = (ra.X * linearA.M22) - (ra.Y * linearA.M21); } //angularB 1 = linear axis 1 x rb if (entityB != null) { angularB.M11 = (linearA.M12 * rb.Z) - (linearA.M13 * rb.Y); angularB.M12 = (linearA.M13 * rb.X) - (linearA.M11 * rb.Z); angularB.M13 = (linearA.M11 * rb.Y) - (linearA.M12 * rb.X); //angularB 2 = linear axis 2 x rb angularB.M21 = (linearA.M22 * rb.Z) - (linearA.M23 * rb.Y); angularB.M22 = (linearA.M23 * rb.X) - (linearA.M21 * rb.Z); angularB.M23 = (linearA.M21 * rb.Y) - (linearA.M22 * rb.X); } //Compute inverse effective mass matrix Matrix2x2 entryA, entryB; //these are the transformed coordinates Matrix2x3 transform; Matrix3x2 transpose; if (entityADynamic) { Matrix2x3.Multiply(ref angularA, ref entityA.inertiaTensorInverse, out transform); Matrix2x3.Transpose(ref angularA, out transpose); Matrix2x2.Multiply(ref transform, ref transpose, out entryA); entryA.M11 += entityA.inverseMass; entryA.M22 += entityA.inverseMass; } else { entryA = new Matrix2x2(); } if (entityBDynamic) { Matrix2x3.Multiply(ref angularB, ref entityB.inertiaTensorInverse, out transform); Matrix2x3.Transpose(ref angularB, out transpose); Matrix2x2.Multiply(ref transform, ref transpose, out entryB); entryB.M11 += entityB.inverseMass; entryB.M22 += entityB.inverseMass; } else { entryB = new Matrix2x2(); } velocityToImpulse.M11 = -entryA.M11 - entryB.M11; velocityToImpulse.M12 = -entryA.M12 - entryB.M12; velocityToImpulse.M21 = -entryA.M21 - entryB.M21; velocityToImpulse.M22 = -entryA.M22 - entryB.M22; Matrix2x2.Invert(ref velocityToImpulse, out velocityToImpulse); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif //Matrix2x3.Transform(ref lambda, ref linearA, out linear); linear.X = accumulatedImpulse.X * linearA.M11 + accumulatedImpulse.Y * linearA.M21; linear.Y = accumulatedImpulse.X * linearA.M12 + accumulatedImpulse.Y * linearA.M22; linear.Z = accumulatedImpulse.X * linearA.M13 + accumulatedImpulse.Y * linearA.M23; if (entityADynamic) { //Matrix2x3.Transform(ref lambda, ref angularA, out angular); angular.X = accumulatedImpulse.X * angularA.M11 + accumulatedImpulse.Y * angularA.M21; angular.Y = accumulatedImpulse.X * angularA.M12 + accumulatedImpulse.Y * angularA.M22; angular.Z = accumulatedImpulse.X * angularA.M13 + accumulatedImpulse.Y * angularA.M23; entityA.ApplyLinearImpulse(ref linear); entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; //Matrix2x3.Transform(ref lambda, ref angularB, out angular); angular.X = accumulatedImpulse.X * angularB.M11 + accumulatedImpulse.Y * angularB.M21; angular.Y = accumulatedImpulse.X * angularB.M12 + accumulatedImpulse.Y * angularB.M22; angular.Z = accumulatedImpulse.X * angularB.M13 + accumulatedImpulse.Y * angularB.M23; entityB.ApplyLinearImpulse(ref linear); entityB.ApplyAngularImpulse(ref angular); } } internal void Setup(ConvexContactManifoldConstraint contactManifoldConstraint) { this.contactManifoldConstraint = contactManifoldConstraint; isActive = true; linearA = new Matrix2x3(); entityA = contactManifoldConstraint.EntityA; entityB = contactManifoldConstraint.EntityB; } internal void CleanUp() { accumulatedImpulse = new Vector2(); contactManifoldConstraint = null; entityA = null; entityB = null; isActive = false; } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //This should never really have to be called. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } } } ================================================ FILE: BEPUphysics/Constraints/Collision/Testing/ContactPenetrationConstraintDETester.cs ================================================ using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { internal class ContactPenetrationConstraintTester { private readonly Contact contact; internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; internal float angularAX, angularAY, angularAZ; internal float angularBX, angularBY, angularBZ; //Inverse effective mass matrix private float bias; internal bool isActive = true; private float linearAX, linearAY, linearAZ; internal int numIterationsAtZeroImpulse; private Entity parentA, parentB; internal float velocityToImpulse; internal ContactPenetrationConstraintTester(Contact contact) { this.contact = contact; } /// /// Computes and applies an impulse to keep the colliders from penetrating. /// /// Impulse applied. internal float ApplyImpulse() { //Compute relative velocity float lambda = (parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ + parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ - //note negatives, since reusing linearA jacobian entry parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ + parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ - bias) //Add in position correction/resitution * velocityToImpulse; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(0, accumulatedImpulse + lambda); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (parentA.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } return lambda; } internal void PreStep(float dt) { parentA = contact.collisionPair.ParentA; parentB = contact.collisionPair.ParentB; //Set up the jacobians. linearAX = contact.Normal.X; linearAY = contact.Normal.Y; linearAZ = contact.Normal.Z; //linearBX = -linearAX; //linearBY = -linearAY; //linearBZ = -linearAZ; //angular A = Ra x N angularAX = (contact.Ra.Y * linearAZ) - (contact.Ra.Z * linearAY); angularAY = (contact.Ra.Z * linearAX) - (contact.Ra.X * linearAZ); angularAZ = (contact.Ra.X * linearAY) - (contact.Ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * contact.Rb.Z) - (linearAZ * contact.Rb.Y); angularBY = (linearAZ * contact.Rb.X) - (linearAX * contact.Rb.Z); angularBZ = (linearAX * contact.Rb.Y) - (linearAY * contact.Rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (parentA.isDynamic) { tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31; tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32; tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass; } else entryA = 0; if (parentB.isDynamic) { tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31; tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32; tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Softness? //Bounciness bias = MathHelper.Min( MathHelper.Max(0, contact.PenetrationDepth - contact.collisionPair.allowedPenetration) * contact.collisionPair.space.simulationSettings.CollisionResponse.PenetrationRecoveryStiffness / dt, contact.collisionPair.space.simulationSettings.CollisionResponse.MaximumPositionCorrectionSpeed); if (contact.collisionPair.Bounciness > 0) { //Compute relative velocity float relativeVelocity = parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ + parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ - parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ + parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ; relativeVelocity *= -1; if (relativeVelocity > contact.collisionPair.space.simulationSettings.CollisionResponse.BouncinessVelocityThreshold) bias = MathHelper.Max(relativeVelocity * contact.collisionPair.Bounciness, bias); } //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (parentA.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; //parentA.ApplyLinearImpulse(ref linear); //parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; //parentB.ApplyLinearImpulse(ref linear); //parentB.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Constraints/Collision/Testing/DirectEnumerationSolver.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Solves the penetration constraints in a collision pair at once. /// internal class DirectEnumerationSolver { private const float ConditionNumberLimit = 1000; private readonly Vector3[] angularA = new Vector3[4]; private readonly Vector3[] angularB = new Vector3[4]; private readonly List contactIndicesUsed = new List(4); private readonly CollisionPair pair; private int contactCount; private Vector3 linear; private Entity parentA, parentB; //Jacobian is of the form //LinA1 AngA1 LinB1 AngB1 //LinA2 AngA2 LinB2 AngB2 //LinA3 AngA3 LinB3 AngB3 //LinA4 AngA4 LinB4 AngB4 //4x12 matrix. //Now, it just so happens that all linear entries are equal (B's are negative). //Angular entries are not equal. //4x12 Jacobian for 4 contacts: //[ n ra1 x n -n n x rb1 ] //[ n ra2 x n -n n x rb2 ] //[ n ra3 x n -n n x rb3 ] //[ n ra4 x n -n n x rb4 ] private Matrix velocityToImpulse; private Matrix3x3 velocityToImpulse3X3; internal DirectEnumerationSolver(CollisionPair pair) { this.pair = pair; } internal void ApplyImpulse() { switch (contactIndicesUsed.Count) { case 4: //Compute relative velocities. #if !WINDOWS Vector4 lambda = new Vector4(); #else Vector4 lambda; #endif lambda.X = -GetRelativeVelocity(0); lambda.Y = -GetRelativeVelocity(1); lambda.Z = -GetRelativeVelocity(2); lambda.W = -GetRelativeVelocity(3); //Transform to impulse Vector4.Transform(ref lambda, ref velocityToImpulse, out lambda); //if the solution is acceptable, apply and return if (lambda.X > 0 && lambda.Y > 0 && lambda.Z > 0 && lambda.W > 0) { #if !WINDOWS Vector3 linearImpulse = new Vector3(), angularImpulse = new Vector3(); #else Vector3 linearImpulse, angularImpulse; #endif float lambdaSum = lambda.X + lambda.Y + lambda.Z + lambda.W; linearImpulse.X = linear.X * lambdaSum; linearImpulse.Y = linear.Y * lambdaSum; linearImpulse.Z = linear.Z * lambdaSum; if (parentA.isDynamic) { angularImpulse.X = angularA[0].X * lambda.X + angularA[1].X * lambda.Y + angularA[2].X * lambda.Z + angularA[3].X * lambda.W; angularImpulse.Y = angularA[0].Y * lambda.X + angularA[1].Y * lambda.Y + angularA[2].Y * lambda.Z + angularA[3].Y * lambda.W; angularImpulse.Z = angularA[0].Z * lambda.X + angularA[1].Z * lambda.Y + angularA[2].Z * lambda.Z + angularA[3].Z * lambda.W; parentA.ApplyLinearImpulse(ref linearImpulse); parentA.ApplyAngularImpulse(ref angularImpulse); } if (parentB.isDynamic) { linearImpulse.X = -linearImpulse.X; linearImpulse.Y = -linearImpulse.Y; linearImpulse.Z = -linearImpulse.Z; angularImpulse.X = angularB[0].X * lambda.X + angularB[1].X * lambda.Y + angularB[2].X * lambda.Z + angularB[3].X * lambda.W; angularImpulse.Y = angularB[0].Y * lambda.X + angularB[1].Y * lambda.Y + angularB[2].Y * lambda.Z + angularB[3].Y * lambda.W; angularImpulse.Z = angularB[0].Z * lambda.X + angularB[1].Z * lambda.Y + angularB[2].Z * lambda.Z + angularB[3].Z * lambda.W; parentB.ApplyLinearImpulse(ref linearImpulse); parentB.ApplyAngularImpulse(ref angularImpulse); } return; } break; case 3: #if !WINDOWS Vector3 lambda3 = new Vector3(); #else Vector3 lambda3; #endif lambda3.X = -GetRelativeVelocity(contactIndicesUsed[0]); lambda3.Y = -GetRelativeVelocity(contactIndicesUsed[1]); lambda3.Z = -GetRelativeVelocity(contactIndicesUsed[2]); //Transform to impulse Matrix3x3.Transform(ref lambda3, ref velocityToImpulse3X3, out lambda3); //if the solution is acceptable, apply and return if (lambda3.X >= 0 && lambda3.Y >= 0 && lambda3.Z >= 0) { #if !WINDOWS Vector3 linearImpulse = new Vector3(), angularImpulse = new Vector3(); #else Vector3 linearImpulse, angularImpulse; #endif float lambdaSum = lambda3.X + lambda3.Y + lambda3.Z; linearImpulse.X = linear.X * lambdaSum; linearImpulse.Y = linear.Y * lambdaSum; linearImpulse.Z = linear.Z * lambdaSum; if (parentA.isDynamic) { angularImpulse.X = angularA[contactIndicesUsed[0]].X * lambda3.X + angularA[contactIndicesUsed[1]].X * lambda3.Y + angularA[contactIndicesUsed[2]].X * lambda3.Z; angularImpulse.Y = angularA[contactIndicesUsed[0]].Y * lambda3.X + angularA[contactIndicesUsed[1]].Y * lambda3.Y + angularA[contactIndicesUsed[2]].Y * lambda3.Z; angularImpulse.Z = angularA[contactIndicesUsed[0]].Z * lambda3.X + angularA[contactIndicesUsed[1]].Z * lambda3.Y + angularA[contactIndicesUsed[2]].Z * lambda3.Z; parentA.ApplyLinearImpulse(ref linearImpulse); parentA.ApplyAngularImpulse(ref angularImpulse); } if (parentB.isDynamic) { linearImpulse.X = -linearImpulse.X; linearImpulse.Y = -linearImpulse.Y; linearImpulse.Z = -linearImpulse.Z; angularImpulse.X = angularB[contactIndicesUsed[0]].X * lambda3.X + angularB[contactIndicesUsed[1]].X * lambda3.Y + angularB[contactIndicesUsed[2]].X * lambda3.Z; angularImpulse.Y = angularB[contactIndicesUsed[0]].Y * lambda3.X + angularB[contactIndicesUsed[1]].Y * lambda3.Y + angularB[contactIndicesUsed[2]].Y * lambda3.Z; angularImpulse.Z = angularB[contactIndicesUsed[0]].Z * lambda3.X + angularB[contactIndicesUsed[1]].Z * lambda3.Y + angularB[contactIndicesUsed[2]].Z * lambda3.Z; parentB.ApplyLinearImpulse(ref linearImpulse); parentB.ApplyAngularImpulse(ref angularImpulse); } return; } break; } //Note: No accumulated impulses yet. //Note: There's no 'sleeping' here. It's going to be real slow. //foreach (Contact c in pair.contacts) //{ // c.penetrationConstraint.applyImpulse(); //} } internal void PreStep(float dt) { parentA = pair.ParentA; parentB = pair.ParentB; contactCount = pair.Contacts.Count; if (contactCount > 0) { //Populate the jacobian linear = pair.Contacts[0].Normal; for (int i = 0; i < contactCount; i++) { angularA[i] = new Vector3(pair.Contacts[i].penetrationConstraint.angularAX, pair.Contacts[i].penetrationConstraint.angularAY, pair.Contacts[i].penetrationConstraint.angularAZ); angularB[i] = new Vector3(pair.Contacts[i].penetrationConstraint.angularBX, pair.Contacts[i].penetrationConstraint.angularBY, pair.Contacts[i].penetrationConstraint.angularBZ); //TODO: Penetration constraint calculates the diagonal entries already. //Could also cache out the angular[i] * I^-1 part, which would then speed up mass matrix calculation. //massMatrixInverse[i][i] = pair.contacts[0].penetrationConstraint.velocityToImpulse; } } contactIndicesUsed.Clear(); switch (contactCount) { case 4: //Create the effective mass matrix //Could optimize this since the penetration constraints need to compute the diagonal anyway. //Also, it does a lot of unnecessary 1/mass calculations. Matrix inverse; if (Get4X4MassMatrix(out inverse, out velocityToImpulse)) { //The 4x4 version is acceptable. contactIndicesUsed.Add(0); contactIndicesUsed.Add(1); contactIndicesUsed.Add(2); contactIndicesUsed.Add(3); } //TODO: Okay, this is a 'bad matrix' even in the ideal 4 point manifold situation. Is this to be expected, or is the above mass matrix entry calculation screwed up? //Yea. //J * M^-1 * JT could be singular in the case of redundant contacts. Pretest? Gramian, condition number.. else { Matrix3x3 inverse3X3; if (Get3X3InverseMassMatrix(1, 2, 3, out inverse3X3, out velocityToImpulse3X3)) { contactIndicesUsed.Add(1); contactIndicesUsed.Add(2); contactIndicesUsed.Add(3); } else if (Get3X3InverseMassMatrix(0, 2, 3, out inverse3X3, out velocityToImpulse3X3)) { contactIndicesUsed.Add(0); contactIndicesUsed.Add(2); contactIndicesUsed.Add(3); } else if (Get3X3InverseMassMatrix(0, 1, 3, out inverse3X3, out velocityToImpulse3X3)) { contactIndicesUsed.Add(0); contactIndicesUsed.Add(1); contactIndicesUsed.Add(3); } else if (Get3X3InverseMassMatrix(0, 1, 2, out inverse3X3, out velocityToImpulse3X3)) { contactIndicesUsed.Add(0); contactIndicesUsed.Add(1); contactIndicesUsed.Add(2); } } break; case 3: break; } } private bool Get2X2InverseMassMatrix(int indexA, int indexB, out Matrix2X2 massMatrix) { massMatrix.M11 = GetMassMatrixEntry(indexA, indexA); massMatrix.M12 = GetMassMatrixEntry(indexA, indexB); massMatrix.M21 = massMatrix.M12; // getMassMatrixEntry(indexB, indexA); massMatrix.M22 = GetMassMatrixEntry(indexB, indexB); return ComputeNorm(ref massMatrix) < ConditionNumberLimit; } private bool Get3X3InverseMassMatrix(int indexA, int indexB, int indexC, out Matrix3x3 inverseMassMatrix, out Matrix3x3 massMatrix) { inverseMassMatrix.M11 = GetMassMatrixEntry(indexA, indexA); inverseMassMatrix.M12 = GetMassMatrixEntry(indexA, indexB); inverseMassMatrix.M13 = GetMassMatrixEntry(indexA, indexC); inverseMassMatrix.M21 = inverseMassMatrix.M12; // getMassMatrixEntry(indexB, indexA); inverseMassMatrix.M22 = GetMassMatrixEntry(indexB, indexB); inverseMassMatrix.M23 = GetMassMatrixEntry(indexB, indexC); inverseMassMatrix.M31 = inverseMassMatrix.M13; // getMassMatrixEntry(indexC, indexA); inverseMassMatrix.M32 = inverseMassMatrix.M23; // getMassMatrixEntry(indexC, indexB); inverseMassMatrix.M33 = GetMassMatrixEntry(indexC, indexC); Matrix3x3.Invert(ref inverseMassMatrix, out massMatrix); return ComputeNorm(ref inverseMassMatrix) * ComputeNorm(ref massMatrix) < ConditionNumberLimit; } private bool Get4X4MassMatrix(out Matrix inverseMassMatrix, out Matrix massMatrix) { #if !WINDOWS inverseMassMatrix = new Matrix(); #endif inverseMassMatrix.M11 = GetMassMatrixEntry(0, 0); inverseMassMatrix.M12 = GetMassMatrixEntry(0, 1); inverseMassMatrix.M13 = GetMassMatrixEntry(0, 2); inverseMassMatrix.M14 = GetMassMatrixEntry(0, 3); inverseMassMatrix.M21 = inverseMassMatrix.M12; // getMassMatrixEntry(1, 0); inverseMassMatrix.M22 = GetMassMatrixEntry(1, 1); inverseMassMatrix.M23 = GetMassMatrixEntry(1, 2); inverseMassMatrix.M24 = GetMassMatrixEntry(1, 3); inverseMassMatrix.M31 = inverseMassMatrix.M13; // getMassMatrixEntry(2, 0); inverseMassMatrix.M32 = inverseMassMatrix.M23; // getMassMatrixEntry(2, 1); inverseMassMatrix.M33 = GetMassMatrixEntry(2, 2); inverseMassMatrix.M34 = GetMassMatrixEntry(2, 3); inverseMassMatrix.M41 = inverseMassMatrix.M14; // getMassMatrixEntry(3, 0); inverseMassMatrix.M42 = inverseMassMatrix.M24; // getMassMatrixEntry(3, 1); inverseMassMatrix.M43 = inverseMassMatrix.M34; // getMassMatrixEntry(3, 2); inverseMassMatrix.M44 = GetMassMatrixEntry(3, 3); Matrix.Invert(ref inverseMassMatrix, out massMatrix); return ComputeNorm(ref inverseMassMatrix) * ComputeNorm(ref massMatrix) < ConditionNumberLimit; } //TODO: DON'T JUST ADD 1/MASS!!!!!! look at Point on Line Joint formulation redux for more information //TODO: ON the other hand, uhh.. normal * normal is always 1 no matter how you look at it. LOOK AT IT MORE private float GetMassMatrixEntry(int i, int j) { //This is wasteful; there are 4 angular jacobians for each entity in 4x4 case. //Even with the reduced 10-element only mass matrix calculation, there is significant //retransforming going on. //Re-used values: //A[0] * Ia^-1 //A[1] * Ia^-1 //A[2] * Ia^-1 //A[3] * Ia^-1 //B[0] * Ib^-1 //B[1] * Ib^-1 //B[2] * Ib^-1 //B[3] * Ib^-1 //A, B inverse mass //The waste gets more wastey when the 4x4 fails and it has to do even more calculations... //In fact, the matrix entries themselves are redundant. //Can construct smaller matrices from bigger matrices????? Maybe... //Since getMassMatrixEntry is the same for 1x1, 2x2, 3x3, and 4x4 versions (1 2 3 4 contacts)... //me^-1 i,j is unique based on i,j alone!!! //Compute "max size matrix" and derive smaller ones directly from it :))) //Technically only the inverse mass matrix shares entries. Post inversion to mass matrix, they will be different. float entryA, entryB; Vector3 transform; if (parentA.isDynamic) { Matrix3x3.Transform(ref angularA[i], ref parentA.inertiaTensorInverse, out transform); Vector3.Dot(ref angularA[j], ref transform, out entryA); entryA += 1 / parentA.mass; } else entryA = 0; if (parentB.isDynamic) { Matrix3x3.Transform(ref angularB[i], ref parentB.inertiaTensorInverse, out transform); Vector3.Dot(ref angularB[j], ref transform, out entryB); entryB += 1 / parentB.mass; } else entryB = 0; return entryA + entryB; } private float GetRelativeVelocity(int i) { float relativeVelocity, dot; Vector3.Dot(ref linear, ref parentA.linearVelocity, out relativeVelocity); Vector3.Dot(ref angularA[i], ref parentA.angularVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref linear, ref parentB.linearVelocity, out dot); relativeVelocity -= dot; Vector3.Dot(ref angularB[i], ref parentB.angularVelocity, out dot); return relativeVelocity + dot; } private float GetSimulatedRelativeVelocity(int i, ref Vector3 linearVelocityA, ref Vector3 linearVelocityB, ref Vector3 angularVelocityA, ref Vector3 angularVelocityB) { float relativeVelocity, dot; Vector3.Dot(ref linear, ref linearVelocityA, out relativeVelocity); Vector3.Dot(ref angularA[i], ref angularVelocityA, out dot); relativeVelocity += dot; Vector3.Dot(ref linear, ref linearVelocityB, out dot); relativeVelocity -= dot; Vector3.Dot(ref angularB[i], ref angularVelocityB, out dot); return relativeVelocity + dot; } #region Norms private static float ComputeNorm(ref Matrix m) { //Would a square-based norm be faster and sufficient ? //Huge number of branches in this float norm = MathHelper.Max(Math.Abs(m.M11), Math.Abs(m.M12)); norm = MathHelper.Max(norm, Math.Abs(m.M13)); norm = MathHelper.Max(norm, Math.Abs(m.M14)); norm = MathHelper.Max(norm, Math.Abs(m.M21)); norm = MathHelper.Max(norm, Math.Abs(m.M22)); norm = MathHelper.Max(norm, Math.Abs(m.M23)); norm = MathHelper.Max(norm, Math.Abs(m.M24)); norm = MathHelper.Max(norm, Math.Abs(m.M31)); norm = MathHelper.Max(norm, Math.Abs(m.M32)); norm = MathHelper.Max(norm, Math.Abs(m.M33)); norm = MathHelper.Max(norm, Math.Abs(m.M34)); norm = MathHelper.Max(norm, Math.Abs(m.M41)); norm = MathHelper.Max(norm, Math.Abs(m.M42)); norm = MathHelper.Max(norm, Math.Abs(m.M43)); norm = MathHelper.Max(norm, Math.Abs(m.M44)); return norm; } private static float ComputeNorm(ref Matrix3x3 m) { //Would a square-based norm be faster and sufficient ? //Huge number of branches in this float norm = MathHelper.Max(Math.Abs(m.M11), Math.Abs(m.M12)); norm = MathHelper.Max(norm, Math.Abs(m.M13)); norm = MathHelper.Max(norm, Math.Abs(m.M21)); norm = MathHelper.Max(norm, Math.Abs(m.M22)); norm = MathHelper.Max(norm, Math.Abs(m.M23)); norm = MathHelper.Max(norm, Math.Abs(m.M31)); norm = MathHelper.Max(norm, Math.Abs(m.M32)); norm = MathHelper.Max(norm, Math.Abs(m.M33)); return norm; } private static float ComputeNorm(ref Matrix2X2 m) { //Would a square-based norm be faster and sufficient ? //Huge number of branches in this float norm = MathHelper.Max(Math.Abs(m.M11), Math.Abs(m.M12)); norm = MathHelper.Max(norm, Math.Abs(m.M21)); norm = MathHelper.Max(norm, Math.Abs(m.M22)); return norm; } #endregion } } ================================================ FILE: BEPUphysics/Constraints/Collision/Testing/SlidingFrictionOneAxisConstraint.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Handles collision pair sliding friction. /// internal class SlidingFrictionOneAxisConstraint { private readonly CollisionPair pair; private float accumulatedImpulse; //Jacobian entries //float linearBX, linearBY, linearBZ; private float angularAX, angularAY, angularAZ; private float angularBX, angularBY, angularBZ; internal bool isActive = true; private float linearAX, linearAY, linearAZ; private float maximumFrictionForce; internal int numIterationsAtZeroImpulse; private Entity parentA, parentB; //Inverse effective mass matrix private float velocityToImpulse; /// /// Constructs a new linear friction constraint. /// /// Collision pair owning this friction constraint. internal SlidingFrictionOneAxisConstraint(CollisionPair pair) { this.pair = pair; } internal float ApplyImpulse() { //Compute relative velocity float lambda = (parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ + parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ - //note negatives, since reusing linearA jacobian entry parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ + parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ) * velocityToImpulse; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maximumFrictionForce, maximumFrictionForce); //instead of maximumFrictionForce, could recompute each iteration... lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (parentA.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } return lambda; } /// /// Initializes the constraint for this frame. /// /// Time since the last frame. /// Computed center of manifold. internal void PreStep(float dt, out Vector3 manifoldCenter) { parentA = pair.ParentA; parentB = pair.ParentB; int count = pair.Contacts.Count; switch (count) { case 1: manifoldCenter = pair.Contacts[0].Position; break; case 2: Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); manifoldCenter.X *= .5f; manifoldCenter.Y *= .5f; manifoldCenter.Z *= .5f; break; case 3: Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .333333333f; manifoldCenter.Y *= .333333333f; manifoldCenter.Z *= .333333333f; break; case 4: //This isn't actually the center of the manifold. Is it good enough? Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter); Vector3.Add(ref pair.Contacts[3].Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .25f; manifoldCenter.Y *= .25f; manifoldCenter.Z *= .25f; break; default: manifoldCenter = Toolbox.NoVector; break; } //Compute the three dimensional relative velocity at the point. Vector3 ra, rb; Vector3.Subtract(ref manifoldCenter, ref parentA.position, out ra); Vector3.Subtract(ref manifoldCenter, ref parentB.position, out rb); Vector3 velocityA, velocityB; Vector3.Cross(ref parentA.angularVelocity, ref ra, out velocityA); Vector3.Add(ref velocityA, ref parentA.linearVelocity, out velocityA); Vector3.Cross(ref parentB.angularVelocity, ref rb, out velocityB); Vector3.Add(ref velocityB, ref parentB.linearVelocity, out velocityB); Vector3 relativeVelocity; Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity); //Get rid of the normal velocity. Vector3 normal = pair.Contacts[0].Normal; float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z; relativeVelocity.X -= normalVelocityScalar * normal.X; relativeVelocity.Y -= normalVelocityScalar * normal.Y; relativeVelocity.Z -= normalVelocityScalar * normal.Z; float friction; //Create the jacobian entry and decide the friction coefficient. float length = relativeVelocity.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float) Math.Sqrt(length); linearAX = relativeVelocity.X / length; linearAY = relativeVelocity.Y / length; linearAZ = relativeVelocity.Z / length; friction = length > pair.space.simulationSettings.CollisionResponse.StaticFrictionVelocityThreshold ? pair.DynamicFriction : pair.StaticFriction; } else { //If there's no velocity, there's no jacobian. Give up. //This is 'fast' in that it will early out on essentially resting objects, //but it may introduce instability. //If it doesn't look good, try the next approach. //isActive = false; //return; //if the above doesn't work well, try using the previous frame's jacobian. if (linearAX != 0 || linearAY != 0 || linearAZ != 0) { friction = pair.StaticFriction; } else { //Can't really do anything here, give up. isActive = false; return; } } maximumFrictionForce = 0; for (int i = 0; i < count; i++) { maximumFrictionForce += pair.Contacts[i].penetrationConstraint.accumulatedImpulse; } maximumFrictionForce *= friction; //angular A = Ra x N angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY); angularAY = (ra.Z * linearAX) - (ra.X * linearAZ); angularAZ = (ra.X * linearAY) - (ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y); angularBY = (linearAZ * rb.X) - (linearAX * rb.Z); angularBZ = (linearAX * rb.Y) - (linearAY * rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (parentA.isDynamic) { tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31; tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32; tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass; } else entryA = 0; if (parentB.isDynamic) { tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31; tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32; tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Softness? //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (parentA.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Constraints/Collision/Testing/SlidingFrictionTwoAxisObsolete.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Handles collision pair sliding friction. /// internal class SlidingFrictionTwoAxisObsolete { private readonly CollisionPair pair; private float accumulatedImpulse; private float accumulatedImpulse2; //Jacobian entries //float linearBX, linearBY, linearBZ; private float angularAX; private float angularAX2; private float angularAY; private float angularAY2; private float angularAZ; private float angularAZ2; private float angularBX; private float angularBX2; private float angularBY; private float angularBY2; private float angularBZ; private float angularBZ2; private int contactCount; private float friction; internal bool isActive = true; private float linearAX; private float linearAX2; private float linearAY; private float linearAY2; private float linearAZ; private float linearAZ2; /* private float maximumFrictionForce; */ internal int numIterationsAtZeroImpulse; private Entity parentA, parentB; //Inverse effective mass matrix private float velocityToImpulse; private float velocityToImpulse2; /// /// Constructs a new linear friction constraint. /// /// Collision pair owning this friction constraint. internal SlidingFrictionTwoAxisObsolete(CollisionPair pair) { this.pair = pair; } internal float ApplyImpulse() { //Compute relative velocity float lambda = (parentA.linearVelocity.X * linearAX + parentA.linearVelocity.Y * linearAY + parentA.linearVelocity.Z * linearAZ + parentA.angularVelocity.X * angularAX + parentA.angularVelocity.Y * angularAY + parentA.angularVelocity.Z * angularAZ - //note negatives, since reusing linearA jacobian entry parentB.linearVelocity.X * linearAX - parentB.linearVelocity.Y * linearAY - parentB.linearVelocity.Z * linearAZ + parentB.angularVelocity.X * angularBX + parentB.angularVelocity.Y * angularBY + parentB.angularVelocity.Z * angularBZ) * velocityToImpulse; //convert to impulse //Compute maximum force float maximumFrictionForce = 0; for (int i = 0; i < contactCount; i++) { maximumFrictionForce += pair.Contacts[i].penetrationConstraint.accumulatedImpulse; } maximumFrictionForce *= friction; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maximumFrictionForce, maximumFrictionForce); //instead of maximumFrictionForce, could recompute each iteration... lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (parentA.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } //Compute relative velocity float lambda2 = (parentA.linearVelocity.X * linearAX2 + parentA.linearVelocity.Y * linearAY2 + parentA.linearVelocity.Z * linearAZ2 + parentA.angularVelocity.X * angularAX2 + parentA.angularVelocity.Y * angularAY2 + parentA.angularVelocity.Z * angularAZ2 - //note negatives, since reusing linearA jacobian entry parentB.linearVelocity.X * linearAX2 - parentB.linearVelocity.Y * linearAY2 - parentB.linearVelocity.Z * linearAZ2 + parentB.angularVelocity.X * angularBX2 + parentB.angularVelocity.Y * angularBY2 + parentB.angularVelocity.Z * angularBZ2) * velocityToImpulse2; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse2 = accumulatedImpulse2; accumulatedImpulse2 = MathHelper.Clamp(accumulatedImpulse2 + lambda2, -maximumFrictionForce, maximumFrictionForce); //instead of maximumFrictionForce, could recompute each iteration... lambda2 = accumulatedImpulse2 - previousAccumulatedImpulse2; //Apply the impulse linear.X = lambda2 * linearAX2; linear.Y = lambda2 * linearAY2; linear.Z = lambda2 * linearAZ2; if (parentA.isDynamic) { angular.X = lambda2 * angularAX2; angular.Y = lambda2 * angularAY2; angular.Z = lambda2 * angularAZ2; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda2 * angularBX2; angular.Y = lambda2 * angularBY2; angular.Z = lambda2 * angularBZ2; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } return lambda; } /// /// Initializes the constraint for this frame. /// /// Time since the last frame. /// Computed center of manifold. internal void PreStep(float dt, out Vector3 manifoldCenter) { numIterationsAtZeroImpulse = 0; parentA = pair.ParentA; parentB = pair.ParentB; contactCount = pair.Contacts.Count; switch (contactCount) { case 1: manifoldCenter = pair.Contacts[0].Position; break; case 2: Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); manifoldCenter.X *= .5f; manifoldCenter.Y *= .5f; manifoldCenter.Z *= .5f; break; case 3: Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .333333333f; manifoldCenter.Y *= .333333333f; manifoldCenter.Z *= .333333333f; break; case 4: //This isn't actually the center of the manifold. Is it good enough? Vector3.Add(ref pair.Contacts[0].Position, ref pair.Contacts[1].Position, out manifoldCenter); Vector3.Add(ref pair.Contacts[2].Position, ref manifoldCenter, out manifoldCenter); Vector3.Add(ref pair.Contacts[3].Position, ref manifoldCenter, out manifoldCenter); manifoldCenter.X *= .25f; manifoldCenter.Y *= .25f; manifoldCenter.Z *= .25f; break; default: manifoldCenter = Toolbox.NoVector; break; } //Compute the three dimensional relative velocity at the point. Vector3 ra; Vector3 rb; Vector3.Subtract(ref manifoldCenter, ref parentA.position, out ra); Vector3.Subtract(ref manifoldCenter, ref parentB.position, out rb); Vector3 velocityA, velocityB; Vector3.Cross(ref parentA.angularVelocity, ref ra, out velocityA); Vector3.Add(ref velocityA, ref parentA.linearVelocity, out velocityA); Vector3.Cross(ref parentB.angularVelocity, ref rb, out velocityB); Vector3.Add(ref velocityB, ref parentB.linearVelocity, out velocityB); Vector3 relativeVelocity; Vector3.Subtract(ref velocityA, ref velocityB, out relativeVelocity); //Get rid of the normal velocity. Vector3 normal = pair.Contacts[0].Normal; float normalVelocityScalar = normal.X * relativeVelocity.X + normal.Y * relativeVelocity.Y + normal.Z * relativeVelocity.Z; relativeVelocity.X -= normalVelocityScalar * normal.X; relativeVelocity.Y -= normalVelocityScalar * normal.Y; relativeVelocity.Z -= normalVelocityScalar * normal.Z; //Create the jacobian entry and decide the friction coefficient. float length = relativeVelocity.LengthSquared(); if (length > Toolbox.Epsilon) { length = (float) Math.Sqrt(length); linearAX = relativeVelocity.X / length; linearAY = relativeVelocity.Y / length; linearAZ = relativeVelocity.Z / length; friction = length > pair.space.simulationSettings.CollisionResponse.StaticFrictionVelocityThreshold ? pair.DynamicFriction : pair.StaticFriction; } else { //If there's no velocity, there's no jacobian. Give up. //This is 'fast' in that it will early out on essentially resting objects, //but it may introduce instability. //If it doesn't look good, try the next approach. //isActive = false; //return; //if the above doesn't work well, try using the previous frame's jacobian. if (linearAX != 0 || linearAY != 0 || linearAZ != 0) { friction = pair.StaticFriction; } else { //Can't really do anything here, give up. isActive = false; return; } } //maximumFrictionForce = 0; //for (int i = 0; i < count; i++) //{ // maximumFrictionForce += pair.contacts[i].penetrationConstraint.accumulatedImpulse; //} //maximumFrictionForce *= friction; //linear axis 2 = normal x N linearAX2 = (normal.Y * linearAZ) - (normal.Z * linearAY); linearAY2 = (normal.Z * linearAX) - (normal.X * linearAZ); linearAZ2 = (normal.X * linearAY) - (normal.Y * linearAX); //angular A = Ra x N angularAX = (ra.Y * linearAZ) - (ra.Z * linearAY); angularAY = (ra.Z * linearAX) - (ra.X * linearAZ); angularAZ = (ra.X * linearAY) - (ra.Y * linearAX); //angular A 2 = Ra x linear axis 2 angularAX2 = (ra.Y * linearAZ2) - (ra.Z * linearAY2); angularAY2 = (ra.Z * linearAX2) - (ra.X * linearAZ2); angularAZ2 = (ra.X * linearAY2) - (ra.Y * linearAX2); //Angular B = N x Rb angularBX = (linearAY * rb.Z) - (linearAZ * rb.Y); angularBY = (linearAZ * rb.X) - (linearAX * rb.Z); angularBZ = (linearAX * rb.Y) - (linearAY * rb.X); //Angular B 2 = linear axis 2 x Rb angularBX2 = (linearAY2 * rb.Z) - (linearAZ2 * rb.Y); angularBY2 = (linearAZ2 * rb.X) - (linearAX2 * rb.Z); angularBZ2 = (linearAX2 * rb.Y) - (linearAY2 * rb.X); //Compute inverse effective mass matrix float entryA, entryB; float entryA2, entryB2; //these are the transformed coordinates float tX, tY, tZ; float tX2, tY2, tZ2; if (parentA.isDynamic) { tX = angularAX * parentA.inertiaTensorInverse.M11 + angularAY * parentA.inertiaTensorInverse.M21 + angularAZ * parentA.inertiaTensorInverse.M31; tY = angularAX * parentA.inertiaTensorInverse.M12 + angularAY * parentA.inertiaTensorInverse.M22 + angularAZ * parentA.inertiaTensorInverse.M32; tZ = angularAX * parentA.inertiaTensorInverse.M13 + angularAY * parentA.inertiaTensorInverse.M23 + angularAZ * parentA.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + 1 / parentA.mass; tX2 = angularAX2 * parentA.inertiaTensorInverse.M11 + angularAY2 * parentA.inertiaTensorInverse.M21 + angularAZ2 * parentA.inertiaTensorInverse.M31; tY2 = angularAX2 * parentA.inertiaTensorInverse.M12 + angularAY2 * parentA.inertiaTensorInverse.M22 + angularAZ2 * parentA.inertiaTensorInverse.M32; tZ2 = angularAX2 * parentA.inertiaTensorInverse.M13 + angularAY2 * parentA.inertiaTensorInverse.M23 + angularAZ2 * parentA.inertiaTensorInverse.M33; entryA2 = tX2 * angularAX2 + tY2 * angularAY2 * tZ2 * angularAZ2 + 1 / parentA.mass; } else { entryA = 0; entryA2 = 0; } if (parentB.isDynamic) { tX = angularBX * parentB.inertiaTensorInverse.M11 + angularBY * parentB.inertiaTensorInverse.M21 + angularBZ * parentB.inertiaTensorInverse.M31; tY = angularBX * parentB.inertiaTensorInverse.M12 + angularBY * parentB.inertiaTensorInverse.M22 + angularBZ * parentB.inertiaTensorInverse.M32; tZ = angularBX * parentB.inertiaTensorInverse.M13 + angularBY * parentB.inertiaTensorInverse.M23 + angularBZ * parentB.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + 1 / parentB.mass; tX2 = angularBX2 * parentB.inertiaTensorInverse.M11 + angularBY2 * parentB.inertiaTensorInverse.M21 + angularBZ2 * parentB.inertiaTensorInverse.M31; tY2 = angularBX2 * parentB.inertiaTensorInverse.M12 + angularBY2 * parentB.inertiaTensorInverse.M22 + angularBZ2 * parentB.inertiaTensorInverse.M32; tZ2 = angularBX2 * parentB.inertiaTensorInverse.M13 + angularBY2 * parentB.inertiaTensorInverse.M23 + angularBZ2 * parentB.inertiaTensorInverse.M33; entryB2 = tX2 * angularBX2 + tY2 * angularBY2 + tZ2 * angularBZ2 + 1 / parentB.mass; } else { entryB = 0; entryB2 = 0; } velocityToImpulse = -1 / (entryA + entryB); //Softness? velocityToImpulse2 = -1 / (entryA2 + entryB2); //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (parentA.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } //Warm starting 2 linear.X = accumulatedImpulse2 * linearAX2; linear.Y = accumulatedImpulse2 * linearAY2; linear.Z = accumulatedImpulse2 * linearAZ2; if (parentA.isDynamic) { angular.X = accumulatedImpulse2 * angularAX2; angular.Y = accumulatedImpulse2 * angularAY2; angular.Z = accumulatedImpulse2 * angularAZ2; parentA.ApplyLinearImpulse(ref linear); parentA.ApplyAngularImpulse(ref angular); } if (parentB.isDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse2 * angularBX2; angular.Y = accumulatedImpulse2 * angularBY2; angular.Z = accumulatedImpulse2 * angularBZ2; parentB.ApplyLinearImpulse(ref linear); parentB.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Constraints/Collision/TwistFrictionConstraint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; using BEPUphysics.Settings; namespace BEPUphysics.Constraints.Collision { /// /// Computes the forces necessary to slow down and stop twisting motion in a collision between two entities. /// public class TwistFrictionConstraint : EntitySolverUpdateable { private readonly float[] leverArms = new float[4]; private ConvexContactManifoldConstraint contactManifoldConstraint; /// /// Gets the contact manifold constraint that owns this constraint. /// public ConvexContactManifoldConstraint ContactManifoldConstraint { get { return contactManifoldConstraint; } } internal float accumulatedImpulse; private float angularX, angularY, angularZ; private int contactCount; private float friction; Entity entityA, entityB; bool entityADynamic, entityBDynamic; private float velocityToImpulse; /// /// Constructs a new twist friction constraint. /// public TwistFrictionConstraint() { isActive = false; } /// /// Gets the torque applied by twist friction. /// public float TotalTorque { get { return accumulatedImpulse; } } /// /// Gets the angular velocity between the associated entities. /// public float RelativeVelocity { get { float lambda = 0; if (entityA != null) lambda = entityA.angularVelocity.X * angularX + entityA.angularVelocity.Y * angularY + entityA.angularVelocity.Z * angularZ; if (entityB != null) lambda -= entityB.angularVelocity.X * angularX + entityB.angularVelocity.Y * angularY + entityB.angularVelocity.Z * angularZ; return lambda; } } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //Compute relative velocity. Collisions can occur between an entity and a non-entity. If it's not an entity, assume it's not moving. float lambda = RelativeVelocity; lambda *= velocityToImpulse; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; float maximumFrictionForce = 0; for (int i = 0; i < contactCount; i++) { maximumFrictionForce += leverArms[i] * contactManifoldConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; } maximumFrictionForce *= friction; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maximumFrictionForce, maximumFrictionForce); //instead of maximumFrictionForce, could recompute each iteration... lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 angular = new Vector3(); #else Vector3 angular; #endif angular.X = lambda * angularX; angular.Y = lambda * angularY; angular.Z = lambda * angularZ; if (entityADynamic) { entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { angular.X = -angular.X; angular.Y = -angular.Y; angular.Z = -angular.Z; entityB.ApplyAngularImpulse(ref angular); } return Math.Abs(lambda); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { entityADynamic = entityA != null && entityA.isDynamic; entityBDynamic = entityB != null && entityB.isDynamic; //Compute the jacobian...... Real hard! Vector3 normal = contactManifoldConstraint.penetrationConstraints.Elements[0].contact.Normal; angularX = normal.X; angularY = normal.Y; angularZ = normal.Z; //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (entityADynamic) { tX = angularX * entityA.inertiaTensorInverse.M11 + angularY * entityA.inertiaTensorInverse.M21 + angularZ * entityA.inertiaTensorInverse.M31; tY = angularX * entityA.inertiaTensorInverse.M12 + angularY * entityA.inertiaTensorInverse.M22 + angularZ * entityA.inertiaTensorInverse.M32; tZ = angularX * entityA.inertiaTensorInverse.M13 + angularY * entityA.inertiaTensorInverse.M23 + angularZ * entityA.inertiaTensorInverse.M33; entryA = tX * angularX + tY * angularY + tZ * angularZ + entityA.inverseMass; } else entryA = 0; if (entityBDynamic) { tX = angularX * entityB.inertiaTensorInverse.M11 + angularY * entityB.inertiaTensorInverse.M21 + angularZ * entityB.inertiaTensorInverse.M31; tY = angularX * entityB.inertiaTensorInverse.M12 + angularY * entityB.inertiaTensorInverse.M22 + angularZ * entityB.inertiaTensorInverse.M32; tZ = angularX * entityB.inertiaTensorInverse.M13 + angularY * entityB.inertiaTensorInverse.M23 + angularZ * entityB.inertiaTensorInverse.M33; entryB = tX * angularX + tY * angularY + tZ * angularZ + entityB.inverseMass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Compute the relative velocity to determine what kind of friction to use float relativeAngularVelocity = RelativeVelocity; //Set up friction and find maximum friction force Vector3 relativeSlidingVelocity = contactManifoldConstraint.SlidingFriction.relativeVelocity; friction = Math.Abs(relativeAngularVelocity) > CollisionResponseSettings.StaticFrictionVelocityThreshold || Math.Abs(relativeSlidingVelocity.X) + Math.Abs(relativeSlidingVelocity.Y) + Math.Abs(relativeSlidingVelocity.Z) > CollisionResponseSettings.StaticFrictionVelocityThreshold ? contactManifoldConstraint.materialInteraction.KineticFriction : contactManifoldConstraint.materialInteraction.StaticFriction; friction *= CollisionResponseSettings.TwistFrictionFactor; contactCount = contactManifoldConstraint.penetrationConstraints.Count; Vector3 contactOffset; for (int i = 0; i < contactCount; i++) { Vector3.Subtract(ref contactManifoldConstraint.penetrationConstraints.Elements[i].contact.Position, ref contactManifoldConstraint.SlidingFriction.manifoldCenter, out contactOffset); leverArms[i] = contactOffset.Length(); } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Apply the warmstarting impulse. #if !WINDOWS Vector3 angular = new Vector3(); #else Vector3 angular; #endif angular.X = accumulatedImpulse * angularX; angular.Y = accumulatedImpulse * angularY; angular.Z = accumulatedImpulse * angularZ; if (entityADynamic) { entityA.ApplyAngularImpulse(ref angular); } if (entityBDynamic) { angular.X = -angular.X; angular.Y = -angular.Y; angular.Z = -angular.Z; entityB.ApplyAngularImpulse(ref angular); } } internal void Setup(ConvexContactManifoldConstraint contactManifoldConstraint) { this.contactManifoldConstraint = contactManifoldConstraint; isActive = true; entityA = contactManifoldConstraint.EntityA; entityB = contactManifoldConstraint.EntityB; } internal void CleanUp() { accumulatedImpulse = 0; contactManifoldConstraint = null; entityA = null; entityB = null; isActive = false; } protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { //This should never really have to be called. if (entityA != null) outputInvolvedEntities.Add(entityA); if (entityB != null) outputInvolvedEntities.Add(entityB); } } } ================================================ FILE: BEPUphysics/Constraints/EntitySolverUpdateable.cs ================================================ using System.Collections.Generic; using BEPUphysics.Constraints.SolverGroups; using BEPUphysics.Entities; using BEPUutilities.DataStructures; using BEPUphysics.SolverSystems; namespace BEPUphysics.Constraints { /// /// Superclass of objects types which require solving by the velocity solver. /// These are updated within the internal iterative solver when owned by a space. /// public abstract class EntitySolverUpdateable : SolverUpdateable { /// /// List of all entities affected by this updateable. /// protected internal readonly RawList involvedEntities = new RawList(2); /// /// Gets the entities that this solver updateable is involved with. /// public ReadOnlyList InvolvedEntities { get { return new ReadOnlyList(involvedEntities); } } /// /// Number of entities used in the solver updateable. /// Note that this is set automatically by the sortInvolvedEntities method /// if it is called. /// protected internal int numberOfInvolvedEntities; /// /// Gets the solver group that manages this solver updateable, if any. /// Null if not owned by a solver group. /// public SolverGroup SolverGroup { get; protected internal set; } /// /// Acquires exclusive access to all entities involved in the solver updateable. /// public override void EnterLock() { for (int i = 0; i < numberOfInvolvedEntities; i++) { if (involvedEntities.Elements[i].isDynamic) //Only need to lock dynamic entities. { involvedEntities.Elements[i].locker.Enter(); //Monitor.Enter(involvedEntities.Elements[i].locker); } } } /// /// Releases exclusive access to the updateable's entities. /// This should be called within a 'finally' block following a 'try' block containing the locked operations. /// public override void ExitLock() { for (int i = numberOfInvolvedEntities - 1; i >= 0; i--) { if (involvedEntities.Elements[i].isDynamic) //Only need to lock dynamic entities. involvedEntities.Elements[i].locker.Exit(); //Monitor.Exit(involvedEntities[i].locker); } } /// /// Attempts to acquire exclusive access to all entities involved in the solver updateable. /// /// Whether or not the lock was entered successfully. public override bool TryEnterLock() { for (int i = 0; i < numberOfInvolvedEntities; i++) { if (involvedEntities.Elements[i].isDynamic) //Only need to lock dynamic entities. if (!involvedEntities.Elements[i].locker.TryEnter()) { //Turns out we can't take all the resources! Immediately drop everything. for (i = i - 1 /*failed on the ith element, so start at the previous*/; i >= 0; i--) { if (involvedEntities[i].isDynamic) involvedEntities.Elements[i].locker.Exit(); } return false; } } return true; //for (int i = 0; i < numberOfInvolvedEntities; i++) //{ // if (involvedEntities[i].isDynamic) //Only need to lock dynamic entities. // if (!Monitor.TryEnter(involvedEntities[i].locker)) // { // //Turns out we can't take all the resources! Immediately drop everything. // for (i = i - 1 /*failed on the ith element, so start at the previous*/; i >= 0; i--) // { // if (involvedEntities[i].isDynamic) // Monitor.Exit(involvedEntities[i].locker); // } // return false; // } //} //return true; } /// /// Handle any bookkeeping needed when the entities involved in this SolverUpdateable change. /// protected internal virtual void OnInvolvedEntitiesChanged() { //First verify that something really changed. bool entitiesChanged = false; RawList newInvolvedEntities = PhysicsResources.GetEntityRawList(); CollectInvolvedEntities(newInvolvedEntities); if (newInvolvedEntities.Count == involvedEntities.Count) { for (int i = 0; i < newInvolvedEntities.Count; i++) { if (newInvolvedEntities.Elements[i] != involvedEntities.Elements[i]) { entitiesChanged = true; break; } } } else { entitiesChanged = true; } if (entitiesChanged) { //Probably need to wake things up given that such a significant change was made. for (int i = 0; i < involvedEntities.Count; i++) { Entity e = involvedEntities.Elements[i]; if (e.isDynamic) { e.activityInformation.Activate(); break;//Don't bother activating other entities; they are all a part of the same simulation island. } } //CollectInvolvedEntities will give the updateable a new simulationIslandConnection and get rid of the old one. CollectInvolvedEntities(); if (SolverGroup != null) SolverGroup.OnInvolvedEntitiesChanged(); //We woke up the FORMER involved entities, now wake up the current involved entities. for (int i = 0; i < involvedEntities.Count; i++) { Entity e = involvedEntities.Elements[i]; if (e.isDynamic) { e.activityInformation.Activate(); break; //Don't bother activating other entities; they are all a part of the same simulation island. } } } PhysicsResources.GiveBack(newInvolvedEntities); } /// /// Collects the entities involved in a solver updateable and sets up the internal listings. /// protected internal void CollectInvolvedEntities() { involvedEntities.Clear(); CollectInvolvedEntities(involvedEntities); SortInvolvedEntities(); UpdateConnectedMembers(); } /// /// Adds entities associated with the solver item to the involved entities list. /// This allows the non-batched multithreading system to lock properly. /// protected internal abstract void CollectInvolvedEntities(RawList outputInvolvedEntities); /// /// Sorts the involved entities according to their hashcode to allow non-batched multithreading to avoid deadlocks. /// protected internal void SortInvolvedEntities() { numberOfInvolvedEntities = involvedEntities.Count; involvedEntities.Sort(comparer); } void UpdateConnectedMembers() { //Since we're about to change this updateable's connections, make sure the //simulation islands hear about it. This is NOT thread safe. var deactivationManager = simulationIslandConnection.DeactivationManager; //Orphan the simulation island connection since it's about to get replaced. //There's three possible situations here: //1) We belong to the DeactivationManager. //2) We don't belong to a DeactivationManager and the connection is slated for removal (we were in the deactivation manager before). // This can happen when a solver updateable associated with a pair gets removed and cleaned up. //3) We don't belong to a DeactivationManager and the connection is not slated for removal (we weren't in a deactivation manager before). //In Case #1, all we have to do is orphan the connection and remove it from the manager. This performs any splits necessary. The replacement connection will force any necessary merges. //In Case #2, we were just removed but the connection is still considered to have an owner. //It won't get cleaned up by the removal, and doing it here would be premature: orphan the connection so the next deactivation manager splits flush cleans it up! //In Case #3, we have full control over the simulation island connection because there is no interaction with a deactivation manager. We can just get rid of it directly. simulationIslandConnection.Owner = null; if (deactivationManager != null) { deactivationManager.Remove(simulationIslandConnection); } else if (!simulationIslandConnection.SlatedForRemoval) //If it's already been removed, cleaning it ourselves would prevent proper simulation island splits in the deactivation manager split flush. PhysicsResources.GiveBack(simulationIslandConnection); //Well, since we're going to orphan the connection, we'll need to take care of its trash. //The SimulationIslandConnection is immutable. //So create a new one! //Assume we've already dealt with the old connection. simulationIslandConnection = PhysicsResources.GetSimulationIslandConnection(); for (int i = 0; i < involvedEntities.Count; i++) { simulationIslandConnection.Add(involvedEntities.Elements[i].activityInformation); } simulationIslandConnection.Owner = this; //Add the new reference back. if (deactivationManager != null) deactivationManager.Add(simulationIslandConnection); } private static EntityComparer comparer = new EntityComparer(); private class EntityComparer : IComparer { #region IComparer Members int IComparer.Compare(Entity x, Entity y) { if (x.InstanceId > y.InstanceId) return 1; if (x.InstanceId < y.InstanceId) return -1; return 0; } #endregion } } } ================================================ FILE: BEPUphysics/Constraints/IJacobians.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Denotes a class that uses a single linear jacobian axis. /// public interface I1DJacobianConstraint { /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. void GetAngularJacobianA(out Vector3 jacobian); /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. void GetAngularJacobianB(out Vector3 jacobian); /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. void GetLinearJacobianA(out Vector3 jacobian); /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. void GetLinearJacobianB(out Vector3 jacobian); /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. void GetMassMatrix(out float outputMassMatrix); } /// /// Denotes a class that uses two linear jacobian axes. /// public interface I2DJacobianConstraint { /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY); /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY); /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY); /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY); /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. void GetMassMatrix(out Matrix2x2 massMatrix); } /// /// Denotes a class that uses three linear jacobian axes. /// public interface I3DJacobianConstraint { /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. /// Third angular jacobian entry for the first connected entity. void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ); /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. /// Third angular jacobian entry for the second connected entity. void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ); /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. /// Third linear jacobian entry for the first connected entity. void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ); /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. /// Third linear jacobian entry for the second connected entity. void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ); /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. void GetMassMatrix(out Matrix3x3 outputMassMatrix); } } ================================================ FILE: BEPUphysics/Constraints/ISolverSettings.cs ================================================ namespace BEPUphysics.Constraints { /// /// Implemented by classes which have solver settings. /// public interface ISolverSettings { /// /// Gets the solver settings for this constraint. /// SolverSettings SolverSettings { get; } } } ================================================ FILE: BEPUphysics/Constraints/ISpringConstraint.cs ================================================ namespace BEPUphysics.Constraints { /// /// Implemented by constraints that support springlike behavior. /// public interface ISpringSettings { /// /// Gets the spring settings used by the constraint. /// SpringSettings SpringSettings { get; } } } ================================================ FILE: BEPUphysics/Constraints/IXDImpulseConstraint.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Implemented by solver updateables which have a one dimensional impulse. /// public interface I1DImpulseConstraint { /// /// Gets the current relative velocity of the constraint. /// Computed based on the current connection velocities and jacobians. /// float RelativeVelocity { get; } /// /// Gets the total impulse a constraint has applied. /// float TotalImpulse { get; } } /// /// Implemented by solver updateables which have a one dimensional impulse. /// public interface I1DImpulseConstraintWithError : I1DImpulseConstraint { /// /// Gets the current constraint error. /// float Error { get; } } /// /// Implemented by solver updateables which have a two dimensional impulse. /// public interface I2DImpulseConstraint { /// /// Gets the current relative velocity of the constraint. /// Computed based on the current connection velocities and jacobians. /// Vector2 RelativeVelocity { get; } /// /// Gets the total impulse a constraint has applied. /// Vector2 TotalImpulse { get; } } /// /// Implemented by solver updateables which have a two dimensional impulse. /// public interface I2DImpulseConstraintWithError : I2DImpulseConstraint { /// /// Gets the current constraint error. /// Vector2 Error { get; } } /// /// Implemented by solver updateables which have a three dimensional impulse. /// public interface I3DImpulseConstraint { /// /// Gets the current relative velocity of the constraint. /// Computed based on the current connection velocities and jacobians. /// Vector3 RelativeVelocity { get; } /// /// Gets the total impulse a constraint has applied. /// Vector3 TotalImpulse { get; } } /// /// Implemented by solver updateables which have a three dimensional impulse. /// public interface I3DImpulseConstraintWithError : I3DImpulseConstraint { /// /// Gets the current constraint error. /// Vector3 Error { get; } } } ================================================ FILE: BEPUphysics/Constraints/JointTransform.cs ================================================ using System; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Defines a three dimensional orthonormal basis used by a constraint. /// public class JointBasis3D { internal Vector3 localPrimaryAxis = Vector3.Backward; internal Vector3 localXAxis = Vector3.Right; internal Vector3 localYAxis = Vector3.Up; internal Vector3 primaryAxis = Vector3.Backward; internal Matrix3x3 rotationMatrix = Matrix3x3.Identity; internal Vector3 xAxis = Vector3.Right; internal Vector3 yAxis = Vector3.Up; /// /// Gets the primary axis of the transform in local space. /// public Vector3 LocalPrimaryAxis { get { return localPrimaryAxis; } } /// /// Gets or sets the local transform of the basis. /// public Matrix3x3 LocalTransform { get { var toReturn = new Matrix3x3 {Right = localXAxis, Up = localYAxis, Backward = localPrimaryAxis}; return toReturn; } set { SetLocalAxes(value); } } /// /// Gets the X axis of the transform in local space. /// public Vector3 LocalXAxis { get { return localXAxis; } } /// /// Gets the Y axis of the transform in local space. /// public Vector3 LocalYAxis { get { return localYAxis; } } /// /// Gets the primary axis of the transform. /// public Vector3 PrimaryAxis { get { return primaryAxis; } } /// /// Gets or sets the rotation matrix used by the joint transform to convert local space axes to world space. /// public Matrix3x3 RotationMatrix { get { return rotationMatrix; } set { rotationMatrix = value; ComputeWorldSpaceAxes(); } } /// /// Gets or sets the world transform of the basis. /// public Matrix3x3 WorldTransform { get { var toReturn = new Matrix3x3 {Right = xAxis, Up = yAxis, Backward = primaryAxis}; return toReturn; } set { SetWorldAxes(value); } } /// /// Gets the X axis of the transform. /// public Vector3 XAxis { get { return xAxis; } } /// /// Gets the Y axis of the transform. /// public Vector3 YAxis { get { return yAxis; } } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Third axis in the transform. /// Matrix to use to transform the local axes into world space. public void SetLocalAxes(Vector3 primaryAxis, Vector3 xAxis, Vector3 yAxis, Matrix3x3 rotationMatrix) { this.rotationMatrix = rotationMatrix; SetLocalAxes(primaryAxis, xAxis, yAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Third axis in the transform. public void SetLocalAxes(Vector3 primaryAxis, Vector3 xAxis, Vector3 yAxis) { if (Math.Abs(Vector3.Dot(primaryAxis, xAxis)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(primaryAxis, yAxis)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(xAxis, yAxis)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two."); localPrimaryAxis = Vector3.Normalize(primaryAxis); localXAxis = Vector3.Normalize(xAxis); localYAxis = Vector3.Normalize(yAxis); ComputeWorldSpaceAxes(); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis. /// The matrix's up vector is used as the y axis. public void SetLocalAxes(Matrix3x3 matrix) { if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(matrix.Backward, matrix.Up)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(matrix.Right, matrix.Up)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two."); localPrimaryAxis = Vector3.Normalize(matrix.Backward); localXAxis = Vector3.Normalize(matrix.Right); localYAxis = Vector3.Normalize(matrix.Up); ComputeWorldSpaceAxes(); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Third axis in the transform. /// Matrix to use to transform the local axes into world space. public void SetWorldAxes(Vector3 primaryAxis, Vector3 xAxis, Vector3 yAxis, Matrix3x3 rotationMatrix) { this.rotationMatrix = rotationMatrix; SetWorldAxes(primaryAxis, xAxis, yAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Third axis in the transform. public void SetWorldAxes(Vector3 primaryAxis, Vector3 xAxis, Vector3 yAxis) { if (Math.Abs(Vector3.Dot(primaryAxis, xAxis)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(primaryAxis, yAxis)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(xAxis, yAxis)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two."); this.primaryAxis = Vector3.Normalize(primaryAxis); this.xAxis = Vector3.Normalize(xAxis); this.yAxis = Vector3.Normalize(yAxis); Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis); Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis); Matrix3x3.TransformTranspose(ref this.yAxis, ref rotationMatrix, out localYAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis. /// The matrix's up vector is used as the y axis. public void SetWorldAxes(Matrix3x3 matrix) { if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(matrix.Backward, matrix.Up)) > Toolbox.BigEpsilon || Math.Abs(Vector3.Dot(matrix.Right, matrix.Up)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform do not form an orthonormal basis. Ensure that each axis is perpendicular to the other two."); primaryAxis = Vector3.Normalize(matrix.Backward); xAxis = Vector3.Normalize(matrix.Right); yAxis = Vector3.Normalize(matrix.Up); Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis); Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis); Matrix3x3.TransformTranspose(ref this.yAxis, ref rotationMatrix, out localYAxis); } internal void ComputeWorldSpaceAxes() { Matrix3x3.Transform(ref localPrimaryAxis, ref rotationMatrix, out primaryAxis); Matrix3x3.Transform(ref localXAxis, ref rotationMatrix, out xAxis); Matrix3x3.Transform(ref localYAxis, ref rotationMatrix, out yAxis); } } /// /// Defines a two axes which are perpendicular to each other used by a constraint. /// public class JointBasis2D { internal Vector3 localPrimaryAxis = Vector3.Backward; internal Vector3 localXAxis = Vector3.Right; internal Vector3 primaryAxis = Vector3.Backward; internal Matrix3x3 rotationMatrix = Matrix3x3.Identity; internal Vector3 xAxis = Vector3.Right; /// /// Gets the primary axis of the transform in local space. /// public Vector3 LocalPrimaryAxis { get { return localPrimaryAxis; } } /// /// Gets the X axis of the transform in local space. /// public Vector3 LocalXAxis { get { return localXAxis; } } /// /// Gets the primary axis of the transform. /// public Vector3 PrimaryAxis { get { return primaryAxis; } } /// /// Gets or sets the rotation matrix used by the joint transform to convert local space axes to world space. /// public Matrix3x3 RotationMatrix { get { return rotationMatrix; } set { rotationMatrix = value; ComputeWorldSpaceAxes(); } } /// /// Gets the X axis of the transform. /// public Vector3 XAxis { get { return xAxis; } } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Matrix to use to transform the local axes into world space. public void SetLocalAxes(Vector3 primaryAxis, Vector3 xAxis, Matrix3x3 rotationMatrix) { this.rotationMatrix = rotationMatrix; SetLocalAxes(primaryAxis, xAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. public void SetLocalAxes(Vector3 primaryAxis, Vector3 xAxis) { if (Math.Abs(Vector3.Dot(primaryAxis, xAxis)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); localPrimaryAxis = Vector3.Normalize(primaryAxis); localXAxis = Vector3.Normalize(xAxis); ComputeWorldSpaceAxes(); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis. public void SetLocalAxes(Matrix3x3 matrix) { if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); localPrimaryAxis = Vector3.Normalize(matrix.Backward); localXAxis = Vector3.Normalize(matrix.Right); ComputeWorldSpaceAxes(); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. /// Matrix to use to transform the local axes into world space. public void SetWorldAxes(Vector3 primaryAxis, Vector3 xAxis, Matrix3x3 rotationMatrix) { this.rotationMatrix = rotationMatrix; SetWorldAxes(primaryAxis, xAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// First axis in the transform. Usually aligned along the main axis of a joint, like the twist axis of a TwistLimit. /// Second axis in the transform. public void SetWorldAxes(Vector3 primaryAxis, Vector3 xAxis) { if (Math.Abs(Vector3.Dot(primaryAxis, xAxis)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); this.primaryAxis = Vector3.Normalize(primaryAxis); this.xAxis = Vector3.Normalize(xAxis); Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis); Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis); } /// /// Sets up the axes of the transform and ensures that it is an orthonormal basis. /// /// Rotation matrix representing the three axes. /// The matrix's backward vector is used as the primary axis. /// The matrix's right vector is used as the x axis. public void SetWorldAxes(Matrix3x3 matrix) { if (Math.Abs(Vector3.Dot(matrix.Backward, matrix.Right)) > Toolbox.BigEpsilon) throw new ArgumentException("The axes provided to the joint transform are not perpendicular. Ensure that the specified axes form a valid constraint."); primaryAxis = Vector3.Normalize(matrix.Backward); xAxis = Vector3.Normalize(matrix.Right); Matrix3x3.TransformTranspose(ref this.primaryAxis, ref rotationMatrix, out localPrimaryAxis); Matrix3x3.TransformTranspose(ref this.xAxis, ref rotationMatrix, out localXAxis); } internal void ComputeWorldSpaceAxes() { Matrix3x3.Transform(ref localPrimaryAxis, ref rotationMatrix, out primaryAxis); Matrix3x3.Transform(ref localXAxis, ref rotationMatrix, out xAxis); } } } ================================================ FILE: BEPUphysics/Constraints/SingleEntity/MaximumAngularVelocityConstraint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SingleEntity { /// /// Prevents the target entity from moving faster than the specified speeds. /// public class MaximumAngularSpeedConstraint : SingleEntityConstraint, I3DImpulseConstraint { private Matrix3x3 effectiveMassMatrix; private float maxForceDt = float.MaxValue; private float maxForceDtSquared = float.MaxValue; private Vector3 accumulatedImpulse; private float maximumForce = float.MaxValue; private float maximumSpeed; private float maximumSpeedSquared; private float softness = .00001f; private float usedSoftness; /// /// Constructs a maximum speed constraint. /// Set its Entity and MaximumSpeed to complete the configuration. /// IsActive also starts as false with this constructor. /// public MaximumAngularSpeedConstraint() { IsActive = false; } /// /// Constructs a maximum speed constraint. /// /// Affected entity. /// Maximum angular speed allowed. public MaximumAngularSpeedConstraint(Entity e, float maxSpeed) { Entity = e; MaximumSpeed = maxSpeed; } /// /// Gets and sets the maximum impulse that the constraint will attempt to apply when satisfying its requirements. /// This field can be used to simulate friction in a constraint. /// public float MaximumForce { get { if (maximumForce > 0) { return maximumForce; } return 0; } set { maximumForce = value >= 0 ? value : 0; } } /// /// Gets or sets the maximum angular speed that this constraint allows. /// public float MaximumSpeed { get { return maximumSpeed; } set { maximumSpeed = MathHelper.Max(0, value); maximumSpeedSquared = maximumSpeed * maximumSpeed; } } /// /// Gets and sets the softness of this constraint. /// Higher values of softness allow the constraint to be violated more. /// Must be greater than zero. /// Sometimes, if a joint system is unstable, increasing the softness of the involved constraints will make it settle down. /// For motors, softness can be used to implement damping. For a damping constant k, the appropriate softness is 1/k. /// public float Softness { get { return softness; } set { softness = Math.Max(0, value); } } #region I3DImpulseConstraint Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// Vector3 I3DImpulseConstraint.RelativeVelocity { get { return entity.angularVelocity; } } /// /// Gets the total impulse applied by the constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } #endregion /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { float angularSpeed = entity.angularVelocity.LengthSquared(); if (angularSpeed > maximumSpeedSquared) { angularSpeed = (float)Math.Sqrt(angularSpeed); Vector3 impulse; //divide by angularSpeed to normalize the velocity. //Multiply by angularSpeed - maximumSpeed to get the 'velocity change vector.' Vector3.Multiply(ref entity.angularVelocity, -(angularSpeed - maximumSpeed) / angularSpeed, out impulse); //incorporate softness Vector3 softnessImpulse; Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse); Vector3.Subtract(ref impulse, ref softnessImpulse, out impulse); //Transform into impulse Matrix3x3.Transform(ref impulse, ref effectiveMassMatrix, out impulse); //Accumulate Vector3 previousAccumulatedImpulse = accumulatedImpulse; Vector3.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse); float forceMagnitude = accumulatedImpulse.LengthSquared(); if (forceMagnitude > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. float multiplier = maxForceDt / (float)Math.Sqrt(forceMagnitude); accumulatedImpulse.X *= multiplier; accumulatedImpulse.Y *= multiplier; accumulatedImpulse.Z *= multiplier; //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z; } entity.ApplyAngularImpulse(ref impulse); return (Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z)); } return 0; } /// /// Calculates necessary information for velocity solving. /// Called automatically by space. /// /// Time in seconds since the last update. public override void Update(float dt) { usedSoftness = softness / dt; effectiveMassMatrix = entity.inertiaTensorInverse; effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); //Determine maximum force if (maximumForce < float.MaxValue) { maxForceDt = maximumForce * dt; maxForceDtSquared = maxForceDt * maxForceDt; } else { maxForceDt = float.MaxValue; maxForceDtSquared = float.MaxValue; } } public override void ExclusiveUpdate() { //Can't do warmstarting due to the strangeness of this constraint (not based on a position error, nor is it really a motor). accumulatedImpulse = Toolbox.ZeroVector; } } } ================================================ FILE: BEPUphysics/Constraints/SingleEntity/MaximumLinearVelocityConstraint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SingleEntity { /// /// Prevents the target entity from moving faster than the specified speeds. /// public class MaximumLinearSpeedConstraint : SingleEntityConstraint, I3DImpulseConstraint { private float effectiveMassMatrix; private float maxForceDt = float.MaxValue; private float maxForceDtSquared = float.MaxValue; private Vector3 accumulatedImpulse; private float maximumForce = float.MaxValue; private float maximumSpeed; private float maximumSpeedSquared; private float softness = .00001f; private float usedSoftness; /// /// Constructs a maximum speed constraint. /// Set its Entity and MaximumSpeed to complete the configuration. /// IsActive also starts as false with this constructor. /// public MaximumLinearSpeedConstraint() { IsActive = false; } /// /// Constructs a maximum speed constraint. /// /// Affected entity. /// Maximum linear speed allowed. public MaximumLinearSpeedConstraint(Entity e, float maxSpeed) { Entity = e; MaximumSpeed = maxSpeed; } /// /// Gets and sets the maximum impulse that the constraint will attempt to apply when satisfying its requirements. /// This field can be used to simulate friction in a constraint. /// public float MaximumForce { get { if (maximumForce > 0) { return maximumForce; } return 0; } set { maximumForce = value >= 0 ? value : 0; } } /// /// Gets or sets the maximum linear speed that this constraint allows. /// public float MaximumSpeed { get { return maximumSpeed; } set { maximumSpeed = MathHelper.Max(0, value); maximumSpeedSquared = maximumSpeed * maximumSpeed; } } /// /// Gets and sets the softness of this constraint. /// Higher values of softness allow the constraint to be violated more. /// Must be greater than zero. /// Sometimes, if a joint system is unstable, increasing the softness of the involved constraints will make it settle down. /// For motors, softness can be used to implement damping. For a damping constant k, the appropriate softness is 1/k. /// public float Softness { get { return softness; } set { softness = Math.Max(0, value); } } #region I3DImpulseConstraint Members /// /// Gets the current relative velocity with respect to the constraint. /// For a single entity constraint, this is pretty straightforward as the /// velocity of the entity. /// Vector3 I3DImpulseConstraint.RelativeVelocity { get { return Entity.LinearVelocity; } } /// /// Gets the total impulse applied by the constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } #endregion /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { float linearSpeed = entity.linearVelocity.LengthSquared(); if (linearSpeed > maximumSpeedSquared) { linearSpeed = (float) Math.Sqrt(linearSpeed); Vector3 impulse; //divide by linearSpeed to normalize the velocity. //Multiply by linearSpeed - maximumSpeed to get the 'velocity change vector.' Vector3.Multiply(ref entity.linearVelocity, -(linearSpeed - maximumSpeed) / linearSpeed, out impulse); //incorporate softness Vector3 softnessImpulse; Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessImpulse); Vector3.Subtract(ref impulse, ref softnessImpulse, out impulse); //Transform into impulse Vector3.Multiply(ref impulse, effectiveMassMatrix, out impulse); //Accumulate Vector3 previousAccumulatedImpulse = accumulatedImpulse; Vector3.Add(ref accumulatedImpulse, ref impulse, out accumulatedImpulse); float forceMagnitude = accumulatedImpulse.LengthSquared(); if (forceMagnitude > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. float multiplier = maxForceDt / (float) Math.Sqrt(forceMagnitude); accumulatedImpulse.X *= multiplier; accumulatedImpulse.Y *= multiplier; accumulatedImpulse.Z *= multiplier; //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. impulse.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; impulse.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; impulse.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z; } entity.ApplyLinearImpulse(ref impulse); return (Math.Abs(impulse.X) + Math.Abs(impulse.Y) + Math.Abs(impulse.Z)); } return 0; } /// /// Calculates necessary information for velocity solving. /// Called automatically by space. /// /// Time in seconds since the last update. public override void Update(float dt) { usedSoftness = softness / dt; effectiveMassMatrix = 1 / (entity.inverseMass + usedSoftness); //Determine maximum force if (maximumForce < float.MaxValue) { maxForceDt = maximumForce * dt; maxForceDtSquared = maxForceDt * maxForceDt; } else { maxForceDt = float.MaxValue; maxForceDtSquared = float.MaxValue; } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Can't do warmstarting due to the strangeness of this constraint (not based on a position error, nor is it really a motor). accumulatedImpulse = Toolbox.ZeroVector; } } } ================================================ FILE: BEPUphysics/Constraints/SingleEntity/SingleEntityAngularMotor.cs ================================================ using System; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SingleEntity { /// /// Constraint which attempts to restrict the relative angular velocity of two entities to some value. /// Can use a target relative orientation to apply additional force. /// public class SingleEntityAngularMotor : SingleEntityConstraint, I3DImpulseConstraintWithError { private readonly JointBasis3D basis = new JointBasis3D(); private readonly MotorSettingsOrientation settings; private Vector3 accumulatedImpulse; private float angle; private Vector3 axis; private Vector3 biasVelocity; private Matrix3x3 effectiveMassMatrix; private float maxForceDt; private float maxForceDtSquared; private float usedSoftness; /// /// Constructs a new constraint which attempts to restrict the relative angular velocity of two entities to some value. /// /// Affected entity. public SingleEntityAngularMotor(Entity entity) { Entity = entity; settings = new MotorSettingsOrientation(this) {servo = {goal = base.entity.orientation}}; //Since no target relative orientation was specified, just use the current relative orientation. Prevents any nasty start-of-sim 'snapping.' //mySettings.myServo.springSettings.stiffnessConstant *= .5f; } /// /// Constructs a new constraint which attempts to restrict the relative angular velocity of two entities to some value. /// This constructor will make the angular motor start with isActive set to false. /// public SingleEntityAngularMotor() { settings = new MotorSettingsOrientation(this); IsActive = false; } /// /// Gets the basis attached to the entity. /// The target velocity/orientation of this motor is transformed by the basis. /// public JointBasis3D Basis { get { return basis; } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettingsOrientation Settings { get { return settings; } } #region I3DImpulseConstraintWithError Members /// /// Gets the current relative velocity with respect to the constraint. /// For single entity constraints, this is pretty straightforward. It is taken directly from the /// entity. /// public Vector3 RelativeVelocity { get { return -Entity.AngularVelocity; } } /// /// Gets the total impulse applied by this constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, error is zero. /// public Vector3 Error { get { return axis * angle; } } #endregion /// /// Applies the corrective impulses required by the constraint. /// public override float SolveIteration() { #if !WINDOWS Vector3 lambda = new Vector3(); #else Vector3 lambda; #endif Vector3 aVel = entity.angularVelocity; lambda.X = -aVel.X + biasVelocity.X - usedSoftness * accumulatedImpulse.X; lambda.Y = -aVel.Y + biasVelocity.Y - usedSoftness * accumulatedImpulse.Y; lambda.Z = -aVel.Z + biasVelocity.Z - usedSoftness * accumulatedImpulse.Z; Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector3 previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse.X += lambda.X; accumulatedImpulse.Y += lambda.Y; accumulatedImpulse.Z += lambda.Z; float sumLengthSquared = accumulatedImpulse.LengthSquared(); if (sumLengthSquared > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. float multiplier = maxForceDt / (float) Math.Sqrt(sumLengthSquared); accumulatedImpulse.X *= multiplier; accumulatedImpulse.Y *= multiplier; accumulatedImpulse.Z *= multiplier; //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; lambda.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z; } entity.ApplyAngularImpulse(ref lambda); return Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z); } /// /// Initializes the constraint for the current frame. /// /// Time between frames. public override void Update(float dt) { basis.rotationMatrix = entity.orientationMatrix; basis.ComputeWorldSpaceAxes(); if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo. { Quaternion currentRelativeOrientation; Matrix worldTransform = Matrix3x3.ToMatrix4X4(basis.WorldTransform); Quaternion.CreateFromRotationMatrix(ref worldTransform, out currentRelativeOrientation); //Compute the relative orientation R' between R and the target relative orientation. Quaternion errorOrientation; Quaternion.Conjugate(ref currentRelativeOrientation, out errorOrientation); Quaternion.Multiply(ref settings.servo.goal, ref errorOrientation, out errorOrientation); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); //Turn this into an axis-angle representation. Toolbox.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle); //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon). if (angle > Toolbox.BigEpsilon) { float velocity = MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction; biasVelocity.X = axis.X * velocity; biasVelocity.Y = axis.Y * velocity; biasVelocity.Z = axis.Z * velocity; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float) Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { //Wouldn't want an old frame's bias velocity to sneak in. biasVelocity = new Vector3(); } } else { usedSoftness = settings.velocityMotor.softness / dt; angle = 0; //Zero out the error; Matrix3x3 transform = basis.WorldTransform; Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity); } //Compute effective mass effectiveMassMatrix = entity.inertiaTensorInverse; effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Apply accumulated impulse entity.ApplyAngularImpulse(ref accumulatedImpulse); } /// /// Computes the maxForceDt and maxForceDtSquared fields. /// private void ComputeMaxForces(float maxForce, float dt) { //Determine maximum force if (maxForce < float.MaxValue) { maxForceDt = maxForce * dt; maxForceDtSquared = maxForceDt * maxForceDt; } else { maxForceDt = float.MaxValue; maxForceDtSquared = float.MaxValue; } } } } ================================================ FILE: BEPUphysics/Constraints/SingleEntity/SingleEntityConstraint.cs ================================================ using BEPUphysics.Entities; using BEPUutilities.DataStructures; namespace BEPUphysics.Constraints.SingleEntity { /// /// Abstract superclass of constraints which control a single entity. /// public abstract class SingleEntityConstraint : EntitySolverUpdateable { /// /// Number of frames so far at effectively zero corrective impulse. /// Set to zero during every preStep(float dt) call and incremented by checkForEarlyOutIterations(Vector3 impulse). /// protected int iterationsAtZeroImpulse; /// /// Entity affected by the constraint. /// protected internal Entity entity; /// /// Gets or sets the entity affected by the constraint. /// public virtual Entity Entity { get { return entity; } set { //TODO: Should this clear accumulated impulses? //For constraints too... entity = value; OnInvolvedEntitiesChanged(); } } /// /// Adds entities associated with the solver item to the involved entities list. /// Ensure that sortInvolvedEntities() is called at the end of the function. /// This allows the non-batched multithreading system to lock properly. /// protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { if (entity != null) //sometimes, the entity is set to null to 'deactivate' it. Don't add null to the involved entities list. outputInvolvedEntities.Add(entity); } } } ================================================ FILE: BEPUphysics/Constraints/SingleEntity/SingleEntityLinearMotor.cs ================================================ using System; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SingleEntity { /// /// Constraint which tries to push an entity to a desired location. /// public class SingleEntityLinearMotor : SingleEntityConstraint, I3DImpulseConstraintWithError { private readonly MotorSettings3D settings; /// /// Sum of forces applied to the constraint in the past. /// private Vector3 accumulatedImpulse = Vector3.Zero; private Vector3 biasVelocity; private Matrix3x3 effectiveMassMatrix; /// /// Maximum impulse that can be applied in a single frame. /// private float maxForceDt; /// /// Maximum impulse that can be applied in a single frame, squared. /// This is computed in the prestep to avoid doing extra multiplies in the more-often called applyImpulse method. /// private float maxForceDtSquared; private Vector3 error; private Vector3 localPoint; private Vector3 worldPoint; private Vector3 r; private float usedSoftness; /// /// Gets or sets the entity affected by the constraint. /// public override Entity Entity { get { return base.Entity; } set { if (Entity != value) accumulatedImpulse = new Vector3(); base.Entity = value; } } /// /// Constructs a new single body linear motor. This motor will try to move a single entity to a goal velocity or to a goal position. /// /// Entity to affect. /// Point in world space attached to the entity that will be motorized. public SingleEntityLinearMotor(Entity entity, Vector3 point) { Entity = entity; Point = point; settings = new MotorSettings3D(this) {servo = {goal = point}}; //Not really necessary, just helps prevent 'snapping'. } /// /// Constructs a new single body linear motor. This motor will try to move a single entity to a goal velocity or to a goal position. /// This constructor will start the motor with isActive = false. /// public SingleEntityLinearMotor() { settings = new MotorSettings3D(this); IsActive = false; } /// /// Point attached to the entity in its local space that is motorized. /// public Vector3 LocalPoint { get { return localPoint; } set { localPoint = value; Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out worldPoint); Vector3.Add(ref worldPoint, ref entity.position, out worldPoint); } } /// /// Point attached to the entity in world space that is motorized. /// public Vector3 Point { get { return worldPoint; } set { worldPoint = value; Vector3.Subtract(ref worldPoint, ref entity.position, out localPoint); Matrix3x3.TransformTranspose(ref localPoint, ref entity.orientationMatrix, out localPoint); } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettings3D Settings { get { return settings; } } #region I3DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector3 RelativeVelocity { get { Vector3 lambda; Vector3.Cross(ref r, ref entity.angularVelocity, out lambda); Vector3.Subtract(ref lambda, ref entity.linearVelocity, out lambda); return lambda; } } /// /// Gets the total impulse applied by this constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, error is zero. /// public Vector3 Error { get { return error; } } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //Compute relative velocity Vector3 lambda; Vector3.Cross(ref r, ref entity.angularVelocity, out lambda); Vector3.Subtract(ref lambda, ref entity.linearVelocity, out lambda); //Add in bias velocity Vector3.Add(ref biasVelocity, ref lambda, out lambda); //Add in softness Vector3 softnessVelocity; Vector3.Multiply(ref accumulatedImpulse, usedSoftness, out softnessVelocity); Vector3.Subtract(ref lambda, ref softnessVelocity, out lambda); //In terms of an impulse (an instantaneous change in momentum), what is it? Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); //Sum the impulse. Vector3 previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse += lambda; //If the impulse it takes to get to the goal is too high for the motor to handle, scale it back. float sumImpulseLengthSquared = accumulatedImpulse.LengthSquared(); if (sumImpulseLengthSquared > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. accumulatedImpulse *= maxForceDt / (float)Math.Sqrt(sumImpulseLengthSquared); //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. lambda = accumulatedImpulse - previousAccumulatedImpulse; } entity.ApplyLinearImpulse(ref lambda); Vector3 taImpulse; Vector3.Cross(ref r, ref lambda, out taImpulse); entity.ApplyAngularImpulse(ref taImpulse); return (Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Transform point into world space. Matrix3x3.Transform(ref localPoint, ref entity.orientationMatrix, out r); Vector3.Add(ref r, ref entity.position, out worldPoint); if (settings.mode == MotorMode.Servomechanism) { Vector3.Subtract(ref settings.servo.goal, ref worldPoint, out error); float separationDistance = error.Length(); if (separationDistance > Toolbox.BigEpsilon) { float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); //The rate of correction can be based on a constant correction velocity as well as a 'spring like' correction velocity. //The constant correction velocity could overshoot the destination, so clamp it. float correctionSpeed = MathHelper.Min(settings.servo.baseCorrectiveSpeed, separationDistance / dt) + separationDistance * errorReduction; Vector3.Multiply(ref error, correctionSpeed / separationDistance, out biasVelocity); //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { //Wouldn't want to use a bias from an earlier frame. biasVelocity = new Vector3(); } } else { usedSoftness = settings.velocityMotor.softness / dt; biasVelocity = settings.velocityMotor.goalVelocity; error = Vector3.Zero; } //Compute the maximum force that can be applied this frame. ComputeMaxForces(settings.maximumForce, dt); //COMPUTE EFFECTIVE MASS MATRIX //Transforms a change in velocity to a change in momentum when multiplied. Matrix3x3 linearComponent; Matrix3x3.CreateScale(entity.inverseMass, out linearComponent); Matrix3x3 rACrossProduct; Matrix3x3.CreateCrossProduct(ref r, out rACrossProduct); Matrix3x3 angularComponentA; Matrix3x3.Multiply(ref rACrossProduct, ref entity.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out effectiveMassMatrix); effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //"Warm start" the constraint by applying a first guess of the solution should be. entity.ApplyLinearImpulse(ref accumulatedImpulse); Vector3 taImpulse; Vector3.Cross(ref r, ref accumulatedImpulse, out taImpulse); entity.ApplyAngularImpulse(ref taImpulse); } /// /// Computes the maxForceDt and maxForceDtSquared fields. /// private void ComputeMaxForces(float maxForce, float dt) { //Determine maximum force if (maxForce < float.MaxValue) { maxForceDt = maxForce * dt; maxForceDtSquared = maxForceDt * maxForceDt; } else { maxForceDt = float.MaxValue; maxForceDtSquared = float.MaxValue; } } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/CustomizableSolverGroup.cs ================================================ namespace BEPUphysics.Constraints.SolverGroups { /// /// Constraint made from other constraints. /// Putting constraints into a solver group can help with organization and, in some cases, performance. /// /// If you have multiple constraints between the same two entities, putting the constraints into a /// CustomizableSolverGroup can lower lock contention. /// /// Be careful about overloading a single solvergroup; it should be kept relatively small to ensure that the multithreading loads stay balanced. /// public class CustomizableSolverGroup : SolverGroup { /// /// Adds a new solver updateable to the solver group. /// /// Solver updateable to add. public new void Add(EntitySolverUpdateable solverUpdateable) { base.Add(solverUpdateable); } /// /// Removes a solver updateable from the solver group. /// /// Solver updateable to remove. public new void Remove(EntitySolverUpdateable solverUpdateable) { base.Remove(solverUpdateable); } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/LineSliderJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts two degrees of linear motion while allowing one degree of angular freedom. /// public class LineSliderJoint : SolverGroup { /// /// Constructs a new constraint which restricts two degrees of linear freedom and two degrees of angular freedom between two entities. /// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner, /// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false). /// public LineSliderJoint() { IsActive = false; PointOnLineJoint = new PointOnLineJoint(); AngularJoint = new RevoluteAngularJoint(); Limit = new LinearAxisLimit(); Motor = new LinearAxisMotor(); Add(PointOnLineJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Constructs a new constraint which restricts two degrees of linear freedom and two degrees of angular freedom between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Location of the anchor for the line to be attached to connectionA in world space. /// Axis in world space to be attached to connectionA along which connectionB can move and rotate. /// Location of the anchor for the point to be attached to connectionB in world space. public LineSliderJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointAnchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor); AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, lineDirection); Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0); Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection); Limit.IsActive = false; Motor.IsActive = false; Add(PointOnLineJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Gets the angular joint which removes two degrees of freedom. /// public RevoluteAngularJoint AngularJoint { get; private set; } /// /// Gets the distance limits for the slider. /// public LinearAxisLimit Limit { get; private set; } /// /// Gets the slider motor. /// public LinearAxisMotor Motor { get; private set; } /// /// Gets the line joint that restricts two linear degrees of freedom. /// public PointOnLineJoint PointOnLineJoint { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/PlaneSliderJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts one linear degree of freedom. /// public class PlaneSliderJoint : SolverGroup { /// /// Constructs a new constraint which restricts one linear degree of 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). /// public PlaneSliderJoint() { IsActive = false; PointOnPlaneJoint = new PointOnPlaneJoint(); LimitX = new LinearAxisLimit(); MotorX = new LinearAxisMotor(); LimitY = new LinearAxisLimit(); MotorY = new LinearAxisMotor(); Add(PointOnPlaneJoint); Add(LimitX); Add(MotorX); Add(LimitY); Add(MotorY); } /// /// Constructs a new constraint which restricts one linear degree of freedom between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Location of the anchor for the plane to be attached to connectionA in world space. /// Normal of the plane constraint in world space. /// Direction in world space along which the X axis LinearAxisLimit and LinearAxisMotor work. /// This is usually chosen to be perpendicular to the planeNormal and the yAxis. /// Direction in world space along which the Y axis LinearAxisLimit and LinearAxisMotor work. /// This is usually chosen to be perpendicular to the planeNormal and the xAxis. /// Location of the anchor for the point to be attached to connectionB in world space. public PlaneSliderJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 planeNormal, Vector3 xAxis, Vector3 yAxis, Vector3 pointAnchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; PointOnPlaneJoint = new PointOnPlaneJoint(connectionA, connectionB, planeAnchor, planeNormal, pointAnchor); LimitX = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, xAxis, 0, 0); MotorX = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, xAxis); LimitY = new LinearAxisLimit(connectionA, connectionB, planeAnchor, pointAnchor, yAxis, 0, 0); MotorY = new LinearAxisMotor(connectionA, connectionB, planeAnchor, pointAnchor, yAxis); LimitX.IsActive = false; MotorX.IsActive = false; LimitY.IsActive = false; MotorY.IsActive = false; Add(PointOnPlaneJoint); Add(LimitX); Add(MotorX); Add(LimitY); Add(MotorY); } /// /// Gets the distance limit for the slider along plane's X axis. /// public LinearAxisLimit LimitX { get; private set; } /// /// Gets the distance limit for the slider along plane's Y axis. /// public LinearAxisLimit LimitY { get; private set; } /// /// Gets the slider motor for the plane's X axis. /// public LinearAxisMotor MotorX { get; private set; } /// /// Gets the slider motor for the plane's Y axis. /// public LinearAxisMotor MotorY { get; private set; } /// /// Gets the plane joint that restricts one linear degree of freedom. /// public PointOnPlaneJoint PointOnPlaneJoint { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/PrismaticJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts two degrees of linear freedom and all three degrees of angular freedom. /// public class PrismaticJoint : SolverGroup { /// /// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom. /// This constructs the internal constraints, but does not configure them. Before using a constraint constructed in this manner, /// ensure that its active constituent constraints are properly configured. The entire group as well as all internal constraints are initially inactive (IsActive = false). /// public PrismaticJoint() { IsActive = false; PointOnLineJoint = new PointOnLineJoint(); AngularJoint = new NoRotationJoint(); Limit = new LinearAxisLimit(); Motor = new LinearAxisMotor(); Add(PointOnLineJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Constructs a new constraint which restricts two degrees of linear freedom and all three degrees of angular freedom. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Location of the anchor for the line to be attached to connectionA in world space. /// Axis in world space to be attached to connectionA along which connectionB can move. /// Location of the anchor for the point to be attached to connectionB in world space. public PrismaticJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointAnchor) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; PointOnLineJoint = new PointOnLineJoint(connectionA, connectionB, lineAnchor, lineDirection, pointAnchor); AngularJoint = new NoRotationJoint(connectionA, connectionB); Limit = new LinearAxisLimit(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection, 0, 0); Motor = new LinearAxisMotor(connectionA, connectionB, lineAnchor, pointAnchor, lineDirection); Limit.IsActive = false; Motor.IsActive = false; Add(PointOnLineJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Gets the angular joint which removes three degrees of freedom. /// public NoRotationJoint AngularJoint { get; private set; } /// /// Gets the distance limits for the slider. /// public LinearAxisLimit Limit { get; private set; } /// /// Gets the slider motor. /// public LinearAxisMotor Motor { get; private set; } /// /// Gets the line joint that restricts two linear degrees of freedom. /// public PointOnLineJoint PointOnLineJoint { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/RevoluteJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts linear motion while allowing one degree of angular freedom. /// Acts like a normal door hinge. /// public class RevoluteJoint : SolverGroup { /// /// 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). /// public RevoluteJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); AngularJoint = new RevoluteAngularJoint(); Limit = new RevoluteLimit(); Motor = new RevoluteMotor(); Add(BallSocketJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Constructs a new constraint which restricts three degrees of linear freedom and two degrees of angular freedom between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Point around which both entities rotate. /// Axis around which the hinge can rotate. public RevoluteJoint(Entity connectionA, Entity connectionB, Vector3 anchor, Vector3 freeAxis) { if (connectionA == null) connectionA = TwoEntityConstraint.WorldEntity; if (connectionB == null) connectionB = TwoEntityConstraint.WorldEntity; BallSocketJoint = new BallSocketJoint(connectionA, connectionB, anchor); AngularJoint = new RevoluteAngularJoint(connectionA, connectionB, freeAxis); Limit = new RevoluteLimit(connectionA, connectionB); Motor = new RevoluteMotor(connectionA, connectionB, freeAxis); Limit.IsActive = false; Motor.IsActive = false; //Ensure that the base and test direction is perpendicular to the free axis. Vector3 baseAxis = anchor - connectionA.position; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) //anchor and connection a in same spot, so try the other way. baseAxis = connectionB.position - anchor; baseAxis -= Vector3.Dot(baseAxis, freeAxis) * freeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = Vector3.Cross(freeAxis, Vector3.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = Vector3.Cross(freeAxis, Vector3.Right); } } Limit.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); Motor.Basis.SetWorldAxes(freeAxis, baseAxis, connectionA.orientationMatrix); baseAxis = connectionB.position - anchor; baseAxis -= Vector3.Dot(baseAxis, freeAxis) * freeAxis; if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { //However, if the free axis is totally aligned (like in an axis constraint), pick another reasonable direction. baseAxis = Vector3.Cross(freeAxis, Vector3.Up); if (baseAxis.LengthSquared() < Toolbox.BigEpsilon) { baseAxis = Vector3.Cross(freeAxis, Vector3.Right); } } Limit.TestAxis = baseAxis; Motor.TestAxis = baseAxis; Add(BallSocketJoint); Add(AngularJoint); Add(Limit); Add(Motor); } /// /// Gets the angular joint which removes two degrees of freedom. /// public RevoluteAngularJoint AngularJoint { get; private set; } /// /// Gets the ball socket joint that restricts linear degrees of freedom. /// public BallSocketJoint BallSocketJoint { get; private set; } /// /// Gets the rotational limit of the hinge. /// public RevoluteLimit Limit { get; private set; } /// /// Gets the motor of the hinge. /// public RevoluteMotor Motor { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/SolverGroup.cs ================================================ using System; using System.Collections.ObjectModel; using BEPUphysics.Entities; using BEPUutilities.DataStructures; using BEPUphysics.SolverSystems; namespace BEPUphysics.Constraints.SolverGroups { /// /// Superclass of constraints that are composed of multiple subconstraints. /// public abstract class SolverGroup : EntitySolverUpdateable { internal readonly RawList solverUpdateables = new RawList(); /// /// Gets the solver updateables managed by this solver group. /// public ReadOnlyList SolverUpdateables { get { return new ReadOnlyList(solverUpdateables); } } /// /// Collects the entities which are affected by the solver group and updates the internal listing. /// protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { foreach (EntitySolverUpdateable item in solverUpdateables) { for (int i = 0; i < item.involvedEntities.Count; i++) { if (!outputInvolvedEntities.Contains(item.involvedEntities.Elements[i])) { outputInvolvedEntities.Add(item.involvedEntities.Elements[i]); } } } } /// /// Sets the activity state of the constraint based on the activity state of its connections. /// Called automatically by the space owning a constaint. If a constraint is a sub-constraint that hasn't been directly added to the space, /// this may need to be called alongside the preStep from within the parent constraint. /// public override void UpdateSolverActivity() { if (isActive) { isActiveInSolver = false; for (int i = 0; i < solverUpdateables.Count; i++) { var item = solverUpdateables.Elements[i]; item.UpdateSolverActivity(); isActiveInSolver |= item.isActiveInSolver; } } else { isActiveInSolver = false; } } protected void UpdateUpdateable(EntitySolverUpdateable item, float dt) { item.SolverSettings.currentIterations = 0; item.SolverSettings.iterationsAtZeroImpulse = 0; if (item.isActiveInSolver) item.Update(dt); } protected void ExclusiveUpdateUpdateable(EntitySolverUpdateable item) { if (item.isActiveInSolver) item.ExclusiveUpdate(); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { for (int i = 0; i < solverUpdateables.Count; i++) { UpdateUpdateable(solverUpdateables.Elements[i], dt); } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { for (int i = 0; i < solverUpdateables.Count; i++) { ExclusiveUpdateUpdateable(solverUpdateables.Elements[i]); } } /// /// Solves a child updateable. Some children may override the group's update method; /// this avoids code repeat. /// /// /// protected void SolveUpdateable(EntitySolverUpdateable item, ref int activeConstraints) { if (item.isActiveInSolver) { SolverSettings subSolverSettings = item.solverSettings; subSolverSettings.currentIterations++; if (subSolverSettings.currentIterations <= solver.iterationLimit && subSolverSettings.currentIterations <= subSolverSettings.maximumIterationCount) { if (item.SolveIteration() < subSolverSettings.minimumImpulse) { subSolverSettings.iterationsAtZeroImpulse++; if (subSolverSettings.iterationsAtZeroImpulse > subSolverSettings.minimumIterationCount) item.isActiveInSolver = false; else { activeConstraints++; } } else { subSolverSettings.iterationsAtZeroImpulse = 0; activeConstraints++; } } else { item.isActiveInSolver = false; } } } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { int activeConstraints = 0; for (int i = 0; i < solverUpdateables.Count; i++) { SolveUpdateable(solverUpdateables.Elements[i], ref activeConstraints); } isActiveInSolver = activeConstraints > 0; return solverSettings.minimumImpulse + 1; //Never let the system deactivate due to low impulses; solver group takes care of itself. } /// /// Adds a solver updateable to the group. /// /// Solver updateable to add. /// Thrown when the SolverUpdateable to add to the SolverGroup already belongs to another SolverGroup or to a Space. protected void Add(EntitySolverUpdateable solverUpdateable) { if (solverUpdateable.solver == null) { if (solverUpdateable.SolverGroup == null) { solverUpdateables.Add(solverUpdateable); solverUpdateable.SolverGroup = this; solverUpdateable.Solver = solver; OnInvolvedEntitiesChanged(); } else { throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a SolverGroup."); } } else { throw new InvalidOperationException("Cannot add SolverUpdateable to SolverGroup; it already belongs to a solver."); } } /// /// Removes a solver updateable from the group. /// /// Solver updateable to remove. /// Thrown when the SolverUpdateable to remove from the SolverGroup doesn't actually belong to this SolverGroup. protected void Remove(EntitySolverUpdateable solverUpdateable) { if (solverUpdateable.SolverGroup == this) { solverUpdateables.Remove(solverUpdateable); solverUpdateable.SolverGroup = null; solverUpdateable.Solver = null; OnInvolvedEntitiesChanged(); } else { throw new InvalidOperationException("Cannot remove SolverUpdateable from SolverGroup; it doesn't belong to this SolverGroup."); } } /// /// Called after the object is added to a space. /// /// public override void OnAdditionToSpace(ISpace newSpace) { for (int i = 0; i < solverUpdateables.Count; i++) { solverUpdateables[i].OnAdditionToSpace(newSpace); } } /// /// Called before an object is removed from its space. /// public override void OnRemovalFromSpace(ISpace oldSpace) { for (int i = 0; i < solverUpdateables.Count; i++) { solverUpdateables[i].OnRemovalFromSpace(oldSpace); } } /// /// Gets the solver to which the solver updateable belongs. /// public override Solver Solver { get { return solver; } internal set { base.Solver = value; for (int i = 0; i < solverUpdateables.Count; i++) { solverUpdateables.Elements[i].Solver = value; } } } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/SwivelHingeJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts linear motion while allowing one degree of angular freedom. /// Acts like a tablet pc monitor hinge. /// public class SwivelHingeJoint : SolverGroup { /// /// 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). /// 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); } /// /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of angular freedom between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Point around which both entities rotate. /// Axis of allowed rotation in world space to be attached to connectionA. Will be kept perpendicular with the twist axis. 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); } /// /// Gets the angular joint which removes one degree of freedom. /// public SwivelHingeAngularJoint AngularJoint { get; private set; } /// /// Gets the ball socket joint that restricts linear degrees of freedom. /// public BallSocketJoint BallSocketJoint { get; private set; } /// /// Gets the rotational limit of the hinge. /// public RevoluteLimit HingeLimit { get; private set; } /// /// Gets the motor of the hinge. /// public RevoluteMotor HingeMotor { get; private set; } /// /// Gets the rotational limit of the swivel hinge. /// public TwistLimit TwistLimit { get; private set; } /// /// Gets the twist motor of the swivel hinge. /// public TwistMotor TwistMotor { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/UniversalJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.JointLimits; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts three degrees of linear motion and one degree of angular motion. /// Acts like two hinges in immediate sequence. /// public class UniversalJoint : SolverGroup { /// /// 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). /// public UniversalJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); TwistJoint = new TwistJoint(); Limit = new TwistLimit(); Motor = new TwistMotor(); Add(BallSocketJoint); Add(TwistJoint); Add(Limit); Add(Motor); } /// /// Constructs a new constraint which restricts three degrees of linear freedom and one degree of twisting angular freedom between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. /// Point around which both entities rotate in world space. 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); } /// /// Gets the ball socket joint that restricts linear degrees of freedom. /// public BallSocketJoint BallSocketJoint { get; private set; } /// /// Gets the rotational limit of the universal joint. /// This constraint overlaps with the twistJoint; if the limit is activated, /// the twistJoint should be generally deactivated and vice versa. /// public TwistLimit Limit { get; private set; } /// /// Gets the motor of the universal joint. /// This constraint overlaps with the twistJoint; if the motor is activated, /// the twistJoint should generally be deactivated and vice versa. /// public TwistMotor Motor { get; private set; } /// /// Gets the angular joint which removes one twisting degree of freedom. /// public TwistJoint TwistJoint { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverGroups/WeldJoint.cs ================================================ using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.Constraints.TwoEntity.Joints; using BEPUphysics.Entities; namespace BEPUphysics.Constraints.SolverGroups { /// /// Restricts the linear and angular motion between two entities. /// public class WeldJoint : SolverGroup { /// /// 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). /// public WeldJoint() { IsActive = false; BallSocketJoint = new BallSocketJoint(); NoRotationJoint = new NoRotationJoint(); Add(BallSocketJoint); Add(NoRotationJoint); } /// /// Constructs a new constraint which restricts the linear and angular motion between two entities. /// /// First entity of the constraint pair. /// Second entity of the constraint pair. 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); } /// /// Gets the ball socket joint that restricts linear degrees of freedom. /// public BallSocketJoint BallSocketJoint { get; private set; } /// /// Gets the no rotation joint that prevents angular motion. /// public NoRotationJoint NoRotationJoint { get; private set; } } } ================================================ FILE: BEPUphysics/Constraints/SolverSettings.cs ================================================ using System; namespace BEPUphysics.Constraints { /// /// Contains information about how a wheel solves its constraints. /// public class SolverSettings { /// /// Used to count how many iterations have taken place so far. /// internal int currentIterations; internal int maximumIterationCount = int.MaxValue; internal int minimumIterationCount = DefaultMinimumIterationCount; internal float minimumImpulse = DefaultMinimumImpulse; internal int iterationsAtZeroImpulse; /// /// Gets or sets the maximum iterations that the wheel constraint can undergo. /// If the space's iteration count is lower than this, the solver will only attempt /// as many iterations as the space iteration count. /// Lower iteration counts are less accurate, but can improve performance. /// public int MaximumIterationCount { get { return maximumIterationCount; } set { maximumIterationCount = Math.Max(value, 0); } } /// /// Gets or sets the minimum number of iterations that will be applied. /// If an impulse of magnitude smaller than the MinimumImpulse is applied, a 'tiny impulses' counter increases. Once it exceeds the MinimumIterations, /// the system can decide to stop solving to save time if appropriate. /// public int MinimumIterationCount { get { return minimumIterationCount; } set { minimumIterationCount = Math.Max(value, 0); } } /// /// Gets or sets the lower limit for impulses. Impulses applied with magnitudes less than this will increment the 'tiny impulse' counter, which is checked /// against the MinimumIterations property. If there's been too many tiny impulses in a row, then the system will stop trying to solve to save time. /// Higher values will allow the system to give up earlier, but can harm accuracy. /// public float MinimumImpulse { get { return minimumImpulse; } set { minimumImpulse = Math.Max(value, 0); } } /// /// The value to assign to new constraints' SolverSettings.MinimumImpulse. /// Impulses with magnitudes below this value will count as effectively zero in determining iteration early outs unless changed in the constraint's solver settings. /// High values quicken the short circuit but can cause instability, while low values will often prevent short circuiting, possibly increasing accuracy but harming performance. /// Defaults to .001f. /// public static float DefaultMinimumImpulse = .001f; /// /// The value to assign to new constraints' SolverSettings.MinimumIterations. /// Constraints are able to skip extra calculations if deemed appropriate after they complete the minimum iterations. /// Higher values force the system to wait longer before trying to early out, possibly improving behavior. /// Defaults to 1. /// public static int DefaultMinimumIterationCount = 1; } } ================================================ FILE: BEPUphysics/Constraints/SpringSettings.cs ================================================ using System; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints { /// /// Contains the error reduction factor and softness of a constraint. /// These can be used to make the same behaviors as the stiffness and damping constants, /// but may provide a more intuitive representation for rigid constraints. /// public class SpringAdvancedSettings { internal float errorReductionFactor = .1f; internal float softness = .00001f; internal bool useAdvancedSettings; /// /// Gets or sets the error reduction parameter of the spring. /// public float ErrorReductionFactor { get { return errorReductionFactor; } set { errorReductionFactor = MathHelper.Clamp(value, 0, 1); } } /// /// Gets or sets the softness of the joint. Higher values allow the constraint to be violated more. /// public float Softness { get { return softness; } set { softness = MathHelper.Max(0, value); } } /// /// Gets or sets whether or not to use the advanced settings. /// If this is set to true, the errorReductionFactor and softness will be used instead /// of the stiffness constant and damping constant. /// public bool UseAdvancedSettings { get { return useAdvancedSettings; } set { useAdvancedSettings = value; } } } /// /// Specifies the way in which a constraint's spring component behaves. /// public class SpringSettings { private readonly SpringAdvancedSettings advanced = new SpringAdvancedSettings(); internal float dampingConstant = 90000; internal float stiffnessConstant = 600000; /// /// Gets an object containing the solver's direct view of the spring behavior. /// public SpringAdvancedSettings Advanced { get { return advanced; } } /// /// Gets or sets the damping constant of this spring. Higher values reduce oscillation more. /// public float DampingConstant { get { return dampingConstant; } set { dampingConstant = MathHelper.Max(0, value); } } /// /// Gets or sets the spring constant of this spring. Higher values make the spring stiffer. /// public float StiffnessConstant { get { return stiffnessConstant; } set { stiffnessConstant = Math.Max(0, value); } } /// /// Computes the error reduction parameter and softness of a constraint based on its constants. /// Automatically called by constraint presteps to compute their per-frame values. /// /// Simulation timestep. /// Error reduction factor to use this frame. /// Adjusted softness of the constraint for this frame. public void ComputeErrorReductionAndSoftness(float dt, out float errorReduction, out float softness) { if (advanced.useAdvancedSettings) { errorReduction = advanced.errorReductionFactor / dt; softness = advanced.softness / dt; } else { if (stiffnessConstant == 0 && dampingConstant == 0) throw new InvalidOperationException("Constraints cannot have both 0 stiffness and 0 damping."); errorReduction = stiffnessConstant / (dt * stiffnessConstant + dampingConstant); softness = 1 / (dt * (dt * stiffnessConstant + dampingConstant)); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/DistanceLimit.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// A modified distance constraint allowing a range of lengths between two anchor points. /// public class DistanceLimit : JointLimit, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private Vector3 anchorA; private Vector3 anchorB; private float biasVelocity; private Vector3 jAngularA, jAngularB; private Vector3 jLinearA, jLinearB; private float error; private Vector3 localAnchorA; private Vector3 localAnchorB; /// /// Maximum distance allowed between the anchors. /// protected float maximumLength; /// /// Minimum distance maintained between the anchors. /// protected float minimumLength; private Vector3 offsetA, offsetB; private float velocityToImpulse; /// /// Constructs a distance limit joint. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the WorldAnchorA and WorldAnchorB (or their entity-local versions) /// and the MinimumLength and MaximumLength. /// This constructor sets the constraint's IsActive property to false by default. /// public DistanceLimit() { IsActive = false; } /// /// Constructs a distance limit joint. /// /// First body connected to the distance limit. /// Second body connected to the distance limit. /// Connection to the spring from the first connected body in world space. /// Connection to the spring from the second connected body in world space. /// Minimum distance maintained between the anchors. /// Maximum distance allowed between the anchors. public DistanceLimit(Entity connectionA, Entity connectionB, Vector3 anchorA, Vector3 anchorB, float minimumLength, float maximumLength) { ConnectionA = connectionA; ConnectionB = connectionB; MinimumLength = minimumLength; MaximumLength = maximumLength; WorldAnchorA = anchorA; WorldAnchorB = anchorB; } /// /// Gets or sets the first entity's connection point in local space. /// public Vector3 LocalAnchorA { get { return localAnchorA; } set { localAnchorA = value; Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out anchorA); anchorA += connectionA.position; } } /// /// Gets or sets the first entity's connection point in local space. /// public Vector3 LocalAnchorB { get { return localAnchorB; } set { localAnchorB = value; Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out anchorB); anchorB += connectionB.position; } } /// /// Gets or sets the maximum distance allowed between the anchors. /// public float MaximumLength { get { return maximumLength; } set { maximumLength = Math.Max(0, value); minimumLength = Math.Min(minimumLength, maximumLength); } } /// /// Gets or sets the minimum distance maintained between the anchors. /// public float MinimumLength { get { return minimumLength; } set { minimumLength = Math.Max(0, value); maximumLength = Math.Max(minimumLength, maximumLength); } } /// /// Gets or sets the connection to the distance constraint from the first connected body in world space. /// public Vector3 WorldAnchorA { get { return anchorA; } set { anchorA = value; localAnchorA = Vector3.Transform(anchorA - connectionA.position, Quaternion.Conjugate(connectionA.orientation)); } } /// /// Gets or sets the connection to the distance constraint from the second connected body in world space. /// public Vector3 WorldAnchorB { get { return anchorB; } set { anchorB = value; localAnchorB = Vector3.Transform(anchorB - connectionB.position, Quaternion.Conjugate(connectionB.orientation)); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { if (isLimitActive) { float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; return lambda; } return 0; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = jLinearA; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = jLinearB; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jAngularA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jAngularB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { //Compute the current relative velocity. float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, lambda, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, lambda, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Calculates necessary information for velocity solving. /// /// Time in seconds since the last update. public override void Update(float dt) { //Transform the anchors and offsets into world space. Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out offsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out offsetB); Vector3.Add(ref connectionA.position, ref offsetA, out anchorA); Vector3.Add(ref connectionB.position, ref offsetB, out anchorB); //Compute the distance. Vector3 separation; Vector3.Subtract(ref anchorB, ref anchorA, out separation); float distance = separation.Length(); if (distance < maximumLength && distance > minimumLength) { isActiveInSolver = false; accumulatedImpulse = 0; error = 0; isLimitActive = false; return; } isLimitActive = true; //Compute jacobians if (distance > maximumLength) { //If it's beyond the max, all of the jacobians are reversed compared to what they are when it's below the min. if (distance > Toolbox.Epsilon) { jLinearA.X = separation.X / distance; jLinearA.Y = separation.Y / distance; jLinearA.Z = separation.Z / distance; } else jLinearB = Toolbox.ZeroVector; jLinearB.X = -jLinearA.X; jLinearB.Y = -jLinearA.Y; jLinearB.Z = -jLinearA.Z; Vector3.Cross(ref jLinearA, ref offsetA, out jAngularA); //Still need to negate angular A. It's done after the effective mass matrix. Vector3.Cross(ref jLinearA, ref offsetB, out jAngularB); } else { if (distance > Toolbox.Epsilon) { jLinearB.X = separation.X / distance; jLinearB.Y = separation.Y / distance; jLinearB.Z = separation.Z / distance; } else jLinearB = Toolbox.ZeroVector; jLinearA.X = -jLinearB.X; jLinearA.Y = -jLinearB.Y; jLinearA.Z = -jLinearB.Z; Vector3.Cross(ref offsetA, ref jLinearB, out jAngularA); //Still need to negate angular A. It's done after the effective mass matrix. Vector3.Cross(ref offsetB, ref jLinearB, out jAngularB); } //Debug.WriteLine("BiasVelocity: " + biasVelocity); //Compute effective mass matrix if (connectionA.isDynamic && connectionB.isDynamic) { Vector3 aAngular; Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular); Vector3.Cross(ref aAngular, ref offsetA, out aAngular); Vector3 bAngular; Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular); Vector3.Cross(ref bAngular, ref offsetB, out bAngular); Vector3.Add(ref aAngular, ref bAngular, out aAngular); Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionA.inverseMass + connectionB.inverseMass; } else if (connectionA.isDynamic) { Vector3 aAngular; Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular); Vector3.Cross(ref aAngular, ref offsetA, out aAngular); Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionA.inverseMass; } else if (connectionB.isDynamic) { Vector3 bAngular; Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular); Vector3.Cross(ref bAngular, ref offsetB, out bAngular); Vector3.Dot(ref bAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionB.inverseMass; } else { //No point in trying to solve with two kinematics. isActiveInSolver = false; accumulatedImpulse = 0; return; } float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); velocityToImpulse = 1 / (softness + velocityToImpulse); //Finish computing jacobian; it's down here as an optimization (since it didn't need to be negated in mass matrix) jAngularA.X = -jAngularA.X; jAngularA.Y = -jAngularA.Y; jAngularA.Z = -jAngularA.Z; //Compute bias velocity if (distance > maximumLength) error = Math.Max(0, distance - maximumLength - Margin); else error = Math.Max(0, minimumLength - Margin - distance); biasVelocity = Math.Min(errorReduction * error, maxCorrectiveVelocity); if (bounciness > 0) { //Compute currently relative velocity for bounciness. float relativeVelocity, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out relativeVelocity); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); relativeVelocity += dot; if (-relativeVelocity > bounceVelocityThreshold) biasVelocity = Math.Max(biasVelocity, -relativeVelocity * bounciness); } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, accumulatedImpulse, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, accumulatedImpulse, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/EllipseSwingLimit.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Constrains the relative orientation of two entities to within an ellipse. /// public class EllipseSwingLimit : JointLimit, I1DImpulseConstraintWithError, I1DJacobianConstraint { private readonly JointBasis3D basis = new JointBasis3D(); private float accumulatedImpulse; private float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; private Vector3 localTwistAxisB; private float maximumAngleX; private float maximumAngleY; private Vector3 worldTwistAxisB; private float velocityToImpulse; /// /// Constructs a new swing limit. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the TwistAxis (or its entity-local version), /// the MaximumAngleX and MaximumAngleY, /// and the Basis. /// This constructor sets the constraint's IsActive property to false by default. /// public EllipseSwingLimit() { SpringSettings.StiffnessConstant /= 5; SpringSettings.Advanced.ErrorReductionFactor /= 5; Margin = .05f; IsActive = false; } /// /// Constructs a new swing limit. /// /// First entity connected by the constraint. /// Second entity connected by the constraint. /// Axis in world space to use as the initial unrestricted twist direction. /// This direction will be transformed to entity A's local space to form the basis's primary axis /// and to entity B's local space to form its twist axis. /// The basis's x and y axis are automatically created from the twist axis. /// Maximum angle of rotation around the basis X axis. /// Maximum angle of rotation around the basis Y axis. public EllipseSwingLimit(Entity connectionA, Entity connectionB, Vector3 twistAxis, float maximumAngleX, float maximumAngleY) { ConnectionA = connectionA; ConnectionB = connectionB; SetupJointTransforms(twistAxis); MaximumAngleX = maximumAngleX; MaximumAngleY = maximumAngleY; SpringSettings.StiffnessConstant /= 5; SpringSettings.Advanced.ErrorReductionFactor /= 5; Margin = .05f; } /// /// Constructs a new swing limit. /// Using this constructor will leave the limit uninitialized. Before using the limit in a simulation, be sure to set the basis axes using /// limit.basis.setLocalAxes or limit.basis.setWorldAxes and b's twist axis using the localTwistAxisB or twistAxisB properties. /// /// First entity connected by the constraint. /// Second entity connected by the constraint. public EllipseSwingLimit(Entity connectionA, Entity connectionB) { ConnectionA = connectionA; ConnectionB = connectionB; SpringSettings.StiffnessConstant /= 5; SpringSettings.Advanced.ErrorReductionFactor /= 5; Margin = .05f; } /// /// Gets the basis attached to entity A. /// The primary axis is the "twist" axis attached to entity A. /// The xAxis is the axis around which the angle will be limited by maximumAngleX. /// Similarly, the yAxis is the axis around which the angle will be limited by maximumAngleY. /// public JointBasis3D Basis { get { return basis; } } /// /// Gets or sets the twist axis attached to entity B in its local space. /// The transformed twist axis will be used to determine the angles around entity A's basis axes. /// public Vector3 LocalTwistAxisB { get { return localTwistAxisB; } set { localTwistAxisB = value; Matrix3x3.Transform(ref localTwistAxisB, ref connectionB.orientationMatrix, out worldTwistAxisB); } } /// /// Gets or sets the maximum angle of rotation around the x axis. /// This can be thought of as the major radius of the swing limit's ellipse. /// public float MaximumAngleX { get { return maximumAngleX; } set { maximumAngleX = MathHelper.Clamp(value, Toolbox.BigEpsilon, MathHelper.Pi); } } /// /// Gets or sets the maximum angle of rotation around the y axis. /// This can be thought of as the minor radius of the swing limit's ellipse. /// public float MaximumAngleY { get { return maximumAngleY; } set { maximumAngleY = MathHelper.Clamp(value, Toolbox.BigEpsilon, MathHelper.Pi); } } /// /// Gets or sets the twist axis attached to entity B in world space. /// The transformed twist axis will be used to determine the angles around entity A's basis axes. /// public Vector3 TwistAxisB { get { return worldTwistAxisB; } set { worldTwistAxisB = value; Matrix3x3.TransformTranspose(ref worldTwistAxisB, ref connectionB.orientationMatrix, out localTwistAxisB); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { if (isLimitActive) { float velocityA, velocityB; Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } return 0; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// /// Axis around which rotation is allowed. public void SetupJointTransforms(Vector3 twistAxis) { //Compute a vector which is perpendicular to the axis. It'll be added in local space to both connections. Vector3 xAxis; Vector3.Cross(ref twistAxis, ref Toolbox.UpVector, out xAxis); float length = xAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref twistAxis, ref Toolbox.RightVector, out xAxis); } Vector3 yAxis; Vector3.Cross(ref twistAxis, ref xAxis, out yAxis); //Put the axes into the joint transform of A. basis.rotationMatrix = connectionA.orientationMatrix; basis.SetWorldAxes(twistAxis, xAxis, yAxis); //Put the axes into the 'joint transform' of B too. TwistAxisB = twistAxis; } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = (-velocityA - velocityB) - biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Min(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Transform the axes into world space. basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); Matrix3x3.Transform(ref localTwistAxisB, ref connectionB.orientationMatrix, out worldTwistAxisB); //Compute the individual swing angles. Quaternion relativeRotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisB, ref basis.primaryAxis, out relativeRotation); Vector3 axis; float angle; Toolbox.GetAxisAngleFromQuaternion(ref relativeRotation, out axis, out angle); #if !WINDOWS Vector3 axisAngle = new Vector3(); #else Vector3 axisAngle; #endif axisAngle.X = axis.X * angle; axisAngle.Y = axis.Y * angle; axisAngle.Z = axis.Z * angle; float angleX; Vector3.Dot(ref axisAngle, ref basis.xAxis, out angleX); float angleY; Vector3.Dot(ref axisAngle, ref basis.yAxis, out angleY); float maxAngleXSquared = maximumAngleX * maximumAngleX; float maxAngleYSquared = maximumAngleY * maximumAngleY; error = angleX * angleX * maxAngleYSquared + angleY * angleY * maxAngleXSquared - maxAngleXSquared * maxAngleYSquared; //float ellipseAngle = (float)Math.Atan2(angleY, angleX) + MathHelper.PiOver2; ////This angle may need to have pi/2 added to it, since it is perpendicular ////Compute the maximum angle, based on the ellipse //float cosAngle = (float)Math.Cos(ellipseAngle); //float sinAngle = (float)Math.Sin(ellipseAngle); //float denominator = myMaximumAngleX * cosAngle; //denominator *= denominator; //float denominator2 = myMaximumAngleY * sinAngle; //denominator += denominator2 * denominator2; //denominator = (float)Math.Sqrt(denominator); //float maximumAngle = myMaximumAngleX * myMaximumAngleY / denominator; //myError = angle - maximumAngle; //myError = angleX * angleX / (myMaximumAngleX * myMaximumAngleX) + angleY * angleY / (myMaximumAngleY * myMaximumAngleY) - 1; if (error <= 0) { isActiveInSolver = false; error = 0; accumulatedImpulse = 0; isLimitActive = false; return; } isLimitActive = true; #if !WINDOWS Vector2 tangent = new Vector2(); #else Vector2 tangent; #endif //tangent.X = angleX * myMaximumAngleY / myMaximumAngleX; //tangent.Y = angleY * myMaximumAngleX / myMaximumAngleY; tangent.X = angleX / maxAngleXSquared; tangent.Y = angleY / maxAngleYSquared; tangent.Normalize(); //Create a rotation which swings our basis 'out' to b's world orientation. Quaternion.Conjugate(ref relativeRotation, out relativeRotation); Vector3 sphereTangentX, sphereTangentY; Vector3.Transform(ref basis.xAxis, ref relativeRotation, out sphereTangentX); Vector3.Transform(ref basis.yAxis, ref relativeRotation, out sphereTangentY); Vector3.Multiply(ref sphereTangentX, tangent.X, out jacobianA); //not actually jA, just storing it there. Vector3.Multiply(ref sphereTangentY, tangent.Y, out jacobianB); //not actually jB, just storing it there. Vector3.Add(ref jacobianA, ref jacobianB, out jacobianA); //jacobianA = tangent.X * sphereTangentX + tangent.Y * sphereTangentY; jacobianB.X = -jacobianA.X; jacobianB.Y = -jacobianA.Y; jacobianB.Z = -jacobianA.Z; float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); //Compute the error correcting velocity error = error - margin; biasVelocity = MathHelper.Min(Math.Max(error, 0) * errorReduction, maxCorrectiveVelocity); if (bounciness > 0) { float relativeVelocity; float dot; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot); relativeVelocity += dot; relativeVelocity /= 5; //HEEAAAAACK if (relativeVelocity > bounceVelocityThreshold) biasVelocity = MathHelper.Max(biasVelocity, bounciness * relativeVelocity); } //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/JointLimit.cs ================================================ using System; using BEPUphysics.Constraints.TwoEntity.Joints; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Superclass of constraints which have a limited area of free movement. /// public abstract class JointLimit : Joint { /// /// Minimum velocity necessary for a bounce to occur at a joint limit. /// protected float bounceVelocityThreshold = 1; /// /// Bounciness of this joint limit. 0 is completely inelastic; 1 is completely elastic. /// protected float bounciness; protected bool isLimitActive; /// /// Small area that the constraint can be violated without applying position correction. Helps avoid jitter. /// protected float margin = 0.005f; /// /// Gets or sets the minimum velocity necessary for a bounce to occur at a joint limit. /// public float BounceVelocityThreshold { get { return bounceVelocityThreshold; } set { bounceVelocityThreshold = Math.Max(0, value); } } /// /// Gets or sets the bounciness of this joint limit. 0 is completely inelastic; 1 is completely elastic. /// public float Bounciness { get { return bounciness; } set { bounciness = MathHelper.Clamp(value, 0, 1); } } /// /// Gets whether or not the limit is currently exceeded. While violated, the constraint will apply impulses in an attempt to stop further violation and to correct any current error. /// This is true whenever the limit is touched. /// public bool IsLimitExceeded { get { return isLimitActive; } } /// /// Gets or sets the small area that the constraint can be violated without applying position correction. Helps avoid jitter. /// public float Margin { get { return margin; } set { margin = MathHelper.Max(value, 0); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/LinearAxisLimit.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Constrains the distance along an axis between anchor points attached to two entities. /// public class LinearAxisLimit : JointLimit, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private float biasVelocity; private Vector3 jAngularA, jAngularB; private Vector3 jLinearA, jLinearB; private Vector3 localAnchorA; private Vector3 localAnchorB; private float massMatrix; private float error; private Vector3 localAxis; private float maximum; private float minimum; private Vector3 worldAxis; private Vector3 rA; //Jacobian entry for entity A. private float unadjustedError; private Vector3 worldAnchorA; private Vector3 worldAnchorB; private Vector3 worldOffsetA, worldOffsetB; /// /// Constructs a constraint which tries to keep anchors on two entities within a certain distance of each other along an axis. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the AnchorA, AnchorB, and Axis (or their entity-local versions), /// and the Minimum and Maximum. /// This constructor sets the constraint's IsActive property to false by default. /// public LinearAxisLimit() { IsActive = false; } /// /// Constructs a constraint which tries to keep anchors on two entities within a certain distance of each other along an axis. /// /// First connection of the pair. /// Second connection of the pair. /// World space point to attach to connection A that will be constrained. /// World space point to attach to connection B that will be constrained. /// Limited axis in world space to attach to connection A. /// Minimum allowed position along the axis. /// Maximum allowed position along the axis. public LinearAxisLimit(Entity connectionA, Entity connectionB, Vector3 anchorA, Vector3 anchorB, Vector3 axis, float minimum, float maximum) { ConnectionA = connectionA; ConnectionB = connectionB; AnchorA = anchorA; AnchorB = anchorB; Axis = axis; Minimum = minimum; Maximum = maximum; } /// /// Gets or sets the anchor point attached to entity A in world space. /// public Vector3 AnchorA { get { return worldAnchorA; } set { worldAnchorA = value; worldOffsetA = worldAnchorA - connectionA.position; Matrix3x3.TransformTranspose(ref worldOffsetA, ref connectionA.orientationMatrix, out localAnchorA); } } /// /// Gets or sets the anchor point attached to entity A in world space. /// public Vector3 AnchorB { get { return worldAnchorB; } set { worldAnchorB = value; worldOffsetB = worldAnchorB - connectionB.position; Matrix3x3.TransformTranspose(ref worldOffsetB, ref connectionB.orientationMatrix, out localAnchorB); } } /// /// Gets or sets the limited axis in world space. /// public Vector3 Axis { get { return worldAxis; } set { worldAxis = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldAxis, ref connectionA.orientationMatrix, out localAxis); } } /// /// Gets or sets the limited axis in the local space of connection A. /// public Vector3 LocalAxis { get { return localAxis; } set { localAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis); } } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetA { get { return localAnchorA; } set { localAnchorA = value; Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); worldAnchorA = connectionA.position + worldOffsetA; } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetB { get { return localAnchorB; } set { localAnchorB = value; Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); worldAnchorB = connectionB.position + worldOffsetB; } } /// /// Gets or sets the maximum allowed distance along the axis. /// public float Maximum { get { return maximum; } set { maximum = value; minimum = MathHelper.Min(minimum, maximum); } } /// /// Gets or sets the minimum allowed distance along the axis. /// public float Minimum { get { return minimum; } set { minimum = value; maximum = MathHelper.Max(minimum, maximum); } } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in world space. /// public Vector3 OffsetA { get { return worldOffsetA; } set { worldOffsetA = value; worldAnchorA = connectionA.position + worldOffsetA; Matrix3x3.TransformTranspose(ref worldOffsetA, ref connectionA.orientationMatrix, out localAnchorA); } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in world space. /// public Vector3 OffsetB { get { return worldOffsetB; } set { worldOffsetB = value; worldAnchorB = connectionB.position + worldOffsetB; Matrix3x3.TransformTranspose(ref worldOffsetB, ref connectionB.orientationMatrix, out localAnchorB); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { if (isLimitActive) { float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; return lambda; } return 0; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion //Jacobians #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = jLinearA; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = jLinearB; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jAngularA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jAngularB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = massMatrix; } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //Compute the current relative velocity. float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= massMatrix; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; if (unadjustedError < 0) accumulatedImpulse = MathHelper.Min(accumulatedImpulse + lambda, 0); else accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, lambda, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, lambda, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Compute the 'pre'-jacobians Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA); Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB); Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA); Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis); //Compute error #if !WINDOWS Vector3 separation = new Vector3(); #else Vector3 separation; #endif separation.X = worldAnchorB.X - worldAnchorA.X; separation.Y = worldAnchorB.Y - worldAnchorA.Y; separation.Z = worldAnchorB.Z - worldAnchorA.Z; Vector3.Dot(ref separation, ref worldAxis, out unadjustedError); //Compute error if (unadjustedError < minimum) unadjustedError = minimum - unadjustedError; else if (unadjustedError > maximum) unadjustedError = maximum - unadjustedError; else { unadjustedError = 0; isActiveInSolver = false; accumulatedImpulse = 0; isLimitActive = false; return; } isLimitActive = true; unadjustedError = -unadjustedError; //Adjust Error if (unadjustedError > 0) error = MathHelper.Max(0, unadjustedError - margin); else if (unadjustedError < 0) error = MathHelper.Min(0, unadjustedError + margin); //Compute jacobians jLinearA = worldAxis; jLinearB.X = -jLinearA.X; jLinearB.Y = -jLinearA.Y; jLinearB.Z = -jLinearA.Z; Vector3.Cross(ref rA, ref jLinearA, out jAngularA); Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB); //Compute bias float errorReductionParameter; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReductionParameter, out softness); biasVelocity = MathHelper.Clamp(errorReductionParameter * error, -maxCorrectiveVelocity, maxCorrectiveVelocity); if (bounciness > 0) { //Compute currently relative velocity for bounciness. float relativeVelocity, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out relativeVelocity); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); relativeVelocity += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); relativeVelocity += dot; if (unadjustedError > 0 && -relativeVelocity > bounceVelocityThreshold) biasVelocity = Math.Max(biasVelocity, -relativeVelocity * bounciness); else if (unadjustedError < 0 && relativeVelocity > bounceVelocityThreshold) biasVelocity = Math.Min(biasVelocity, -relativeVelocity * bounciness); } //compute mass matrix float entryA, entryB; Vector3 intermediate; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularA, out entryA); entryA += connectionA.inverseMass; } else entryA = 0; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularB, out entryB); entryB += connectionB.inverseMass; } else entryB = 0; massMatrix = 1 / (entryA + entryB + softness); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, accumulatedImpulse, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, accumulatedImpulse, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/RevoluteLimit.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Constraint which prevents the connected entities from rotating relative to each other around an axis beyond given limits. /// public class RevoluteLimit : JointLimit, I2DImpulseConstraintWithError, I2DJacobianConstraint { private readonly JointBasis2D basis = new JointBasis2D(); private Vector2 accumulatedImpulse; private Vector2 biasVelocity; private Vector3 jacobianMaxA; private Vector3 jacobianMaxB; private Vector3 jacobianMinA; private Vector3 jacobianMinB; private bool maxIsActive; private bool minIsActive; private Vector2 error; private Vector3 localTestAxis; /// /// Naximum angle that entities can twist. /// protected float maximumAngle; /// /// Minimum angle that entities can twist. /// protected float minimumAngle; private Vector3 worldTestAxis; private Vector2 velocityToImpulse; /// /// Constructs a new constraint which prevents the connected entities from rotating relative to each other around an axis beyond given limits. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the TestAxis (or its entity-local version) and the Basis. /// This constructor sets the constraint's IsActive property to false by default. /// public RevoluteLimit() { IsActive = false; } /// /// Constructs a new constraint which prevents the connected entities from rotating relative to each other around an axis beyond given limits. /// /// First connection of the pair. /// Second connection of the pair. /// Axis of rotation to be limited. /// Axis attached to connectionB that is tested to determine the current angle. /// Will also be used as the base rotation axis representing 0 degrees. /// Minimum twist angle allowed. /// Maximum twist angle allowed. public RevoluteLimit(Entity connectionA, Entity connectionB, Vector3 limitedAxis, Vector3 testAxis, float minimumAngle, float maximumAngle) { ConnectionA = connectionA; ConnectionB = connectionB; //Put the axes into the joint transform of A. basis.rotationMatrix = this.connectionA.orientationMatrix; basis.SetWorldAxes(limitedAxis, testAxis); //Put the axes into the 'joint transform' of B too. TestAxis = basis.xAxis; MinimumAngle = minimumAngle; MaximumAngle = maximumAngle; } /// /// Constructs a new constraint which prevents the connected entities from rotating relative to each other around an axis beyond given limits. /// Using this constructor will leave the limit uninitialized. Before using the limit in a simulation, be sure to set the basis axes using /// Basis.SetLocalAxes or Basis.SetWorldAxes and the test axis using the LocalTestAxis or TestAxis properties. /// /// First connection of the pair. /// Second connection of the pair. public RevoluteLimit(Entity connectionA, Entity connectionB) { ConnectionA = connectionA; ConnectionB = connectionB; } /// /// Gets the basis attached to entity A. /// The primary axis represents the limited axis of rotation. The 'measurement plane' which the test axis is tested against is based on this primary axis. /// The x axis defines the 'base' direction on the measurement plane corresponding to 0 degrees of relative rotation. /// public JointBasis2D Basis { get { return basis; } } /// /// Gets or sets the axis attached to entity B in its local space that will be tested against the limits. /// public Vector3 LocalTestAxis { get { return localTestAxis; } set { localTestAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis); } } /// /// Gets or sets the maximum angle that entities can twist. /// public float MaximumAngle { get { return maximumAngle; } set { maximumAngle = value % (MathHelper.TwoPi); if (minimumAngle > MathHelper.Pi) minimumAngle -= MathHelper.TwoPi; if (minimumAngle <= -MathHelper.Pi) minimumAngle += MathHelper.TwoPi; } } /// /// Gets or sets the minimum angle that entities can twist. /// public float MinimumAngle { get { return minimumAngle; } set { minimumAngle = value % (MathHelper.TwoPi); if (minimumAngle > MathHelper.Pi) minimumAngle -= MathHelper.TwoPi; if (minimumAngle <= -MathHelper.Pi) minimumAngle += MathHelper.TwoPi; } } /// /// Gets or sets the axis attached to entity B in world space that will be tested against the limits. /// public Vector3 TestAxis { get { return worldTestAxis; } set { worldTestAxis = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldTestAxis, ref connectionB.orientationMatrix, out localTestAxis); } } #region I2DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// The revolute limit is special; internally, it is sometimes two constraints. /// The X value of the vector is the "minimum" plane of the limit, and the Y value is the "maximum" plane. /// If a plane isn't active, its error is zero. /// public Vector2 RelativeVelocity { get { if (isLimitActive) { float velocityA, velocityB; Vector2 toReturn = Vector2.Zero; if (minIsActive) { Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out velocityB); toReturn.X = velocityA + velocityB; } if (maxIsActive) { Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out velocityB); toReturn.Y = velocityA + velocityB; } return toReturn; } return new Vector2(); } } /// /// Gets the total impulse applied by this constraint. /// The x component corresponds to the minimum plane limit, /// while the y component corresponds to the maximum plane limit. /// public Vector2 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// The x component corresponds to the minimum plane limit, /// while the y component corresponds to the maximum plane limit. /// public Vector2 Error { get { return error; } } #endregion //Newer version of revolute limit will use up to two planes. This is sort of like being a double-constraint, with two jacobians and everything. //Not going to solve both plane limits simultaneously because they can be redundant. De-linking them will let the system deal with redundancy better. #region I2DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = jacobianMinA; jacobianY = jacobianMaxA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = jacobianMinB; jacobianY = jacobianMaxB; } /// /// Gets the mass matrix of the revolute limit. /// The revolute limit is special; in terms of solving, it is /// actually sometimes TWO constraints; a minimum plane, and a /// maximum plane. The M11 field represents the minimum plane mass matrix /// and the M22 field represents the maximum plane mass matrix. /// /// Mass matrix of the constraint. public void GetMassMatrix(out Matrix2x2 massMatrix) { massMatrix.M11 = velocityToImpulse.X; massMatrix.M22 = velocityToImpulse.Y; massMatrix.M12 = 0; massMatrix.M21 = 0; } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { float lambda; float lambdaTotal = 0; float velocityA, velocityB; float previousAccumulatedImpulse; if (minIsActive) { //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out velocityB); //Add in the constraint space bias velocity lambda = -(velocityA + velocityB) + biasVelocity.X - softness * accumulatedImpulse.X; //Transform to an impulse lambda *= velocityToImpulse.X; //Clamp accumulated impulse (can't go negative) previousAccumulatedImpulse = accumulatedImpulse.X; accumulatedImpulse.X = MathHelper.Max(accumulatedImpulse.X + lambda, 0); lambda = accumulatedImpulse.X - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianMinA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianMinB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } lambdaTotal += Math.Abs(lambda); } if (maxIsActive) { //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out velocityB); //Add in the constraint space bias velocity lambda = -(velocityA + velocityB) + biasVelocity.Y - softness * accumulatedImpulse.Y; //Transform to an impulse lambda *= velocityToImpulse.Y; //Clamp accumulated impulse (can't go negative) previousAccumulatedImpulse = accumulatedImpulse.Y; accumulatedImpulse.Y = MathHelper.Max(accumulatedImpulse.Y + lambda, 0); lambda = accumulatedImpulse.Y - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianMaxA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianMaxB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } lambdaTotal += Math.Abs(lambda); } return lambdaTotal; } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Transform the axes into world space. basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis); //Compute the plane normals. Vector3 minPlaneNormal, maxPlaneNormal; //Rotate basisA y axis around the basisA primary axis. Matrix rotation; Matrix.CreateFromAxisAngle(ref basis.primaryAxis, minimumAngle + MathHelper.PiOver2, out rotation); Vector3.TransformNormal(ref basis.xAxis, ref rotation, out minPlaneNormal); Matrix.CreateFromAxisAngle(ref basis.primaryAxis, maximumAngle - MathHelper.PiOver2, out rotation); Vector3.TransformNormal(ref basis.xAxis, ref rotation, out maxPlaneNormal); //Compute the errors along the two normals. float planePositionMin, planePositionMax; Vector3.Dot(ref minPlaneNormal, ref worldTestAxis, out planePositionMin); Vector3.Dot(ref maxPlaneNormal, ref worldTestAxis, out planePositionMax); float span = GetDistanceFromMinimum(maximumAngle); //Early out and compute the determine the plane normal. if (span >= MathHelper.Pi) { if (planePositionMax > 0 || planePositionMin > 0) { //It's in a perfectly valid configuration, so skip. isActiveInSolver = false; minIsActive = false; maxIsActive = false; error = Vector2.Zero; accumulatedImpulse = Vector2.Zero; isLimitActive = false; return; } if (planePositionMax > planePositionMin) { //It's quicker to escape out to the max plane than the min plane. error.X = 0; error.Y = -planePositionMax; accumulatedImpulse.X = 0; minIsActive = false; maxIsActive = true; } else { //It's quicker to escape out to the min plane than the max plane. error.X = -planePositionMin; error.Y = 0; accumulatedImpulse.Y = 0; minIsActive = true; maxIsActive = false; } //There's never a non-degenerate situation where having both planes active with a span //greater than pi is useful. } else { if (planePositionMax > 0 && planePositionMin > 0) { //It's in a perfectly valid configuration, so skip. isActiveInSolver = false; minIsActive = false; maxIsActive = false; error = Vector2.Zero; accumulatedImpulse = Vector2.Zero; isLimitActive = false; return; } if (planePositionMin <= 0 && planePositionMax <= 0) { //Escape upward. //Activate both planes. error.X = -planePositionMin; error.Y = -planePositionMax; minIsActive = true; maxIsActive = true; } else if (planePositionMin <= 0) { //It's quicker to escape out to the min plane than the max plane. error.X = -planePositionMin; error.Y = 0; accumulatedImpulse.Y = 0; minIsActive = true; maxIsActive = false; } else { //It's quicker to escape out to the max plane than the min plane. error.X = 0; error.Y = -planePositionMax; accumulatedImpulse.X = 0; minIsActive = false; maxIsActive = true; } } isLimitActive = true; //****** VELOCITY BIAS ******// //Compute the correction velocity float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); //Compute the jacobians if (minIsActive) { Vector3.Cross(ref minPlaneNormal, ref worldTestAxis, out jacobianMinA); if (jacobianMinA.LengthSquared() < Toolbox.Epsilon) { //The plane normal is aligned with the test axis. //Use the basis's free axis. jacobianMinA = basis.primaryAxis; } jacobianMinA.Normalize(); jacobianMinB.X = -jacobianMinA.X; jacobianMinB.Y = -jacobianMinA.Y; jacobianMinB.Z = -jacobianMinA.Z; } if (maxIsActive) { Vector3.Cross(ref maxPlaneNormal, ref worldTestAxis, out jacobianMaxA); if (jacobianMaxA.LengthSquared() < Toolbox.Epsilon) { //The plane normal is aligned with the test axis. //Use the basis's free axis. jacobianMaxA = basis.primaryAxis; } jacobianMaxA.Normalize(); jacobianMaxB.X = -jacobianMaxA.X; jacobianMaxB.Y = -jacobianMaxA.Y; jacobianMaxB.Z = -jacobianMaxA.Z; } //Error is always positive if (minIsActive) { biasVelocity.X = MathHelper.Min(MathHelper.Max(0, error.X - margin) * errorReduction, maxCorrectiveVelocity); if (bounciness > 0) { float relativeVelocity; float dot; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMinA, out relativeVelocity); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMinB, out dot); relativeVelocity += dot; if (-relativeVelocity > bounceVelocityThreshold) biasVelocity.X = MathHelper.Max(biasVelocity.X, -bounciness * relativeVelocity); } } if (maxIsActive) { biasVelocity.Y = MathHelper.Min(MathHelper.Max(0, error.Y - margin) * errorReduction, maxCorrectiveVelocity); if (bounciness > 0) { //Find the velocity contribution from each connection if (maxIsActive) { float relativeVelocity; Vector3.Dot(ref connectionA.angularVelocity, ref jacobianMaxA, out relativeVelocity); float dot; Vector3.Dot(ref connectionB.angularVelocity, ref jacobianMaxB, out dot); relativeVelocity += dot; if (-relativeVelocity > bounceVelocityThreshold) biasVelocity.Y = MathHelper.Max(biasVelocity.Y, -bounciness * relativeVelocity); } } } //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float minEntryA, minEntryB; float maxEntryA, maxEntryB; Vector3 transformedAxis; if (connectionA.isDynamic) { if (minIsActive) { Matrix3x3.Transform(ref jacobianMinA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianMinA, out minEntryA); } else minEntryA = 0; if (maxIsActive) { Matrix3x3.Transform(ref jacobianMaxA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianMaxA, out maxEntryA); } else maxEntryA = 0; } else { minEntryA = 0; maxEntryA = 0; } //Connection B's contribution to the mass matrix if (connectionB.isDynamic) { if (minIsActive) { Matrix3x3.Transform(ref jacobianMinB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianMinB, out minEntryB); } else minEntryB = 0; if (maxIsActive) { Matrix3x3.Transform(ref jacobianMaxB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianMaxB, out maxEntryB); } else maxEntryB = 0; } else { minEntryB = 0; maxEntryB = 0; } //Compute the inverse mass matrix //Notice that the mass matrix isn't linked, it's two separate ones. velocityToImpulse.X = 1 / (softness + minEntryA + minEntryB); velocityToImpulse.Y = 1 / (softness + maxEntryA + maxEntryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse if (connectionA.isDynamic) { var impulse = new Vector3(); if (minIsActive) { Vector3.Multiply(ref jacobianMinA, accumulatedImpulse.X, out impulse); } if (maxIsActive) { Vector3 temp; Vector3.Multiply(ref jacobianMaxA, accumulatedImpulse.Y, out temp); Vector3.Add(ref impulse, ref temp, out impulse); } connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { var impulse = new Vector3(); if (minIsActive) { Vector3.Multiply(ref jacobianMinB, accumulatedImpulse.X, out impulse); } if (maxIsActive) { Vector3 temp; Vector3.Multiply(ref jacobianMaxB, accumulatedImpulse.Y, out temp); Vector3.Add(ref impulse, ref temp, out impulse); } connectionB.ApplyAngularImpulse(ref impulse); } } private float GetDistanceFromMinimum(float angle) { if (minimumAngle > 0) { if (angle >= minimumAngle) return angle - minimumAngle; if (angle > 0) return MathHelper.TwoPi - minimumAngle + angle; return MathHelper.TwoPi - minimumAngle + angle; } if (angle < minimumAngle) return MathHelper.TwoPi - minimumAngle + angle; return angle - minimumAngle; //else //if (currentAngle >= 0) // return angle - minimumAngle; } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/SwingLimit.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Keeps the angle between the axes attached to two entities below some maximum value. /// public class SwingLimit : JointLimit, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private float biasVelocity; private Vector3 hingeAxis; private float minimumCosine = 1; private float error; private Vector3 localAxisA; private Vector3 localAxisB; private Vector3 worldAxisA; private Vector3 worldAxisB; private float velocityToImpulse; /// /// Constructs a new constraint which attempts to restrict the maximum relative angle of two entities to some value. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the WorldAxisA, WorldAxisB (or their entity-local versions) and the MaximumAngle. /// This constructor sets the constraint's IsActive property to false by default. /// public SwingLimit() { IsActive = false; } /// /// Constructs a new constraint which attempts to restrict the maximum relative angle of two entities to some value. /// /// First connection of the pair. /// Second connection of the pair. /// Axis attached to the first connected entity. /// Axis attached to the second connected entity. /// Maximum angle between the axes allowed. public SwingLimit(Entity connectionA, Entity connectionB, Vector3 axisA, Vector3 axisB, float maximumAngle) { ConnectionA = connectionA; ConnectionB = connectionB; WorldAxisA = axisA; WorldAxisB = axisB; MaximumAngle = maximumAngle; } /// /// Gets or sets the axis attached to the first connected entity in its local space. /// public Vector3 LocalAxisA { get { return localAxisA; } set { localAxisA = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); } } /// /// Gets or sets the axis attached to the first connected entity in its local space. /// public Vector3 LocalAxisB { get { return localAxisB; } set { localAxisB = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisB, ref connectionA.orientationMatrix, out worldAxisB); } } /// /// Maximum angle allowed between the two axes, from 0 to pi. /// public float MaximumAngle { get { return (float)Math.Acos(minimumCosine); } set { minimumCosine = (float)Math.Cos(MathHelper.Clamp(value, 0, MathHelper.Pi)); } } /// /// Gets or sets the axis attached to the first connected entity in world space. /// public Vector3 WorldAxisA { get { return worldAxisA; } set { worldAxisA = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionA.orientation, out conjugate); Vector3.Transform(ref worldAxisA, ref conjugate, out localAxisA); } } /// /// Gets or sets the axis attached to the first connected entity in world space. /// public Vector3 WorldAxisB { get { return worldAxisB; } set { worldAxisB = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionB.orientation, out conjugate); Vector3.Transform(ref worldAxisB, ref conjugate, out localAxisB); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { if (isLimitActive) { Vector3 relativeVelocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity); float lambda; Vector3.Dot(ref relativeVelocity, ref hingeAxis, out lambda); return lambda; } return 0; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = hingeAxis; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = -hingeAxis; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Applies the sequential impulse. /// public override float SolveIteration() { float lambda; Vector3 relativeVelocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity); //Transform the velocity to with the jacobian Vector3.Dot(ref relativeVelocity, ref hingeAxis, out lambda); //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; Vector3.Multiply(ref hingeAxis, lambda, out impulse); if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Initializes the constraint for this frame. /// /// Time since the last frame. public override void Update(float dt) { Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); float dot; Vector3.Dot(ref worldAxisA, ref worldAxisB, out dot); //Keep in mind, the dot is the cosine of the angle. //1: 0 radians //0: pi/2 radians //-1: pi radians if (dot > minimumCosine) { isActiveInSolver = false; error = 0; accumulatedImpulse = 0; isLimitActive = false; return; } isLimitActive = true; //Hinge axis is actually the jacobian entry for angular A (negative angular B). Vector3.Cross(ref worldAxisA, ref worldAxisB, out hingeAxis); float lengthSquared = hingeAxis.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { //Vector3.Divide(ref hingeAxis, (float)Math.Sqrt(lengthSquared), out hingeAxis); } else { //They're parallel; for the sake of continuity, pick some axis which is perpendicular to both that ISN'T the zero vector. Vector3.Cross(ref worldAxisA, ref Toolbox.UpVector, out hingeAxis); lengthSquared = hingeAxis.LengthSquared(); if (lengthSquared > Toolbox.Epsilon) { //Vector3.Divide(ref hingeAxis, (float)Math.Sqrt(lengthSquared), out hingeAxis); } else { //That's improbable; b's world axis was apparently parallel with the up vector! //So just use the right vector (it can't be parallel with both the up and right vectors). Vector3.Cross(ref worldAxisA, ref Toolbox.RightVector, out hingeAxis); //hingeAxis.Normalize(); } } float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); //Further away from 0 degrees is further negative; if the dot is below the minimum cosine, it means the angle is above the maximum angle. error = Math.Max(0, minimumCosine - dot - margin); biasVelocity = MathHelper.Clamp(errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity); if (bounciness > 0) { //Compute the speed around the axis. float relativeSpeed; Vector3 relativeVelocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out relativeVelocity); Vector3.Dot(ref relativeVelocity, ref hingeAxis, out relativeSpeed); if (relativeSpeed < -bounceVelocityThreshold) { biasVelocity = Math.Max(biasVelocity, bounciness * relativeSpeed); } } //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref hingeAxis, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref hingeAxis, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref hingeAxis, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); } public override void ExclusiveUpdate() { //Apply accumulated impulse Vector3 impulse; Vector3.Multiply(ref hingeAxis, accumulatedImpulse, out impulse); if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/JointLimits/TwistLimit.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.JointLimits { /// /// Prevents the connected entities from twisting relative to each other beyond given limits. /// public class TwistLimit : JointLimit, I1DImpulseConstraintWithError, I1DJacobianConstraint { private readonly JointBasis3D basisA = new JointBasis3D(); private readonly JointBasis2D basisB = new JointBasis2D(); private float accumulatedImpulse; private float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; /// /// Naximum angle that entities can twist. /// protected float maximumAngle; /// /// Minimum angle that entities can twist. /// protected float minimumAngle; private float velocityToImpulse; /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other beyond given limits. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the BasisA, BasisB and the MinimumAngle and MaximumAngle. /// This constructor sets the constraint's IsActive property to false by default. /// public TwistLimit() { IsActive = false; } /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other beyond given limits. /// /// First connection of the pair. /// Second connection of the pair. /// Twist axis attached to the first connected entity. /// Twist axis attached to the second connected entity. /// Minimum twist angle allowed. /// Maximum twist angle allowed. public TwistLimit(Entity connectionA, Entity connectionB, Vector3 axisA, Vector3 axisB, float minimumAngle, float maximumAngle) { ConnectionA = connectionA; ConnectionB = connectionB; SetupJointTransforms(axisA, axisB); MinimumAngle = minimumAngle; MaximumAngle = maximumAngle; } /// /// Gets the basis attached to entity A. /// The primary axis represents the twist axis attached to entity A. /// The x axis and y axis represent a plane against which entity B's attached x axis is projected to determine the twist angle. /// public JointBasis3D BasisA { get { return basisA; } } /// /// Gets the basis attached to entity B. /// The primary axis represents the twist axis attached to entity A. /// The x axis is projected onto the plane defined by localTransformA's x and y axes /// to get the twist angle. /// public JointBasis2D BasisB { get { return basisB; } } /// /// Gets or sets the maximum angle that entities can twist. /// public float MaximumAngle { get { return maximumAngle; } set { maximumAngle = value % (MathHelper.TwoPi); if (minimumAngle > MathHelper.Pi) minimumAngle -= MathHelper.TwoPi; if (minimumAngle <= -MathHelper.Pi) minimumAngle += MathHelper.TwoPi; } } /// /// Gets or sets the minimum angle that entities can twist. /// public float MinimumAngle { get { return minimumAngle; } set { minimumAngle = value % (MathHelper.TwoPi); if (minimumAngle > MathHelper.Pi) minimumAngle -= MathHelper.TwoPi; if (minimumAngle <= -MathHelper.Pi) minimumAngle += MathHelper.TwoPi; } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { if (isLimitActive) { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } return 0; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// /// Twist axis in world space to attach to entity A. /// Twist axis in world space to attach to entity B. public void SetupJointTransforms(Vector3 worldTwistAxisA, Vector3 worldTwistAxisB) { worldTwistAxisA.Normalize(); worldTwistAxisB.Normalize(); Vector3 worldXAxis; Vector3.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis); float length = worldXAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis); } worldXAxis.Normalize(); //Complete A's basis. Vector3 worldYAxis; Vector3.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis); basisA.rotationMatrix = connectionA.orientationMatrix; basisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis); //Rotate the axis to B since it could be arbitrarily rotated. Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation); Vector3.Transform(ref worldXAxis, ref rotation, out worldXAxis); basisB.rotationMatrix = connectionB.orientationMatrix; basisB.SetWorldAxes(worldTwistAxisB, worldXAxis); } /// /// Solves for velocity. /// public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Clamp accumulated impulse (can't go negative) float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Max(accumulatedImpulse + lambda, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return Math.Abs(lambda); } /// /// Do any necessary computations to prepare the constraint for this frame. /// /// Simulation step length. public override void Update(float dt) { basisA.rotationMatrix = connectionA.orientationMatrix; basisB.rotationMatrix = connectionB.orientationMatrix; basisA.ComputeWorldSpaceAxes(); basisB.ComputeWorldSpaceAxes(); Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation); //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement. Vector3 twistMeasureAxis; Vector3.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis); //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value. float y, x; Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y); Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x); var angle = (float) Math.Atan2(y, x); float distanceFromCurrent, distanceFromMaximum; if (IsAngleValid(angle, out distanceFromCurrent, out distanceFromMaximum)) { isActiveInSolver = false; accumulatedImpulse = 0; error = 0; isLimitActive = false; return; } isLimitActive = true; //Compute the jacobian. if (error > 0) { Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB); if (jacobianB.LengthSquared() < Toolbox.Epsilon) { //A nasty singularity can show up if the axes are aligned perfectly. //In a 'real' situation, this is impossible, so just ignore it. isActiveInSolver = false; return; } jacobianB.Normalize(); jacobianA.X = -jacobianB.X; jacobianA.Y = -jacobianB.Y; jacobianA.Z = -jacobianB.Z; } else { //Reverse the jacobian so that the solver loop is easier. Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianA); if (jacobianA.LengthSquared() < Toolbox.Epsilon) { //A nasty singularity can show up if the axes are aligned perfectly. //In a 'real' situation, this is impossible, so just ignore it. isActiveInSolver = false; return; } jacobianA.Normalize(); jacobianB.X = -jacobianA.X; jacobianB.Y = -jacobianA.Y; jacobianB.Z = -jacobianA.Z; } //****** VELOCITY BIAS ******// //Compute the correction velocity. error = ComputeAngleError(distanceFromCurrent, distanceFromMaximum); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); //biasVelocity = MathHelper.Clamp(-error * myCorrectionStrength / dt, -myMaxCorrectiveVelocity, myMaxCorrectiveVelocity); biasVelocity = MathHelper.Min(MathHelper.Max(0, Math.Abs(error) - margin) * errorReduction, maxCorrectiveVelocity); if (bounciness > 0) { float relativeVelocity; float dot; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out relativeVelocity); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out dot); relativeVelocity += dot; if (-relativeVelocity > bounceVelocityThreshold) biasVelocity = MathHelper.Max(biasVelocity, -bounciness * relativeVelocity); } //The nice thing about this approach is that the jacobian entry doesn't flip. //Instead, the error can be negative due to the use of Atan2. //This is important for limits which have a unique high and low value. //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } private static float ComputeAngleError(float distanceFromCurrent, float distanceFromMaximum) { float errorFromMin = MathHelper.TwoPi - distanceFromCurrent; float errorFromMax = distanceFromCurrent - distanceFromMaximum; return errorFromMax > errorFromMin ? errorFromMin : -errorFromMax; } private float GetDistanceFromMinimum(float angle) { if (minimumAngle > 0) { if (angle >= minimumAngle) return angle - minimumAngle; if (angle > 0) return MathHelper.TwoPi - minimumAngle + angle; return MathHelper.TwoPi - minimumAngle + angle; } if (angle < minimumAngle) return MathHelper.TwoPi - minimumAngle + angle; return angle - minimumAngle; //else //if (currentAngle >= 0) // return angle - myMinimumAngle; } private bool IsAngleValid(float currentAngle, out float distanceFromCurrent, out float distanceFromMaximum) { distanceFromCurrent = GetDistanceFromMinimum(currentAngle); distanceFromMaximum = GetDistanceFromMinimum(maximumAngle); return distanceFromCurrent < distanceFromMaximum; } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/BallSocketJoint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; using System.Diagnostics; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Connects two entities with a spherical joint. Acts like an unrestricted shoulder joint. /// public class BallSocketJoint : Joint, I3DImpulseConstraintWithError, I3DJacobianConstraint { private Vector3 accumulatedImpulse; private Vector3 biasVelocity; private Vector3 localAnchorA; private Vector3 localAnchorB; private Matrix3x3 massMatrix; private Vector3 error; private Matrix3x3 rACrossProduct; private Matrix3x3 rBCrossProduct; private Vector3 worldOffsetA, worldOffsetB; /// /// Constructs a spherical joint. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the offsets (OffsetA, OffsetB or LocalOffsetA, LocalOffsetB). /// This constructor sets the constraint's IsActive property to false by default. /// public BallSocketJoint() { IsActive = false; } /// /// Constructs a spherical joint. /// /// First connected entity. /// Second connected entity. /// Location of the socket. public BallSocketJoint(Entity connectionA, Entity connectionB, Vector3 anchorLocation) { ConnectionA = connectionA; ConnectionB = connectionB; OffsetA = anchorLocation - ConnectionA.position; OffsetB = anchorLocation - ConnectionB.position; } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetA { get { return localAnchorA; } set { localAnchorA = value; Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetB { get { return localAnchorB; } set { localAnchorB = value; Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); } } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in world space. /// public Vector3 OffsetA { get { return worldOffsetA; } set { worldOffsetA = value; Matrix3x3.TransformTranspose(ref worldOffsetA, ref connectionA.orientationMatrix, out localAnchorA); } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in world space. /// public Vector3 OffsetB { get { return worldOffsetB; } set { worldOffsetB = value; Matrix3x3.TransformTranspose(ref worldOffsetB, ref connectionB.orientationMatrix, out localAnchorB); } } #region I3DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector3 RelativeVelocity { get { Vector3 cross; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross); Vector3.Add(ref connectionA.linearVelocity, ref cross, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross); Vector3.Add(ref connectionB.linearVelocity, ref cross, out bVel); return aVel - bVel; } } /// /// Gets the total impulse applied by this constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public Vector3 Error { get { return error; } } #endregion #region I3DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. /// Third linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. /// Third linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. /// Third angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = rACrossProduct.Right; jacobianY = rACrossProduct.Up; jacobianZ = rACrossProduct.Forward; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. /// Third angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = rBCrossProduct.Right; jacobianY = rBCrossProduct.Up; jacobianZ = rBCrossProduct.Forward; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out Matrix3x3 outputMassMatrix) { outputMassMatrix = massMatrix; } #endregion /// /// Calculates necessary information for velocity solving. /// Called by preStep(float dt) /// /// Time in seconds since the last update. public override void Update(float dt) { Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); float errorReductionParameter; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReductionParameter, out softness); //Mass Matrix Matrix3x3 k; Matrix3x3 linearComponent; Matrix3x3.CreateCrossProduct(ref worldOffsetA, out rACrossProduct); Matrix3x3.CreateCrossProduct(ref worldOffsetB, out rBCrossProduct); if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.CreateScale(connectionA.inverseMass + connectionB.inverseMass, out linearComponent); Matrix3x3 angularComponentA, angularComponentB; Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k); Matrix3x3.Subtract(ref k, ref angularComponentB, out k); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3x3.CreateScale(connectionA.inverseMass, out linearComponent); Matrix3x3 angularComponentA; Matrix3x3.Multiply(ref rACrossProduct, ref connectionA.inertiaTensorInverse, out angularComponentA); Matrix3x3.Multiply(ref angularComponentA, ref rACrossProduct, out angularComponentA); Matrix3x3.Subtract(ref linearComponent, ref angularComponentA, out k); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.CreateScale(connectionB.inverseMass, out linearComponent); Matrix3x3 angularComponentB; Matrix3x3.Multiply(ref rBCrossProduct, ref connectionB.inertiaTensorInverse, out angularComponentB); Matrix3x3.Multiply(ref angularComponentB, ref rBCrossProduct, out angularComponentB); Matrix3x3.Subtract(ref linearComponent, ref angularComponentB, out k); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } k.M11 += softness; k.M22 += softness; k.M33 += softness; Matrix3x3.Invert(ref k, out massMatrix); Vector3.Add(ref connectionB.position, ref worldOffsetB, out error); Vector3.Subtract(ref error, ref connectionA.position, out error); Vector3.Subtract(ref error, ref worldOffsetA, out error); Vector3.Multiply(ref error, -errorReductionParameter, out biasVelocity); //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref accumulatedImpulse); #if !WINDOWS Vector3 linear = new Vector3(); #else Vector3 linear; #endif if (connectionA.isDynamic) { linear.X = -accumulatedImpulse.X; linear.Y = -accumulatedImpulse.Y; linear.Z = -accumulatedImpulse.Z; connectionA.ApplyLinearImpulse(ref linear); Vector3 taImpulse; Vector3.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref accumulatedImpulse); Vector3 tbImpulse; Vector3.Cross(ref worldOffsetB, ref accumulatedImpulse, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } } /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { #if !WINDOWS Vector3 lambda = new Vector3(); #else Vector3 lambda; #endif //Velocity along the length. Vector3 cross; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref worldOffsetA, out cross); Vector3.Add(ref connectionA.linearVelocity, ref cross, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref worldOffsetB, out cross); Vector3.Add(ref connectionB.linearVelocity, ref cross, out bVel); lambda.X = aVel.X - bVel.X + biasVelocity.X - softness * accumulatedImpulse.X; lambda.Y = aVel.Y - bVel.Y + biasVelocity.Y - softness * accumulatedImpulse.Y; lambda.Z = aVel.Z - bVel.Z + biasVelocity.Z - softness * accumulatedImpulse.Z; //Turn the velocity into an impulse. Matrix3x3.Transform(ref lambda, ref massMatrix, out lambda); //ACcumulate the impulse Vector3.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); //Apply the impulse //Constraint.applyImpulse(myConnectionA, myConnectionB, ref rA, ref rB, ref impulse); #if !WINDOWS Vector3 linear = new Vector3(); #else Vector3 linear; #endif if (connectionA.isDynamic) { linear.X = -lambda.X; linear.Y = -lambda.Y; linear.Z = -lambda.Z; connectionA.ApplyLinearImpulse(ref linear); Vector3 taImpulse; Vector3.Cross(ref worldOffsetA, ref linear, out taImpulse); connectionA.ApplyAngularImpulse(ref taImpulse); } if (connectionB.isDynamic) { connectionB.ApplyLinearImpulse(ref lambda); Vector3 tbImpulse; Vector3.Cross(ref worldOffsetB, ref lambda, out tbImpulse); connectionB.ApplyAngularImpulse(ref tbImpulse); } return (Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/DistanceJoint.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constraint which tries to maintain the distance between points on two entities. /// public class DistanceJoint : Joint, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private Vector3 anchorA; private Vector3 anchorB; private float biasVelocity; private Vector3 jAngularA, jAngularB; private Vector3 jLinearA, jLinearB; /// /// Distance maintained between the anchors. /// protected float distance; private float error; private Vector3 localAnchorA; private Vector3 localAnchorB; private Vector3 offsetA, offsetB; private float velocityToImpulse; /// /// Constructs a distance joint. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the anchors (WorldAnchorA, WorldAnchorB or LocalAnchorA, LocalAnchorB) /// and the desired Distance. /// This constructor sets the constraint's IsActive property to false by default. /// public DistanceJoint() { IsActive = false; ConnectionA = connectionA; ConnectionB = connectionB; Distance = (anchorA - anchorB).Length(); WorldAnchorA = anchorA; WorldAnchorB = anchorB; } /// /// Constructs a distance joint. /// /// First body connected to the distance joint. /// Second body connected to the distance joint. /// Connection to the distance joint from the first connected body in world space. /// Connection to the distance joint from the second connected body in world space. public DistanceJoint(Entity connectionA, Entity connectionB, Vector3 anchorA, Vector3 anchorB) { ConnectionA = connectionA; ConnectionB = connectionB; Distance = (anchorA - anchorB).Length(); WorldAnchorA = anchorA; WorldAnchorB = anchorB; } /// /// Gets or sets the distance maintained between the anchors. /// public float Distance { get { return distance; } set { distance = Math.Max(0, value); } } /// /// Gets or sets the first entity's connection point in local space. /// public Vector3 LocalAnchorA { get { return localAnchorA; } set { localAnchorA = value; Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out anchorA); anchorA += connectionA.position; } } /// /// Gets or sets the first entity's connection point in local space. /// public Vector3 LocalAnchorB { get { return localAnchorB; } set { localAnchorB = value; Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out anchorB); anchorB += connectionB.position; } } /// /// Gets or sets the connection to the distance constraint from the first connected body in world space. /// public Vector3 WorldAnchorA { get { return anchorA; } set { anchorA = value; localAnchorA = Vector3.Transform(anchorA - connectionA.position, Quaternion.Conjugate(connectionA.orientation)); } } /// /// Gets or sets the connection to the distance constraint from the second connected body in world space. /// public Vector3 WorldAnchorB { get { return anchorB; } set { anchorB = value; localAnchorB = Vector3.Transform(anchorB - connectionB.position, Quaternion.Conjugate(connectionB.orientation)); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; return lambda; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = jLinearA; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = jLinearB; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jAngularA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jAngularB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { //Compute the current relative velocity. float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate impulse accumulatedImpulse += lambda; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, lambda, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, lambda, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Calculates necessary information for velocity solving. /// /// Time in seconds since the last update. public override void Update(float dt) { //Transform the anchors and offsets into world space. Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out offsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out offsetB); Vector3.Add(ref connectionA.position, ref offsetA, out anchorA); Vector3.Add(ref connectionB.position, ref offsetB, out anchorB); //Compute the distance. Vector3 separation; Vector3.Subtract(ref anchorB, ref anchorA, out separation); float currentDistance = separation.Length(); //Compute jacobians if (currentDistance > Toolbox.Epsilon) { jLinearB.X = separation.X / currentDistance; jLinearB.Y = separation.Y / currentDistance; jLinearB.Z = separation.Z / currentDistance; } else jLinearB = Toolbox.ZeroVector; jLinearA.X = -jLinearB.X; jLinearA.Y = -jLinearB.Y; jLinearA.Z = -jLinearB.Z; Vector3.Cross(ref offsetA, ref jLinearB, out jAngularA); //Still need to negate angular A. It's done after the effective mass matrix. Vector3.Cross(ref offsetB, ref jLinearB, out jAngularB); //Compute effective mass matrix if (connectionA.isDynamic && connectionB.isDynamic) { Vector3 aAngular; Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular); Vector3.Cross(ref aAngular, ref offsetA, out aAngular); Vector3 bAngular; Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular); Vector3.Cross(ref bAngular, ref offsetB, out bAngular); Vector3.Add(ref aAngular, ref bAngular, out aAngular); Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionA.inverseMass + connectionB.inverseMass; } else if (connectionA.isDynamic) { Vector3 aAngular; Matrix3x3.Transform(ref jAngularA, ref connectionA.localInertiaTensorInverse, out aAngular); Vector3.Cross(ref aAngular, ref offsetA, out aAngular); Vector3.Dot(ref aAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionA.inverseMass; } else if (connectionB.isDynamic) { Vector3 bAngular; Matrix3x3.Transform(ref jAngularB, ref connectionB.localInertiaTensorInverse, out bAngular); Vector3.Cross(ref bAngular, ref offsetB, out bAngular); Vector3.Dot(ref bAngular, ref jLinearB, out velocityToImpulse); velocityToImpulse += connectionB.inverseMass; } else { //No point in trying to solve with two kinematics. isActiveInSolver = false; accumulatedImpulse = 0; return; } float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); velocityToImpulse = 1 / (softness + velocityToImpulse); //Finish computing jacobian; it's down here as an optimization (since it didn't need to be negated in mass matrix) jAngularA.X = -jAngularA.X; jAngularA.Y = -jAngularA.Y; jAngularA.Z = -jAngularA.Z; //Compute bias velocity error = distance - currentDistance; biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, accumulatedImpulse, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, accumulatedImpulse, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/Joint.cs ================================================ using System; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Superclass of position-based constraints. /// public abstract class Joint : TwoEntityConstraint, ISpringSettings { /// /// Maximum extra velocity that the constraint will apply in an effort to correct constraint error. /// protected float maxCorrectiveVelocity = float.MaxValue; /// /// Squared maximum extra velocity that the constraint will apply in an effort to correct constraint error. /// protected float maxCorrectiveVelocitySquared = float.MaxValue; protected float softness; /// /// Spring settings define how a constraint responds to velocity and position error. /// protected SpringSettings springSettings = new SpringSettings(); /// /// Gets or sets the maximum extra velocity that the constraint will apply in an effort to correct any constraint error. /// public float MaxCorrectiveVelocity { get { return maxCorrectiveVelocity; } set { maxCorrectiveVelocity = Math.Max(0, value); if (maxCorrectiveVelocity >= float.MaxValue) { maxCorrectiveVelocitySquared = float.MaxValue; } else { maxCorrectiveVelocitySquared = maxCorrectiveVelocity * maxCorrectiveVelocity; } } } #region ISpringSettings Members /// /// Gets the spring settings used by the constraint. /// Spring settings define how a constraint responds to velocity and position error. /// public SpringSettings SpringSettings { get { return springSettings; } } #endregion } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/NoRotationJoint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constrains two entities so that they cannot rotate relative to each other. /// public class NoRotationJoint : Joint, I3DImpulseConstraintWithError, I3DJacobianConstraint { private Vector3 accumulatedImpulse; private Vector3 biasVelocity; private Matrix3x3 effectiveMassMatrix; private Quaternion initialQuaternionConjugateA; private Quaternion initialQuaternionConjugateB; private Vector3 error; /// /// Constructs a new constraint which prevents relative angular motion between the two connected bodies. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) and the initial orientations /// (InitialOrientationA, InitialOrientationB). /// This constructor sets the constraint's IsActive property to false by default. /// public NoRotationJoint() { IsActive = false; } /// /// Constructs a new constraint which prevents relative angular motion between the two connected bodies. /// /// First connection of the pair. /// Second connection of the pair. public NoRotationJoint(Entity connectionA, Entity connectionB) { ConnectionA = connectionA; ConnectionB = connectionB; initialQuaternionConjugateA = Quaternion.Conjugate(ConnectionA.orientation); initialQuaternionConjugateB = Quaternion.Conjugate(ConnectionB.orientation); } /// /// Gets or sets the initial orientation of the first connected entity. /// The constraint will try to maintain the relative orientation between the initialOrientationA and initialOrientationB. /// public Quaternion InitialOrientationA { get { return Quaternion.Conjugate(initialQuaternionConjugateA); } set { initialQuaternionConjugateA = Quaternion.Conjugate(value); } } /// /// Gets or sets the initial orientation of the second connected entity. /// The constraint will try to maintain the relative orientation between the initialOrientationA and initialOrientationB. /// public Quaternion InitialOrientationB { get { return Quaternion.Conjugate(initialQuaternionConjugateB); } set { initialQuaternionConjugateB = Quaternion.Conjugate(value); } } #region I3DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector3 RelativeVelocity { get { Vector3 velocityDifference; Vector3.Subtract(ref connectionB.angularVelocity, ref connectionA.angularVelocity, out velocityDifference); return velocityDifference; } } /// /// Gets the total impulse applied by this constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public Vector3 Error { get { return error; } } #endregion #region I3DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. /// Third linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; jacobianZ = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. /// Third linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; jacobianZ = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. /// Third angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. /// Third angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out Matrix3x3 outputMassMatrix) { outputMassMatrix = effectiveMassMatrix; } #endregion /// /// Applies the corrective impulses required by the constraint. /// public override float SolveIteration() { Vector3 velocityDifference; Vector3.Subtract(ref connectionB.angularVelocity, ref connectionA.angularVelocity, out velocityDifference); Vector3 softnessVector; Vector3.Multiply(ref accumulatedImpulse, softness, out softnessVector); Vector3 lambda; Vector3.Add(ref velocityDifference, ref biasVelocity, out lambda); Vector3.Subtract(ref lambda, ref softnessVector, out lambda); Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector3.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse); if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref lambda); } if (connectionB.isDynamic) { Vector3 torqueB; Vector3.Negate(ref lambda, out torqueB); connectionB.ApplyAngularImpulse(ref torqueB); } return Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z); } /// /// Initializes the constraint for the current frame. /// /// Time between frames. public override void Update(float dt) { Quaternion quaternionA; Quaternion.Multiply(ref connectionA.orientation, ref initialQuaternionConjugateA, out quaternionA); Quaternion quaternionB; Quaternion.Multiply(ref connectionB.orientation, ref initialQuaternionConjugateB, out quaternionB); Quaternion.Conjugate(ref quaternionB, out quaternionB); Quaternion intermediate; Quaternion.Multiply(ref quaternionA, ref quaternionB, out intermediate); float angle; Vector3 axis; Toolbox.GetAxisAngleFromQuaternion(ref intermediate, out axis, out angle); error.X = axis.X * angle; error.Y = axis.Y * angle; error.Z = axis.Z * angle; float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); errorReduction = -errorReduction; biasVelocity.X = errorReduction * error.X; biasVelocity.Y = errorReduction * error.Y; biasVelocity.Z = errorReduction * error.Z; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float) Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix); effectiveMassMatrix.M11 += softness; effectiveMassMatrix.M22 += softness; effectiveMassMatrix.M33 += softness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //apply accumulated impulse if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref accumulatedImpulse); } if (connectionB.isDynamic) { Vector3 torqueB; Vector3.Negate(ref accumulatedImpulse, out torqueB); connectionB.ApplyAngularImpulse(ref torqueB); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/PointOnLineJoint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constrains two entities so that one has a point that stays on a line defined by the other. /// public class PointOnLineJoint : Joint, I2DImpulseConstraintWithError, I2DJacobianConstraint { private Vector2 accumulatedImpulse; private Vector3 angularA1; private Vector3 angularA2; private Vector3 angularB1; private Vector3 angularB2; private Vector2 biasVelocity; private Vector3 localRestrictedAxis1; //on a private Vector3 localRestrictedAxis2; //on a private Vector2 error; private Vector3 localAxisAnchor; //on a private Vector3 localLineDirection; //on a private Vector3 localPoint; //on b private Vector3 worldLineAnchor; private Vector3 worldLineDirection; private Vector3 worldPoint; private Matrix2x2 negativeEffectiveMassMatrix; //Jacobians //(Linear jacobians are just: // axis 1 -axis 1 // axis 2 -axis 2 ) private Vector3 rA, rB; private Vector3 worldRestrictedAxis1, worldRestrictedAxis2; /// /// Constructs a joint which constrains a point of one body to be on a line based on the other body. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB), /// the LineAnchor, the LineDirection, and the Point (or the entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public PointOnLineJoint() { IsActive = false; } /// /// Constructs a joint which constrains a point of one body to be on a line based on the other body. /// /// First connected entity which defines the line. /// Second connected entity which has a point. /// Location off of which the line is based in world space. /// Direction of the line in world space. /// Location of the point anchored to connectionB in world space. public PointOnLineJoint(Entity connectionA, Entity connectionB, Vector3 lineAnchor, Vector3 lineDirection, Vector3 pointLocation) { ConnectionA = connectionA; ConnectionB = connectionB; LineAnchor = lineAnchor; LineDirection = lineDirection; Point = pointLocation; } /// /// Gets or sets the line anchor in world space. /// public Vector3 LineAnchor { get { return worldLineAnchor; } set { localAxisAnchor = value - connectionA.position; Matrix3x3.TransformTranspose(ref localAxisAnchor, ref connectionA.orientationMatrix, out localAxisAnchor); worldLineAnchor = value; } } /// /// Gets or sets the line direction in world space. /// public Vector3 LineDirection { get { return worldLineDirection; } set { worldLineDirection = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldLineDirection, ref connectionA.orientationMatrix, out localLineDirection); UpdateRestrictedAxes(); } } /// /// Gets or sets the line anchor in connection A's local space. /// public Vector3 LocalLineAnchor { get { return localAxisAnchor; } set { localAxisAnchor = value; Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor); Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor); } } /// /// Gets or sets the line direction in connection A's local space. /// public Vector3 LocalLineDirection { get { return localLineDirection; } set { localLineDirection = Vector3.Normalize(value); Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection); UpdateRestrictedAxes(); } } /// /// Gets or sets the point's location in connection B's local space. /// The point is the location that is attached to the line. /// public Vector3 LocalPoint { get { return localPoint; } set { localPoint = value; Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out worldPoint); Vector3.Add(ref worldPoint, ref connectionB.position, out worldPoint); } } /// /// Gets the offset from A to the connection point between the entities. /// public Vector3 OffsetA { get { return rA; } } /// /// Gets the offset from B to the connection point between the entities. /// public Vector3 OffsetB { get { return rB; } } /// /// Gets or sets the point's location in world space. /// The point is the location on connection B that is attached to the line. /// public Vector3 Point { get { return worldPoint; } set { worldPoint = value; localPoint = worldPoint - connectionB.position; Matrix3x3.TransformTranspose(ref localPoint, ref connectionB.orientationMatrix, out localPoint); } } #region I2DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector2 RelativeVelocity { get { #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif Vector3 dv; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3.Subtract(ref aVel, ref bVel, out dv); Vector3.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X); Vector3.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y); return lambda; } } /// /// Gets the total impulse applied by this constraint. /// public Vector2 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public Vector2 Error { get { return error; } } #endregion #region I2DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = worldRestrictedAxis1; jacobianY = worldRestrictedAxis2; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = -worldRestrictedAxis1; jacobianY = -worldRestrictedAxis2; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = angularA1; jacobianY = angularA2; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = angularB1; jacobianY = angularB2; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out Matrix2x2 massMatrix) { Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out massMatrix); } #endregion /// /// Calculates and applies corrective impulses. /// Called automatically by space. /// public override float SolveIteration() { #region Theory //lambda = -mc * (Jv + b) // PraT = [ bx by bz ] * [ 0 raz -ray ] = [ (-by * raz + bz * ray) (bx * raz - bz * rax) (-bx * ray + by * rax) ] // [ cx cy cz ] [ -raz 0 rax ] [ (-cy * raz + cz * ray) (cx * raz - cz * rax) (-cx * ray + cy * rax) ] // [ ray -rax 0 ] // // PrbT = [ bx by bz ] * [ 0 rbz -rby ] = [ (-by * rbz + bz * rby) (bx * rbz - bz * rbx) (-bx * rby + by * rbx) ] // [ cx cy cz ] [ -rbz 0 rbx ] [ (-cy * rbz + cz * rby) (cx * rbz - cz * rbx) (-cx * rby + cy * rbx) ] // [ rby -rbx 0 ] // Jv = [ bx by bz PraT -bx -by -bz -Prbt ] * [ vax ] // [ cx cy cz -cx -cy -cz ] [ vay ] // [ vaz ] // [ wax ] // [ way ] // [ waz ] // [ vbx ] // [ vby ] // [ vbz ] // [ wbx ] // [ wby ] // [ wbz ] // va' = [ bx * vax + by * vay + bz * vaz ] = [ b * va ] // [ cx * vax + cy * vay + cz * vaz ] [ c * va ] // wa' = [ (PraT row 1) * wa ] // [ (PraT row 2) * wa ] // vb' = [ -bx * vbx - by * vby - bz * vbz ] = [ -b * vb ] // [ -cx * vbx - cy * vby - cz * vbz ] [ -c * vb ] // wb' = [ -(PrbT row 1) * wb ] // [ -(PrbT row 2) * wb ] // Jv = [ b * va + (PraT row 1) * wa - b * vb - (PrbT row 1) * wb ] // [ c * va + (PraT row 2) * wa - c * vb - (PrbT row 2) * wb ] // Jv = [ b * (va + wa x ra - vb - wb x rb) ] // [ c * (va + wa x ra - vb - wb x rb) ] //P = JT * lambda #endregion #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif //float va1, va2, wa1, wa2, vb1, vb2, wb1, wb2; //Vector3.Dot(ref worldAxis1, ref myParentA.myInternalLinearVelocity, out va1); //Vector3.Dot(ref worldAxis2, ref myParentA.myInternalLinearVelocity, out va2); //wa1 = prAT.M11 * myParentA.myInternalAngularVelocity.X + prAT.M12 * myParentA.myInternalAngularVelocity.Y + prAT.M13 * myParentA.myInternalAngularVelocity.Z; //wa2 = prAT.M21 * myParentA.myInternalAngularVelocity.X + prAT.M22 * myParentA.myInternalAngularVelocity.Y + prAT.M23 * myParentA.myInternalAngularVelocity.Z; //Vector3.Dot(ref worldAxis1, ref myParentB.myInternalLinearVelocity, out vb1); //Vector3.Dot(ref worldAxis2, ref myParentB.myInternalLinearVelocity, out vb2); //wb1 = prBT.M11 * myParentB.myInternalAngularVelocity.X + prBT.M12 * myParentB.myInternalAngularVelocity.Y + prBT.M13 * myParentB.myInternalAngularVelocity.Z; //wb2 = prBT.M21 * myParentB.myInternalAngularVelocity.X + prBT.M22 * myParentB.myInternalAngularVelocity.Y + prBT.M23 * myParentB.myInternalAngularVelocity.Z; //lambda.X = va1 + wa1 - vb1 - wb1 + biasVelocity.X + mySoftness * accumulatedImpulse.X; //lambda.Y = va2 + wa2 - vb2 - wb2 + biasVelocity.Y + mySoftness * accumulatedImpulse.Y; Vector3 dv; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3.Subtract(ref aVel, ref bVel, out dv); Vector3.Dot(ref dv, ref worldRestrictedAxis1, out lambda.X); Vector3.Dot(ref dv, ref worldRestrictedAxis2, out lambda.Y); lambda.X += biasVelocity.X + softness * accumulatedImpulse.X; lambda.Y += biasVelocity.Y + softness * accumulatedImpulse.Y; //Convert to impulse Matrix2x2.Transform(ref lambda, ref negativeEffectiveMassMatrix, out lambda); Vector2.Add(ref lambda, ref accumulatedImpulse, out accumulatedImpulse); float x = lambda.X; float y = lambda.Y; //Apply impulse #if !WINDOWS Vector3 impulse = new Vector3(); Vector3 torque= new Vector3(); #else Vector3 impulse; Vector3 torque; #endif impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y; impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y; impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y; if (connectionA.isDynamic) { torque.X = x * angularA1.X + y * angularA2.X; torque.Y = x * angularA1.Y + y * angularA2.Y; torque.Z = x * angularA1.Z + y * angularA2.Z; connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { impulse.X = -impulse.X; impulse.Y = -impulse.Y; impulse.Z = -impulse.Z; torque.X = x * angularB1.X + y * angularB2.X; torque.Y = x * angularB1.Y + y * angularB2.Y; torque.Z = x * angularB1.Z + y * angularB2.Z; connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } return (Math.Abs(lambda.X) + Math.Abs(lambda.Y)); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Transform local axes into world space Matrix3x3.Transform(ref localRestrictedAxis1, ref connectionA.orientationMatrix, out worldRestrictedAxis1); Matrix3x3.Transform(ref localRestrictedAxis2, ref connectionA.orientationMatrix, out worldRestrictedAxis2); Matrix3x3.Transform(ref localAxisAnchor, ref connectionA.orientationMatrix, out worldLineAnchor); Vector3.Add(ref worldLineAnchor, ref connectionA.position, out worldLineAnchor); Matrix3x3.Transform(ref localLineDirection, ref connectionA.orientationMatrix, out worldLineDirection); //Transform local Matrix3x3.Transform(ref localPoint, ref connectionB.orientationMatrix, out rB); Vector3.Add(ref rB, ref connectionB.position, out worldPoint); //Find the point on the line closest to the world point. Vector3 offset; Vector3.Subtract(ref worldPoint, ref worldLineAnchor, out offset); float distanceAlongAxis; Vector3.Dot(ref offset, ref worldLineDirection, out distanceAlongAxis); Vector3 worldNearPoint; Vector3.Multiply(ref worldLineDirection, distanceAlongAxis, out offset); Vector3.Add(ref worldLineAnchor, ref offset, out worldNearPoint); Vector3.Subtract(ref worldNearPoint, ref connectionA.position, out rA); //Error Vector3 error3D; Vector3.Subtract(ref worldPoint, ref worldNearPoint, out error3D); Vector3.Dot(ref error3D, ref worldRestrictedAxis1, out error.X); Vector3.Dot(ref error3D, ref worldRestrictedAxis2, out error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); float bias = -errorReduction; biasVelocity.X = bias * error.X; biasVelocity.Y = bias * error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float)Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } //Set up the jacobians Vector3.Cross(ref rA, ref worldRestrictedAxis1, out angularA1); Vector3.Cross(ref worldRestrictedAxis1, ref rB, out angularB1); Vector3.Cross(ref rA, ref worldRestrictedAxis2, out angularA2); Vector3.Cross(ref worldRestrictedAxis2, ref rB, out angularB2); float m11 = 0, m22 = 0, m1221 = 0; float inverseMass; Vector3 intermediate; //Compute the effective mass matrix. if (connectionA.isDynamic) { inverseMass = connectionA.inverseMass; Matrix3x3.Transform(ref angularA1, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA1, out m11); m11 += inverseMass; Vector3.Dot(ref intermediate, ref angularA2, out m1221); Matrix3x3.Transform(ref angularA2, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularA2, out m22); m22 += inverseMass; } #region Mass Matrix B if (connectionB.isDynamic) { float extra; inverseMass = connectionB.inverseMass; Matrix3x3.Transform(ref angularB1, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB1, out extra); m11 += inverseMass + extra; Vector3.Dot(ref intermediate, ref angularB2, out extra); m1221 += extra; Matrix3x3.Transform(ref angularB2, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref angularB2, out extra); m22 += inverseMass + extra; } #endregion negativeEffectiveMassMatrix.M11 = m11 + softness; negativeEffectiveMassMatrix.M12 = m1221; negativeEffectiveMassMatrix.M21 = m1221; negativeEffectiveMassMatrix.M22 = m22 + softness; Matrix2x2.Invert(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix); Matrix2x2.Negate(ref negativeEffectiveMassMatrix, out negativeEffectiveMassMatrix); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 impulse = new Vector3(); Vector3 torque= new Vector3(); #else Vector3 impulse; Vector3 torque; #endif float x = accumulatedImpulse.X; float y = accumulatedImpulse.Y; impulse.X = worldRestrictedAxis1.X * x + worldRestrictedAxis2.X * y; impulse.Y = worldRestrictedAxis1.Y * x + worldRestrictedAxis2.Y * y; impulse.Z = worldRestrictedAxis1.Z * x + worldRestrictedAxis2.Z * y; if (connectionA.isDynamic) { torque.X = x * angularA1.X + y * angularA2.X; torque.Y = x * angularA1.Y + y * angularA2.Y; torque.Z = x * angularA1.Z + y * angularA2.Z; connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { impulse.X = -impulse.X; impulse.Y = -impulse.Y; impulse.Z = -impulse.Z; torque.X = x * angularB1.X + y * angularB2.X; torque.Y = x * angularB1.Y + y * angularB2.Y; torque.Z = x * angularB1.Z + y * angularB2.Z; connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } } private void UpdateRestrictedAxes() { localRestrictedAxis1 = Vector3.Cross(Vector3.Up, localLineDirection); if (localRestrictedAxis1.LengthSquared() < .001f) { localRestrictedAxis1 = Vector3.Cross(Vector3.Right, localLineDirection); } localRestrictedAxis2 = Vector3.Cross(localLineDirection, localRestrictedAxis1); localRestrictedAxis1.Normalize(); localRestrictedAxis2.Normalize(); } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/PointOnPlaneJoint.cs ================================================ using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constrains a point on one body to be on a plane defined by another body. /// public class PointOnPlaneJoint : Joint, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private float biasVelocity; private float error; private Vector3 localPlaneAnchor; private Vector3 localPlaneNormal; private Vector3 localPointAnchor; private Vector3 worldPlaneAnchor; private Vector3 worldPlaneNormal; private Vector3 worldPointAnchor; private float negativeEffectiveMass; private Vector3 rA; private Vector3 rAcrossN; private Vector3 rB; private Vector3 rBcrossN; /// /// Constructs a new point on plane constraint. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the PlaneAnchor, PlaneNormal, and PointAnchor (or their entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public PointOnPlaneJoint() { IsActive = false; } /// /// Constructs a new point on plane constraint. /// /// Entity to which the constraint's plane is attached. /// Entity to which the constraint's point is attached. /// A point on the plane. /// Direction, attached to the first connected entity, defining the plane's normal /// The point to constrain to the plane, attached to the second connected object. public PointOnPlaneJoint(Entity connectionA, Entity connectionB, Vector3 planeAnchor, Vector3 normal, Vector3 pointAnchor) { ConnectionA = connectionA; ConnectionB = connectionB; PointAnchor = pointAnchor; PlaneAnchor = planeAnchor; PlaneNormal = normal; } /// /// Gets or sets the plane's anchor in entity A's local space. /// public Vector3 LocalPlaneAnchor { get { return localPlaneAnchor; } set { localPlaneAnchor = value; Matrix3x3.Transform(ref localPlaneAnchor, ref connectionA.orientationMatrix, out worldPlaneAnchor); Vector3.Add(ref connectionA.position, ref worldPlaneAnchor, out worldPlaneAnchor); } } /// /// Gets or sets the plane's normal in entity A's local space. /// public Vector3 LocalPlaneNormal { get { return localPlaneNormal; } set { localPlaneNormal = Vector3.Normalize(value); Matrix3x3.Transform(ref localPlaneNormal, ref connectionA.orientationMatrix, out worldPlaneNormal); } } /// /// Gets or sets the point anchor in entity B's local space. /// public Vector3 LocalPointAnchor { get { return localPointAnchor; } set { localPointAnchor = value; Matrix3x3.Transform(ref localPointAnchor, ref connectionB.orientationMatrix, out worldPointAnchor); Vector3.Add(ref worldPointAnchor, ref connectionB.position, out worldPointAnchor); } } /// /// Gets the offset from A to the connection point between the entities. /// public Vector3 OffsetA { get { return rA; } } /// /// Gets the offset from B to the connection point between the entities. /// public Vector3 OffsetB { get { return rB; } } /// /// Gets or sets the plane anchor in world space. /// public Vector3 PlaneAnchor { get { return worldPlaneAnchor; } set { worldPlaneAnchor = value; localPlaneAnchor = value - connectionA.position; Matrix3x3.TransformTranspose(ref localPlaneAnchor, ref connectionA.orientationMatrix, out localPlaneAnchor); } } /// /// Gets or sets the plane's normal in world space. /// public Vector3 PlaneNormal { get { return worldPlaneNormal; } set { worldPlaneNormal = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldPlaneNormal, ref connectionA.orientationMatrix, out localPlaneNormal); } } /// /// Gets or sets the point anchor in world space. /// public Vector3 PointAnchor { get { return worldPointAnchor; } set { worldPointAnchor = value; localPointAnchor = value - connectionB.position; Matrix3x3.TransformTranspose(ref localPointAnchor, ref connectionB.orientationMatrix, out localPointAnchor); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { Vector3 dv; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3.Subtract(ref aVel, ref bVel, out dv); float velocityDifference; Vector3.Dot(ref dv, ref worldPlaneNormal, out velocityDifference); return velocityDifference; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = worldPlaneNormal; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = -worldPlaneNormal; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = rAcrossN; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = -rBcrossN; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = -negativeEffectiveMass; } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //TODO: This could technically be faster. //Form the jacobian explicitly. //Cross cross add add subtract dot //vs //dot dot dot dot and then scalar adds Vector3 dv; Vector3 aVel, bVel; Vector3.Cross(ref connectionA.angularVelocity, ref rA, out aVel); Vector3.Add(ref aVel, ref connectionA.linearVelocity, out aVel); Vector3.Cross(ref connectionB.angularVelocity, ref rB, out bVel); Vector3.Add(ref bVel, ref connectionB.linearVelocity, out bVel); Vector3.Subtract(ref aVel, ref bVel, out dv); float velocityDifference; Vector3.Dot(ref dv, ref worldPlaneNormal, out velocityDifference); //if(velocityDifference > 0) // Debug.WriteLine("Velocity difference: " + velocityDifference); //Debug.WriteLine("softness velocity: " + softness * accumulatedImpulse); float lambda = negativeEffectiveMass * (velocityDifference + biasVelocity + softness * accumulatedImpulse); accumulatedImpulse += lambda; Vector3 impulse; Vector3 torque; Vector3.Multiply(ref worldPlaneNormal, lambda, out impulse); if (connectionA.isDynamic) { Vector3.Multiply(ref rAcrossN, lambda, out torque); connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); Vector3.Multiply(ref rBcrossN, lambda, out torque); connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } return lambda; } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { Matrix3x3.Transform(ref localPlaneNormal, ref connectionA.orientationMatrix, out worldPlaneNormal); Matrix3x3.Transform(ref localPlaneAnchor, ref connectionA.orientationMatrix, out worldPlaneAnchor); Vector3.Add(ref worldPlaneAnchor, ref connectionA.position, out worldPlaneAnchor); Matrix3x3.Transform(ref localPointAnchor, ref connectionB.orientationMatrix, out rB); Vector3.Add(ref rB, ref connectionB.position, out worldPointAnchor); //Find rA and rB. //So find the closest point on the plane to worldPointAnchor. float pointDistance, planeDistance; Vector3.Dot(ref worldPointAnchor, ref worldPlaneNormal, out pointDistance); Vector3.Dot(ref worldPlaneAnchor, ref worldPlaneNormal, out planeDistance); float distanceChange = planeDistance - pointDistance; Vector3 closestPointOnPlane; Vector3.Multiply(ref worldPlaneNormal, distanceChange, out closestPointOnPlane); Vector3.Add(ref closestPointOnPlane, ref worldPointAnchor, out closestPointOnPlane); Vector3.Subtract(ref closestPointOnPlane, ref connectionA.position, out rA); Vector3.Cross(ref rA, ref worldPlaneNormal, out rAcrossN); Vector3.Cross(ref rB, ref worldPlaneNormal, out rBcrossN); Vector3.Negate(ref rBcrossN, out rBcrossN); Vector3 offset; Vector3.Subtract(ref worldPointAnchor, ref closestPointOnPlane, out offset); Vector3.Dot(ref offset, ref worldPlaneNormal, out error); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); biasVelocity = MathHelper.Clamp(-errorReduction * error, -maxCorrectiveVelocity, maxCorrectiveVelocity); if (connectionA.IsDynamic && connectionB.IsDynamic) { Vector3 IrACrossN, IrBCrossN; Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN); Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN); float angularA, angularB; Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA); Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB); negativeEffectiveMass = connectionA.inverseMass + connectionB.inverseMass + angularA + angularB; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else if (connectionA.IsDynamic && !connectionB.IsDynamic) { Vector3 IrACrossN; Matrix3x3.Transform(ref rAcrossN, ref connectionA.inertiaTensorInverse, out IrACrossN); float angularA; Vector3.Dot(ref rAcrossN, ref IrACrossN, out angularA); negativeEffectiveMass = connectionA.inverseMass + angularA; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else if (!connectionA.IsDynamic && connectionB.IsDynamic) { Vector3 IrBCrossN; Matrix3x3.Transform(ref rBcrossN, ref connectionB.inertiaTensorInverse, out IrBCrossN); float angularB; Vector3.Dot(ref rBcrossN, ref IrBCrossN, out angularB); negativeEffectiveMass = connectionB.inverseMass + angularB; negativeEffectiveMass = -1 / (negativeEffectiveMass + softness); } else negativeEffectiveMass = 0; } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm Starting Vector3 impulse; Vector3 torque; Vector3.Multiply(ref worldPlaneNormal, accumulatedImpulse, out impulse); if (connectionA.isDynamic) { Vector3.Multiply(ref rAcrossN, accumulatedImpulse, out torque); connectionA.ApplyLinearImpulse(ref impulse); connectionA.ApplyAngularImpulse(ref torque); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); Vector3.Multiply(ref rBcrossN, accumulatedImpulse, out torque); connectionB.ApplyLinearImpulse(ref impulse); connectionB.ApplyAngularImpulse(ref torque); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/RevoluteAngularJoint.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constrains two entities to rotate only around a single axis. /// Acts like the angular portion of a hinge joint. /// public class RevoluteAngularJoint : Joint, I2DImpulseConstraintWithError, I2DJacobianConstraint { private Vector2 accumulatedImpulse; private Vector2 biasVelocity; private Matrix2x2 effectiveMassMatrix; private Vector3 localAxisA, localAxisB; private Vector3 localConstrainedAxis1, localConstrainedAxis2; //Not a and b because they are both based on a... private Vector2 error; private Vector3 worldAxisA, worldAxisB; private Vector3 worldConstrainedAxis1, worldConstrainedAxis2; /// /// Constructs a new orientation joint. /// Orientation joints can be used to simulate the angular portion of a hinge. /// Orientation joints allow rotation around only a single axis. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the WorldFreeAxisA and WorldFreeAxisB (or their entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public RevoluteAngularJoint() { IsActive = false; } /// /// Constructs a new orientation joint. /// Orientation joints can be used to simulate the angular portion of a hinge. /// Orientation joints allow rotation around only a single axis. /// /// First entity connected in the orientation joint. /// Second entity connected in the orientation joint. /// Axis allowed to rotate freely in world space. public RevoluteAngularJoint(Entity connectionA, Entity connectionB, Vector3 freeAxis) { ConnectionA = connectionA; ConnectionB = connectionB; //rA and rB store the local version of the axis. WorldFreeAxisA = freeAxis; WorldFreeAxisB = freeAxis; } /// /// Gets or sets the free axis in connection A's local space. /// Updates the internal restricted axes. /// public Vector3 LocalFreeAxisA { get { return localAxisA; } set { localAxisA = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); UpdateRestrictedAxes(); } } /// /// Gets or sets the free axis in connection B's local space. /// public Vector3 LocalFreeAxisB { get { return localAxisB; } set { localAxisB = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); } } /// /// Gets or sets the free axis attached to connection A in world space. /// This does not change the other connection's free axis. /// Updates the internal restricted axes. /// public Vector3 WorldFreeAxisA { get { return worldAxisA; } set { worldAxisA = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldAxisA, ref connectionA.orientationMatrix, out localAxisA); UpdateRestrictedAxes(); } } /// /// Gets or sets the free axis attached to connection A in world space. /// This does not change the other connection's free axis. /// public Vector3 WorldFreeAxisB { get { return worldAxisB; } set { worldAxisB = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldAxisB, ref connectionB.orientationMatrix, out localAxisB); } } #region I2DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector2 RelativeVelocity { get { Vector3 velocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity); #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif Vector3.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X); Vector3.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y); return lambda; } } /// /// Gets the total impulse applied by this constraint. /// public Vector2 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public Vector2 Error { get { return error; } } #endregion #region I2DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = worldConstrainedAxis1; jacobianY = worldConstrainedAxis2; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY) { jacobianX = -worldConstrainedAxis1; jacobianY = -worldConstrainedAxis2; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out Matrix2x2 massMatrix) { massMatrix = effectiveMassMatrix; } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { // lambda = -mc * (Jv + b) // P = JT * lambda Vector3 velocity; Vector3.Subtract(ref connectionA.angularVelocity, ref connectionB.angularVelocity, out velocity); #if !WINDOWS Vector2 lambda = new Vector2(); #else Vector2 lambda; #endif Vector3.Dot(ref worldConstrainedAxis1, ref velocity, out lambda.X); Vector3.Dot(ref worldConstrainedAxis2, ref velocity, out lambda.Y); Vector2.Add(ref lambda, ref biasVelocity, out lambda); Vector2 softnessImpulse; Vector2.Multiply(ref accumulatedImpulse, softness, out softnessImpulse); Vector2.Add(ref lambda, ref softnessImpulse, out lambda); Matrix2x2.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector2.Add(ref accumulatedImpulse, ref lambda, out accumulatedImpulse); #if !WINDOWS Vector3 impulse = new Vector3(); #else Vector3 impulse; #endif impulse.X = worldConstrainedAxis1.X * lambda.X + worldConstrainedAxis2.X * lambda.Y; impulse.Y = worldConstrainedAxis1.Y * lambda.X + worldConstrainedAxis2.Y * lambda.Y; impulse.Z = worldConstrainedAxis1.Z * lambda.X + worldConstrainedAxis2.Z * lambda.Y; if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda.X) + Math.Abs(lambda.Y)); } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); Matrix3x3.Transform(ref localConstrainedAxis1, ref connectionA.orientationMatrix, out worldConstrainedAxis1); Matrix3x3.Transform(ref localConstrainedAxis2, ref connectionA.orientationMatrix, out worldConstrainedAxis2); Vector3 error; Vector3.Cross(ref worldAxisA, ref worldAxisB, out error); Vector3.Dot(ref error, ref worldConstrainedAxis1, out this.error.X); Vector3.Dot(ref error, ref worldConstrainedAxis2, out this.error.Y); float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); errorReduction = -errorReduction; biasVelocity.X = errorReduction * this.error.X; biasVelocity.Y = errorReduction * this.error.Y; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > maxCorrectiveVelocitySquared) { float multiplier = maxCorrectiveVelocity / (float) Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; } Vector3 axis1I, axis2I; if (connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3 inertiaTensorSum; Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out inertiaTensorSum); Matrix3x3.Transform(ref worldConstrainedAxis1, ref inertiaTensorSum, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref inertiaTensorSum, out axis2I); } else if (connectionA.isDynamic && !connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionA.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionA.inertiaTensorInverse, out axis2I); } else if (!connectionA.isDynamic && connectionB.isDynamic) { Matrix3x3.Transform(ref worldConstrainedAxis1, ref connectionB.inertiaTensorInverse, out axis1I); Matrix3x3.Transform(ref worldConstrainedAxis2, ref connectionB.inertiaTensorInverse, out axis2I); } else { throw new InvalidOperationException("Cannot constrain two kinematic bodies."); } Vector3.Dot(ref axis1I, ref worldConstrainedAxis1, out effectiveMassMatrix.M11); Vector3.Dot(ref axis1I, ref worldConstrainedAxis2, out effectiveMassMatrix.M12); Vector3.Dot(ref axis2I, ref worldConstrainedAxis1, out effectiveMassMatrix.M21); Vector3.Dot(ref axis2I, ref worldConstrainedAxis2, out effectiveMassMatrix.M22); effectiveMassMatrix.M11 += softness; effectiveMassMatrix.M22 += softness; Matrix2x2.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); Matrix2x2.Negate(ref effectiveMassMatrix, out effectiveMassMatrix); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm Starting #if !WINDOWS Vector3 impulse = new Vector3(); #else Vector3 impulse; #endif impulse.X = worldConstrainedAxis1.X * accumulatedImpulse.X + worldConstrainedAxis2.X * accumulatedImpulse.Y; impulse.Y = worldConstrainedAxis1.Y * accumulatedImpulse.X + worldConstrainedAxis2.Y * accumulatedImpulse.Y; impulse.Z = worldConstrainedAxis1.Z * accumulatedImpulse.X + worldConstrainedAxis2.Z * accumulatedImpulse.Y; if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Negate(ref impulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } private void UpdateRestrictedAxes() { localConstrainedAxis1 = Vector3.Cross(Vector3.Up, localAxisA); if (localConstrainedAxis1.LengthSquared() < .001f) { localConstrainedAxis1 = Vector3.Cross(Vector3.Right, localAxisA); } localConstrainedAxis2 = Vector3.Cross(localAxisA, localConstrainedAxis1); localConstrainedAxis1.Normalize(); localConstrainedAxis2.Normalize(); } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/SwivelHingeAngularJoint.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Constrains two bodies so that they can rotate relative to each other like a modified door hinge. /// Instead of removing two degrees of freedom, only one is removed so that the second connection to the constraint can twist. /// public class SwivelHingeAngularJoint : Joint, I1DImpulseConstraintWithError, I1DJacobianConstraint { private float accumulatedImpulse; private float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; private Vector3 localHingeAxis; private Vector3 localTwistAxis; private Vector3 worldHingeAxis; private Vector3 worldTwistAxis; private float velocityToImpulse; /// /// Constructs a new constraint which allows relative angular motion around a hinge axis and a twist axis. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the WorldHingeAxis and WorldTwistAxis (or their entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public SwivelHingeAngularJoint() { IsActive = false; } /// /// Constructs a new constraint which allows relative angular motion around a hinge axis and a twist axis. /// /// First connection of the pair. /// Second connection of the pair. /// Hinge axis attached to connectionA. /// The connected entities will be able to rotate around this axis relative to each other. /// Twist axis attached to connectionB. /// The connected entities will be able to rotate around this axis relative to each other. public SwivelHingeAngularJoint(Entity connectionA, Entity connectionB, Vector3 worldHingeAxis, Vector3 worldTwistAxis) { ConnectionA = connectionA; ConnectionB = connectionB; WorldHingeAxis = worldHingeAxis; WorldTwistAxis = worldTwistAxis; } /// /// Gets or sets the hinge axis attached to entity A in its local space. /// public Vector3 LocalHingeAxis { get { return localHingeAxis; } set { localHingeAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localHingeAxis, ref connectionA.orientationMatrix, out worldHingeAxis); } } /// /// Gets or sets the twist axis attached to entity B in its local space. /// public Vector3 LocalTwistAxis { get { return localTwistAxis; } set { localTwistAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localTwistAxis, ref connectionB.orientationMatrix, out worldTwistAxis); } } /// /// Gets or sets the hinge axis attached to entity A in world space. /// public Vector3 WorldHingeAxis { get { return worldHingeAxis; } set { worldHingeAxis = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionA.orientation, out conjugate); Vector3.Transform(ref worldHingeAxis, ref conjugate, out localHingeAxis); } } /// /// Gets or sets the axis attached to the first connected entity in world space. /// public Vector3 WorldTwistAxis { get { return worldTwistAxis; } set { worldTwistAxis = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionB.orientation, out conjugate); Vector3.Transform(ref worldTwistAxis, ref conjugate, out localTwistAxis); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Solves for velocity. /// public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) - biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate the impulse accumulatedImpulse += lambda; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Do any necessary computations to prepare the constraint for this frame. /// /// Simulation step length. public override void Update(float dt) { //Transform the axes into world space. Matrix3x3.Transform(ref localHingeAxis, ref connectionA.orientationMatrix, out worldHingeAxis); Matrix3x3.Transform(ref localTwistAxis, ref connectionB.orientationMatrix, out worldTwistAxis); //****** VELOCITY BIAS ******// Vector3.Dot(ref worldHingeAxis, ref worldTwistAxis, out error); //Compute the correction velocity. float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); biasVelocity = MathHelper.Clamp(error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity); //Compute the jacobian Vector3.Cross(ref worldHingeAxis, ref worldTwistAxis, out jacobianA); float length = jacobianA.LengthSquared(); if (length > Toolbox.Epsilon) Vector3.Divide(ref jacobianA, (float)Math.Sqrt(length), out jacobianA); else jacobianA = new Vector3(); jacobianB.X = -jacobianA.X; jacobianB.Y = -jacobianA.Y; jacobianB.Z = -jacobianA.Z; //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Joints/TwistJoint.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Joints { /// /// Prevents the connected entities from twisting relative to each other. /// Acts like the angular part of a universal joint. /// public class TwistJoint : Joint, I1DImpulseConstraintWithError, I1DJacobianConstraint { private Vector3 aLocalAxisY, aLocalAxisZ; private float accumulatedImpulse; private Vector3 bLocalAxisY; private float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; private Vector3 localAxisA; private Vector3 localAxisB; private Vector3 worldAxisA; private Vector3 worldAxisB; private float velocityToImpulse; /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the WorldAxisA and WorldAxisB (or their entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public TwistJoint() { IsActive = false; } /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other. /// /// First connection of the pair. /// Second connection of the pair. /// Twist axis attached to the first connected entity. /// Twist axis attached to the second connected entity. public TwistJoint(Entity connectionA, Entity connectionB, Vector3 axisA, Vector3 axisB) { ConnectionA = connectionA; ConnectionB = connectionB; WorldAxisA = axisA; WorldAxisB = axisB; } /// /// Gets or sets the axis attached to the first connected entity in its local space. /// public Vector3 LocalAxisA { get { return localAxisA; } set { localAxisA = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Initialize(); } } /// /// Gets or sets the axis attached to the first connected entity in its local space. /// public Vector3 LocalAxisB { get { return localAxisB; } set { localAxisB = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxisB, ref connectionA.orientationMatrix, out worldAxisB); Initialize(); } } /// /// Gets or sets the axis attached to the first connected entity in world space. /// public Vector3 WorldAxisA { get { return worldAxisA; } set { worldAxisA = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionA.orientation, out conjugate); Vector3.Transform(ref worldAxisA, ref conjugate, out localAxisA); Initialize(); } } /// /// Gets or sets the axis attached to the first connected entity in world space. /// public Vector3 WorldAxisB { get { return worldAxisB; } set { worldAxisB = Vector3.Normalize(value); Quaternion conjugate; Quaternion.Conjugate(ref connectionA.orientation, out conjugate); Vector3.Transform(ref worldAxisB, ref conjugate, out localAxisB); Initialize(); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float velocityA, velocityB; Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Solves for velocity. /// public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) + biasVelocity - softness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate the impulse accumulatedImpulse += lambda; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } /// /// Do any necessary computations to prepare the constraint for this frame. /// /// Simulation step length. public override void Update(float dt) { Vector3 aAxisY, aAxisZ; Vector3 bAxisY; Matrix3x3.Transform(ref localAxisA, ref connectionA.orientationMatrix, out worldAxisA); Matrix3x3.Transform(ref aLocalAxisY, ref connectionA.orientationMatrix, out aAxisY); Matrix3x3.Transform(ref aLocalAxisZ, ref connectionA.orientationMatrix, out aAxisZ); Matrix3x3.Transform(ref localAxisB, ref connectionB.orientationMatrix, out worldAxisB); Matrix3x3.Transform(ref bLocalAxisY, ref connectionB.orientationMatrix, out bAxisY); Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldAxisB, ref worldAxisA, out rotation); //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement. Vector3 twistMeasureAxis; Vector3.Transform(ref bAxisY, ref rotation, out twistMeasureAxis); //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value. float y, x; Vector3.Dot(ref twistMeasureAxis, ref aAxisZ, out y); Vector3.Dot(ref twistMeasureAxis, ref aAxisY, out x); error = (float) Math.Atan2(y, x); //Debug.WriteLine("Angle: " + angle); //The nice thing about this approach is that the jacobian entry doesn't flip. //Instead, the error can be negative due to the use of Atan2. //This is important for limits which have a unique high and low value. //Compute the jacobian. Vector3.Add(ref worldAxisA, ref worldAxisB, out jacobianB); if (jacobianB.LengthSquared() < Toolbox.Epsilon) { //A nasty singularity can show up if the axes are aligned perfectly. //In a 'real' situation, this is impossible, so just ignore it. isActiveInSolver = false; return; } jacobianB.Normalize(); jacobianA.X = -jacobianB.X; jacobianA.Y = -jacobianB.Y; jacobianA.Z = -jacobianB.Z; //****** VELOCITY BIAS ******// //Compute the correction velocity. float errorReduction; springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out softness); biasVelocity = MathHelper.Clamp(-error * errorReduction, -maxCorrectiveVelocity, maxCorrectiveVelocity); //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (softness + entryA + entryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } private void Initialize() { //Compute a vector which is perpendicular to the axis. It'll be added in local space to both connections. Vector3 yAxis; Vector3.Cross(ref worldAxisA, ref Toolbox.UpVector, out yAxis); float length = yAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref worldAxisA, ref Toolbox.RightVector, out yAxis); } yAxis.Normalize(); //Put the axis into the local space of A. Quaternion conjugate; Quaternion.Conjugate(ref connectionA.orientation, out conjugate); Vector3.Transform(ref yAxis, ref conjugate, out aLocalAxisY); //Complete A's basis. Vector3.Cross(ref localAxisA, ref aLocalAxisY, out aLocalAxisZ); //Rotate the axis to B since it could be arbitrarily rotated. Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldAxisA, ref worldAxisB, out rotation); Vector3.Transform(ref yAxis, ref rotation, out yAxis); //Put it into local space. Quaternion.Conjugate(ref connectionB.orientation, out conjugate); Vector3.Transform(ref yAxis, ref conjugate, out bLocalAxisY); } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/AngularMotor.cs ================================================ using System; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Constraint which attempts to restrict the relative angular motion of two entities. /// Can use a target relative orientation to apply additional force. /// public class AngularMotor : Motor, I3DImpulseConstraintWithError, I3DJacobianConstraint { private readonly JointBasis3D basis = new JointBasis3D(); private readonly MotorSettingsOrientation settings; private Vector3 accumulatedImpulse; private float angle; private Vector3 axis; private Vector3 biasVelocity; private Matrix3x3 effectiveMassMatrix; /// /// Constructs a new constraint which attempts to restrict the relative angular motion of two entities. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB). /// This constructor sets the constraint's IsActive property to false by default. /// public AngularMotor() { IsActive = false; settings = new MotorSettingsOrientation(this); } /// /// Constructs a new constraint which attempts to restrict the relative angular motion of two entities. /// /// First connection of the pair. /// Second connection of the pair. public AngularMotor(Entity connectionA, Entity connectionB) { ConnectionA = connectionA; ConnectionB = connectionB; settings = new MotorSettingsOrientation(this); //Compute the rotation from A to B in A's local space. Quaternion orientationAConjugate; Quaternion.Conjugate(ref connectionA.orientation, out orientationAConjugate); Quaternion.Concatenate(ref connectionB.orientation, ref orientationAConjugate, out settings.servo.goal); } /// /// Gets the basis attached to entity A. /// The target velocity/orientation of this motor is transformed by the basis. /// public JointBasis3D Basis { get { return basis; } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettingsOrientation Settings { get { return settings; } } #region I3DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public Vector3 RelativeVelocity { get { return connectionA.angularVelocity - connectionB.angularVelocity; } } /// /// Gets the total impulse applied by this constraint. /// public Vector3 TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, error is zero. /// public Vector3 Error { get { return axis * angle; } } #endregion #region I3DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// First linear jacobian entry for the first connected entity. /// Second linear jacobian entry for the first connected entity. /// Third linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; jacobianZ = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// First linear jacobian entry for the second connected entity. /// Second linear jacobian entry for the second connected entity. /// Third linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.ZeroVector; jacobianY = Toolbox.ZeroVector; jacobianZ = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// First angular jacobian entry for the first connected entity. /// Second angular jacobian entry for the first connected entity. /// Third angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// First angular jacobian entry for the second connected entity. /// Second angular jacobian entry for the second connected entity. /// Third angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobianX, out Vector3 jacobianY, out Vector3 jacobianZ) { jacobianX = Toolbox.RightVector; jacobianY = Toolbox.UpVector; jacobianZ = Toolbox.BackVector; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out Matrix3x3 outputMassMatrix) { outputMassMatrix = effectiveMassMatrix; } #endregion /// /// Applies the corrective impulses required by the constraint. /// public override float SolveIteration() { #if !WINDOWS Vector3 lambda = new Vector3(); #else Vector3 lambda; #endif Vector3 aVel = connectionA.angularVelocity; Vector3 bVel = connectionB.angularVelocity; lambda.X = bVel.X - aVel.X - biasVelocity.X - usedSoftness * accumulatedImpulse.X; lambda.Y = bVel.Y - aVel.Y - biasVelocity.Y - usedSoftness * accumulatedImpulse.Y; lambda.Z = bVel.Z - aVel.Z - biasVelocity.Z - usedSoftness * accumulatedImpulse.Z; Matrix3x3.Transform(ref lambda, ref effectiveMassMatrix, out lambda); Vector3 previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse.X += lambda.X; accumulatedImpulse.Y += lambda.Y; accumulatedImpulse.Z += lambda.Z; float sumLengthSquared = accumulatedImpulse.LengthSquared(); if (sumLengthSquared > maxForceDtSquared) { //max / impulse gives some value 0 < x < 1. Basically, normalize the vector (divide by the length) and scale by the maximum. float multiplier = maxForceDt / (float) Math.Sqrt(sumLengthSquared); accumulatedImpulse.X *= multiplier; accumulatedImpulse.Y *= multiplier; accumulatedImpulse.Z *= multiplier; //Since the limit was exceeded by this corrective impulse, limit it so that the accumulated impulse remains constrained. lambda.X = accumulatedImpulse.X - previousAccumulatedImpulse.X; lambda.Y = accumulatedImpulse.Y - previousAccumulatedImpulse.Y; lambda.Z = accumulatedImpulse.Z - previousAccumulatedImpulse.Z; } if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref lambda); } if (connectionB.isDynamic) { Vector3 torqueB; Vector3.Negate(ref lambda, out torqueB); connectionB.ApplyAngularImpulse(ref torqueB); } return (Math.Abs(lambda.X) + Math.Abs(lambda.Y) + Math.Abs(lambda.Z)); } /// /// Initializes the constraint for the current frame. /// /// Time between frames. public override void Update(float dt) { basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); if (settings.mode == MotorMode.Servomechanism) //Only need to do the bulk of this work if it's a servo. { //The error is computed using this equation: //GoalRelativeOrientation * ConnectionA.Orientation * Error = ConnectionB.Orientation //GoalRelativeOrientation is the original rotation from A to B in A's local space. //Multiplying by A's orientation gives us where B *should* be. //Of course, B won't be exactly where it should be after initialization. //The Error component holds the difference between what is and what should be. //Error = (GoalRelativeOrientation * ConnectionA.Orientation)^-1 * ConnectionB.Orientation //ConnectionA.Orientation is replaced in the above by the world space basis orientation. Quaternion worldBasis = Matrix3x3.CreateQuaternion(basis.WorldTransform); Quaternion bTarget; Quaternion.Concatenate(ref settings.servo.goal, ref worldBasis, out bTarget); Quaternion bTargetConjugate; Quaternion.Conjugate(ref bTarget, out bTargetConjugate); Quaternion error; Quaternion.Concatenate(ref bTargetConjugate, ref connectionB.orientation, out error); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); //Turn this into an axis-angle representation. Toolbox.GetAxisAngleFromQuaternion(ref error, out axis, out angle); //Scale the axis by the desired velocity if the angle is sufficiently large (epsilon). if (angle > Toolbox.BigEpsilon) { float velocity = -(MathHelper.Min(settings.servo.baseCorrectiveSpeed, angle / dt) + angle * errorReduction); biasVelocity.X = axis.X * velocity; biasVelocity.Y = axis.Y * velocity; biasVelocity.Z = axis.Z * velocity; //Ensure that the corrective velocity doesn't exceed the max. float length = biasVelocity.LengthSquared(); if (length > settings.servo.maxCorrectiveVelocitySquared) { float multiplier = settings.servo.maxCorrectiveVelocity / (float) Math.Sqrt(length); biasVelocity.X *= multiplier; biasVelocity.Y *= multiplier; biasVelocity.Z *= multiplier; } } else { biasVelocity.X = 0; biasVelocity.Y = 0; biasVelocity.Z = 0; } } else { usedSoftness = settings.velocityMotor.softness / dt; angle = 0; //Zero out the error; Matrix3x3 transform = basis.WorldTransform; Matrix3x3.Transform(ref settings.velocityMotor.goalVelocity, ref transform, out biasVelocity); } //Compute effective mass Matrix3x3.Add(ref connectionA.inertiaTensorInverse, ref connectionB.inertiaTensorInverse, out effectiveMassMatrix); effectiveMassMatrix.M11 += usedSoftness; effectiveMassMatrix.M22 += usedSoftness; effectiveMassMatrix.M33 += usedSoftness; Matrix3x3.Invert(ref effectiveMassMatrix, out effectiveMassMatrix); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Apply accumulated impulse if (connectionA.isDynamic) { connectionA.ApplyAngularImpulse(ref accumulatedImpulse); } if (connectionB.isDynamic) { Vector3 torqueB; Vector3.Negate(ref accumulatedImpulse, out torqueB); connectionB.ApplyAngularImpulse(ref torqueB); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/LinearAxisMotor.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Constrains anchors on two entities to move relative to each other on a line. /// public class LinearAxisMotor : Motor, I1DImpulseConstraintWithError, I1DJacobianConstraint { private readonly MotorSettings1D settings; private float accumulatedImpulse; private float biasVelocity; private Vector3 jAngularA, jAngularB; private Vector3 jLinearA, jLinearB; private Vector3 localAnchorA; private Vector3 localAnchorB; private float massMatrix; private float error; private Vector3 localAxis; private Vector3 worldAxis; private Vector3 rA; //Jacobian entry for entity A. private Vector3 worldAnchorA; private Vector3 worldAnchorB; private Vector3 worldOffsetA, worldOffsetB; /// /// Constrains anchors on two entities to move relative to each other on a line. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the AnchorA, AnchorB and the Axis (or their entity-local versions). /// This constructor sets the constraint's IsActive property to false by default. /// public LinearAxisMotor() { settings = new MotorSettings1D(this); IsActive = false; } /// /// Constrains anchors on two entities to move relative to each other on a line. /// /// First connection of the pair. /// Second connection of the pair. /// World space point to attach to connection A that will be constrained. /// World space point to attach to connection B that will be constrained. /// Limited axis in world space to attach to connection A. public LinearAxisMotor(Entity connectionA, Entity connectionB, Vector3 anchorA, Vector3 anchorB, Vector3 axis) { ConnectionA = connectionA; ConnectionB = connectionB; AnchorA = anchorA; AnchorB = anchorB; Axis = axis; settings = new MotorSettings1D(this); } /// /// Gets or sets the anchor point attached to entity A in world space. /// public Vector3 AnchorA { get { return worldAnchorA; } set { worldAnchorA = value; worldOffsetA = worldAnchorA - connectionA.position; Matrix3x3.TransformTranspose(ref worldOffsetA, ref connectionA.orientationMatrix, out localAnchorA); } } /// /// Gets or sets the anchor point attached to entity A in world space. /// public Vector3 AnchorB { get { return worldAnchorB; } set { worldAnchorB = value; worldOffsetB = worldAnchorB - connectionB.position; Matrix3x3.TransformTranspose(ref worldOffsetB, ref connectionB.orientationMatrix, out localAnchorB); } } /// /// Gets or sets the motorized axis in world space. /// public Vector3 Axis { get { return worldAxis; } set { worldAxis = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldAxis, ref connectionA.orientationMatrix, out localAxis); } } /// /// Gets or sets the limited axis in the local space of connection A. /// public Vector3 LocalAxis { get { return localAxis; } set { localAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis); } } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetA { get { return localAnchorA; } set { localAnchorA = value; Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); worldAnchorA = connectionA.position + worldOffsetA; } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in its local space. /// public Vector3 LocalOffsetB { get { return localAnchorB; } set { localAnchorB = value; Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); worldAnchorB = connectionB.position + worldOffsetB; } } /// /// Gets or sets the offset from the first entity's center of mass to the anchor point in world space. /// public Vector3 OffsetA { get { return worldOffsetA; } set { worldOffsetA = value; worldAnchorA = connectionA.position + worldOffsetA; Matrix3x3.TransformTranspose(ref worldOffsetA, ref connectionA.orientationMatrix, out localAnchorA); //Looks weird, but localAnchorA is "localOffsetA." } } /// /// Gets or sets the offset from the second entity's center of mass to the anchor point in world space. /// public Vector3 OffsetB { get { return worldOffsetB; } set { worldOffsetB = value; worldAnchorB = connectionB.position + worldOffsetB; Matrix3x3.TransformTranspose(ref worldOffsetB, ref connectionB.orientationMatrix, out localAnchorB);//Looks weird, but localAnchorB is "localOffsetB." } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettings1D Settings { get { return settings; } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; return lambda; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, the error will be zero. /// public float Error { get { return error; } } #endregion //Jacobians #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = jLinearA; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = jLinearB; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jAngularA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jAngularB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = massMatrix; } #endregion /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { //Compute the current relative velocity. float lambda, dot; Vector3.Dot(ref jLinearA, ref connectionA.linearVelocity, out lambda); Vector3.Dot(ref jAngularA, ref connectionA.angularVelocity, out dot); lambda += dot; Vector3.Dot(ref jLinearB, ref connectionB.linearVelocity, out dot); lambda += dot; Vector3.Dot(ref jAngularB, ref connectionB.angularVelocity, out dot); lambda += dot; //Add in the constraint space bias velocity lambda = -lambda + biasVelocity - usedSoftness * accumulatedImpulse; //Transform to an impulse lambda *= massMatrix; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForceDt, maxForceDt); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, lambda, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, lambda, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return (Math.Abs(lambda)); } public override void Update(float dt) { //Compute the 'pre'-jacobians Matrix3x3.Transform(ref localAnchorA, ref connectionA.orientationMatrix, out worldOffsetA); Matrix3x3.Transform(ref localAnchorB, ref connectionB.orientationMatrix, out worldOffsetB); Vector3.Add(ref worldOffsetA, ref connectionA.position, out worldAnchorA); Vector3.Add(ref worldOffsetB, ref connectionB.position, out worldAnchorB); Vector3.Subtract(ref worldAnchorB, ref connectionA.position, out rA); Matrix3x3.Transform(ref localAxis, ref connectionA.orientationMatrix, out worldAxis); if (settings.mode == MotorMode.Servomechanism) { //Compute error #if !WINDOWS Vector3 separation = new Vector3(); #else Vector3 separation; #endif separation.X = worldAnchorB.X - worldAnchorA.X; separation.Y = worldAnchorB.Y - worldAnchorA.Y; separation.Z = worldAnchorB.Z - worldAnchorA.Z; Vector3.Dot(ref separation, ref worldAxis, out error); //Compute error error = error - settings.servo.goal; //Compute bias float absErrorOverDt = Math.Abs(error / dt); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction; biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity); } else { biasVelocity = -settings.velocityMotor.goalVelocity; usedSoftness = settings.velocityMotor.softness / dt; error = 0; } //Compute jacobians jLinearA = worldAxis; jLinearB.X = -jLinearA.X; jLinearB.Y = -jLinearA.Y; jLinearB.Z = -jLinearA.Z; Vector3.Cross(ref rA, ref jLinearA, out jAngularA); Vector3.Cross(ref worldOffsetB, ref jLinearB, out jAngularB); //compute mass matrix float entryA, entryB; Vector3 intermediate; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jAngularA, ref connectionA.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularA, out entryA); entryA += connectionA.inverseMass; } else entryA = 0; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jAngularB, ref connectionB.inertiaTensorInverse, out intermediate); Vector3.Dot(ref intermediate, ref jAngularB, out entryB); entryB += connectionB.inverseMass; } else entryB = 0; massMatrix = 1 / (entryA + entryB + usedSoftness); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //Warm starting Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jLinearA, accumulatedImpulse, out impulse); connectionA.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jLinearB, accumulatedImpulse, out impulse); connectionB.ApplyLinearImpulse(ref impulse); Vector3.Multiply(ref jAngularB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/Motor.cs ================================================ namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Superclass of constraints which do work and change the velocity of connected entities, but have no specific position target. /// public abstract class Motor : TwoEntityConstraint { protected float maxForceDt = float.MaxValue; protected float maxForceDtSquared = float.MaxValue; /// /// Softness divided by the timestep to maintain timestep independence. /// internal float usedSoftness; /// /// Computes the maxForceDt and maxForceDtSquared fields. /// protected void ComputeMaxForces(float maxForce, float dt) { //Determine maximum force if (maxForce < float.MaxValue) { maxForceDt = maxForce * dt; maxForceDtSquared = maxForceDt * maxForceDt; } else { maxForceDt = float.MaxValue; maxForceDtSquared = float.MaxValue; } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/MotorSettings.cs ================================================ using System; using Microsoft.Xna.Framework; namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Defines the behavior style of a motor. /// public enum MotorMode { /// /// Velocity motors only work to try to reach some relative velocity. /// They have no position goal. /// /// When this type is selected, the motor settings' velocityMotor data will be used. /// VelocityMotor, /// /// Servomechanisms change their velocity in order to reach some position goal. /// /// When this type is selected, the motor settings' servo data will be used. /// Servomechanism } /// /// Contains genereal settings for motors. /// public abstract class MotorSettings { internal EntitySolverUpdateable motor; internal float maximumForce = float.MaxValue; internal MotorMode mode = MotorMode.VelocityMotor; internal MotorSettings(EntitySolverUpdateable motor) { this.motor = motor; } /// /// Gets and sets the maximum impulse that the constraint will attempt to apply when satisfying its requirements. /// This field can be used to simulate friction in a constraint. /// public float MaximumForce { get { if (maximumForce > 0) { return maximumForce; } return 0; } set { value = value >= 0 ? value : 0; if (value != maximumForce) { maximumForce = value; WakeUpEntities(); } } } /// /// Gets or sets what kind of motor this is. /// /// If velocityMotor is chosen, the motor will try to achieve some velocity using the VelocityMotorSettings. /// If servomechanism is chosen, the motor will try to reach some position using the ServoSettings. /// public MotorMode Mode { get { return mode; } set { if (mode != value) { mode = value; WakeUpEntities(); } } } internal void WakeUpEntities() { for (int i = 0; i < motor.involvedEntities.Count; i++) { if (motor.involvedEntities[i].isDynamic) { //Only need to wake up one dynamic entity. That will wake up the rest. //Wouldn't want to pointlessly force-wake a kinematic object. motor.involvedEntities[i].activityInformation.Activate(); break; } } } } /// /// Contains settings for motors which act on one degree of freedom. /// public class MotorSettings1D : MotorSettings { internal ServoSettings1D servo; internal VelocityMotorSettings1D velocityMotor; internal MotorSettings1D(Motor motor) : base(motor) { servo = new ServoSettings1D(this); velocityMotor = new VelocityMotorSettings1D(this); } /// /// Gets the settings that govern the behavior of this motor if it is a servomechanism. /// public ServoSettings1D Servo { get { return servo; } } /// /// Gets the settings that govern the behavior of this motor if it is a velocity motor. /// public VelocityMotorSettings1D VelocityMotor { get { return velocityMotor; } } } /// /// Contains settings for motors which act on three degrees of freedom. /// public class MotorSettings3D : MotorSettings { internal ServoSettings3D servo; internal VelocityMotorSettings3D velocityMotor; internal MotorSettings3D(EntitySolverUpdateable motor) : base(motor) { servo = new ServoSettings3D(this); velocityMotor = new VelocityMotorSettings3D(this); } /// /// Gets the settings that govern the behavior of this motor if it is a servomechanism. /// public ServoSettings3D Servo { get { return servo; } } /// /// Gets the settings that govern the behavior of this motor if it is a velocity motor. /// public VelocityMotorSettings3D VelocityMotor { get { return velocityMotor; } } } /// /// Contains settings for motors which act on two entities' relative orientation. /// public class MotorSettingsOrientation : MotorSettings { internal ServoSettingsOrientation servo; internal VelocityMotorSettings3D velocityMotor; internal MotorSettingsOrientation(EntitySolverUpdateable motor) : base(motor) { servo = new ServoSettingsOrientation(this); velocityMotor = new VelocityMotorSettings3D(this); } /// /// Gets the settings that govern the behavior of this motor if it is a servomechanism. /// public ServoSettingsOrientation Servo { get { return servo; } } /// /// Gets the settings that govern the behavior of this motor if it is a velocity motor. /// public VelocityMotorSettings3D VelocityMotor { get { return velocityMotor; } } } /// /// Defines the behavior of a servo. /// Used when the MotorSettings' motorType is set to servomechanism. /// public class ServoSettings : ISpringSettings { internal MotorSettings motorSettings; /// /// Speed at which the servo will try to achieve its goal. /// internal float baseCorrectiveSpeed; /// /// Maximum extra velocity that the constraint will apply in an effort to correct constraint error. /// internal float maxCorrectiveVelocity = float.MaxValue; /// /// Squared maximum extra velocity that the constraint will apply in an effort to correct constraint error. /// internal float maxCorrectiveVelocitySquared = float.MaxValue; /// /// Spring settings define how a constraint responds to velocity and position error. /// internal SpringSettings springSettings = new SpringSettings(); internal ServoSettings(MotorSettings motorSettings) { this.motorSettings = motorSettings; } /// /// Gets and sets the speed at which the servo will try to achieve its goal. /// This is inactive if the constraint is not in servo mode. /// public float BaseCorrectiveSpeed { get { return baseCorrectiveSpeed; } set { value = value < 0 ? 0 : value; if (value != baseCorrectiveSpeed) { baseCorrectiveSpeed = value; motorSettings.WakeUpEntities(); } } } /// /// Gets or sets the maximum extra velocity that the constraint will apply in an effort to correct any constraint error. /// public float MaxCorrectiveVelocity { get { return maxCorrectiveVelocity; } set { value = Math.Max(0, value); if (maxCorrectiveVelocity != value) { maxCorrectiveVelocity = value; if (maxCorrectiveVelocity >= float.MaxValue) { maxCorrectiveVelocitySquared = float.MaxValue; } else { maxCorrectiveVelocitySquared = maxCorrectiveVelocity * maxCorrectiveVelocity; } motorSettings.WakeUpEntities(); } } } #region ISpringSettings Members /// /// Gets the spring settings used by the constraint. /// Spring settings define how a constraint responds to velocity and position error. /// public SpringSettings SpringSettings { get { return springSettings; } } #endregion } /// /// Defines the behavior of a servo that works on one degree of freedom. /// Used when the MotorSettings' motorType is set to servomechanism. /// public class ServoSettings1D : ServoSettings { internal float goal; internal ServoSettings1D(MotorSettings motorSettings) : base(motorSettings) { } /// /// Gets or sets the goal position of the servo. /// public float Goal { get { return goal; } set { if (goal != value) { goal = value; motorSettings.WakeUpEntities(); } } } } /// /// Defines the behavior of a servo that works on three degrees of freedom. /// Used when the MotorSettings' motorType is set to servomechanism. /// public class ServoSettings3D : ServoSettings { internal Vector3 goal; internal ServoSettings3D(MotorSettings motorSettings) : base(motorSettings) { } /// /// Gets or sets the goal position of the servo. /// public Vector3 Goal { get { return goal; } set { if (goal != value) { goal = value; motorSettings.WakeUpEntities(); } } } } /// /// Defines the behavior of a servo that works on the relative orientation of two entities. /// Used when the MotorSettings' motorType is set to servomechanism. /// public class ServoSettingsOrientation : ServoSettings { internal Quaternion goal; internal ServoSettingsOrientation(MotorSettings motorSettings) : base(motorSettings) { } /// /// Gets or sets the goal orientation of the servo. /// public Quaternion Goal { get { return goal; } set { if (goal != value) { goal = value; motorSettings.WakeUpEntities(); } } } } /// /// Defines the behavior of a velocity motor. /// Used when the MotorSettings' motorType is set to velocityMotor. /// public class VelocityMotorSettings { internal MotorSettings motorSettings; /// /// Softness of this constraint. /// Higher values of softness allow the constraint to be violated more. /// Must be greater than zero. /// Sometimes, if a joint system is unstable, increasing the softness of the involved constraints will make it settle down. /// internal float softness = .0001f; internal VelocityMotorSettings(MotorSettings motorSettings) { this.motorSettings = motorSettings; } /// /// Gets and sets the softness of this constraint. /// Higher values of softness allow the constraint to be violated more. /// Must be greater than zero. /// Sometimes, if a joint system is unstable, increasing the softness of the involved constraints will make it settle down. /// For motors, softness can be used to implement damping. For a damping constant k, the appropriate softness is 1/k. /// public float Softness { get { return softness; } set { value = value < 0 ? 0 : value; if (softness != value) { softness = value; motorSettings.WakeUpEntities(); } } } } /// /// Defines the behavior of a velocity motor that works on one degree of freedom. /// Used when the MotorSettings' motorType is set to velocityMotor. /// public class VelocityMotorSettings1D : VelocityMotorSettings { internal float goalVelocity; internal VelocityMotorSettings1D(MotorSettings motorSettings) : base(motorSettings) { } /// /// Gets or sets the goal velocity of the motor. /// public float GoalVelocity { get { return goalVelocity; } set { goalVelocity = value; motorSettings.WakeUpEntities(); } } } /// /// Defines the behavior of a velocity motor that works on three degrees of freedom. /// Used when the MotorSettings' motorType is set to velocityMotor. /// public class VelocityMotorSettings3D : VelocityMotorSettings { internal Vector3 goalVelocity; internal VelocityMotorSettings3D(MotorSettings motorSettings) : base(motorSettings) { } /// /// Gets or sets the goal position of the servo. /// public Vector3 GoalVelocity { get { return goalVelocity; } set { goalVelocity = value; motorSettings.WakeUpEntities(); } } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/RevoluteMotor.cs ================================================ using System; using System.Diagnostics; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; using Microsoft.Xna.Framework.Input; namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Tries to rotate two entities so that they reach a specified relative orientation or speed around an axis. /// public class RevoluteMotor : Motor, I1DImpulseConstraintWithError, I1DJacobianConstraint { private readonly JointBasis2D basis = new JointBasis2D(); private readonly MotorSettings1D settings; private float accumulatedImpulse; protected float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; private Vector3 localTestAxis; private Vector3 worldTestAxis; private float velocityToImpulse; /// /// Constructs a new constraint tries to rotate two entities so that they reach a specified relative orientation around an axis. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the Basis and TestAxis. /// This constructor sets the constraint's IsActive property to false by default. /// public RevoluteMotor() { settings = new MotorSettings1D(this); IsActive = false; } /// /// Constructs a new constraint tries to rotate two entities so that they reach a specified relative orientation around an axis. /// /// First connection of the pair. /// Second connection of the pair. /// Rotation axis to control in world space. public RevoluteMotor(Entity connectionA, Entity connectionB, Vector3 motorizedAxis) { ConnectionA = connectionA; ConnectionB = connectionB; SetupJointTransforms(motorizedAxis); settings = new MotorSettings1D(this); } /// /// Gets the basis attached to entity A. /// The primary axis represents the motorized axis of rotation. The 'measurement plane' which the test axis is tested against is based on this primary axis. /// The x axis defines the 'base' direction on the measurement plane corresponding to 0 degrees of relative rotation. /// public JointBasis2D Basis { get { return basis; } } /// /// Gets or sets the axis attached to entity B in its local space. /// This axis is projected onto the x and y axes of transformA to determine the hinge angle. /// public Vector3 LocalTestAxis { get { return localTestAxis; } set { localTestAxis = Vector3.Normalize(value); Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis); } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettings1D Settings { get { return settings; } } /// /// Gets or sets the axis attached to entity B in world space. /// This axis is projected onto the x and y axes of the Basis attached to entity A to determine the hinge angle. /// public Vector3 TestAxis { get { return worldTestAxis; } set { worldTestAxis = Vector3.Normalize(value); Matrix3x3.TransformTranspose(ref worldTestAxis, ref connectionB.orientationMatrix, out localTestAxis); } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float velocityA, velocityB; Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, the error is zero. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// /// Axis around which the motor acts. public void SetupJointTransforms(Vector3 motorizedAxis) { //Compute a vector which is perpendicular to the axis. It'll be added in local space to both connections. Vector3 xAxis; Vector3.Cross(ref motorizedAxis, ref Toolbox.UpVector, out xAxis); float length = xAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref motorizedAxis, ref Toolbox.RightVector, out xAxis); } //Put the axes into the joint transform of A. basis.rotationMatrix = connectionA.orientationMatrix; basis.SetWorldAxes(motorizedAxis, xAxis); //Put the axes into the 'joint transform' of B too. TestAxis = basis.xAxis; } /// /// Performs the frame's configuration step. /// ///Timestep duration. public override void Update(float dt) { //Transform the axes into world space. basis.rotationMatrix = connectionA.orientationMatrix; basis.ComputeWorldSpaceAxes(); Matrix3x3.Transform(ref localTestAxis, ref connectionB.orientationMatrix, out worldTestAxis); if (settings.mode == MotorMode.Servomechanism) { float y, x; Vector3 yAxis; Vector3.Cross(ref basis.primaryAxis, ref basis.xAxis, out yAxis); Vector3.Dot(ref worldTestAxis, ref yAxis, out y); Vector3.Dot(ref worldTestAxis, ref basis.xAxis, out x); var angle = (float)Math.Atan2(y, x); //****** VELOCITY BIAS ******// //Compute the correction velocity. error = GetDistanceFromGoal(angle); float absErrorOverDt = Math.Abs(error / dt); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction; biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity); } else { biasVelocity = settings.velocityMotor.goalVelocity; usedSoftness = settings.velocityMotor.softness / dt; error = 0; } //Compute the jacobians jacobianA = basis.primaryAxis; jacobianB.X = -jacobianA.X; jacobianB.Y = -jacobianA.Y; jacobianB.Z = -jacobianA.Z; //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (usedSoftness + entryA + entryB); //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) - biasVelocity - usedSoftness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate the impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForceDt, maxForceDt); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return Math.Abs(lambda); } private float GetDistanceFromGoal(float angle) { float forwardDistance; float goalAngle = MathHelper.WrapAngle(settings.servo.goal); if (goalAngle > 0) { if (angle > goalAngle) forwardDistance = angle - goalAngle; else if (angle > 0) forwardDistance = MathHelper.TwoPi - goalAngle + angle; else //if (angle <= 0) forwardDistance = MathHelper.TwoPi - goalAngle + angle; } else { if (angle < goalAngle) forwardDistance = MathHelper.TwoPi - goalAngle + angle; else //if (angle < 0) forwardDistance = angle - goalAngle; //else //if (currentAngle >= 0) // return angle - myMinimumAngle; } return forwardDistance > MathHelper.Pi ? MathHelper.TwoPi - forwardDistance : -forwardDistance; } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/Motors/TwistMotor.cs ================================================ using System; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Constraints.TwoEntity.Motors { /// /// Attempts to achieve some defined relative twist angle between the entities. /// public class TwistMotor : Motor, I1DImpulseConstraintWithError, I1DJacobianConstraint { private readonly JointBasis3D basisA = new JointBasis3D(); private readonly JointBasis2D basisB = new JointBasis2D(); private readonly MotorSettings1D settings; private float accumulatedImpulse; /// /// Velocity needed to get closer to the goal. /// protected float biasVelocity; private Vector3 jacobianA, jacobianB; private float error; private float velocityToImpulse; /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other. /// To finish the initialization, specify the connections (ConnectionA and ConnectionB) /// as well as the BasisA and BasisB. /// This constructor sets the constraint's IsActive property to false by default. /// public TwistMotor() { IsActive = false; settings = new MotorSettings1D(this); } /// /// Constructs a new constraint which prevents the connected entities from twisting relative to each other. /// /// First connection of the pair. /// Second connection of the pair. /// Twist axis attached to the first connected entity. /// Twist axis attached to the second connected entity. public TwistMotor(Entity connectionA, Entity connectionB, Vector3 axisA, Vector3 axisB) { ConnectionA = connectionA; ConnectionB = connectionB; SetupJointTransforms(axisA, axisB); settings = new MotorSettings1D(this); } /// /// Gets the basis attached to entity A. /// The primary axis represents the twist axis attached to entity A. /// The x axis and y axis represent a plane against which entity B's attached x axis is projected to determine the twist angle. /// public JointBasis3D BasisA { get { return basisA; } } /// /// Gets the basis attached to entity B. /// The primary axis represents the twist axis attached to entity A. /// The x axis is projected onto the plane defined by localTransformA's x and y axes /// to get the twist angle. /// public JointBasis2D BasisB { get { return basisB; } } /// /// Gets the motor's velocity and servo settings. /// public MotorSettings1D Settings { get { return settings; } } #region I1DImpulseConstraintWithError Members /// /// Gets the current relative velocity between the connected entities with respect to the constraint. /// public float RelativeVelocity { get { float velocityA, velocityB; Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); return velocityA + velocityB; } } /// /// Gets the total impulse applied by this constraint. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the current constraint error. /// If the motor is in velocity only mode, the error will be zero. /// public float Error { get { return error; } } #endregion #region I1DJacobianConstraint Members /// /// Gets the linear jacobian entry for the first connected entity. /// /// Linear jacobian entry for the first connected entity. public void GetLinearJacobianA(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the linear jacobian entry for the second connected entity. /// /// Linear jacobian entry for the second connected entity. public void GetLinearJacobianB(out Vector3 jacobian) { jacobian = Toolbox.ZeroVector; } /// /// Gets the angular jacobian entry for the first connected entity. /// /// Angular jacobian entry for the first connected entity. public void GetAngularJacobianA(out Vector3 jacobian) { jacobian = jacobianA; } /// /// Gets the angular jacobian entry for the second connected entity. /// /// Angular jacobian entry for the second connected entity. public void GetAngularJacobianB(out Vector3 jacobian) { jacobian = jacobianB; } /// /// Gets the mass matrix of the constraint. /// /// Constraint's mass matrix. public void GetMassMatrix(out float outputMassMatrix) { outputMassMatrix = velocityToImpulse; } #endregion /// /// Sets up the joint transforms by automatically creating perpendicular vectors to complete the bases. /// /// Twist axis in world space to attach to entity A. /// Twist axis in world space to attach to entity B. public void SetupJointTransforms(Vector3 worldTwistAxisA, Vector3 worldTwistAxisB) { worldTwistAxisA.Normalize(); worldTwistAxisB.Normalize(); Vector3 worldXAxis; Vector3.Cross(ref worldTwistAxisA, ref Toolbox.UpVector, out worldXAxis); float length = worldXAxis.LengthSquared(); if (length < Toolbox.Epsilon) { Vector3.Cross(ref worldTwistAxisA, ref Toolbox.RightVector, out worldXAxis); } worldXAxis.Normalize(); //Complete A's basis. Vector3 worldYAxis; Vector3.Cross(ref worldTwistAxisA, ref worldXAxis, out worldYAxis); basisA.rotationMatrix = connectionA.orientationMatrix; basisA.SetWorldAxes(worldTwistAxisA, worldXAxis, worldYAxis); //Rotate the axis to B since it could be arbitrarily rotated. Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref worldTwistAxisA, ref worldTwistAxisB, out rotation); Vector3.Transform(ref worldXAxis, ref rotation, out worldXAxis); basisB.rotationMatrix = connectionB.orientationMatrix; basisB.SetWorldAxes(worldTwistAxisB, worldXAxis); } /// /// Solves for velocity. /// public override float SolveIteration() { float velocityA, velocityB; //Find the velocity contribution from each connection Vector3.Dot(ref connectionA.angularVelocity, ref jacobianA, out velocityA); Vector3.Dot(ref connectionB.angularVelocity, ref jacobianB, out velocityB); //Add in the constraint space bias velocity float lambda = -(velocityA + velocityB) + biasVelocity - usedSoftness * accumulatedImpulse; //Transform to an impulse lambda *= velocityToImpulse; //Accumulate the impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForceDt, maxForceDt); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, lambda, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, lambda, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } return Math.Abs(lambda); } /// /// Do any necessary computations to prepare the constraint for this frame. /// /// Simulation step length. public override void Update(float dt) { basisA.rotationMatrix = connectionA.orientationMatrix; basisB.rotationMatrix = connectionB.orientationMatrix; basisA.ComputeWorldSpaceAxes(); basisB.ComputeWorldSpaceAxes(); if (settings.mode == MotorMode.Servomechanism) { Quaternion rotation; Toolbox.GetQuaternionBetweenNormalizedVectors(ref basisB.primaryAxis, ref basisA.primaryAxis, out rotation); //Transform b's 'Y' axis so that it is perpendicular with a's 'X' axis for measurement. Vector3 twistMeasureAxis; Vector3.Transform(ref basisB.xAxis, ref rotation, out twistMeasureAxis); //By dotting the measurement vector with a 2d plane's axes, we can get a local X and Y value. float y, x; Vector3.Dot(ref twistMeasureAxis, ref basisA.yAxis, out y); Vector3.Dot(ref twistMeasureAxis, ref basisA.xAxis, out x); var angle = (float) Math.Atan2(y, x); //Compute goal velocity. error = GetDistanceFromGoal(angle); float absErrorOverDt = Math.Abs(error / dt); float errorReduction; settings.servo.springSettings.ComputeErrorReductionAndSoftness(dt, out errorReduction, out usedSoftness); biasVelocity = Math.Sign(error) * MathHelper.Min(settings.servo.baseCorrectiveSpeed, absErrorOverDt) + error * errorReduction; biasVelocity = MathHelper.Clamp(biasVelocity, -settings.servo.maxCorrectiveVelocity, settings.servo.maxCorrectiveVelocity); } else { biasVelocity = settings.velocityMotor.goalVelocity; usedSoftness = settings.velocityMotor.softness / dt; error = 0; } //The nice thing about this approach is that the jacobian entry doesn't flip. //Instead, the error can be negative due to the use of Atan2. //This is important for limits which have a unique high and low value. //Compute the jacobian. Vector3.Add(ref basisA.primaryAxis, ref basisB.primaryAxis, out jacobianB); if (jacobianB.LengthSquared() < Toolbox.Epsilon) { //A nasty singularity can show up if the axes are aligned perfectly. //In a 'real' situation, this is impossible, so just ignore it. isActiveInSolver = false; return; } jacobianB.Normalize(); jacobianA.X = -jacobianB.X; jacobianA.Y = -jacobianB.Y; jacobianA.Z = -jacobianB.Z; //Update the maximum force ComputeMaxForces(settings.maximumForce, dt); //****** EFFECTIVE MASS MATRIX ******// //Connection A's contribution to the mass matrix float entryA; Vector3 transformedAxis; if (connectionA.isDynamic) { Matrix3x3.Transform(ref jacobianA, ref connectionA.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianA, out entryA); } else entryA = 0; //Connection B's contribution to the mass matrix float entryB; if (connectionB.isDynamic) { Matrix3x3.Transform(ref jacobianB, ref connectionB.inertiaTensorInverse, out transformedAxis); Vector3.Dot(ref transformedAxis, ref jacobianB, out entryB); } else entryB = 0; //Compute the inverse mass matrix velocityToImpulse = 1 / (usedSoftness + entryA + entryB); } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { //****** WARM STARTING ******// //Apply accumulated impulse Vector3 impulse; if (connectionA.isDynamic) { Vector3.Multiply(ref jacobianA, accumulatedImpulse, out impulse); connectionA.ApplyAngularImpulse(ref impulse); } if (connectionB.isDynamic) { Vector3.Multiply(ref jacobianB, accumulatedImpulse, out impulse); connectionB.ApplyAngularImpulse(ref impulse); } } private float GetDistanceFromGoal(float angle) { float forwardDistance; float goalAngle = MathHelper.WrapAngle(settings.servo.goal); if (goalAngle > 0) { if (angle > goalAngle) forwardDistance = angle - goalAngle; else if (angle > 0) forwardDistance = MathHelper.TwoPi - goalAngle + angle; else //if (angle <= 0) forwardDistance = MathHelper.TwoPi - goalAngle + angle; } else { if (angle < goalAngle) forwardDistance = MathHelper.TwoPi - goalAngle + angle; else //if (angle < 0) forwardDistance = angle - goalAngle; //else //if (currentAngle >= 0) // return angle - myMinimumAngle; } return forwardDistance > MathHelper.Pi ? MathHelper.TwoPi - forwardDistance : -forwardDistance; } } } ================================================ FILE: BEPUphysics/Constraints/TwoEntity/TwoEntityConstraint.cs ================================================ using BEPUphysics.Entities; using BEPUphysics.Entities.Prefabs; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.Constraints.TwoEntity { /// /// Abstract superclass of constraints involving two bodies. /// public abstract class TwoEntityConstraint : EntitySolverUpdateable { /// /// Entity that constraints connect to when they are given a null connection. /// public static readonly Entity WorldEntity = new Sphere(Vector3.Zero, 0); /// /// First connection to the constraint. /// protected internal Entity connectionA; /// /// Second connection to the constraint. /// protected internal Entity connectionB; /// /// Gets or sets the first connection to the constraint. /// public Entity ConnectionA { get { //if (myConnectionA == nullSphere) // return null; return connectionA; } set { connectionA = value ?? WorldEntity; OnInvolvedEntitiesChanged(); } } /// /// Gets or sets the second connection to the constraint. /// public Entity ConnectionB { get { //if (myConnectionB == nullSphere) // return null; return connectionB; } set { connectionB = value ?? WorldEntity; OnInvolvedEntitiesChanged(); } } /// /// Adds entities associated with the solver item to the involved entities list. /// Ensure that sortInvolvedEntities() is called at the end of the function. /// This allows the non-batched multithreading system to lock properly. /// protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { if (connectionA != null && connectionA != WorldEntity) outputInvolvedEntities.Add(connectionA); if (connectionB != null && connectionB != WorldEntity) outputInvolvedEntities.Add(connectionB); } } } ================================================ FILE: BEPUphysics/DataStructures/BoundingBoxTree.cs ================================================ using System.Collections.Generic; using Microsoft.Xna.Framework; using BEPUphysics.BroadPhaseSystems; namespace BEPUphysics.DataStructures { /// /// Acceleration structure of objects surrounded by axis aligned bounding boxes, supporting various speedy queries. /// public class BoundingBoxTree where T : IBoundingBoxOwner { /// /// Gets the bounding box surrounding the tree. /// public BoundingBox BoundingBox { get { if (root != null) return root.BoundingBox; else return new BoundingBox(); } } Node root; /// /// Constructs a new tree. /// /// Data to use to construct the tree. public BoundingBoxTree(IList elements) { Reconstruct(elements); } /// /// Reconstructs the tree based on the current data. /// public void Reconstruct(IList elements) { root = null; int count = elements.Count; for (int i = 0; i < count; i++) { //Use a permuted version of the elements instead of the actual elements list. //Permuting makes the input basically random, improving the quality of the tree. Add(elements[(int)((982451653L * i) % count)]); } } /// /// Refits the tree based on the current data. /// This process is cheaper to perform than a reconstruction when the topology of the mesh /// does not change. /// public void Refit() { if (root != null) root.Refit(); } void Analyze(out List depths, out int minDepth, out int maxDepth, out int nodeCount) { depths = new List(); nodeCount = 0; root.Analyze(depths, 0, ref nodeCount); maxDepth = 0; minDepth = int.MaxValue; for (int i = 0; i < depths.Count; i++) { if (depths[i] > maxDepth) maxDepth = depths[i]; if (depths[i] < minDepth) minDepth = depths[i]; } } /// /// Adds an element to the tree. /// If a list of objects is available, using the Reconstruct method is recommended. /// /// Element to add. public void Add(T element) { //Insertions can easily be performed stacklessly. //Only one path is chosen at each step and nothing is returned, so the history of the 'recursion' is completely forgotten. var node = new LeafNode(element); if (root == null) { //Empty tree. This is the first and only node. root = node; } else { if (root.IsLeaf) //Root is alone. root.TryToInsert(node, out root); else { //The caller is responsible for the merge. BoundingBox.CreateMerged(ref node.BoundingBox, ref root.BoundingBox, out root.BoundingBox); Node treeNode = root; while (!treeNode.TryToInsert(node, out treeNode)) ;//TryToInsert returns the next node, if any, and updates node bounding box. } } } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingBox boundingBox, IList outputOverlappedElements) { if (root != null) { bool intersects; root.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) root.GetOverlaps(ref boundingBox, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingSphere boundingSphere, IList outputOverlappedElements) { if (root != null) { bool intersects; root.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) root.GetOverlaps(ref boundingSphere, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingFrustum boundingFrustum, IList outputOverlappedElements) { if (root != null) { bool intersects; boundingFrustum.Intersects(ref root.BoundingBox, out intersects); if (intersects) root.GetOverlaps(ref boundingFrustum, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(Ray ray, IList outputOverlappedElements) { if (root != null) { float? result; ray.Intersects(ref root.BoundingBox, out result); if (result != null) root.GetOverlaps(ref ray, float.MaxValue, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Maximum length of the ray in units of the ray's length. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(Ray ray, float maximumLength, IList outputOverlappedElements) { if (root != null) { float? result; ray.Intersects(ref root.BoundingBox, out result); if (result != null) root.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the pairs of elements in each tree with overlapping bounding boxes. /// /// Type of the elements in the opposing tree. /// Other tree to test. /// List of overlaps found by the query. /// Whether or not any overlaps were found. public bool GetOverlaps(BoundingBoxTree tree, IList> outputOverlappedElements) where TElement : IBoundingBoxOwner { bool intersects; root.BoundingBox.Intersects(ref tree.root.BoundingBox, out intersects); if (intersects) { root.GetOverlaps(tree.root, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } internal abstract class Node { internal BoundingBox BoundingBox; internal abstract void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements); internal abstract void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements); internal abstract void GetOverlaps(BoundingBoxTree.Node opposingNode, IList> outputOverlappedElements) where TElement : IBoundingBoxOwner; internal abstract bool IsLeaf { get; } internal abstract Node ChildA { get; } internal abstract Node ChildB { get; } internal abstract T Element { get; } internal abstract bool TryToInsert(LeafNode node, out Node treeNode); internal abstract void Analyze(List depths, int depth, ref int nodeCount); internal abstract void Refit(); } internal sealed class InternalNode : Node { internal Node childA; internal Node childB; internal override Node ChildA { get { return childA; } } internal override Node ChildB { get { return childB; } } internal override T Element { get { return default(T); } } internal override bool IsLeaf { get { return false; } } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Users of the GetOverlaps method will have to check the bounding box before calling //root.getoverlaps. This is actually desired in some cases, since the outer bounding box is used //to determine a pair, and further overlap tests shouldn't bother retesting the root. bool intersects; childA.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) childA.GetOverlaps(ref boundingBox, outputOverlappedElements); childB.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) childB.GetOverlaps(ref boundingBox, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { bool intersects; childA.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) childA.GetOverlaps(ref boundingSphere, outputOverlappedElements); childB.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) childB.GetOverlaps(ref boundingSphere, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { bool intersects; boundingFrustum.Intersects(ref childA.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(ref boundingFrustum, outputOverlappedElements); boundingFrustum.Intersects(ref childB.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(ref boundingFrustum, outputOverlappedElements); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { float? result; ray.Intersects(ref childA.BoundingBox, out result); if (result != null && result < maximumLength) childA.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); ray.Intersects(ref childB.BoundingBox, out result); if (result != null && result < maximumLength) childB.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); } internal override void GetOverlaps(BoundingBoxTree.Node opposingNode, IList> outputOverlappedElements) { bool intersects; if (opposingNode.IsLeaf) { //If it's a leaf, go deeper in our hierarchy, but not the opposition. childA.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingNode, outputOverlappedElements); childB.BoundingBox.Intersects(ref opposingNode.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingNode, outputOverlappedElements); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in both hierarchies. childA.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingChildA, outputOverlappedElements); childA.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childA.GetOverlaps(opposingChildB, outputOverlappedElements); childB.BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingChildA, outputOverlappedElements); childB.BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) childB.GetOverlaps(opposingChildB, outputOverlappedElements); } } internal override bool TryToInsert(LeafNode node, out Node treeNode) { ////The following can make the tree shorter, but it actually hurt query times in testing. //bool aIsLeaf = childA.IsLeaf; //bool bIsLeaf = childB.IsLeaf; //if (aIsLeaf && !bIsLeaf) //{ // //Just put us with the leaf. Keeps the tree shallower. // BoundingBox merged; // BoundingBox.CreateMerged(ref childA.BoundingBox, ref node.BoundingBox, out merged); // childA = new InternalNode() { BoundingBox = merged, childA = this.childA, childB = node }; // treeNode = null; // return true; //} //else if (!aIsLeaf && bIsLeaf) //{ // //Just put us with the leaf. Keeps the tree shallower. // BoundingBox merged; // BoundingBox.CreateMerged(ref childB.BoundingBox, ref node.BoundingBox, out merged); // childB = new InternalNode() { BoundingBox = merged, childA = node, childB = this.childB }; // treeNode = null; // return true; //} //Since we are an internal node, we know we have two children. //Regardless of what kind of nodes they are, figure out which would be a better choice to merge the new node with. //Use the path which produces the smallest 'volume.' BoundingBox mergedA, mergedB; BoundingBox.CreateMerged(ref childA.BoundingBox, ref node.BoundingBox, out mergedA); BoundingBox.CreateMerged(ref childB.BoundingBox, ref node.BoundingBox, out mergedB); Vector3 offset; float originalAVolume, originalBVolume; Vector3.Subtract(ref childA.BoundingBox.Max, ref childA.BoundingBox.Min, out offset); originalAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref childB.BoundingBox.Max, ref childB.BoundingBox.Min, out offset); originalBVolume = offset.X * offset.Y * offset.Z; float mergedAVolume, mergedBVolume; Vector3.Subtract(ref mergedA.Max, ref mergedA.Min, out offset); mergedAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref mergedB.Max, ref mergedB.Min, out offset); mergedBVolume = offset.X * offset.Y * offset.Z; //Could use factor increase or absolute difference if (mergedAVolume - originalAVolume < mergedBVolume - originalBVolume) { //merging A produces a better result. if (childA.IsLeaf) { childA = new InternalNode() { BoundingBox = mergedA, childA = this.childA, childB = node }; treeNode = null; return true; } else { childA.BoundingBox = mergedA; treeNode = childA; return false; } } else { //merging B produces a better result. if (childB.IsLeaf) { //Target is a leaf! Return. childB = new InternalNode() { BoundingBox = mergedB, childA = node, childB = this.childB }; treeNode = null; return true; } else { childB.BoundingBox = mergedB; treeNode = childB; return false; } } } public override string ToString() { return "{" + childA + ", " + childB + "}"; } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; childA.Analyze(depths, depth + 1, ref nodeCount); childB.Analyze(depths, depth + 1, ref nodeCount); } internal override void Refit() { childA.Refit(); childB.Refit(); BoundingBox.CreateMerged(ref childA.BoundingBox, ref childB.BoundingBox, out BoundingBox); } } /// /// The tiny extra margin added to leaf bounding boxes that allow the volume cost metric to function properly even in degenerate cases. /// public static float LeafMargin = .001f; internal sealed class LeafNode : Node { T element; internal override Node ChildA { get { return null; } } internal override Node ChildB { get { return null; } } internal override T Element { get { return element; } } internal override bool IsLeaf { get { return true; } } internal LeafNode(T element) { this.element = element; BoundingBox = element.BoundingBox; //Having an ever-so-slight margin allows the hierarchy use a volume metric even for degenerate shapes (consider a flat tessellated plane). BoundingBox.Max.X += LeafMargin; BoundingBox.Max.Y += LeafMargin; BoundingBox.Max.Z += LeafMargin; BoundingBox.Min.X -= LeafMargin; BoundingBox.Min.Y -= LeafMargin; BoundingBox.Min.Z -= LeafMargin; } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Our parent already tested the bounding box. All that's left is to add myself to the list. outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { outputOverlappedElements.Add(element); } internal override void GetOverlaps(BoundingBoxTree.Node opposingNode, IList> outputOverlappedElements) { bool intersects; if (opposingNode.IsLeaf) { //We're both leaves! Our parents have already done the testing for us, so we know we're overlapping. outputOverlappedElements.Add(new TreeOverlapPair(element, opposingNode.Element)); } else { var opposingChildA = opposingNode.ChildA; var opposingChildB = opposingNode.ChildB; //If it's not a leaf, try to go deeper in the opposing hierarchy. BoundingBox.Intersects(ref opposingChildA.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildA, outputOverlappedElements); BoundingBox.Intersects(ref opposingChildB.BoundingBox, out intersects); if (intersects) GetOverlaps(opposingChildB, outputOverlappedElements); } } internal override bool TryToInsert(LeafNode node, out Node treeNode) { var newTreeNode = new InternalNode(); BoundingBox.CreateMerged(ref BoundingBox, ref node.BoundingBox, out newTreeNode.BoundingBox); newTreeNode.childA = this; newTreeNode.childB = node; treeNode = newTreeNode; return true; } public override string ToString() { return element.ToString(); } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; depths.Add(depth); } internal override void Refit() { BoundingBox = element.BoundingBox; //Having an ever-so-slight margin allows the hierarchy use a volume metric even for degenerate shapes (consider a flat tessellated plane). BoundingBox.Max.X += LeafMargin; BoundingBox.Max.Y += LeafMargin; BoundingBox.Max.Z += LeafMargin; BoundingBox.Min.X -= LeafMargin; BoundingBox.Min.Y -= LeafMargin; BoundingBox.Min.Z -= LeafMargin; } } } } ================================================ FILE: BEPUphysics/DataStructures/MeshBoundingBoxTree.cs ================================================ using System.Collections.Generic; using System.Globalization; using Microsoft.Xna.Framework; namespace BEPUphysics.DataStructures { /// /// Acceleration structure of triangles surrounded by axis aligned bounding boxes, supporting various speedy queries. /// public class MeshBoundingBoxTree { MeshBoundingBoxTreeData data; /// /// Gets the bounding box surrounding the tree. /// public BoundingBox BoundingBox { get { if (root != null) return root.BoundingBox; else return new BoundingBox(); } } Node root; /// /// Gets or sets the data used to construct the tree. /// When set, the tree will be reconstructed. /// public MeshBoundingBoxTreeData Data { get { return data; } set { this.data = value; Reconstruct(); } } /// /// Constructs a new tree. /// /// Data to use to construct the tree. public MeshBoundingBoxTree(MeshBoundingBoxTreeData data) { Data = data; } /// /// Reconstructs the tree based on the current data. /// public void Reconstruct() { root = null; for (int i = 0; i < data.IndexCount; i += 3) { //Use a permuted version of the triangles instead of the actual triangle list. //Permuting makes the input basically random, improving the quality of the tree. Insert((int)(((982451653L * (i / 3)) % (data.IndexCount / 3)) * 3)); } } /// /// Refits the tree based on the current data. /// This process is cheaper to perform than a reconstruction when the topology of the mesh /// does not change. /// public void Refit() { if (root != null) root.Refit(data); } void Analyze(out List depths, out int minDepth, out int maxDepth, out int nodeCount) { depths = new List(); nodeCount = 0; root.Analyze(depths, 0, ref nodeCount); maxDepth = 0; minDepth = int.MaxValue; for (int i = 0; i < depths.Count; i++) { if (depths[i] > maxDepth) maxDepth = depths[i]; if (depths[i] < minDepth) minDepth = depths[i]; } } void Insert(int triangleIndex) { //Insertions can easily be performed stacklessly. //Only one path is chosen at each step and nothing is returned, so the history of the 'recursion' is completely forgotten. var node = new LeafNode(triangleIndex, data); if (root == null) { //Empty tree. This is the first and only node. root = node; } else { if (root.IsLeaf) //Root is alone. root.TryToInsert(node, out root); else { //The caller is responsible for the merge. BoundingBox.CreateMerged(ref node.BoundingBox, ref root.BoundingBox, out root.BoundingBox); Node treeNode = root; while (!treeNode.TryToInsert(node, out treeNode)) ;//TryToInsert returns the next node, if any, and updates node bounding box. } } } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingBox boundingBox, IList outputOverlappedElements) { if (root != null) { bool intersects; root.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) root.GetOverlaps(ref boundingBox, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingSphere boundingSphere, IList outputOverlappedElements) { if (root != null) { bool intersects; root.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) root.GetOverlaps(ref boundingSphere, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(BoundingFrustum boundingFrustum, IList outputOverlappedElements) { if (root != null) { bool intersects; boundingFrustum.Intersects(ref root.BoundingBox, out intersects); if (intersects) root.GetOverlaps(ref boundingFrustum, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(Ray ray, IList outputOverlappedElements) { if (root != null) { float? result; ray.Intersects(ref root.BoundingBox, out result); if (result != null) root.GetOverlaps(ref ray, float.MaxValue, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } /// /// Gets the triangles whose bounding boxes are overlapped by the query. /// /// Shape to query against the tree. /// Maximum length of the ray in units of the ray's length. /// Indices of triangles in the index buffer with bounding boxes which are overlapped by the query. /// Whether or not any elements were overlapped. public bool GetOverlaps(Ray ray, float maximumLength, IList outputOverlappedElements) { if (root != null) { float? result; ray.Intersects(ref root.BoundingBox, out result); if (result != null) root.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); } return outputOverlappedElements.Count > 0; } abstract class Node { internal BoundingBox BoundingBox; internal abstract void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements); internal abstract void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements); internal abstract void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements); internal abstract bool IsLeaf { get; } internal abstract bool TryToInsert(LeafNode node, out Node treeNode); internal abstract void Analyze(List depths, int depth, ref int nodeCount); internal abstract void Refit(MeshBoundingBoxTreeData data); } sealed class InternalNode : Node { internal Node ChildA; internal Node ChildB; internal override bool IsLeaf { get { return false; } } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Users of the GetOverlaps method will have to check the bounding box before calling //root.getoverlaps. This is actually desired in some cases, since the outer bounding box is used //to determine a pair, and further overlap tests shouldn't bother retesting the root. bool intersects; ChildA.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) ChildA.GetOverlaps(ref boundingBox, outputOverlappedElements); ChildB.BoundingBox.Intersects(ref boundingBox, out intersects); if (intersects) ChildB.GetOverlaps(ref boundingBox, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { bool intersects; ChildA.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) ChildA.GetOverlaps(ref boundingSphere, outputOverlappedElements); ChildB.BoundingBox.Intersects(ref boundingSphere, out intersects); if (intersects) ChildB.GetOverlaps(ref boundingSphere, outputOverlappedElements); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { bool intersects; boundingFrustum.Intersects(ref ChildA.BoundingBox, out intersects); if (intersects) ChildA.GetOverlaps(ref boundingFrustum, outputOverlappedElements); boundingFrustum.Intersects(ref ChildB.BoundingBox, out intersects); if (intersects) ChildB.GetOverlaps(ref boundingFrustum, outputOverlappedElements); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { float? result; ray.Intersects(ref ChildA.BoundingBox, out result); if (result != null && result < maximumLength) ChildA.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); ray.Intersects(ref ChildB.BoundingBox, out result); if (result != null && result < maximumLength) ChildB.GetOverlaps(ref ray, maximumLength, outputOverlappedElements); } internal override bool TryToInsert(LeafNode node, out Node treeNode) { ////The following can make the tree shorter, but it actually hurt query times in testing. //bool aIsLeaf = childA.IsLeaf; //bool bIsLeaf = childB.IsLeaf; //if (aIsLeaf && !bIsLeaf) //{ // //Just put us with the leaf. Keeps the tree shallower. // BoundingBox merged; // BoundingBox.CreateMerged(ref childA.BoundingBox, ref node.BoundingBox, out merged); // childA = new InternalNode() { BoundingBox = merged, childA = this.childA, childB = node }; // treeNode = null; // return true; //} //else if (!aIsLeaf && bIsLeaf) //{ // //Just put us with the leaf. Keeps the tree shallower. // BoundingBox merged; // BoundingBox.CreateMerged(ref childB.BoundingBox, ref node.BoundingBox, out merged); // childB = new InternalNode() { BoundingBox = merged, childA = node, childB = this.childB }; // treeNode = null; // return true; //} //Since we are an internal node, we know we have two children. //Regardless of what kind of nodes they are, figure out which would be a better choice to merge the new node with. //Use the path which produces the smallest 'volume.' BoundingBox mergedA, mergedB; BoundingBox.CreateMerged(ref ChildA.BoundingBox, ref node.BoundingBox, out mergedA); BoundingBox.CreateMerged(ref ChildB.BoundingBox, ref node.BoundingBox, out mergedB); Vector3 offset; float originalAVolume, originalBVolume; Vector3.Subtract(ref ChildA.BoundingBox.Max, ref ChildA.BoundingBox.Min, out offset); originalAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref ChildB.BoundingBox.Max, ref ChildB.BoundingBox.Min, out offset); originalBVolume = offset.X * offset.Y * offset.Z; float mergedAVolume, mergedBVolume; Vector3.Subtract(ref mergedA.Max, ref mergedA.Min, out offset); mergedAVolume = offset.X * offset.Y * offset.Z; Vector3.Subtract(ref mergedB.Max, ref mergedB.Min, out offset); mergedBVolume = offset.X * offset.Y * offset.Z; //Could use factor increase or absolute difference if (mergedAVolume - originalAVolume < mergedBVolume - originalBVolume) { //merging A produces a better result. if (ChildA.IsLeaf) { ChildA = new InternalNode() { BoundingBox = mergedA, ChildA = this.ChildA, ChildB = node }; treeNode = null; return true; } else { ChildA.BoundingBox = mergedA; treeNode = ChildA; return false; } } else { //merging B produces a better result. if (ChildB.IsLeaf) { //Target is a leaf! Return. ChildB = new InternalNode() { BoundingBox = mergedB, ChildA = node, ChildB = this.ChildB }; treeNode = null; return true; } else { ChildB.BoundingBox = mergedB; treeNode = ChildB; return false; } } } public override string ToString() { return "{" + ChildA.ToString() + ", " + ChildB.ToString() + "}"; } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; ChildA.Analyze(depths, depth + 1, ref nodeCount); ChildB.Analyze(depths, depth + 1, ref nodeCount); } internal override void Refit(MeshBoundingBoxTreeData data) { ChildA.Refit(data); ChildB.Refit(data); BoundingBox.CreateMerged(ref ChildA.BoundingBox, ref ChildB.BoundingBox, out BoundingBox); } } /// /// The tiny extra margin added to leaf bounding boxes that allow the volume cost metric to function properly even in degenerate cases. /// public static float LeafMargin = .001f; sealed class LeafNode : Node { int LeafIndex; internal override bool IsLeaf { get { return true; } } internal LeafNode(int leafIndex, MeshBoundingBoxTreeData data) { LeafIndex = leafIndex; data.GetBoundingBox(leafIndex, out BoundingBox); //Having an ever-so-slight margin allows the hierarchy use a volume metric even for degenerate shapes (consider a flat tessellated plane). BoundingBox.Max.X += LeafMargin; BoundingBox.Max.Y += LeafMargin; BoundingBox.Max.Z += LeafMargin; BoundingBox.Min.X -= LeafMargin; BoundingBox.Min.Y -= LeafMargin; BoundingBox.Min.Z -= LeafMargin; } internal override void GetOverlaps(ref BoundingBox boundingBox, IList outputOverlappedElements) { //Our parent already tested the bounding box. All that's left is to add myself to the list. outputOverlappedElements.Add(LeafIndex); } internal override void GetOverlaps(ref BoundingSphere boundingSphere, IList outputOverlappedElements) { outputOverlappedElements.Add(LeafIndex); } internal override void GetOverlaps(ref BoundingFrustum boundingFrustum, IList outputOverlappedElements) { outputOverlappedElements.Add(LeafIndex); } internal override void GetOverlaps(ref Ray ray, float maximumLength, IList outputOverlappedElements) { outputOverlappedElements.Add(LeafIndex); } internal override bool TryToInsert(LeafNode node, out Node treeNode) { var newTreeNode = new InternalNode(); BoundingBox.CreateMerged(ref BoundingBox, ref node.BoundingBox, out newTreeNode.BoundingBox); newTreeNode.ChildA = this; newTreeNode.ChildB = node; treeNode = newTreeNode; return true; } public override string ToString() { return LeafIndex.ToString(CultureInfo.InvariantCulture); } internal override void Analyze(List depths, int depth, ref int nodeCount) { nodeCount++; depths.Add(depth); } internal override void Refit(MeshBoundingBoxTreeData data) { data.GetBoundingBox(LeafIndex, out BoundingBox); //Having an ever-so-slight margin allows the hierarchy use a volume metric even for degenerate shapes (consider a flat tessellated plane). BoundingBox.Max.X += LeafMargin; BoundingBox.Max.Y += LeafMargin; BoundingBox.Max.Z += LeafMargin; BoundingBox.Min.X -= LeafMargin; BoundingBox.Min.Y -= LeafMargin; BoundingBox.Min.Z -= LeafMargin; } } } } ================================================ FILE: BEPUphysics/DataStructures/MeshBoundingBoxTreeData.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.DataStructures { /// /// Superclass of the data used to create triangle mesh bounding box trees. /// public abstract class MeshBoundingBoxTreeData { internal uint[] indices; /// /// Gets or sets the indices of the triangle mesh. /// public uint[] Indices { get { return indices; } set { indices = value; } } public int IndexCount; internal Vector3[] vertices; /// /// Gets or sets the vertices of the triangle mesh. /// public Vector3[] Vertices { get { return vertices; } set { vertices = value; } } /// /// Gets the bounding box of an element in the data. /// /// Index of the triangle in the data. /// Bounding box of the triangle. public void GetBoundingBox(int triangleIndex, out BoundingBox boundingBox) { Vector3 v1, v2, v3; GetTriangle(triangleIndex, out v1, out v2, out v3); Vector3.Min(ref v1, ref v2, out boundingBox.Min); Vector3.Min(ref boundingBox.Min, ref v3, out boundingBox.Min); Vector3.Max(ref v1, ref v2, out boundingBox.Max); Vector3.Max(ref boundingBox.Max, ref v3, out boundingBox.Max); } /// /// Gets the triangle vertex positions at a given index. /// ///First index of a triangle's vertices in the index buffer. ///First vertex of the triangle. ///Second vertex of the triangle. ///Third vertex of the triangle. public abstract void GetTriangle(int triangleIndex, out Vector3 v1, out Vector3 v2, out Vector3 v3); /// /// Gets the position of a vertex in the data. /// ///Index of the vertex. ///Position of the vertex. public abstract void GetVertexPosition(int i, out Vector3 vertex); } } ================================================ FILE: BEPUphysics/DataStructures/StaticMeshData.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.DataStructures { /// /// Collection of triangle mesh data that directly returns vertices from its vertex buffer instead of transforming them first. /// public class StaticMeshData : MeshBoundingBoxTreeData { /// /// Constructs the triangle mesh data. /// ///Vertices to use in the data. ///Indices to use in the data. public StaticMeshData(Vector3[] vertices, uint[] indices, int indexCount) { Vertices = vertices; Indices = indices; IndexCount = indexCount; } /// /// Gets the triangle vertex positions at a given index. /// ///First index of a triangle's vertices in the index buffer. ///First vertex of the triangle. ///Second vertex of the triangle. ///Third vertex of the triangle. public override void GetTriangle(int triangleIndex, out Vector3 v1, out Vector3 v2, out Vector3 v3) { v1 = vertices[indices[triangleIndex]]; v2 = vertices[indices[triangleIndex + 1]]; v3 = vertices[indices[triangleIndex + 2]]; } /// /// Gets the position of a vertex in the data. /// ///Index of the vertex. ///Position of the vertex. public override void GetVertexPosition(int i, out Vector3 vertex) { vertex = vertices[i]; } } } ================================================ FILE: BEPUphysics/DataStructures/TransformableMeshData.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.DataStructures { /// /// Collection of mesh data which transforms its vertices before returning them. /// public class TransformableMeshData : MeshBoundingBoxTreeData { /// /// Constructs the mesh data. /// ///Vertices to use in the mesh data. ///Indices to use in the mesh data. public TransformableMeshData(Vector3[] vertices, uint[] indices, int indexCount) { Vertices = vertices; Indices = indices; IndexCount = indexCount; } /// /// Constructs the mesh data. /// ///Vertice sto use in the mesh data. ///Indices to use in the mesh data. ///Transform to apply to vertices before returning their positions. public TransformableMeshData(Vector3[] vertices, uint[] indices, int indexCount, AffineTransform worldTransform) { this.worldTransform = worldTransform; Vertices = vertices; Indices = indices; IndexCount = indexCount; } internal AffineTransform worldTransform = AffineTransform.Identity; /// /// Gets or sets the transform to apply to the vertices before returning their position. /// public AffineTransform WorldTransform { get { return worldTransform; } set { worldTransform = value; } } /// /// Gets the triangle vertex positions at a given index. /// ///First index of a triangle's vertices in the index buffer. ///First vertex of the triangle. ///Second vertex of the triangle. ///Third vertex of the triangle. public override void GetTriangle(int triangleIndex, out Vector3 v1, out Vector3 v2, out Vector3 v3) { AffineTransform.Transform(ref vertices[indices[triangleIndex]], ref worldTransform, out v1); AffineTransform.Transform(ref vertices[indices[triangleIndex + 1]], ref worldTransform, out v2); AffineTransform.Transform(ref vertices[indices[triangleIndex + 2]], ref worldTransform, out v3); } /// /// Gets the position of a vertex in the data. /// ///Index of the vertex. ///Position of the vertex. public override void GetVertexPosition(int i, out Vector3 vertex) { AffineTransform.Transform(ref vertices[i], ref worldTransform, out vertex); } } } ================================================ FILE: BEPUphysics/DataStructures/TreeOverlapPair.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace BEPUphysics.DataStructures { /// /// Result of an overlap test between two trees of specified type. /// ///Type of elements in the first tree. ///Type of elements in the second tree. public struct TreeOverlapPair { /// /// Overlap owned by the first tree. /// public T1 OverlapA; /// /// Overlap owned by the second tree. /// public T2 OverlapB; /// /// Constructs a new overlap pair. /// /// Overlap owned by the first tree. /// Overlap owned by the second tree. public TreeOverlapPair(T1 overlapA, T2 overlapB) { OverlapA = overlapA; OverlapB = overlapB; } } } ================================================ FILE: BEPUphysics/DataStructures/TriangleMesh.cs ================================================ using System.Collections.Generic; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using Microsoft.Xna.Framework.Graphics; namespace BEPUphysics.DataStructures { /// /// Data structure containing triangle mesh data and its associated bounding box tree. /// public class TriangleMesh { private MeshBoundingBoxTreeData data; /// /// Gets or sets the bounding box data used in the mesh. /// public MeshBoundingBoxTreeData Data { get { return data; } set { data = value; tree.Data = data; } } private MeshBoundingBoxTree tree; /// /// Gets the bounding box tree that accelerates queries to this triangle mesh. /// public MeshBoundingBoxTree Tree { get { return tree; } } /// /// Constructs a new triangle mesh. /// ///Data to use to construct the mesh. public TriangleMesh(MeshBoundingBoxTreeData data) { this.data = data; tree = new MeshBoundingBoxTree(data); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. ///Number of hits between the ray and the mesh. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, out int hitCount) { var rayHits = CommonResources.GetRayHitList(); bool toReturn = RayCast(ray, rayHits); hitCount = rayHits.Count; CommonResources.GiveBack(rayHits); return toReturn; } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, out RayHit rayHit) { return RayCast(ray, float.MaxValue, TriangleSidedness.DoubleSided, out rayHit); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Sidedness to apply to the mesh for the ray cast. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, TriangleSidedness sidedness, out RayHit rayHit) { return RayCast(ray, float.MaxValue, sidedness, out rayHit); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, IList hits) { return RayCast(ray, float.MaxValue, TriangleSidedness.DoubleSided, hits); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Sidedness to apply to the mesh for the ray cast. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, TriangleSidedness sidedness, IList hits) { return RayCast(ray, float.MaxValue, sidedness, hits); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Maximum length of the ray in units of the ray direction's length. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, out RayHit rayHit) { return RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, out rayHit); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Maximum length of the ray in units of the ray direction's length. /// Sidedness to apply to the mesh for the ray cast. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, out RayHit rayHit) { var rayHits = CommonResources.GetRayHitList(); bool toReturn = RayCast(ray, maximumLength, sidedness, rayHits); if (toReturn) { rayHit = rayHits[0]; for (int i = 1; i < rayHits.Count; i++) { RayHit hit = rayHits[i]; if (hit.T < rayHit.T) rayHit = hit; } } else rayHit = new RayHit(); CommonResources.GiveBack(rayHits); return toReturn; } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Maximum length of the ray in units of the ray direction's length. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, IList hits) { return RayCast(ray, maximumLength, TriangleSidedness.DoubleSided, hits); } /// /// Tests a ray against the triangle mesh. /// ///Ray to test against the mesh. /// Maximum length of the ray in units of the ray direction's length. /// Sidedness to apply to the mesh for the ray cast. ///Hit data for the ray, if any. ///Whether or not the ray hit the mesh. public bool RayCast(Ray ray, float maximumLength, TriangleSidedness sidedness, IList hits) { var hitElements = CommonResources.GetIntList(); tree.GetOverlaps(ray, maximumLength, hitElements); for (int i = 0; i < hitElements.Count; i++) { Vector3 v1, v2, v3; data.GetTriangle(hitElements[i], out v1, out v2, out v3); RayHit hit; if (Toolbox.FindRayTriangleIntersection(ref ray, maximumLength, sidedness, ref v1, ref v2, ref v3, out hit)) { hits.Add(hit); } } CommonResources.GiveBack(hitElements); return hits.Count > 0; } #region Vertex extraction helpers /// /// Gets an array of vertices and indices from the provided model. /// /// Model to use for the collision shape. /// Compiled set of vertices from the model. /// Compiled set of indices from the model. public static void GetVerticesAndIndicesFromModel(Model collisionModel, out Vector3[] vertices, out int[] indices) { var verticesList = new List(); var indicesList = new List(); var transforms = new Matrix[collisionModel.Bones.Count]; collisionModel.CopyAbsoluteBoneTransformsTo(transforms); Matrix transform; foreach (ModelMesh mesh in collisionModel.Meshes) { if (mesh.ParentBone != null) transform = transforms[mesh.ParentBone.Index]; else transform = Matrix.Identity; AddMesh(mesh, transform, verticesList, indicesList); } vertices = verticesList.ToArray(); indices = indicesList.ToArray(); } /// /// Adds a mesh's vertices and indices to the given lists. /// /// Model to use for the collision shape. /// Transform to apply to the mesh. /// List to receive vertices from the mesh. /// List to receive indices from the mesh. public static void AddMesh(ModelMesh collisionModelMesh, Matrix transform, List vertices, IList indices) { foreach (ModelMeshPart meshPart in collisionModelMesh.MeshParts) { int startIndex = vertices.Count; var meshPartVertices = new Vector3[meshPart.NumVertices]; //Grab position data from the mesh part. int stride = meshPart.VertexBuffer.VertexDeclaration.VertexStride; meshPart.VertexBuffer.GetData( meshPart.VertexOffset * stride, meshPartVertices, 0, meshPart.NumVertices, stride); //Transform it so its vertices are located in the model's space as opposed to mesh part space. Vector3.Transform(meshPartVertices, ref transform, meshPartVertices); vertices.AddRange(meshPartVertices); if (meshPart.IndexBuffer.IndexElementSize == IndexElementSize.ThirtyTwoBits) { var meshIndices = new int[meshPart.PrimitiveCount * 3]; meshPart.IndexBuffer.GetData(meshPart.StartIndex * 4, meshIndices, 0, meshPart.PrimitiveCount * 3); for (int k = 0; k < meshIndices.Length; k++) { indices.Add(startIndex + meshIndices[k]); } } else { var meshIndices = new ushort[meshPart.PrimitiveCount * 3]; meshPart.IndexBuffer.GetData(meshPart.StartIndex * 2, meshIndices, 0, meshPart.PrimitiveCount * 3); for (int k = 0; k < meshIndices.Length; k++) { indices.Add(startIndex + meshIndices[k]); } } } } #endregion } } ================================================ FILE: BEPUphysics/DeactivationManagement/DeactivationManager.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.Threading; using BEPUutilities; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUphysics.DeactivationManagement { /// /// Manages the sleeping states of objects. /// public class DeactivationManager : MultithreadedProcessingStage { private int maximumDeactivationAttemptsPerFrame = 100; private int deactivationIslandIndex; internal float velocityLowerLimit = .26f; internal float velocityLowerLimitSquared = .26f * .26f; internal float lowVelocityTimeMinimum = 1f; /// /// Gets or sets the velocity under which the deactivation system will consider /// objects to be deactivation candidates (if their velocity stays below the limit /// for the LowVelocityTimeMinimum). /// Defaults to 0.26. /// public float VelocityLowerLimit { get { return velocityLowerLimit; } set { velocityLowerLimit = Math.Max(0, value); velocityLowerLimitSquared = velocityLowerLimit * velocityLowerLimit; } } /// /// Gets or sets the time limit above which the deactivation system will consider /// objects to be deactivation candidates (if their velocity stays below the VelocityLowerLimit for the duration). /// Defaults to 1. /// public float LowVelocityTimeMinimum { get { return lowVelocityTimeMinimum; } set { if (value <= 0) throw new ArgumentException("Must use a positive, non-zero value for deactivation time minimum."); lowVelocityTimeMinimum = value; } } internal bool useStabilization = true; /// /// Gets or sets whether or not to use a stabilization effect on nearly motionless objects. /// This removes a lot of energy from a system when things are settling down, allowing them to go /// to sleep faster. It also makes most simulations appear a lot more robust. /// Defaults to true. /// public bool UseStabilization { get { return useStabilization; } set { useStabilization = value; } } //TryToSplit is NOT THREAD SAFE. Only one TryToSplit should ever be run. Queue member1Friends = new Queue(), member2Friends = new Queue(); List searchedMembers1 = new List(), searchedMembers2 = new List(); /// /// Gets or sets the maximum number of objects to attempt to deactivate each frame. /// Defaults to 100. /// public int MaximumDeactivationAttemptsPerFrame { get { return maximumDeactivationAttemptsPerFrame; } set { maximumDeactivationAttemptsPerFrame = value; } } TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the deactivation manager. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } set { timeStepSettings = value; } } /// /// Constructs a deactivation manager. /// ///The time step settings used by the manager. public DeactivationManager(TimeStepSettings timeStepSettings) { Enabled = true; multithreadedCandidacyLoopDelegate = MultithreadedCandidacyLoop; this.timeStepSettings = timeStepSettings; } /// /// Constructs a deactivation manager. /// ///The time step settings used by the manager. /// Thread manager used by the manager. public DeactivationManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : this(timeStepSettings) { ThreadManager = threadManager; AllowMultithreading = true; } //TODO: Deactivation Candidate Detection //-Could scan the entities of CURRENTLY ACTIVE simulation islands. //-Requires a List-format of active sim islands. //-Requires sim islands have a list-format entity set. //-Simulation islands of different sizes won't load-balance well on the xbox360; it would be fine on the pc though. //TODO: Simulation Island Deactivation RawList simulationIslandMembers = new RawList(); RawList simulationIslands = new RawList(); /// /// Gets the simulation islands currently in the manager. /// public ReadOnlyList SimulationIslands { get { return new ReadOnlyList(simulationIslands); } } UnsafeResourcePool islandPool = new UnsafeResourcePool(); void GiveBackIsland(SimulationIsland island) { island.CleanUp(); islandPool.GiveBack(island); } /// /// Adds a simulation island member to the manager. /// ///Member to add. ///Thrown if the member already belongs to a manager. public void Add(SimulationIslandMember simulationIslandMember) { if (simulationIslandMember.DeactivationManager == null) { simulationIslandMember.Activate(); simulationIslandMember.DeactivationManager = this; simulationIslandMembers.Add(simulationIslandMember); if (simulationIslandMember.IsDynamic) { AddSimulationIslandToMember(simulationIslandMember); } else { RemoveSimulationIslandFromMember(simulationIslandMember); } } else throw new ArgumentException("Cannot add that member to this DeactivationManager; it already belongs to a manager."); } /// /// Removes the member from this island. /// /// Removes the member from the manager. public void Remove(SimulationIslandMember simulationIslandMember) { if (simulationIslandMember.DeactivationManager == this) { simulationIslandMember.DeactivationManager = null; simulationIslandMembers.Remove(simulationIslandMember); RemoveSimulationIslandFromMember(simulationIslandMember); } else throw new ArgumentException("Cannot remove that member from this DeactivationManager; it belongs to a different or no manager."); } Action multithreadedCandidacyLoopDelegate; void MultithreadedCandidacyLoop(int i) { simulationIslandMembers.Elements[i].UpdateDeactivationCandidacy(timeStepSettings.TimeStepDuration); } protected override void UpdateMultithreaded() { FlushSplits(); ThreadManager.ForLoop(0, simulationIslandMembers.Count, multithreadedCandidacyLoopDelegate); DeactivateObjects(); } //RawList debugConnections = new RawList(); protected override void UpdateSingleThreaded() { FlushSplits(); for (int i = 0; i < simulationIslandMembers.Count; i++) simulationIslandMembers.Elements[i].UpdateDeactivationCandidacy(timeStepSettings.TimeStepDuration); DeactivateObjects(); } Queue splitAttempts = new Queue(); static float maximumSplitAttemptsFraction = .01f; /// /// Gets or sets the fraction of splits that the deactivation manager will attempt in a single frame. /// The total splits queued multiplied by this value results in the number of splits managed. /// Defaults to .04f. /// public static float MaximumSplitAttemptsFraction { get { return maximumSplitAttemptsFraction; } set { if (value > 1 || value < 0) throw new ArgumentException("Value must be from zero to one."); maximumSplitAttemptsFraction = value; } } static int minimumSplitAttempts = 3; /// /// Gets or sets the minimum number of splits attempted in a single frame. /// Defaults to 5. /// public static int MinimumSplitAttempts { get { return minimumSplitAttempts; } set { if (value >= 0) throw new ArgumentException("Minimum split count must be nonnegative."); minimumSplitAttempts = value; } } void FlushSplits() { //Only do a portion of the total splits. int maxAttempts = Math.Max(minimumSplitAttempts, (int)(splitAttempts.Count * maximumSplitAttemptsFraction)); int attempts = 0; while (attempts < maxAttempts && splitAttempts.Count > 0) { var attempt = splitAttempts.Dequeue(); if (attempt.SlatedForRemoval) //If it was re-added, don't split! { attempt.SlatedForRemoval = false; //Reset the removal state so that future adds will add back references, since we're about to remove them. attempt.RemoveReferencesFromConnectedMembers(); bool triedToSplit = false; for (int i = 0; i < attempt.entries.Count; i++) { for (int j = i + 1; j < attempt.entries.Count; j++) { triedToSplit |= TryToSplit(attempt.entries.Elements[i].Member, attempt.entries.Elements[j].Member); } } //Only count the split if it does any work. if (triedToSplit) attempts++; if (attempt.Owner == null) { //It's an orphan connection. No one owns it, and now that it's been dequeued from the deactivation manager, //it has no home at all. //Don't let it rot- return it to the pool! PhysicsResources.GiveBack(attempt); //This occurs when a constraint changes members. //Because connections need to be immutable for this scheme to work, //the old connection is orphaned and put into the deactivation manager's removal queue //while a new one from the pool takes its place. } } } } void DeactivateObjects() { //Deactivate only some objects each frame. int numberOfEntitiesDeactivated = 0; int numberOfIslandsChecked = 0; int originalIslandCount = simulationIslands.Count; while (numberOfEntitiesDeactivated < maximumDeactivationAttemptsPerFrame && simulationIslands.Count > 0 && numberOfIslandsChecked < originalIslandCount) { deactivationIslandIndex = (deactivationIslandIndex + 1) % simulationIslands.Count; var island = simulationIslands.Elements[deactivationIslandIndex]; if (island.memberCount == 0) { //Found an orphan island left over from merge procedures or removal procedures. //Shoo it on out. simulationIslands.FastRemoveAt(deactivationIslandIndex); GiveBackIsland(island); } else { island.TryToDeactivate(); numberOfEntitiesDeactivated += island.memberCount; } numberOfIslandsChecked++; } } /// /// Adds a simulation island connection to the deactivation manager. /// ///Connection to add. ///Thrown if the connection already belongs to a manager. public void Add(SimulationIslandConnection connection) { //DO A MERGE IF NECESSARY if (connection.DeactivationManager == null) { connection.DeactivationManager = this; if (connection.entries.Count > 0) { var island = connection.entries.Elements[0].Member.SimulationIsland; for (int i = 1; i < connection.entries.Count; i++) { SimulationIsland opposingIsland; if (island != (opposingIsland = connection.entries.Elements[i].Member.SimulationIsland)) { //Need to do a merge between the two islands. //Note that this merge may eliminate the need for a merge with subsequent connection if they belong to the same island. island = Merge(island, opposingIsland); } } if (connection.SlatedForRemoval) connection.SlatedForRemoval = false; //If it was slated for removal, that means connections are still present. Just set the flag and the removal system will ignore the removal order. else connection.AddReferencesToConnectedMembers(); //debugConnections.Add(connection); } } else { throw new ArgumentException("Cannot add connection to deactivation manager; it already belongs to one."); } } private SimulationIsland Merge(SimulationIsland s1, SimulationIsland s2) { //Pull the smaller island into the larger island and set all members //of the smaller island to refer to the new island. //The simulation islands can be null; a connection can be a kinematic entity, which has no simulation island. //'Merging' a null island with an island simply gets back the island. if (s1 == null) { //Should still activate the island, though. s2.Activate(); return s2; } if (s2 == null) { //Should still activate the island, though. s1.Activate(); return s1; } //Swap if needed so s1 is the bigger island if (s1.memberCount < s2.memberCount) { var biggerIsland = s2; s2 = s1; s1 = biggerIsland; } s1.Activate(); s2.immediateParent = s1; //This is a bit like a 'union by rank.' //But don't get confused- simulation islands are not a union find structure. //This parenting simply avoids the need for maintaining a list of members in each simulation island. //In the subsequent frame, the deactivation candidacy update will go through the parents and eat away //at the child simulation island. Then, in a later TryToDeactivate phase, the then-empty simulation island //will be removed. //The larger one survives. return s1; } /// /// Removes a simulation island connection from the manager. /// ///Connection to remove from the manager. ///Thrown if the connection does not belong to this manager. public void Remove(SimulationIslandConnection connection) { if (connection.DeactivationManager == this) { connection.DeactivationManager = null; //TODO: Should it remove here, or in the deferred final case? probably here, since otherwise Add-Remove-Add would throw an exception! //Try to split by examining the connections and breadth-first searching outward. //If it is determined that a split is required, grab a new island and add it. //This is a little tricky because it's a theoretically N-way split. //For two members which have the same simulation island (they will initially), try to split. //debugConnections.Remove(connection); connection.SlatedForRemoval = true; //Don't immediately do the removal. //Defer them! splitAttempts.Enqueue(connection); //connection.RemoveReferencesFromConnectedMembers(); //for (int i = 0; i < connection.members.count; i++) //{ // for (int j = i + 1; j < connection.members.count; j++) // { // //Notice that the splits are not performed immediately! They are deferred and spread over multiple frames. // //Split operations aren't cheap, and overdoing them can lead to pointless re-merging and re-splitting. // splitAttempts.Enqueue(new SplitAttempt() { a = connection.members.Elements[i], b = connection.members.Elements[j] }); // //TryToSplit(connection.ConnectedMembers[i], connection.ConnectedMembers[j]); // } //} } else { throw new ArgumentException("Cannot remove connection from activity manager; it is owned by a different or no activity manager."); } } /// /// Tries to split connections between the two island members. /// /// First island member. /// Second island member. /// Whether a split operation was run. This does not mean a split was /// successful, just that the expensive test was performed. private bool TryToSplit(SimulationIslandMember member1, SimulationIslandMember member2) { //Can't split if they aren't even in the same island. //This also covers the case where the connection involves a kinematic entity that has no //simulation island at all. if (member1.SimulationIsland != member2.SimulationIsland || member1.SimulationIsland == null || member2.SimulationIsland == null) return false; //By now, we know the members belong to the same island and are not null. //Start a BFS starting from each member. //Two-way can complete the search quicker. member1Friends.Enqueue(member1); member2Friends.Enqueue(member2); searchedMembers1.Add(member1); searchedMembers2.Add(member2); member1.searchState = SimulationIslandSearchState.OwnedByFirst; member2.searchState = SimulationIslandSearchState.OwnedBySecond; while (member1Friends.Count > 0 && member2Friends.Count > 0) { SimulationIslandMember currentNode = member1Friends.Dequeue(); for (int i = 0; i < currentNode.connections.Count; i++) { for (int j = 0; j < currentNode.connections.Elements[i].entries.Count; j++) { SimulationIslandMember connectedNode; if ((connectedNode = currentNode.connections.Elements[i].entries.Elements[j].Member) != currentNode && connectedNode.SimulationIsland != null) //The connection could be connected to something that isn't in the Space and has no island, or it's not dynamic. { switch (connectedNode.searchState) { case SimulationIslandSearchState.Unclaimed: //Found a new friend :) member1Friends.Enqueue(connectedNode); connectedNode.searchState = SimulationIslandSearchState.OwnedByFirst; searchedMembers1.Add(connectedNode); break; case SimulationIslandSearchState.OwnedBySecond: //Found our way to member2Friends set; cannot split! member1Friends.Clear(); member2Friends.Clear(); goto ResetSearchStates; } } } } currentNode = member2Friends.Dequeue(); for (int i = 0; i < currentNode.connections.Count; i++) { for (int j = 0; j < currentNode.connections.Elements[i].entries.Count; j++) { SimulationIslandMember connectedNode; if ((connectedNode = currentNode.connections.Elements[i].entries.Elements[j].Member) != currentNode && connectedNode.SimulationIsland != null) //The connection could be connected to something that isn't in the Space and has no island, or it's not dynamic. { switch (connectedNode.searchState) { case SimulationIslandSearchState.Unclaimed: //Found a new friend :) member2Friends.Enqueue(connectedNode); connectedNode.searchState = SimulationIslandSearchState.OwnedBySecond; searchedMembers2.Add(connectedNode); break; case SimulationIslandSearchState.OwnedByFirst: //Found our way to member1Friends set; cannot split! member1Friends.Clear(); member2Friends.Clear(); goto ResetSearchStates; } } } } } //If one of the queues empties out without finding anything, it means it's isolated. The other one will never find it. //Now we can do a split. Grab a new Island, fill it with the isolated search stuff. Remove the isolated search stuff from the old Island. SimulationIsland newIsland = islandPool.Take(); simulationIslands.Add(newIsland); if (member1Friends.Count == 0) { //Member 1 is isolated, give it its own simulation island! for (int i = 0; i < searchedMembers1.Count; i++) { searchedMembers1[i].simulationIsland.Remove(searchedMembers1[i]); newIsland.Add(searchedMembers1[i]); } member2Friends.Clear(); } else if (member2Friends.Count == 0) { //Member 2 is isolated, give it its own simulation island! for (int i = 0; i < searchedMembers2.Count; i++) { searchedMembers2[i].simulationIsland.Remove(searchedMembers2[i]); newIsland.Add(searchedMembers2[i]); } member1Friends.Clear(); } //Force the system awake. //Technically, the members should already be awake. //However, calling Activate on them resets the members' //deactivation candidacy timers. This prevents the island //from instantly going back to sleep, which could leave //objects hanging in mid-air. member1.Activate(); member2.Activate(); ResetSearchStates: for (int i = 0; i < searchedMembers1.Count; i++) { searchedMembers1[i].searchState = SimulationIslandSearchState.Unclaimed; } for (int i = 0; i < searchedMembers2.Count; i++) { searchedMembers2[i].searchState = SimulationIslandSearchState.Unclaimed; } searchedMembers1.Clear(); searchedMembers2.Clear(); return true; } /// /// Strips a member of its simulation island. /// ///Member to be stripped. public void RemoveSimulationIslandFromMember(SimulationIslandMember member) { //Becoming kinematic eliminates the member as a possible path. //Splits must be attempted between its connected members. //Don't need to split same-connection members. Splitting one non-null entry against a non null entry in each of the other connections will do the trick. if (member.simulationIsland != null) { //Note that this is using the most immediate simulation island. This is because the immediate simulation island //is the one who 'owns' the member; not the root parent. The root parent will own the member in the next frame //after the deactivation candidacy loop runs. SimulationIsland island = member.simulationIsland; island.Remove(member); if (island.memberCount == 0) { simulationIslands.Remove(island); GiveBackIsland(island); //Even though we appear to have connections, the island was only me! //We can stop now. //Note that we do NOT remove the island from the simulation islands list here. //That would take an O(n) search. Instead, orphan it and let the TryToDeactivate loop find it. return; } } if (member.connections.Count > 0) { for (int i = 0; i < member.connections.Count; i++) { //Find a member with a non-null island to represent connection i. SimulationIslandMember representativeA = null; for (int j = 0; j < member.connections.Elements[i].entries.Count; j++) { if (member.connections.Elements[i].entries.Elements[j].Member.SimulationIsland != null) { representativeA = member.connections.Elements[i].entries.Elements[j].Member; break; } } if (representativeA == null) { //There was no representative! That means it was a connection in which //no member had a simulation island. Consider removing a dynamic box from the space //while it sits on a kinematic box. Neither object has a simulation island. //In this case, simply try the next connection. continue; } //Activate the representative. This must be performed even if no split occurs; connected objects must be activated! representativeA.Activate(); //Split the representative against representatives from other connections. for (int j = i + 1; j < member.connections.Count; j++) { //Find a representative for another connection. SimulationIslandMember representativeB = null; for (int k = 0; k < member.connections.Elements[j].entries.Count; k++) { if (member.connections.Elements[j].entries.Elements[k].Member.SimulationIsland != null) { representativeB = member.connections.Elements[j].entries.Elements[k].Member; break; } } if (representativeB == null) { //There was no representative! Same idea as above. //Try the next connection. continue; } //Activate the representative. This must be performed even if no split occurs; connected objects must be activated! representativeB.Activate(); //Try to split the representatives. //Don't bother doing any deferring; this is a rare activity //and it's best just to do it up front. TryToSplit(representativeA, representativeB); } } } } /// /// Adds a simulation island to a member. /// ///Member to gain a simulation island. ///Thrown if the member already has a simulation island. public void AddSimulationIslandToMember(SimulationIslandMember member) { if (member.SimulationIsland != null) { throw new ArgumentException("Cannot initialize member's simulation island; it already has one."); } if (member.connections.Count > 0) { SimulationIsland island = null; //Find a simulation starting island to live in. for (int i = 0; i < member.connections.Count; i++) { for (int j = 0; j < member.connections.Elements[i].entries.Count; j++) { island = member.connections.Elements[i].entries.Elements[j].Member.SimulationIsland; if (island != null) { island.Add(member); break; } } if (island != null) break; } if (member.SimulationIsland == null) { //No non-null entries in any connections. That's weird. //Maybe it's connected to a bunch of kinematics, or maybe it's a vehicle-like situation //where the body is associated with a 'vehicle' connection which sometimes contains only the body. //No friends to merge with. SimulationIsland newIsland = islandPool.Take(); simulationIslands.Add(newIsland); newIsland.Add(member); return; } //Becoming dynamic adds a new path. //Merges must be attempted between its connected members. for (int i = 0; i < member.connections.Count; i++) { for (int j = 0; j < member.connections.Elements[i].entries.Count; j++) { if (member.connections.Elements[i].entries.Elements[j].Member == member) continue; //Don't bother trying to compare against ourselves. That would cause an erroneous early-out sometimes. SimulationIsland opposingIsland = member.connections.Elements[i].entries.Elements[j].Member.SimulationIsland; if (opposingIsland != null) { if (island != opposingIsland) { island = Merge(island, opposingIsland); } //All non-null simulation islands in a single connection are guaranteed to be the same island due to previous merges. //Once we find one, we can stop. break; } } } } else { //No friends to merge with. SimulationIsland newIsland = islandPool.Take(); simulationIslands.Add(newIsland); newIsland.Add(member); } } } } ================================================ FILE: BEPUphysics/DeactivationManagement/ISimulationIslandConnection.cs ================================================ using System.Collections.ObjectModel; using BEPUutilities.DataStructures; namespace BEPUphysics.DeactivationManagement { /// /// Defines an object which connects simulation islands together. /// public interface ISimulationIslandConnection { /// /// Gets or sets the deactivation member that owns this connection. /// DeactivationManager DeactivationManager { get; set; } /// /// Gets the simulation island members associated with this connection. /// ReadOnlyList ConnectedMembers { get; } /// /// Adds references to the connection to all connected members. /// void AddReferencesToConnectedMembers(); /// /// Removes references to the connection from all connected members. /// void RemoveReferencesFromConnectedMembers(); //When connections are modified, simulation island needs to be updated. //Could have a custom collection that shoots off events when it is changed, or otherwise notifies... //But then again, whatever is doing the changing can do the notification without a custom collection. //It would need to call merge/trysplit.. Not everything has the ability to change connections, but some do. //Leave it up to the implementors :) //When "Added," it attempts to merge the simulation islands of its connected members. //When "Removed," it attempts to split the simulation islands of its connected members. //The SimulationIslandConnection is not itself a thing that is added/removed to a set somewhere. //It is a description of some object. The act of 'adding' a simulation island connection is completed in full by //merging simulation islands and the associated bookkeeping- there does not need to be a list anywhere. //Likewise, removing a simulation island is completed in full by the split attempt. //However, whatever is doing the management of things that happen to be ISimulationIslandConnections must know how to deal with them. //(Using the SimulationIsland.Merge, TrySplit methods). //This leaves a lot of responsibility in the implementation's hands. //Maybe this is just fine. Consider that this isn't exactly a common situation. //Known, in-engine scenarios: //Constraints //Collision Pairs. //When constraints are added or removed to the space (or maybe more directly the solver, to unify with cp's), the merge happens. //When they are removed from the space, the split happens. //All SolverItems are also simulation connections, which is why it makes sense to do it at the solver time.... HOWEVER... //It is possible to have a simulation island connection that isn't a solver item at all. Think particle collision. //In the particle case, the engine doesn't 'know' anything about the type- it's 100% custom. //The OnAdditionToSpace methods come in handy. Particle collision says, well, the system won't do it for me since I'm not going to be added to the solver. //So instead, whatever the thing is that handles the 'force application' part of the particle system would perform the necessary simulation island management. //Sidenote: The particle collision system would, in practice, probably just be a Solver item with 1 iteration. } } ================================================ FILE: BEPUphysics/DeactivationManagement/ISimulationIslandConnectionOwner.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace BEPUphysics.DeactivationManagement { /// /// Denotes a class which owns a simulation island connection. /// public interface ISimulationIslandConnectionOwner { /// /// Gets the connection associated with the object. /// SimulationIslandConnection SimulationIslandConnection { get; } } } ================================================ FILE: BEPUphysics/DeactivationManagement/ISimulationIslandMemberOwner.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace BEPUphysics.DeactivationManagement { /// /// Defines an object which owns a SimulationIslandMember. /// public interface ISimulationIslandMemberOwner { /// /// Gets the simulation island member associated with the object. /// SimulationIslandMember ActivityInformation { get; } } } ================================================ FILE: BEPUphysics/DeactivationManagement/SimulationIsland.cs ================================================ using System; using System.Threading; using BEPUutilities.DataStructures; using System.Collections.ObjectModel; namespace BEPUphysics.DeactivationManagement { /// /// A collection of simulation island members bound together with connections. /// An island is activated and deactivated as a group. /// public class SimulationIsland { internal SimulationIsland immediateParent; internal SimulationIsland Parent { get { return immediateParent == this ? this : immediateParent.Parent; } } internal bool allowDeactivation = true; internal bool isActive = true; /// /// Gets whether or not the island is currently active. /// public bool IsActive { get { return isActive; } set { isActive = value; } } internal int memberCount; /// /// Gets the number of simulation island members within this simulation island. /// public int MemberCount { get { return memberCount; } } internal int deactivationCandidateCount; /// /// Gets the number of simulation island members in the simulation island which are prepared to go to sleep. /// public int DeactivationCandidateCount { get { return deactivationCandidateCount; } } /// /// Constructs a simulation island. /// public SimulationIsland() { memberActivatedDelegate = MemberActivated; becameDeactivationCandidateDelegate = BecameDeactivationCandidate; becameNonDeactivationCandidateDelegate = BecameNonDeactivationCandidate; CleanUp(); } Action memberActivatedDelegate; void MemberActivated(SimulationIslandMember member) { Activate(); } Action becameDeactivationCandidateDelegate; void BecameDeactivationCandidate(SimulationIslandMember member) { Interlocked.Increment(ref deactivationCandidateCount); //The reason why this does not deactivate when count == members.count is that deactivation candidate count will go up and down in parallel. //The actual deactivation process is not designed to be thread safe. Perhaps doable, but perhaps not worth the effort. } Action becameNonDeactivationCandidateDelegate; void BecameNonDeactivationCandidate(SimulationIslandMember member) { Interlocked.Decrement(ref deactivationCandidateCount); } /// /// Activates the simulation island. /// public void Activate() { //TODO: CONSIDER ACTIVE ISLAND WITH FORCE-DEACTIVATED MEMBER. ACTIVATING SIMULATION ISLAND WILL NOT WAKE FORCE-DEACTIVATED MEMBER. DESIRED? if (!isActive) { isActive = true; } } /// /// Attempts to deactivate the simulation island. /// ///Whether or not the simulation island was successfully deactivated. public bool TryToDeactivate() { if (allowDeactivation) { //TODO: Check the deactivation count. If it's a fully deactivated simulation island, then try to deactivate !:) //DO NOT WORRY ABOUT THREAD SAFETY HERE. //TryToDeactivate will be called sequentially in a 'limited work per frame' scheme. //Avoids load balancing problems and makes implementation easier. if (isActive && deactivationCandidateCount == memberCount) { isActive = false; return true; } return false; } else { //Reset the allow deactivation flag so we don't stay inactive forever. allowDeactivation = true; return false; } } /// /// Adds a member to the simulation island. /// ///Member to add. ///Thrown when the member being added is either non-dynamic or already has a simulation island. public void Add(SimulationIslandMember member) { //This method is not thread safe. //TODO: Should it wake the island up? if (member.IsDynamic && member.simulationIsland == null) { member.simulationIsland = this; memberCount++; member.Activated += memberActivatedDelegate; member.BecameDeactivationCandidate += becameDeactivationCandidateDelegate; member.BecameNonDeactivationCandidate += becameNonDeactivationCandidateDelegate; if (member.IsDeactivationCandidate) { deactivationCandidateCount++; } } else throw new ArgumentException("Member either is not dynamic or already has a simulation island; cannot add."); } /// /// Removes a member from the simulation island. /// ///Member to remove. ///Thrown when the member does not belong to this simulation island. public void Remove(SimulationIslandMember member) { //Is this method ever used? What if old islands are simply cleared and a new one is repopulated instead? //More amenable to UFBRPC approach, probably quicker/simpler overall than removing even with lists //Consider a single block leaving a large island. BFS will quickly find out the necessary information to quickly //remove everything from the old island. //Event handlers will hold references still if not cleaned up via removal... //This method is not thread safe. //TODO: Should it wake the island up? if (member.simulationIsland == this) { memberCount--; member.simulationIsland = null; member.Activated -= memberActivatedDelegate; member.BecameDeactivationCandidate -= becameDeactivationCandidateDelegate; member.BecameNonDeactivationCandidate -= becameNonDeactivationCandidateDelegate; if (member.IsDeactivationCandidate) { deactivationCandidateCount--; } } else throw new ArgumentException("Member does not belong to island; cannot remove."); } internal void CleanUp() { isActive = true; deactivationCandidateCount = 0; memberCount = 0; immediateParent = this; } } } ================================================ FILE: BEPUphysics/DeactivationManagement/SimulationIslandConnection.cs ================================================ using System.Threading; using BEPUutilities.DataStructures; namespace BEPUphysics.DeactivationManagement { /// /// Connects simulation island members together. /// public class SimulationIslandConnection { /// /// Stores members and the index at which this connection is located in that member's connections list. /// This allows connections to be removed from members in constant time rather than linear time. /// internal struct Entry { internal SimulationIslandMember Member; internal int Index; } internal RawList entries = new RawList(2); /// /// Gets a list of members connected by the connection. /// public SimulationIslandMemberList Members { get { return new SimulationIslandMemberList(entries); } } /// /// Gets or sets the owner of the connection. /// public ISimulationIslandConnectionOwner Owner { get; set; } /// /// Gets whether or not this connection is going to be removed /// by the next DeactivationManager stage run. Connections /// slated for removal should not be considered to be part of /// a member's 'real' connections. /// public bool SlatedForRemoval { get; internal set; } /// /// Adds the connection to the connected members. /// public void AddReferencesToConnectedMembers() { //Add back the references to this to entities for (int i = 0; i < entries.Count; i++) { entries.Elements[i].Index = entries.Elements[i].Member.AddConnectionReference(this); } } /// /// Removes the connection from the connected members. /// public void RemoveReferencesFromConnectedMembers() { //Clean out the references entities may have had to this solver updateable. for (int i = 0; i < entries.Count; i++) { entries.Elements[i].Member.RemoveConnectionReference(this, entries.Elements[i].Index); } } /// /// Searches the list of members related to this connection and sets the index associated with this connection to the given value. /// /// Member to change the index for. /// New index of this connection in the member's connections list. internal void SetListIndex(SimulationIslandMember member, int index) { for (int i = 0; i < entries.Count; i++) { if (member == entries.Elements[i].Member) { entries.Elements[i].Index = index; break; } } } /// /// Gets or sets the deactivation manager that owns the connection. /// public DeactivationManager DeactivationManager { get; internal set; } internal void CleanUp() { SlatedForRemoval = false; entries.Clear(); Owner = null; DeactivationManager = null; } /// /// Adds the member to the connection. /// /// Member to add. internal void Add(SimulationIslandMember simulationIslandMember) { //Note that the simulation member does not yet know about this connection, so the index is assigned to -1. entries.Add(new Entry { Index = -1, Member = simulationIslandMember }); } } } ================================================ FILE: BEPUphysics/DeactivationManagement/SimulationIslandMember.cs ================================================ using System; using BEPUutilities.DataStructures; using BEPUphysics.Entities; namespace BEPUphysics.DeactivationManagement { /// /// Object owned by an entity which lives in a simulation island. /// Can be considered the entity's deactivation system proxy, just as the CollisionInformation property stores the collision pipeline proxy. /// public class SimulationIslandMember { //This system could be expanded to allow non-entity simulation island members. //However, there are no such objects on the near horizon, and it is unlikely that anyone will be interested in developing custom simulation island members. Entity owner; float previousVelocity; internal float velocityTimeBelowLimit; internal bool isSlowing; /// /// Gets the entity that owns this simulation island member. /// public Entity Owner { get { return owner; } } internal SimulationIslandMember(Entity owner) { this.owner = owner; } internal RawList connections = new RawList(16); /// /// Gets the connections associated with this member. /// public ReadOnlyList Connections { get { return new ReadOnlyList(connections); } } /// /// Updates the member's deactivation state. /// ///Timestep duration. public void UpdateDeactivationCandidacy(float dt) { //Get total velocity, and see if the entity is losing energy. float velocity = owner.linearVelocity.LengthSquared() + owner.angularVelocity.LengthSquared(); bool isActive = IsActive; if (isActive) { TryToCompressIslandHierarchy(); isSlowing = velocity <= previousVelocity; if (IsDynamic) { //Update time entity's been under the low-velocity limit, or reset if it's not if (velocity < DeactivationManager.velocityLowerLimitSquared) velocityTimeBelowLimit += dt; else velocityTimeBelowLimit = 0; if (!IsAlwaysActive) { if (!isDeactivationCandidate) { //See if the velocity has been low long enough to make this object a deactivation candidate. if (velocityTimeBelowLimit > DeactivationManager.lowVelocityTimeMinimum && isSlowing) //Only deactivate if it is NOT increasing in speed. { IsDeactivationCandidate = true; } } else { //See if velocity is high enough to make this object not a deactivation candidate. if (velocityTimeBelowLimit <= DeactivationManager.lowVelocityTimeMinimum) { IsDeactivationCandidate = false; } } } else IsDeactivationCandidate = false; } else { //If it's not dynamic, then deactivation candidacy is based entirely on whether or not the object has velocity (and the IsAlwaysActive state). IsDeactivationCandidate = velocity == 0 && !IsAlwaysActive; if (IsDeactivationCandidate) { //Update the flagging system. //If time <= 0, the entity is considered active. //Forcing a kinematic active needs to allow the system to run for a whole frame. //This means that in here, if it is < 0, we set it to zero. It will still update for the rest of the frame. //Then, next frame, when its == 0, set it to 1. It will be considered inactive unless it was activated manually again. if (velocityTimeBelowLimit == 0) velocityTimeBelowLimit = 1; else if (velocityTimeBelowLimit < 0) velocityTimeBelowLimit = 0; } else { //If velocity is not zero, then the flag is set to 'this is active.' velocityTimeBelowLimit = -1; } if (velocityTimeBelowLimit <= 0) { //There's a single oddity we need to worry about in this case. //An active kinematic object has no simulation island. Without intervention, //an active kinematic object will not keep an island awake. //To solve this, when we encounter active kinematic objects, //tell simulation islands associated with connected objects that they aren't allowed to deactivate. for (int i = 0; i < connections.Count; i++) { var connectedMembers = connections.Elements[i].entries; for (int j = connectedMembers.Count - 1; j >= 0; j--) { //The change locker must be obtained before attempting to access the SimulationIsland. //Path compression can force the simulation island to evaluate to null briefly. //Do not permit the object to undergo path compression during this (brief) operation. connectedMembers.Elements[j].Member.simulationIslandChangeLocker.Enter(); var island = connectedMembers.Elements[j].Member.SimulationIsland; if (island != null) { //It's possible a kinematic entity to go inactive for one frame, allowing nearby entities to go to sleep. //The next frame, it could wake up again. Because kinematics do not have simulation islands and thus //do not automatically wake up touching dynamic entities, we must do so manually. //This is safe because the island.Activate command is a single boolean set. //We're also inside the island change locker, so we don't have to worry about the island changing beneath our feet. island.Activate(); island.allowDeactivation = false; } connectedMembers.Elements[j].Member.simulationIslandChangeLocker.Exit(); } } } } } previousVelocity = velocity; //These will be 'eventually right.' if (previouslyActive && !isActive) OnDeactivated(); else if (!previouslyActive && isActive) OnActivated(); previouslyActive = isActive; } bool isDeactivationCandidate; /// /// Gets or sets whether or not the object is a deactivation candidate. /// public bool IsDeactivationCandidate { get { return isDeactivationCandidate; } private set { if (value && !isDeactivationCandidate) { isDeactivationCandidate = true; OnBecameDeactivationCandidate(); } else if (!value && isDeactivationCandidate) { isDeactivationCandidate = false; OnBecameNonDeactivationCandidate(); } if (!value) { velocityTimeBelowLimit = 0; } } } internal BEPUutilities.SpinLock simulationIslandChangeLocker = new BEPUutilities.SpinLock(); void TryToCompressIslandHierarchy() { var currentSimulationIsland = simulationIsland; if (currentSimulationIsland != null) { if (currentSimulationIsland.immediateParent != currentSimulationIsland) { //Only remove ourselves from the owning simulation island, not all the way up the chain. //The change locker must be obtained first to prevent kinematic notifications in the candidacy update //from attempting to evaluate the SimulationIsland while we are reorganizing things. simulationIslandChangeLocker.Enter(); lock (currentSimulationIsland) currentSimulationIsland.Remove(this); currentSimulationIsland = currentSimulationIsland.Parent; //Add ourselves to the new owner. lock (currentSimulationIsland) currentSimulationIsland.Add(this); simulationIslandChangeLocker.Exit(); //TODO: Should it activate the new island? This might avoid a possible corner case. //It could interfere with the activated event meaningfulness, since that is triggered //at the end of the update candidacy loop.. //currentSimulationIsland.isActive = true; } } } bool previouslyActive = true; /// /// Gets whether or not the member is active. /// public bool IsActive { get { var currentSimulationIsland = SimulationIsland; if (currentSimulationIsland != null) { return currentSimulationIsland.isActive; } else { //If the simulation island is null, //then this member has either not been added to a deactivation manager, //or it is a kinematic entity. //In either case, using the previous velocity is a reasonable approach. //This previous velocity is represented here by a flagging system that uses the velocityTimeBelowLimit. //-A kinematic entity with a velocityTimeBelowLimit of -1 was found to be active during the last deactivation candidacy analysis due to its velocity, //or it was recently activated, or IsAlwaysActive is set to true. //-A kinematic entity with a velocityTimeBelowLimit of 0 did not have its activity refreshed during the deactivation candidacy, //but we still consider it active so that a full frame can complete. //-A kinematic entity with a velocityTimeBelowLimit of 1 did not have its activity refreshed in the last two frames so we can consider it inactive. return velocityTimeBelowLimit <= 0; } } } /// /// Attempts to activate the entity. /// public void Activate() { //If we're trying to activate, always set the deactivation candidacy to false. This resets the timer if necessary. IsDeactivationCandidate = false; var currentSimulationIsland = SimulationIsland; if (currentSimulationIsland != null) { //We can force-activate an island. //Note that this does nothing for objects not in a space //or kinematic objects that don't have an island. //"Activating" a kinematic object is meaningless- their activity state //is entirely defined by their velocity. currentSimulationIsland.IsActive = true; } else { //"Wake up" the kinematic entity. //The time is used as a flag. If time <= 0, that means the object will be considered active until the subsequent update. velocityTimeBelowLimit = -1; } } bool isAlwaysActive; /// /// Gets or sets whether or not this member is always active. /// public bool IsAlwaysActive { get { return isAlwaysActive; } set { isAlwaysActive = value; if (isAlwaysActive) Activate(); } } internal bool allowStabilization = true; /// /// Gets or sets whether or not the entity can be stabilized by the deactivation system. This allows systems of objects to go to sleep faster. /// Defaults to true. /// public bool AllowStabilization { get { return allowStabilization; } set { allowStabilization = value; } } //simulationisland should hook into the activated event. If it is fired and the simulation island is inactive, the simulation island should activate. //Obviously only call event if it goes from inactive to active. /// /// Fired when the object activates. /// public event Action Activated; /// /// Fired when the object becomes a deactivation candidate. /// public event Action BecameDeactivationCandidate; //semi-horrible name /// /// Fired when the object is no longer a deactivation candidate. /// public event Action BecameNonDeactivationCandidate; //horrible name /// /// Fired when the object deactivates. /// public event Action Deactivated; protected internal void OnActivated() { if (Activated != null) Activated(this); } protected internal void OnBecameDeactivationCandidate() { if (BecameDeactivationCandidate != null) BecameDeactivationCandidate(this); } protected internal void OnBecameNonDeactivationCandidate() { if (BecameNonDeactivationCandidate != null) BecameNonDeactivationCandidate(this); } protected internal void OnDeactivated() { if (Deactivated != null) Deactivated(this); } internal SimulationIsland simulationIsland; /// /// Gets the simulation island that owns this member. /// public SimulationIsland SimulationIsland { get { return simulationIsland != null ? simulationIsland.Parent : null; } internal set { simulationIsland = value; } } /// /// Gets the deactivation manager that is managing this member. /// public DeactivationManager DeactivationManager { get; internal set; } //This is appropriate because it allows kinematic entities, while still technically members (they inherit ISimulationIslandMember), to act as dead-ends. /// /// Gets whether or not the object is dynamic. /// Non-dynamic members act as dead-ends in connection graphs. /// public bool IsDynamic { get { return owner.isDynamic; } } /// /// Gets or sets the current search state of the simulation island member. This is used by the simulation island system /// to efficiently split islands. /// internal SimulationIslandSearchState searchState; /// /// Removes a connection reference from the member. /// ///Reference to remove. ///Index of the connection in this member's list internal void RemoveConnectionReference(SimulationIslandConnection connection, int index) { if (connections.Count > index) { connections.FastRemoveAt(index); if (connections.Count > index) connections.Elements[index].SetListIndex(this, index); } } /// /// Adds a connection reference to the member. /// ///Reference to add. ///Index of the connection in the member's list. internal int AddConnectionReference(SimulationIslandConnection connection) { connections.Add(connection); return connections.Count - 1; } } /// /// Defines the current state of a simulation island member in a split attempt. /// public enum SimulationIslandSearchState { Unclaimed, OwnedByFirst, OwnedBySecond } } ================================================ FILE: BEPUphysics/DeactivationManagement/SimulationIslandMemberList.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using BEPUutilities.DataStructures; namespace BEPUphysics.DeactivationManagement { /// /// Read only list containing the simulation island members associated with a simulation island connection. /// public struct SimulationIslandMemberList : IList { RawList entries; internal SimulationIslandMemberList(RawList entries) { this.entries = entries; } /// /// Determines the index of a specific item in the . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the . public int IndexOf(SimulationIslandMember item) { return Array.IndexOf(entries.Elements, item, 0, entries.Count); } void IList.Insert(int index, SimulationIslandMember item) { throw new NotSupportedException("The list is read-only."); } void IList.RemoveAt(int index) { throw new NotSupportedException("The list is read-only."); } /// /// Gets the element at the specified index. /// /// /// The element at the specified index. /// /// The zero-based index of the element to get or set. is not a valid index in the .The property is set and the is read-only. public SimulationIslandMember this[int index] { get { return entries.Elements[index].Member; } set { throw new NotSupportedException("The list is read-only."); } } void ICollection.Add(SimulationIslandMember item) { throw new NotSupportedException("The list is read-only."); } void ICollection.Clear() { throw new NotSupportedException("The list is read-only."); } /// /// Determines whether the contains a specific value. /// /// /// true if is found in the ; otherwise, false. /// /// The object to locate in the . public bool Contains(SimulationIslandMember item) { return IndexOf(item) > -1; } /// /// Copies the elements of the to an , starting at a particular index. /// /// The one-dimensional that is the destination of the elements copied from . The must have zero-based indexing. /// The zero-based index in at which copying begins. public void CopyTo(SimulationIslandMember[] array, int arrayIndex) { for (int i = 0; i < entries.Count; i++) { array[i + arrayIndex] = entries.Elements[i].Member; } } /// /// Gets the number of elements contained in the . /// /// /// The number of elements contained in the . /// public int Count { get { return entries.Count; } } bool ICollection.IsReadOnly { get { return true; } } bool ICollection.Remove(SimulationIslandMember item) { throw new NotSupportedException("The list is read-only."); } public IEnumerator GetEnumerator() { return new Enumerator(entries); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(entries); } /// /// Enumerators the simulation island members in the member list. /// public struct Enumerator : IEnumerator { private RawList entries; private int index; internal Enumerator(RawList entries) { this.entries = entries; index = -1; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public SimulationIslandMember Current { get { return entries.Elements[index].Member; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } /// /// Gets the current element in the collection. /// /// /// The current element in the collection. /// /// The enumerator is positioned before the first element of the collection or after the last element.2 object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. 2 public bool MoveNext() { index++; return index >= entries.Count; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. 2 public void Reset() { index = -1; } } } } ================================================ FILE: BEPUphysics/Entities/Entity.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; namespace BEPUphysics.Entities { /// /// Superclass of all entities which have a defined collidable type. /// After construction, the collidable on this sort of Entity cannot be changed. /// It can be constructed directly, or one of its prefab children (Box, Sphere, etc.) can be used. /// /// If the collidable needs to be changed after construction, consider using the MorphableEntity. ///Type of EntityCollidable to use for the entity. public class Entity : Entity where T : EntityCollidable { /// /// Gets the collidable used by the entity. /// public new T CollisionInformation { get { return (T)collisionInformation; } } protected internal Entity() { } /// /// Constructs a kinematic Entity. /// ///Collidable for the entity. public Entity(T collisionInformation) { Initialize(collisionInformation); } /// /// Constructs a dynamic Entity. /// ///Collidable for the entity. /// Mass of the entity. public Entity(T collisionInformation, float mass) { Initialize(collisionInformation, mass); } /// /// Constructs a dynamic Entity. /// ///Collidable for the entity. /// Mass of the entity. /// Inertia of the entity. public Entity(T collisionInformation, float mass, Matrix3x3 inertiaTensor) { Initialize(collisionInformation, mass, inertiaTensor); } /// /// Constructs a dynamic Entity. /// ///Collidable for the entity. /// Mass of the entity. /// Inertia of the entity. /// Volume of the entity. public Entity(T collisionInformation, float mass, Matrix3x3 inertiaTensor, float volume) { Initialize(collisionInformation, mass, inertiaTensor, volume); } } } ================================================ FILE: BEPUphysics/Entities/EntityBase.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.DeactivationManagement; using BEPUphysics.EntityStateManagement; using BEPUphysics.OtherSpaceStages; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUphysics.Materials; using BEPUphysics.CollisionShapes; using BEPUphysics.CollisionRuleManagement; using MathChecker = BEPUutilities.MathChecker; namespace BEPUphysics.Entities { /// /// Superclass of movable rigid bodies. Contains information for /// both dynamic and kinematic simulation. /// public class Entity : IBroadPhaseEntryOwner, IDeferredEventCreatorOwner, ISimulationIslandMemberOwner, ICCDPositionUpdateable, IForceUpdateable, ISpaceObject, IMaterialOwner, ICollisionRulesOwner { internal Vector3 position; internal Quaternion orientation = Quaternion.Identity; internal Matrix3x3 orientationMatrix = Matrix3x3.Identity; internal Vector3 linearVelocity; internal Vector3 linearMomentum; internal Vector3 angularVelocity; internal Vector3 angularMomentum; internal bool isDynamic; /// /// Gets or sets the position of the Entity. This Position acts /// as the center of mass for dynamic entities. /// public Vector3 Position { get { return position; } set { position = value; activityInformation.Activate(); MathChecker.Validate(position); } } /// /// Gets or sets the orientation quaternion of the entity. /// public Quaternion Orientation { get { return orientation; } set { Quaternion.Normalize(ref value, out orientation); Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix); //Update inertia tensors for consistency. Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); activityInformation.Activate(); MathChecker.Validate(orientation); } } /// /// Gets or sets the orientation matrix of the entity. /// public Matrix3x3 OrientationMatrix { get { return orientationMatrix; } set { Matrix3x3.CreateQuaternion(ref value, out orientation); Orientation = orientation; //normalizes and sets. } } /// /// Gets or sets the world transform of the entity. /// The upper left 3x3 part is the Orientation, and the translation is the Position. /// When setting this property, ensure that the rotation matrix component does not include /// any scaling or shearing. /// public Matrix WorldTransform { get { Matrix worldTransform; Matrix3x3.ToMatrix4X4(ref orientationMatrix, out worldTransform); worldTransform.Translation = position; return worldTransform; } set { Quaternion.CreateFromRotationMatrix(ref value, out orientation); Orientation = orientation; //normalizes and sets. position = value.Translation; activityInformation.Activate(); MathChecker.Validate(position); } } /// /// Gets or sets the angular velocity of the entity. /// public Vector3 AngularVelocity { get { return angularVelocity; } set { angularVelocity = value; Matrix3x3.Transform(ref value, ref inertiaTensor, out angularMomentum); activityInformation.Activate(); MathChecker.Validate(angularVelocity); MathChecker.Validate(angularMomentum); } } /// /// Gets or sets the angular momentum of the entity. /// public Vector3 AngularMomentum { get { if (MotionSettings.ConserveAngularMomentum) return angularMomentum; else { Vector3 v; Matrix3x3.Transform(ref angularVelocity, ref inertiaTensor, out v); return v; } } set { angularMomentum = value; Matrix3x3.Transform(ref value, ref inertiaTensorInverse, out angularVelocity); activityInformation.Activate(); MathChecker.Validate(angularVelocity); MathChecker.Validate(angularMomentum); } } /// /// Gets or sets the linear velocity of the entity. /// public Vector3 LinearVelocity { get { return linearVelocity; } set { linearVelocity = value; Vector3.Multiply(ref linearVelocity, mass, out linearMomentum); activityInformation.Activate(); MathChecker.Validate(linearVelocity); MathChecker.Validate(linearMomentum); } } /// /// Gets or sets the linear momentum of the entity. /// public Vector3 LinearMomentum { get { return linearMomentum; } set { linearMomentum = value; Vector3.Multiply(ref linearMomentum, inverseMass, out linearVelocity); activityInformation.Activate(); MathChecker.Validate(linearVelocity); MathChecker.Validate(linearMomentum); } } /// /// Gets or sets the position, orientation, linear velocity, and angular velocity of the entity. /// public MotionState MotionState { get { MotionState toReturn; toReturn.Position = position; toReturn.Orientation = orientation; toReturn.LinearVelocity = linearVelocity; toReturn.AngularVelocity = angularVelocity; return toReturn; } set { Position = value.Position; Orientation = value.Orientation; LinearVelocity = value.LinearVelocity; AngularVelocity = value.AngularVelocity; } } /// /// Gets whether or not the entity is dynamic. /// Dynamic entities have finite mass and respond /// to collisions. Kinematic (non-dynamic) entities /// have infinite mass and inertia and will plow through anything. /// public bool IsDynamic { get { return isDynamic; } } bool isAffectedByGravity = true; /// /// Gets or sets whether or not the entity can be affected by gravity applied by the ForceUpdater. /// public bool IsAffectedByGravity { get { return isAffectedByGravity; } set { isAffectedByGravity = value; } } /// /// Gets the buffered states of the entity. If the Space.BufferedStates manager is enabled, /// this property provides access to the buffered and interpolated states of the entity. /// Buffered states are the most recent completed update values, while interpolated states are the previous values blended /// with the current frame's values. Interpolated states are helpful when updating the engine with internal time stepping, /// giving entity motion a smooth appearance even when updates aren't occurring consistently every frame. /// Both are buffered for asynchronous access. /// public EntityBufferedStates BufferedStates { get; private set; } internal Matrix3x3 inertiaTensorInverse; /// /// Gets the world space inertia tensor inverse of the entity. /// public Matrix3x3 InertiaTensorInverse { get { return inertiaTensorInverse; } } internal Matrix3x3 inertiaTensor; /// /// Gets the world space inertia tensor of the entity. /// public Matrix3x3 InertiaTensor { get { return inertiaTensor; } } internal Matrix3x3 localInertiaTensor; /// /// Gets or sets the local inertia tensor of the entity. /// public Matrix3x3 LocalInertiaTensor { get { return localInertiaTensor; } set { localInertiaTensor = value; Matrix3x3.AdaptiveInvert(ref localInertiaTensor, out localInertiaTensorInverse); Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); localInertiaTensor.Validate(); localInertiaTensorInverse.Validate(); } } internal Matrix3x3 localInertiaTensorInverse; /// /// Gets or sets the local inertia tensor inverse of the entity. /// public Matrix3x3 LocalInertiaTensorInverse { get { return localInertiaTensorInverse; } set { localInertiaTensorInverse = value; Matrix3x3.AdaptiveInvert(ref localInertiaTensorInverse, out localInertiaTensor); //Update the world space versions. Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); localInertiaTensor.Validate(); localInertiaTensorInverse.Validate(); } } internal float mass; /// /// Gets or sets the mass of the entity. Setting this to an invalid value, such as a non-positive number, NaN, or infinity, makes the entity kinematic. /// Setting it to a valid positive number will also scale the inertia tensor if it was already dynamic, or force the calculation of a new inertia tensor /// if it was previously kinematic. /// public float Mass { get { return mass; } set { if (value <= 0 || float.IsNaN(value) || float.IsInfinity(value)) BecomeKinematic(); else { if (isDynamic) { //If it's already dynamic, then we don't need to recompute the inertia tensor. //Instead, scale the one we have already. Matrix3x3 newInertia; Matrix3x3.Multiply(ref localInertiaTensor, value * inverseMass, out newInertia); BecomeDynamic(value, newInertia); } else { BecomeDynamic(value); } } } } internal float inverseMass; /// /// Gets or sets the inverse mass of the entity. /// public float InverseMass { get { return inverseMass; } set { if (value > 0) Mass = 1 / value; else Mass = 0; } } internal float volume; /// /// Gets the volume of the entity. /// This is computed along with other physical properties at initialization, /// but it's only used for auxiliary systems like the FluidVolume. /// public float Volume { get { return volume; } } /// /// Fires when the entity's position and orientation is updated. /// public event Action PositionUpdated; protected EntityCollidable collisionInformation; /// /// Gets the collidable used by the entity. /// public EntityCollidable CollisionInformation { get { return collisionInformation; } protected set { if (collisionInformation != null) collisionInformation.Shape.ShapeChanged -= shapeChangedDelegate; collisionInformation = value; if (collisionInformation != null) collisionInformation.Shape.ShapeChanged += shapeChangedDelegate; //Entity constructors do their own initialization when the collision information changes. //Might be able to condense it up here, but don't really need it right now. //ShapeChangedHandler(collisionInformation.shape); } } //protected internal object locker = new object(); ///// ///// Gets the synchronization object used by systems that need ///// exclusive access to the entity's properties. ///// //public object Locker //{ // get // { // return locker; // } //} protected internal SpinLock locker = new SpinLock(); /// /// Gets the synchronization object used by systems that need /// exclusive access to the entity's properties. /// public SpinLock Locker { get { return locker; } } internal Material material; //NOT thread safe due to material change pair update. /// /// Gets or sets the material used by the entity. /// public Material Material { get { return material; } set { if (material != null) material.MaterialChanged -= materialChangedDelegate; material = value; if (material != null) material.MaterialChanged += materialChangedDelegate; OnMaterialChanged(material); } } Action materialChangedDelegate; void OnMaterialChanged(Material newMaterial) { for (int i = 0; i < collisionInformation.pairs.Count; i++) { collisionInformation.pairs[i].UpdateMaterialProperties(); } } /// /// Gets all the EntitySolverUpdateables associated with this entity. /// public EntitySolverUpdateableCollection SolverUpdateables { get { return new EntitySolverUpdateableCollection(activityInformation.connections); } } /// /// Gets the two-entity constraints associated with this entity (a subset of the solver updateables). /// public EntityConstraintCollection Constraints { get { return new EntityConstraintCollection(activityInformation.connections); } } #region Construction protected Entity() { InitializeId(); BufferedStates = new EntityBufferedStates(this); material = new Material(); materialChangedDelegate = OnMaterialChanged; material.MaterialChanged += materialChangedDelegate; shapeChangedDelegate = OnShapeChanged; activityInformation = new SimulationIslandMember(this); } /// /// Constructs a new kinematic entity. /// ///Collidable to use with the entity. public Entity(EntityCollidable collisionInformation) : this() { Initialize(collisionInformation); } /// /// Constructs a new entity. /// ///Collidable to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. public Entity(EntityCollidable collisionInformation, float mass) : this() { Initialize(collisionInformation, mass); } /// /// Constructs a new entity. /// ///Collidable to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. /// Inertia tensor of the entity. Only used for a dynamic entity. public Entity(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor) : this() { Initialize(collisionInformation, mass, inertiaTensor); } /// /// Constructs a new entity. /// ///Collidable to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. /// Inertia tensor of the entity. Only used for a dynamic entity. /// Volume of the entity. public Entity(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor, float volume) : this() { Initialize(collisionInformation, mass, inertiaTensor, volume); } /// /// Constructs a new kinematic entity. /// ///Shape to use with the entity. public Entity(EntityShape shape) : this() { Initialize(shape.GetCollidableInstance()); } /// /// Constructs a new entity. /// ///Shape to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. public Entity(EntityShape shape, float mass) : this() { Initialize(shape.GetCollidableInstance(), mass); } /// /// Constructs a new entity. /// ///Shape to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. /// Inertia tensor of the entity. Only used for a dynamic entity. public Entity(EntityShape shape, float mass, Matrix3x3 inertiaTensor) : this() { Initialize(shape.GetCollidableInstance(), mass, inertiaTensor); } /// /// Constructs a new entity. /// ///Shape to use with the entity. ///Mass of the entity. If positive, the entity will be dynamic. Otherwise, it will be kinematic. /// Inertia tensor of the entity. Only used for a dynamic entity. /// Volume of the entity. public Entity(EntityShape shape, float mass, Matrix3x3 inertiaTensor, float volume) : this() { Initialize(shape.GetCollidableInstance(), mass, inertiaTensor, volume); } //These initialize methods make it easier to construct some Entity prefab types. protected internal void Initialize(EntityCollidable collisionInformation) { CollisionInformation = collisionInformation; BecomeKinematic(); collisionInformation.Entity = this; } protected internal void Initialize(EntityCollidable collisionInformation, float mass) { CollisionInformation = collisionInformation; if (mass > 0) { ShapeDistributionInformation shapeInfo; collisionInformation.Shape.ComputeDistributionInformation(out shapeInfo); Matrix3x3.Multiply(ref shapeInfo.VolumeDistribution, mass * InertiaHelper.InertiaTensorScale, out shapeInfo.VolumeDistribution); volume = shapeInfo.Volume; BecomeDynamic(mass, shapeInfo.VolumeDistribution); } else { volume = collisionInformation.Shape.ComputeVolume(); BecomeKinematic(); } collisionInformation.Entity = this; } protected internal void Initialize(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor) { CollisionInformation = collisionInformation; volume = collisionInformation.Shape.ComputeVolume(); if (mass > 0) BecomeDynamic(mass, inertiaTensor); else BecomeKinematic(); collisionInformation.Entity = this; } protected internal void Initialize(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor, float volume) { CollisionInformation = collisionInformation; this.volume = volume; if (mass > 0) BecomeDynamic(mass, inertiaTensor); else BecomeKinematic(); collisionInformation.Entity = this; } #endregion #region IDeferredEventCreatorOwner Members IDeferredEventCreator IDeferredEventCreatorOwner.EventCreator { get { return CollisionInformation.Events; } } #endregion internal SimulationIslandMember activityInformation; public SimulationIslandMember ActivityInformation { get { return activityInformation; } } bool IForceUpdateable.IsActive { get { return activityInformation.IsActive; } } bool IPositionUpdateable.IsActive { get { return activityInformation.IsActive; } } /// /// Applies an impulse to the entity. /// ///Location to apply the impulse. ///Impulse to apply. public void ApplyImpulse(Vector3 location, Vector3 impulse) { ApplyImpulse(ref location, ref impulse); } /// /// Applies an impulse to the entity. /// ///Location to apply the impulse. ///Impulse to apply. public void ApplyImpulse(ref Vector3 location, ref Vector3 impulse) { if (isDynamic) { ApplyLinearImpulse(ref impulse); #if WINDOWS Vector3 positionDifference; #else Vector3 positionDifference = new Vector3(); #endif positionDifference.X = location.X - position.X; positionDifference.Y = location.Y - position.Y; positionDifference.Z = location.Z - position.Z; Vector3 cross; Vector3.Cross(ref positionDifference, ref impulse, out cross); ApplyAngularImpulse(ref cross); activityInformation.Activate(); } } //These methods are very direct and quick. They don't activate the object or anything. /// /// Applies a linear velocity change to the entity using the given impulse. /// This method does not wake up the object or perform any other nonessential operation; /// it is meant to be used for performance-sensitive constraint solving. /// Consider equivalently adding to the LinearMomentum property for convenience instead. /// /// Impulse to apply. public void ApplyLinearImpulse(ref Vector3 impulse) { #if WINDOWS_PHONE //Some XNA math methods support SIMD on the phone. //This would most likely be inlined on the PC anyway, but the XBOX360 is a questionmark. //Just inline those platforms manually. Vector3.Add(ref linearMomentum, ref impulse, out linearMomentum); Vector3.Multiply(ref linearMomentum, inverseMass, out linearVelocity); #else linearMomentum.X += impulse.X; linearMomentum.Y += impulse.Y; linearMomentum.Z += impulse.Z; linearVelocity.X = linearMomentum.X * inverseMass; linearVelocity.Y = linearMomentum.Y * inverseMass; linearVelocity.Z = linearMomentum.Z * inverseMass; #endif MathChecker.Validate(linearVelocity); MathChecker.Validate(linearMomentum); } /// /// Applies an angular velocity change to the entity using the given impulse. /// This method does not wake up the object or perform any other nonessential operation; /// it is meant to be used for performance-sensitive constraint solving. /// Consider equivalently adding to the AngularMomentum property for convenience instead. /// /// Impulse to apply. public void ApplyAngularImpulse(ref Vector3 impulse) { //There's some room here for SIMD-friendliness. However, since the phone doesn't accelerate non-XNA types, the matrix3x3 operations don't gain much. angularMomentum.X += impulse.X; angularMomentum.Y += impulse.Y; angularMomentum.Z += impulse.Z; if (MotionSettings.ConserveAngularMomentum) { angularVelocity.X = angularMomentum.X * inertiaTensorInverse.M11 + angularMomentum.Y * inertiaTensorInverse.M21 + angularMomentum.Z * inertiaTensorInverse.M31; angularVelocity.Y = angularMomentum.X * inertiaTensorInverse.M12 + angularMomentum.Y * inertiaTensorInverse.M22 + angularMomentum.Z * inertiaTensorInverse.M32; angularVelocity.Z = angularMomentum.X * inertiaTensorInverse.M13 + angularMomentum.Y * inertiaTensorInverse.M23 + angularMomentum.Z * inertiaTensorInverse.M33; } else { angularVelocity.X += impulse.X * inertiaTensorInverse.M11 + impulse.Y * inertiaTensorInverse.M21 + impulse.Z * inertiaTensorInverse.M31; angularVelocity.Y += impulse.X * inertiaTensorInverse.M12 + impulse.Y * inertiaTensorInverse.M22 + impulse.Z * inertiaTensorInverse.M32; angularVelocity.Z += impulse.X * inertiaTensorInverse.M13 + impulse.Y * inertiaTensorInverse.M23 + impulse.Z * inertiaTensorInverse.M33; } MathChecker.Validate(angularVelocity); MathChecker.Validate(angularMomentum); } /// /// Gets or sets whether or not to ignore shape changes. When true, changing the entity's collision shape will not update the volume, density, or inertia tensor. /// public bool IgnoreShapeChanges { get; set; } Action shapeChangedDelegate; protected void OnShapeChanged(CollisionShape shape) { if (!IgnoreShapeChanges) { //When the shape changes, force the entity awake so that it performs any necessary updates. activityInformation.Activate(); ShapeDistributionInformation shapeInfo; collisionInformation.Shape.ComputeDistributionInformation(out shapeInfo); volume = shapeInfo.Volume; if (isDynamic) { Matrix3x3.Multiply(ref shapeInfo.VolumeDistribution, InertiaHelper.InertiaTensorScale * mass, out shapeInfo.VolumeDistribution); LocalInertiaTensor = shapeInfo.VolumeDistribution; } else { LocalInertiaTensorInverse = new Matrix3x3(); } } } //TODO: Include warnings about multithreading. These modify things outside of the entity and use single-thread-only helpers. /// /// Forces the entity to become kinematic. Kinematic entities have infinite mass and inertia. /// public void BecomeKinematic() { bool previousState = isDynamic; isDynamic = false; LocalInertiaTensorInverse = new Matrix3x3(); mass = 0; inverseMass = 0; //Notify simulation island of the change. if (previousState) { if (activityInformation.DeactivationManager != null) activityInformation.DeactivationManager.RemoveSimulationIslandFromMember(activityInformation); if (((IForceUpdateable)this).ForceUpdater != null) ((IForceUpdateable)this).ForceUpdater.ForceUpdateableBecomingKinematic(this); } //Change the collision group if it was using the default. if (collisionInformation.CollisionRules.Group == CollisionRules.DefaultDynamicCollisionGroup || collisionInformation.CollisionRules.Group == null) collisionInformation.CollisionRules.Group = CollisionRules.DefaultKinematicCollisionGroup; activityInformation.Activate(); //Preserve velocity and reinitialize momentum for new state. LinearVelocity = linearVelocity; AngularVelocity = angularVelocity; } /// /// Forces the entity to become dynamic. Dynamic entities respond to collisions and have finite mass and inertia. /// ///Mass to use for the entity. public void BecomeDynamic(float mass) { Matrix3x3 inertiaTensor = collisionInformation.Shape.ComputeVolumeDistribution(); Matrix3x3.Multiply(ref inertiaTensor, mass * InertiaHelper.InertiaTensorScale, out inertiaTensor); BecomeDynamic(mass, inertiaTensor); } /// /// Forces the entity to become dynamic. Dynamic entities respond to collisions and have finite mass and inertia. /// ///Mass to use for the entity. /// Inertia tensor to use for the entity. public void BecomeDynamic(float mass, Matrix3x3 localInertiaTensor) { if (mass <= 0 || float.IsInfinity(mass) || float.IsNaN(mass)) throw new InvalidOperationException("Cannot use a mass of " + mass + " for a dynamic entity. Consider using a kinematic entity instead."); bool previousState = isDynamic; isDynamic = true; LocalInertiaTensor = localInertiaTensor; this.mass = mass; this.inverseMass = 1 / mass; //Notify simulation island system of the change. if (!previousState) { if (activityInformation.DeactivationManager != null) activityInformation.DeactivationManager.AddSimulationIslandToMember(activityInformation); if (((IForceUpdateable)this).ForceUpdater != null) ((IForceUpdateable)this).ForceUpdater.ForceUpdateableBecomingDynamic(this); } //Change the group if it was using the defaults. if (collisionInformation.CollisionRules.Group == CollisionRules.DefaultKinematicCollisionGroup || collisionInformation.CollisionRules.Group == null) collisionInformation.CollisionRules.Group = CollisionRules.DefaultDynamicCollisionGroup; activityInformation.Activate(); //Preserve velocity and reinitialize momentum for new state. LinearVelocity = linearVelocity; AngularVelocity = angularVelocity; } void IForceUpdateable.UpdateForForces(float dt) { //Linear velocity if (IsAffectedByGravity) { Vector3.Add(ref forceUpdater.gravityDt, ref linearVelocity, out linearVelocity); } //Boost damping at very low velocities. This is a strong stabilizer; removes a ton of energy from the system. if (activityInformation.DeactivationManager.useStabilization && activityInformation.allowStabilization && (activityInformation.isSlowing || activityInformation.velocityTimeBelowLimit > activityInformation.DeactivationManager.lowVelocityTimeMinimum)) { float energy = linearVelocity.LengthSquared() + angularVelocity.LengthSquared(); if (energy < activityInformation.DeactivationManager.velocityLowerLimitSquared) { float boost = 1 - (float)(Math.Sqrt(energy) / (2f * activityInformation.DeactivationManager.velocityLowerLimit)); ModifyAngularDamping(boost); ModifyLinearDamping(boost); } } //Damping float linear = LinearDamping + linearDampingBoost; if (linear > 0) { Vector3.Multiply(ref linearVelocity, (float)Math.Pow(MathHelper.Clamp(1 - linear, 0, 1), dt), out linearVelocity); } //When applying angular damping, the momentum or velocity is damped depending on the conservation setting. float angular = AngularDamping + angularDampingBoost; if (angular > 0 && MotionSettings.ConserveAngularMomentum) { Vector3.Multiply(ref angularMomentum, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularMomentum); } else if (angular > 0) { Vector3.Multiply(ref angularVelocity, (float)Math.Pow(MathHelper.Clamp(1 - angular, 0, 1), dt), out angularVelocity); } linearDampingBoost = 0; angularDampingBoost = 0; //Linear momentum Vector3.Multiply(ref linearVelocity, mass, out linearMomentum); //Update world inertia tensors. Matrix3x3 multiplied; Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensorInverse, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensorInverse); Matrix3x3.MultiplyTransposed(ref orientationMatrix, ref localInertiaTensor, out multiplied); Matrix3x3.Multiply(ref multiplied, ref orientationMatrix, out inertiaTensor); //Update angular velocity or angular momentum. if (MotionSettings.ConserveAngularMomentum) { Matrix3x3.Transform(ref angularMomentum, ref inertiaTensorInverse, out angularVelocity); } else { Matrix3x3.Transform(ref angularVelocity, ref inertiaTensor, out angularMomentum); } MathChecker.Validate(linearVelocity); MathChecker.Validate(linearMomentum); MathChecker.Validate(angularVelocity); MathChecker.Validate(angularMomentum); } private ForceUpdater forceUpdater; ForceUpdater IForceUpdateable.ForceUpdater { get { return forceUpdater; } set { forceUpdater = value; } } #region ISpaceObject ISpace space; ISpace ISpaceObject.Space { get { return space; } set { space = value; } } /// /// Gets the space that owns the entity. /// public ISpace Space { get { return space; } } void ISpaceObject.OnAdditionToSpace(ISpace newSpace) { OnAdditionToSpace(newSpace); } protected virtual void OnAdditionToSpace(ISpace newSpace) { } void ISpaceObject.OnRemovalFromSpace(ISpace oldSpace) { OnRemovalFromSpace(oldSpace); } protected virtual void OnRemovalFromSpace(ISpace oldSpace) { } #endregion #region ICCDPositionUpdateable PositionUpdater IPositionUpdateable.PositionUpdater { get; set; } PositionUpdateMode positionUpdateMode = MotionSettings.DefaultPositionUpdateMode; /// /// Gets the position update mode of the entity. /// public PositionUpdateMode PositionUpdateMode { get { return positionUpdateMode; } set { var previous = positionUpdateMode; positionUpdateMode = value; //Notify our owner of the change, if needed. if (positionUpdateMode != previous && ((IPositionUpdateable)this).PositionUpdater != null && (((IPositionUpdateable)this).PositionUpdater as ContinuousPositionUpdater) != null) { (((IPositionUpdateable)this).PositionUpdater as ContinuousPositionUpdater).UpdateableModeChanged(this, previous); } } } void ICCDPositionUpdateable.UpdateTimesOfImpact(float dt) { //I am a continuous object. If I am in a pair with another object, even if I am inactive, //I must order the pairs to compute a time of impact. //The pair method works in such a way that, when this method is run asynchronously, there will be no race conditions. for (int i = 0; i < collisionInformation.pairs.Count; i++) { //Only perform CCD if we're either supposed to test against no solver pairs or if this isn't a no solver pair. if (MotionSettings.PairAllowsCCD(this, collisionInformation.pairs.Elements[i])) collisionInformation.pairs.Elements[i].UpdateTimeOfImpact(collisionInformation, dt); } } void ICCDPositionUpdateable.ResetTimesOfImpact() { //Reset all of the times of impact to 1, allowing the entity to move all the way through its velocity-defined motion. for (int i = 0; i < collisionInformation.pairs.Count; i++) { collisionInformation.pairs.Elements[i].timeOfImpact = 1; } } void ICCDPositionUpdateable.UpdatePositionContinuously(float dt) { float minimumToi = 1; for (int i = 0; i < collisionInformation.pairs.Count; i++) { if (collisionInformation.pairs.Elements[i].timeOfImpact < minimumToi) minimumToi = collisionInformation.pairs.Elements[i].timeOfImpact; } //The orientation was already updated by the PreUpdatePosition. //However, to be here, this object is not a discretely updated object. //That means we still need to update the linear motion. Vector3 increment; Vector3.Multiply(ref linearVelocity, dt * minimumToi, out increment); Vector3.Add(ref position, ref increment, out position); collisionInformation.UpdateWorldTransform(ref position, ref orientation); if (PositionUpdated != null) PositionUpdated(this); MathChecker.Validate(linearMomentum); MathChecker.Validate(linearVelocity); MathChecker.Validate(angularMomentum); MathChecker.Validate(angularVelocity); MathChecker.Validate(position); MathChecker.Validate(orientation); } void IPositionUpdateable.PreUpdatePosition(float dt) { Vector3 increment; if (MotionSettings.UseRk4AngularIntegration && isDynamic) { Toolbox.UpdateOrientationRK4(ref orientation, ref localInertiaTensorInverse, ref angularMomentum, dt, out orientation); } else { Vector3.Multiply(ref angularVelocity, dt * .5f, out increment); var multiplier = new Quaternion(increment.X, increment.Y, increment.Z, 0); Quaternion.Multiply(ref multiplier, ref orientation, out multiplier); Quaternion.Add(ref orientation, ref multiplier, out orientation); orientation.Normalize(); } Matrix3x3.CreateFromQuaternion(ref orientation, out orientationMatrix); //Only do the linear motion if this object doesn't obey CCD. if (PositionUpdateMode == PositionUpdateMode.Discrete) { Vector3.Multiply(ref linearVelocity, dt, out increment); Vector3.Add(ref position, ref increment, out position); collisionInformation.UpdateWorldTransform(ref position, ref orientation); //The position update is complete if this is a discretely updated object. if (PositionUpdated != null) PositionUpdated(this); } collisionInformation.UpdateWorldTransform(ref position, ref orientation); MathChecker.Validate(linearMomentum); MathChecker.Validate(linearVelocity); MathChecker.Validate(angularMomentum); MathChecker.Validate(angularVelocity); MathChecker.Validate(position); MathChecker.Validate(orientation); } #endregion float linearDampingBoost, angularDampingBoost; float angularDamping = .15f; float linearDamping = .03f; /// /// Gets or sets the angular damping of the entity. /// Values range from 0 to 1, corresponding to a fraction of angular momentum removed /// from the entity over a unit of time. /// public float AngularDamping { get { return angularDamping; } set { angularDamping = MathHelper.Clamp(value, 0, 1); } } /// /// Gets or sets the linear damping of the entity. /// Values range from 0 to 1, correspondong to a fraction of linear momentum removed /// from the entity over a unit of time. /// public float LinearDamping { get { return linearDamping; } set { linearDamping = MathHelper.Clamp(value, 0, 1); } } /// /// Temporarily adjusts the linear damping by an amount. After the value is used, the /// damping returns to the base value. /// /// Damping to add. public void ModifyLinearDamping(float damping) { float totalDamping = LinearDamping + linearDampingBoost; float remainder = 1 - totalDamping; linearDampingBoost += damping * remainder; } /// /// Temporarily adjusts the angular damping by an amount. After the value is used, the /// damping returns to the base value. /// /// Damping to add. public void ModifyAngularDamping(float damping) { float totalDamping = AngularDamping + angularDampingBoost; float remainder = 1 - totalDamping; angularDampingBoost += damping * remainder; } /// /// Gets or sets the user data associated with the entity. /// This is separate from the entity's collidable's tag. /// If a tag needs to be accessed from within the collision /// detection pipeline, consider using the entity.CollisionInformation.Tag. /// public object Tag { get; set; } CollisionRules ICollisionRulesOwner.CollisionRules { get { return collisionInformation.collisionRules; } set { collisionInformation.CollisionRules = value; } } BroadPhaseEntry IBroadPhaseEntryOwner.Entry { get { return collisionInformation; } } public override string ToString() { if (Tag == null) return base.ToString(); else return base.ToString() + ", " + Tag; } #if WINDOWS_PHONE static int idCounter; /// /// Gets the entity's unique instance id. /// public int InstanceId { get; private set; } #else static long idCounter; /// /// Gets the entity's unique instance id. /// public long InstanceId { get; private set; } #endif void InitializeId() { InstanceId = System.Threading.Interlocked.Increment(ref idCounter); hashCode = (int)((((ulong)InstanceId) * 4294967311UL) % 4294967296UL); } int hashCode; public override int GetHashCode() { return hashCode; } } } ================================================ FILE: BEPUphysics/Entities/EntityConstraintCollection.cs ================================================ using System.Collections.Generic; using BEPUphysics.Constraints.TwoEntity; using BEPUphysics.DeactivationManagement; using BEPUutilities.DataStructures; namespace BEPUphysics.Entities { /// /// Convenience collection for easily scanning the two entity constraints connected to an entity. /// public class EntityConstraintCollection : IEnumerable { private RawList connections; /// /// Constructs a new constraint collection. /// /// Solver updateables to enumerate over. public EntityConstraintCollection(RawList connections) { this.connections = connections; } /// /// Gets an enumerator for the collection. /// /// Enumerator for the collection. public Enumerator GetEnumerator() { return new Enumerator(connections); } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(connections); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(connections); } /// /// Enumerator for the EntityConstraintCollection. /// public struct Enumerator : IEnumerator { private RawList connections; private int index; private TwoEntityConstraint current; /// /// Constructs an enumerator for the solver updateables list. /// /// List of solver updateables to enumerate. public Enumerator(RawList connections) { this.connections = connections; index = -1; current = null; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public TwoEntityConstraint Current { get { return current; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. 2 public bool MoveNext() { while (++index < connections.Count) { if (!connections.Elements[index].SlatedForRemoval) { current = connections.Elements[index].Owner as TwoEntityConstraint; if (current != null) return true; } } return false; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. 2 public void Reset() { index = -1; current = null; } } } } ================================================ FILE: BEPUphysics/Entities/EntitySolverUpdateableCollection.cs ================================================ using System.Collections.Generic; using BEPUphysics.Constraints; using BEPUphysics.DeactivationManagement; using BEPUutilities.DataStructures; namespace BEPUphysics.Entities { /// /// Convenience collection for easily scanning the two entity constraints connected to an entity. /// public class EntitySolverUpdateableCollection : IEnumerable { private RawList connections; /// /// Constructs a new constraint collection. /// /// Solver updateables to enumerate over. public EntitySolverUpdateableCollection(RawList connections) { this.connections = connections; } /// /// Gets an enumerator for the collection. /// /// Enumerator for the collection. public Enumerator GetEnumerator() { return new Enumerator(connections); } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(connections); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(connections); } /// /// Enumerator for the EntityConstraintCollection. /// public struct Enumerator : IEnumerator { private RawList connections; private int index; private EntitySolverUpdateable current; /// /// Constructs an enumerator for the solver updateables list. /// /// List of solver updateables to enumerate. public Enumerator(RawList connections) { this.connections = connections; index = -1; current = null; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public EntitySolverUpdateable Current { get { return current; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. 2 public bool MoveNext() { while (++index < connections.Count) { if (!connections.Elements[index].SlatedForRemoval) { current = connections.Elements[index].Owner as EntitySolverUpdateable; if (current != null) return true; } } return false; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. 2 public void Reset() { index = -1; current = null; } } } } ================================================ FILE: BEPUphysics/Entities/MorphableEntity.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; using BEPUphysics.CollisionShapes; namespace BEPUphysics.Entities { /// /// Entity with modifiable collision information. /// public class MorphableEntity : Entity { /// /// Gets or sets the collidable associated with the entity. /// public new EntityCollidable CollisionInformation { get { return base.CollisionInformation; } set { SetCollisionInformation(value); } } /// /// Constructs a new morphable entity. /// ///Collidable to use with the entity. public MorphableEntity(EntityCollidable collisionInformation) : base(collisionInformation) { } /// /// Constructs a new morphable entity. /// ///Collidable to use with the entity. ///Mass of the entity. public MorphableEntity(EntityCollidable collisionInformation, float mass) : base(collisionInformation, mass) { } /// /// Constructs a new morphable entity. /// ///Collidable to use with the entity. ///Mass of the entity. /// Inertia tensor of the entity. public MorphableEntity(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor) : base(collisionInformation, mass, inertiaTensor) { } /// /// Constructs a new morphable entity. /// ///Collidable to use with the entity. ///Mass of the entity. /// Inertia tensor of the entity. /// Volume of the entity. public MorphableEntity(EntityCollidable collisionInformation, float mass, Matrix3x3 inertiaTensor, float volume) : base(collisionInformation, mass, inertiaTensor, volume) { } /// /// Constructs a new morphable entity. /// ///Shape to use with the entity. public MorphableEntity(EntityShape shape) : base(shape) { } /// /// Constructs a new morphable entity. /// ///Shape to use with the entity. ///Mass of the entity. public MorphableEntity(EntityShape shape, float mass) : base(shape, mass) { } /// /// Constructs a new morphable entity. /// ///Shape to use with the entity. ///Mass of the entity. /// Inertia tensor of the entity. public MorphableEntity(EntityShape shape, float mass, Matrix3x3 inertiaTensor) : base(shape, mass, inertiaTensor) { } /// /// Constructs a new morphable entity. /// ///Shape to use with the entity. ///Mass of the entity. /// Inertia tensor of the entity. /// Volume of the entity. public MorphableEntity(EntityShape shape, float mass, Matrix3x3 inertiaTensor, float volume) : base(shape, mass, inertiaTensor, volume) { } /// /// Sets the collision information of the entity to another collidable. /// /// New collidable to use. public void SetCollisionInformation(EntityCollidable newCollisionInformation) { //Temporarily remove the object from the space. //The reset process will update any systems that need to be updated. //This is not thread safe, but this operation should not be performed mid-frame anyway. ISpace space = Space; if (space != null) Space.Remove(this); CollisionInformation.Entity = null; if (isDynamic) Initialize(newCollisionInformation, mass); else Initialize(newCollisionInformation); if (space != null) space.Add(this); } /// /// Sets the collision information of the entity to another collidable. /// /// New collidable to use. /// New mass to use for the entity. public void SetCollisionInformation(EntityCollidable newCollisionInformation, float newMass) { //Temporarily remove the object from the space. //The reset process will update any systems that need to be updated. //This is not thread safe, but this operation should not be performed mid-frame anyway. ISpace space = Space; if (space != null) Space.Remove(this); CollisionInformation.Entity = null; Initialize(newCollisionInformation, newMass); if (space != null) space.Add(this); } /// /// Sets the collision information of the entity to another collidable. /// /// New collidable to use. /// New mass to use for the entity. /// New inertia tensor to use for the entity. public void SetCollisionInformation(EntityCollidable newCollisionInformation, float newMass, Matrix3x3 newInertia) { //Temporarily remove the object from the space. //The reset process will update any systems that need to be updated. //This is not thread safe, but this operation should not be performed mid-frame anyway. ISpace space = Space; if (space != null) Space.Remove(this); CollisionInformation.Entity = null; Initialize(newCollisionInformation, newMass, newInertia); if (space != null) space.Add(this); } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Box.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Box-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Box : Entity> { private Box(float width, float height, float length) :base(new ConvexCollidable(new BoxShape(width, height, length))) { } private Box(float width, float height, float length, float mass) :base(new ConvexCollidable(new BoxShape(width, height, length)), mass) { } /// /// Constructs a physically simulated box. /// /// Position of the box. /// Width of the box. /// Length of the box. /// Height of the box. /// Mass of the object. public Box(Vector3 pos, float width, float height, float length, float mass) : this(width, height, length, mass) { Position = pos; } /// /// Constructs a nondynamic box. /// /// Position of the box. /// Width of the box. /// Length of the box. /// Height of the box. public Box(Vector3 pos, float width, float height, float length) : this(width, height, length) { Position = pos; } /// /// Constructs a physically simulated box. /// /// Motion state specifying the entity's initial state. /// Width of the box. /// Length of the box. /// Height of the box. /// Mass of the object. public Box(MotionState motionState, float width, float height, float length, float mass) : this(width, height, length, mass) { MotionState = motionState; } /// /// Constructs a nondynamic box. /// /// Motion state specifying the entity's initial state. /// Width of the box. /// Length of the box. /// Height of the box. public Box(MotionState motionState, float width, float height, float length) : this(width, height, length) { MotionState = motionState; } /// /// Width of the box divided by two. /// public float HalfWidth { get { return CollisionInformation.Shape.HalfWidth; } set { CollisionInformation.Shape.HalfWidth = value; } } /// /// Height of the box divided by two. /// public float HalfHeight { get { return CollisionInformation.Shape.HalfHeight; } set { CollisionInformation.Shape.HalfHeight = value; } } /// /// Length of the box divided by two. /// public float HalfLength { get { return CollisionInformation.Shape.HalfLength; } set { CollisionInformation.Shape.HalfLength = value; } } /// /// Width of the box. /// public float Width { get { return CollisionInformation.Shape.Width; } set { CollisionInformation.Shape.Width = value; } } /// /// Height of the box. /// public float Height { get { return CollisionInformation.Shape.Height; } set { CollisionInformation.Shape.Height = value; } } /// /// Length of the box. /// public float Length { get { return CollisionInformation.Shape.Length; } set { CollisionInformation.Shape.Length = value; } } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Capsule.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Pill-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Capsule : Entity> { /// /// Gets or sets the length of the capsule. /// public float Length { get { return CollisionInformation.Shape.Length; } set { CollisionInformation.Shape.Length = value; } } /// /// Gets or sets the radius of the capsule. /// public float Radius { get { return CollisionInformation.Shape.Radius; } set { CollisionInformation.Shape.Radius = value; } } private Capsule(float len, float rad) : base(new ConvexCollidable(new CapsuleShape(len, rad))) { } private Capsule(float len, float rad, float mass) : base(new ConvexCollidable(new CapsuleShape(len, rad)), mass) { } /// /// Computes an orientation and length from a line segment. /// ///Starting point of the line segment. ///Endpoint of the line segment. ///Orientation of a line that fits the line segment. ///Length of the line segment. public static void GetCapsuleInformation(ref Vector3 start, ref Vector3 end, out Quaternion orientation, out float length) { Vector3 segmentDirection; Vector3.Subtract(ref end, ref start, out segmentDirection); length = segmentDirection.Length(); if (length > 0) { Vector3.Divide(ref segmentDirection, length, out segmentDirection); Toolbox.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref segmentDirection, out orientation); } else orientation = Quaternion.Identity; } /// /// Constructs a new kinematic capsule. /// ///Line segment start point. ///Line segment end point. ///Radius of the capsule to expand the line segment by. public Capsule(Vector3 start, Vector3 end, float radius) : this((end - start).Length(), radius) { float length; Quaternion orientation; GetCapsuleInformation(ref start, ref end, out orientation, out length); this.Orientation = orientation; Vector3 position; Vector3.Add(ref start, ref end, out position); Vector3.Multiply(ref position, .5f, out position); this.Position = position; } /// /// Constructs a new dynamic capsule. /// ///Line segment start point. ///Line segment end point. ///Radius of the capsule to expand the line segment by. /// Mass of the entity. public Capsule(Vector3 start, Vector3 end, float radius, float mass) : this((end - start).Length(), radius, mass) { float length; Quaternion orientation; GetCapsuleInformation(ref start, ref end, out orientation, out length); this.Orientation = orientation; Vector3 position; Vector3.Add(ref start, ref end, out position); Vector3.Multiply(ref position, .5f, out position); this.Position = position; } /// /// Constructs a physically simulated capsule. /// /// Position of the capsule. /// Length of the capsule. /// Radius of the capsule. /// Mass of the object. public Capsule(Vector3 position, float length, float radius, float mass) : this(length, radius, mass) { Position = position; } /// /// Constructs a nondynamic capsule. /// /// Position of the capsule. /// Length of the capsule. /// Radius of the capsule. public Capsule(Vector3 position, float length, float radius) : this(length, radius) { Position = position; } /// /// Constructs a dynamic capsule. /// /// Motion state specifying the entity's initial state. /// Length of the capsule. /// Radius of the capsule. /// Mass of the object. public Capsule(MotionState motionState, float length, float radius, float mass) : this(length, radius, mass) { MotionState = motionState; } /// /// Constructs a nondynamic capsule. /// /// Motion state specifying the entity's initial state. /// Length of the capsule. /// Radius of the capsule. public Capsule(MotionState motionState, float length, float radius) : this(length, radius) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/CompoundBody.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes; using BEPUutilities; using System.Collections.ObjectModel; namespace BEPUphysics.Entities.Prefabs { /// /// Acts as a grouping of multiple other objects. Can be used to form physically simulated concave shapes. /// public class CompoundBody : Entity { /// /// Gets the list of shapes in the compound. /// public ReadOnlyList Shapes { get { return CollisionInformation.Shape.Shapes; } } /// /// Creates a new kinematic CompoundBody with the given subbodies. /// /// List of entities to use as subbodies of the compound body. /// Thrown when the bodies list is empty or there is a mix of kinematic and dynamic entities in the body list. public CompoundBody(IList bodies) { Vector3 center; var shape = new CompoundShape(bodies, out center); Initialize(new CompoundCollidable(shape)); Position = center; } /// /// Creates a new dynamic CompoundBody with the given subbodies. /// /// List of entities to use as subbodies of the compound body. /// Mass of the compound. /// Thrown when the bodies list is empty or there is a mix of kinematic and dynamic entities in the body list. public CompoundBody(IList bodies, float mass) { Vector3 center; var shape = new CompoundShape(bodies, out center); Initialize(new CompoundCollidable(shape), mass); Position = center; } /// /// Constructs a kinematic compound body from the children data. /// ///Children data to construct the compound from. public CompoundBody(IList children) { Vector3 center; var collidable = new CompoundCollidable(children, out center); Initialize(collidable); Position = center; } /// /// Constructs a dynamic compound body from the children data. /// ///Children data to construct the compound from. ///Mass of the compound body. public CompoundBody(IList children, float mass) { Vector3 center; var collidable = new CompoundCollidable(children, out center); Initialize(collidable, mass); Position = center; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Cone.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Cone-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Cone : Entity> { /// /// Gets or sets the length of the cone. /// public float Height { get { return CollisionInformation.Shape.Height; } set { CollisionInformation.Shape.Height = value; } } /// /// Gets or sets the radius of the cone. /// public float Radius { get { return CollisionInformation.Shape.Radius; } set { CollisionInformation.Shape.Radius = value; } } private Cone(float high, float rad) :base(new ConvexCollidable(new ConeShape(high, rad))) { } private Cone(float high, float rad, float mass) :base(new ConvexCollidable(new ConeShape(high, rad)), mass) { } /// /// Constructs a physically simulated cone. /// /// Position of the cone. /// Height of the cone. /// Radius of the cone. /// Mass of the object. public Cone(Vector3 position, float height, float radius, float mass) : this(height, radius, mass) { Position = position; } /// /// Constructs a nondynamic cone. /// /// Position of the cone. /// Height of the cone. /// Radius of the cone. public Cone(Vector3 position, float height, float radius) : this(height, radius) { Position = position; } /// /// Constructs a physically simulated cone. /// /// Motion state specifying the entity's initial state. /// Height of the cone. /// Radius of the cone. /// Mass of the object. public Cone(MotionState motionState, float height, float radius, float mass) : this(height, radius, mass) { MotionState = motionState; } /// /// Constructs a nondynamic cone. /// /// Motion state specifying the entity's initial state. /// Height of the cone. /// Radius of the cone. public Cone(MotionState motionState, float height, float radius) : this(height, radius) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/ConvexHull.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using System.Collections.ObjectModel; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities.DataStructures; namespace BEPUphysics.Entities.Prefabs { /// /// Shape that can collide and move based on the convex 'outer layer' of a list of points. After making an entity, add it to a Space so that the engine can manage it. /// public class ConvexHull : Entity> { /// /// List of the points composing the surface of the convex hull in local space. /// public ReadOnlyList Vertices { get { return CollisionInformation.Shape.Vertices; } } /// /// Constructs a nondynamic convex hull of points. /// /// List of points in the object. public ConvexHull(IList points) { Vector3 center; var shape = new ConvexHullShape(points, out center); Initialize(new ConvexCollidable(shape)); Position = center; } /// /// Constructs a physically simulated convex hull of points. /// /// List of points in the object. /// Mass of the object. public ConvexHull(IList points, float mass) { Vector3 center; var shape = new ConvexHullShape(points, out center); Initialize(new ConvexCollidable(shape), mass); Position = center; } /// /// Constructs a physically simulated convex hull of points. /// /// Position to place the convex hull. /// List of points in the object. /// Mass of the object. public ConvexHull(Vector3 position, IList points, float mass) : this(points, mass) { Position = position; } /// /// Constructs a nondynamic convex hull of points. /// /// Position to place the convex hull. /// List of points in the object. public ConvexHull(Vector3 position, IList points) : this(points) { Position = position; } /// /// Constructs a physically simulated convex hull of points. /// /// Motion state specifying the entity's initial state. /// List of points in the object. /// Mass of the object. public ConvexHull(MotionState motionState, IList points, float mass) : this(points, mass) { MotionState = motionState; } /// /// Constructs a nondynamic convex hull of points. /// /// Motion state specifying the entity's initial state. /// List of points in the object. public ConvexHull(MotionState motionState, IList points) : this(points) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Cylinder.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Cylinder-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Cylinder : Entity> { /// /// Gets or sets the height of the cylinder. /// public float Height { get { return CollisionInformation.Shape.Height; } set { CollisionInformation.Shape.Height = value; } } /// /// Gets or sets the radius of the cylinder. /// public float Radius { get { return CollisionInformation.Shape.Radius; } set { CollisionInformation.Shape.Radius = value; } } private Cylinder(float high, float rad, float mass) : base(new ConvexCollidable(new CylinderShape(high, rad)), mass) { } private Cylinder(float high, float rad) : base(new ConvexCollidable(new CylinderShape(high, rad))) { } /// /// Constructs a physically simulated cylinder. /// /// Position of the cylinder. /// Height of the cylinder. /// Radius of the cylinder. /// Mass of the object. public Cylinder(Vector3 position, float height, float radius, float mass) : this(height, radius, mass) { Position = position; } /// /// Constructs a nondynamic cylinder. /// /// Position of the cylinder. /// Height of the cylinder. /// Radius of the cylinder. public Cylinder(Vector3 position, float height, float radius) : this(height, radius) { Position = position; } /// /// Constructs a physically simulated cylinder. /// /// Motion state specifying the entity's initial state. /// Height of the cylinder. /// Radius of the cylinder. /// Mass of the object. public Cylinder(MotionState motionState, float height, float radius, float mass) : this(height, radius, mass) { MotionState = motionState; } /// /// Constructs a nondynamic cylinder. /// /// Motion state specifying the entity's initial state. /// Height of the cylinder. /// Radius of the cylinder. public Cylinder(MotionState motionState, float height, float radius) : this(height, radius) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/MinkowskiSum.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Shape representing the sweeping of one entity through another. Can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class MinkowskiSum : Entity> { /// /// First entity in the sum. /// public Entity EntityA; /// /// Second entity in the sum. /// public Entity EntityB; private MinkowskiSum(OrientedConvexShapeEntry a, OrientedConvexShapeEntry b, float m) :base(new ConvexCollidable(new MinkowskiSumShape(a, b)), m) { Position = -CollisionInformation.Shape.LocalOffset; } private MinkowskiSum(OrientedConvexShapeEntry a, OrientedConvexShapeEntry b) : base(new ConvexCollidable(new MinkowskiSumShape(a, b))) { Position = -CollisionInformation.Shape.LocalOffset; } /// /// Constructs a dynamic minkowski sum. /// /// Position of the resulting shape. /// First entity in the sum. /// Second entity in the sum. /// Mass of the object. public MinkowskiSum(Vector3 position, OrientedConvexShapeEntry a, OrientedConvexShapeEntry b, float mass) : this(a, b, mass) { Position = position; } /// /// Constructs a nondynamic minkowski sum of two entities. /// /// Position of the resulting shape. /// First entity in the sum. /// Second entity in the sum. public MinkowskiSum(Vector3 position, OrientedConvexShapeEntry a, OrientedConvexShapeEntry b) : this(a, b) { Position = position; } /// /// Constructs a dynamic minkowski sum of two entities. /// /// Motion state specifying the entity's initial state. /// First entity in the sum. /// Second entity in the sum. /// Mass of the object. public MinkowskiSum(MotionState motionState, OrientedConvexShapeEntry a, OrientedConvexShapeEntry b, float mass) : this(a, b, mass) { MotionState = motionState; } /// /// Constructs a nondynamic minkowski sum of two entities. /// /// Motion state specifying the entity's initial state. /// First entity in the sum. /// Second entity in the sum. public MinkowskiSum(MotionState motionState, OrientedConvexShapeEntry a, OrientedConvexShapeEntry b) : this(a, b) { MotionState = motionState; } /// /// Constructs a dynamic minkowski sum entity. /// /// Motion state specifying the entity's initial state. /// List of shapes to make the sum frmo. /// Mass of the object. public MinkowskiSum(MotionState motionState, IList shapes, float mass) : base(new ConvexCollidable(new MinkowskiSumShape(shapes)), mass) { MotionState = motionState; } /// /// Constructs a nondynamic minkowski sum. /// /// Motion state specifying the entity's initial state. /// List of shapes to make the sum frmo. public MinkowskiSum(MotionState motionState, IList shapes) : base(new ConvexCollidable(new MinkowskiSumShape(shapes))) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/MobileMesh.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUphysics.CollisionShapes; using BEPUutilities; using System.Collections.ObjectModel; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Acts as a grouping of multiple other objects. Can be used to form physically simulated concave shapes. /// public class MobileMesh : Entity { /// /// Creates a new kinematic MobileMesh. /// /// Vertices in the mesh. /// Indices of the mesh. /// Affine transform to apply to the vertices. /// Solidity/sidedness of the mesh. "Solid" is only permitted if the mesh is closed. public MobileMesh(Vector3[] vertices, uint[] indices, AffineTransform localTransform, MobileMeshSolidity solidity) { ShapeDistributionInformation info; var shape = new MobileMeshShape(vertices, indices, localTransform, solidity, out info); Initialize(new MobileMeshCollidable(shape)); Position = info.Center; } /// /// Creates a new dynamic MobileMesh. /// /// Vertices in the mesh. /// Indices of the mesh. /// Affine transform to apply to the vertices. /// Solidity/sidedness of the mesh. "Solid" is only permitted if the mesh is closed. /// Mass of the mesh. public MobileMesh(Vector3[] vertices, uint[] indices, AffineTransform localTransform, MobileMeshSolidity solidity, float mass) { ShapeDistributionInformation info; var shape = new MobileMeshShape(vertices, indices, localTransform, solidity, out info); Matrix3x3 inertia; Matrix3x3.Multiply(ref info.VolumeDistribution, mass * InertiaHelper.InertiaTensorScale, out inertia); Initialize(new MobileMeshCollidable(shape), mass, inertia, info.Volume); Position = info.Center; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Sphere.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// Ball-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Sphere : Entity> { /// /// Radius of the sphere. /// public float Radius { get { return CollisionInformation.Shape.Radius; } set { CollisionInformation.Shape.Radius = value; } } private Sphere(float radius) :base(new ConvexCollidable(new SphereShape(radius))) { } private Sphere(float radius, float mass) :base(new ConvexCollidable(new SphereShape(radius)), mass) { } /// /// Constructs a physically simulated sphere. /// /// Position of the sphere. /// Radius of the sphere. /// Mass of the object. public Sphere(Vector3 position, float radius, float mass) : this(radius, mass) { Position = position; } /// /// Constructs a nondynamic sphere. /// /// Position of the sphere. /// Radius of the sphere. public Sphere(Vector3 position, float radius) : this(radius) { Position = position; } /// /// Constructs a physically simulated sphere. /// /// Motion state specifying the entity's initial state. /// Radius of the sphere. /// Mass of the object. public Sphere(MotionState motionState, float radius, float mass) : this(radius, mass) { MotionState = motionState; } /// /// Constructs a nondynamic sphere. /// /// Motion state specifying the entity's initial state. /// Radius of the sphere. public Sphere(MotionState motionState, float radius) : this(radius) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/TransformableEntity.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; namespace BEPUphysics.Entities.Prefabs { /// /// Ball-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class TransformableEntity : Entity> { /// /// Gets the shape on which the transformable shape is based. /// public ConvexShape Shape { get { return CollisionInformation.Shape.Shape; } set { CollisionInformation.Shape.Shape = value; } } /// /// Gets the linear transform that the shape uses to transform its base shape. /// public Matrix3x3 Transform { get { return CollisionInformation.Shape.Transform; } set { CollisionInformation.Shape.Transform = value; } } private TransformableEntity(ConvexShape shape, Matrix3x3 transform) : base(new ConvexCollidable(new TransformableShape(shape, transform))) { } private TransformableEntity(ConvexShape shape, Matrix3x3 transform, float mass) : base(new ConvexCollidable(new TransformableShape(shape, transform)), mass) { } /// /// Constructs a dynamic transformable entity. /// /// Position of the entity. /// Shape to transform. /// Transform to apply to the shape. /// Mass of the object. public TransformableEntity(Vector3 position, ConvexShape shape, Matrix3x3 transform, float mass) : this(shape, transform, mass) { Position = position; } /// /// Constructs a kinematic transformable entity. /// /// Position of the entity. /// Shape to transform. /// Transform to apply to the shape. public TransformableEntity(Vector3 position, ConvexShape shape, Matrix3x3 transform) : this(shape, transform) { Position = position; } /// /// Constructs a dynamic transformable entity. /// /// Initial motion state of the entity. /// Shape to transform. /// Transform to apply to the shape. /// Mass of the object. public TransformableEntity(MotionState motionState, ConvexShape shape, Matrix3x3 transform, float mass) : this(shape, transform, mass) { MotionState = motionState; } /// /// Constructs a kinematic transformable entity. /// /// Initial motion state of the entity. /// Shape to transform. /// Transform to apply to the shape. public TransformableEntity(MotionState motionState, ConvexShape shape, Matrix3x3 transform) : this(shape, transform) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/Triangle.cs ================================================ using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities; namespace BEPUphysics.Entities.Prefabs { /// /// Triangle-shaped object that can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class Triangle : Entity> { /// /// Gets or sets the first vertex of the triangle in local space. /// public Vector3 LocalVertexA { get { return CollisionInformation.Shape.VertexA; } set { CollisionInformation.Shape.VertexA = value; } } /// /// Gets or sets the second vertex of the triangle in local space. /// public Vector3 LocalVertexB { get { return CollisionInformation.Shape.VertexB; } set { CollisionInformation.Shape.VertexB = value; } } /// /// Gets or sets the third vertex of the triangle in local space. /// public Vector3 LocalVertexC { get { return CollisionInformation.Shape.VertexC; } set { CollisionInformation.Shape.VertexC = value; } } /// /// Gets or sets the first vertex of the triangle in world space. /// public Vector3 VertexA { get { return Matrix3x3.Transform(CollisionInformation.Shape.VertexA, orientationMatrix) + position; } set { CollisionInformation.Shape.VertexA = Matrix3x3.TransformTranspose(value - position, orientationMatrix); } } /// /// Gets or sets the second vertex of the triangle in world space. /// public Vector3 VertexB { get { return Matrix3x3.Transform(CollisionInformation.Shape.VertexB, orientationMatrix) + position; } set { CollisionInformation.Shape.VertexB = Matrix3x3.TransformTranspose(value - position, orientationMatrix); } } /// /// Gets or sets the third vertex of the triangle in world space. /// public Vector3 VertexC { get { return Matrix3x3.Transform(CollisionInformation.Shape.VertexC, orientationMatrix) + position; } set { CollisionInformation.Shape.VertexC = Matrix3x3.TransformTranspose(value - position, orientationMatrix); } } /// /// Gets or sets the sidedness of the triangle. /// public TriangleSidedness Sidedness { get { return CollisionInformation.Shape.Sidedness; } set { CollisionInformation.Shape.Sidedness = value; } } /// /// Constructs a dynamic triangle. /// /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. /// Mass of the object. public Triangle(Vector3 v1, Vector3 v2, Vector3 v3, float mass) { Vector3 center; var shape = new TriangleShape(v1, v2, v3, out center); Initialize(new ConvexCollidable(shape), mass); Position = center; } /// /// Constructs a nondynamic triangle. /// /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. public Triangle(Vector3 v1, Vector3 v2, Vector3 v3) { Vector3 center; var shape = new TriangleShape(v1, v2, v3, out center); Initialize(new ConvexCollidable(shape)); Position = center; } /// /// Constructs a dynamic triangle. /// /// Position where the triangle is initialy centered. /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. /// Mass of the object. public Triangle(Vector3 pos, Vector3 v1, Vector3 v2, Vector3 v3, float mass) : this(v1, v2, v3, mass) { Position = pos; } /// /// Constructs a nondynamic triangle. /// /// Position where the triangle is initially centered. /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. public Triangle(Vector3 pos, Vector3 v1, Vector3 v2, Vector3 v3) : this(v1, v2, v3) { Position = pos; } /// /// Constructs a dynamic triangle. /// /// Motion state specifying the entity's initial state. /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. /// Mass of the object. public Triangle(MotionState motionState, Vector3 v1, Vector3 v2, Vector3 v3, float mass) : this(v1, v2, v3, mass) { MotionState = motionState; } /// /// Constructs a nondynamic triangle. /// /// Motion state specifying the entity's initial state. /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. public Triangle(MotionState motionState, Vector3 v1, Vector3 v2, Vector3 v3) : this(v1, v2, v3) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/Entities/Prefabs/WrappedBody.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.DataStructures; using BEPUphysics.EntityStateManagement; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.Entities.Prefabs { /// /// A shape formed from the convex hull around its subbodies. Can collide and move. After making an entity, add it to a Space so that the engine can manage it. /// public class WrappedBody : Entity> { /// /// Gets the list of shapes in the wrapped body. /// public ObservableList Shapes { get { return CollisionInformation.Shape.Shapes; } } /// Thrown when the subbodies list contains zero entities. private WrappedBody(IList subShapes, float mass) { Vector3 center; var shape = new WrappedShape(subShapes, out center); Initialize(new ConvexCollidable(shape), mass); Position = center; } /// Thrown when the subbodies list contains zero entities. private WrappedBody(IList subShapes) { Vector3 center; var shape = new WrappedShape(subShapes, out center); Initialize(new ConvexCollidable(shape)); Position = center; } /// /// Constructs a physically simulated box. /// /// Position of the box. /// List of entities composing the body. /// Mass of the object. public WrappedBody(Vector3 position, IList subBodies, float mass) : this(subBodies, mass) { Position = position; } /// /// Constructs a nondynamic wrapped body. /// /// Position of the box. /// List of entities composing the body. public WrappedBody(Vector3 position, IList subBodies) : this(subBodies) { Position = position; } /// /// Constructs a dynamic wrapped body. /// /// Motion state specifying the entity's initial state. /// List of entities composing the body. /// Mass of the object. public WrappedBody(MotionState motionState, IList subBodies, float mass) : this(subBodies, mass) { MotionState = motionState; } /// /// Constructs a nondynamic wrapped body. /// /// Motion state specifying the entity's initial state. /// List of entities composing the body. public WrappedBody(MotionState motionState, IList subBodies) : this(subBodies) { MotionState = motionState; } } } ================================================ FILE: BEPUphysics/EntityStateManagement/BufferedStatesAccessor.cs ================================================ using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.EntityStateManagement { /// /// Acts as an entity's view into the buffered states system. /// Buffered states are updated each frame and contain the latest known states. /// These states can also be written to. Writes will not be immediately visible; /// the next frame's write buffer flush will write the changes to the entities. /// public class BufferedStatesAccessor { internal EntityBufferedStates bufferedStates; /// /// Gets and sets the states write buffer used when buffered properties are written. /// public EntityStateWriteBuffer WriteBuffer { get; set; } /// /// Constructs a new accessor. /// ///The owning states system. public BufferedStatesAccessor(EntityBufferedStates bufferedStates) { this.bufferedStates = bufferedStates; } bool IsReadBufferAccessible() { return bufferedStates.BufferedStatesManager != null && bufferedStates.BufferedStatesManager.Enabled && bufferedStates.BufferedStatesManager.ReadBuffers.Enabled; } bool IsWriteBufferAccessible() { return WriteBuffer != null && WriteBuffer.Enabled; } /// /// Gets or sets the buffered position of the entity. /// public Vector3 Position { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).Position; return bufferedStates.Entity.Position; } set { if (IsWriteBufferAccessible()) WriteBuffer.EnqueuePosition(bufferedStates.Entity, ref value); else bufferedStates.Entity.Position = value; } } /// /// Gets or sets the buffered orientation quaternion of the entity. /// public Quaternion Orientation { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).Orientation; return bufferedStates.Entity.Orientation; } set { if (IsWriteBufferAccessible()) WriteBuffer.EnqueueOrientation(bufferedStates.Entity, ref value); else bufferedStates.Entity.Orientation = value; } } /// /// Gets or sets the buffered orientation matrix of the entity. /// public Matrix3x3 OrientationMatrix { get { Matrix3x3 toReturn; if (IsReadBufferAccessible()) { Quaternion o = bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).Orientation; Matrix3x3.CreateFromQuaternion(ref o, out toReturn); } else Matrix3x3.CreateFromQuaternion(ref bufferedStates.Entity.orientation, out toReturn); return toReturn; } set { if (IsWriteBufferAccessible()) { Quaternion toSet = Quaternion.Normalize(Quaternion.CreateFromRotationMatrix(Matrix3x3.ToMatrix4X4(value))); WriteBuffer.EnqueueOrientation(bufferedStates.Entity, ref toSet); } else { bufferedStates.Entity.OrientationMatrix = value; } } } /// /// Gets or sets the buffered linear velocity of the entity. /// public Vector3 LinearVelocity { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).LinearVelocity; return bufferedStates.Entity.LinearVelocity; } set { if (IsWriteBufferAccessible()) WriteBuffer.EnqueueLinearVelocity(bufferedStates.Entity, ref value); else bufferedStates.Entity.LinearVelocity = value; } } /// /// Gets or sets the buffered angular velocity of the entity. /// public Vector3 AngularVelocity { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).AngularVelocity; return bufferedStates.Entity.AngularVelocity; } set { if (IsWriteBufferAccessible()) WriteBuffer.EnqueueAngularVelocity(bufferedStates.Entity, ref value); else bufferedStates.Entity.AngularVelocity = value; } } /// /// Gets or sets the buffered world transform of the entity. /// public Matrix WorldTransform { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex).WorldTransform; return bufferedStates.Entity.WorldTransform; } set { if (IsWriteBufferAccessible()) { Vector3 translation = value.Translation; Quaternion orientation; Quaternion.CreateFromRotationMatrix(ref value, out orientation); orientation.Normalize(); WriteBuffer.EnqueueOrientation(bufferedStates.Entity, ref orientation); WriteBuffer.EnqueuePosition(bufferedStates.Entity, ref translation); } else { bufferedStates.Entity.WorldTransform = value; } } } /// /// Gets or sets the buffered motion state of the entity. /// public MotionState MotionState { get { if (IsReadBufferAccessible()) return bufferedStates.BufferedStatesManager.ReadBuffers.GetState(bufferedStates.motionStateIndex); return bufferedStates.Entity.MotionState; } set { if (IsWriteBufferAccessible()) { WriteBuffer.EnqueueLinearVelocity(bufferedStates.Entity, ref value.LinearVelocity); WriteBuffer.EnqueueAngularVelocity(bufferedStates.Entity, ref value.AngularVelocity); WriteBuffer.EnqueueOrientation(bufferedStates.Entity, ref value.Orientation); WriteBuffer.EnqueuePosition(bufferedStates.Entity, ref value.Position); } else { bufferedStates.Entity.MotionState = value; } } } } } ================================================ FILE: BEPUphysics/EntityStateManagement/BufferedStatesManager.cs ================================================ using System; using System.Collections.Generic; using System.Collections.ObjectModel; using BEPUphysics.Threading; using BEPUutilities; using BEPUphysics.Entities; using BEPUutilities.DataStructures; namespace BEPUphysics.EntityStateManagement { /// /// Manages the buffered and interpolated states of entities. /// public class BufferedStatesManager { /// /// Gets the buffers of last known entity states. /// public StateReadBuffers ReadBuffers { get; private set; } /// /// Gets the entity states blended between the current frame and previous frame based on /// the time remaining in internal time stepping. /// public InterpolatedStatesManager InterpolatedStates { get; private set; } internal RawList entities = new RawList(); /// /// Gets the list of entities in the manager. /// public ReadOnlyList Entities { get { return new ReadOnlyList(entities); } } bool enabled; /// /// Gets or sets whether or not the buffered states manager and its submanagers are updating. /// public bool Enabled { get { return enabled; } set { if (!enabled && value) { ReadBuffers.Enabled = true; InterpolatedStates.Enabled = true; } else if (enabled && !value) { InterpolatedStates.Enabled = false; ReadBuffers.Enabled = false; } enabled = value; } } /// /// Constructs a new manager. /// public BufferedStatesManager() { InterpolatedStates = new InterpolatedStatesManager(this); ReadBuffers = new StateReadBuffers(this); } /// /// Constructs a new manager. /// ///Thread manager to be used by the manager. public BufferedStatesManager(IThreadManager threadManager) { InterpolatedStates = new InterpolatedStatesManager(this, threadManager); ReadBuffers = new StateReadBuffers(this, threadManager); } /// /// Adds an entity to the manager. /// ///Entity to add. ///Thrown if the entity already belongs to a states manager. public void Add(Entity e) { lock (InterpolatedStates.FlipLocker) { lock (ReadBuffers.FlipLocker) { if (e.BufferedStates.BufferedStatesManager == null) { e.BufferedStates.BufferedStatesManager = this; e.BufferedStates.motionStateIndex = entities.Count; entities.Add(e); if (ReadBuffers.Enabled) ReadBuffers.Add(e); if (InterpolatedStates.Enabled) InterpolatedStates.Add(e); } else throw new InvalidOperationException("Entity already belongs to a BufferedStatesManager; cannot add."); } } } /// /// Removes an entity from the manager. /// ///Entity to remove. ///Thrown if the entity does not belong to this manager. public void Remove(Entity e) { lock (InterpolatedStates.FlipLocker) { lock (ReadBuffers.FlipLocker) { if (e.BufferedStates.BufferedStatesManager == this) { int index = entities.IndexOf(e); int endIndex = entities.Count - 1; entities[index] = entities[endIndex]; entities.RemoveAt(endIndex); if (index < entities.Count) entities[index].BufferedStates.motionStateIndex = index; if (ReadBuffers.Enabled) ReadBuffers.Remove(index, endIndex); if (InterpolatedStates.Enabled) InterpolatedStates.Remove(index, endIndex); e.BufferedStates.BufferedStatesManager = null; } else throw new InvalidOperationException("Entity does not belong to this BufferedStatesManager; cannot remove."); } } } } } ================================================ FILE: BEPUphysics/EntityStateManagement/EntityBufferedStates.cs ================================================ using BEPUphysics.Entities; namespace BEPUphysics.EntityStateManagement { /// /// Contains a single entity's buffered states. /// public class EntityBufferedStates { /// /// Gets the buffered states manager that owns this entry. /// public BufferedStatesManager BufferedStatesManager { get; internal set; } /// /// Gets the buffered states accessor for this entity. /// Contains the current snapshot of the entity's states. /// public BufferedStatesAccessor States { get; private set; } /// /// Gets the interpolated states accessor for this entity. /// Contains a blended snapshot between the previous and current states based on the /// internal timestepping remainder. /// public InterpolatedStatesAccessor InterpolatedStates { get; private set; } internal int motionStateIndex; /// /// Gets the motion state index of this entity. /// public int MotionStateIndex { get { return motionStateIndex; } internal set { motionStateIndex = value; } } /// /// Constructs a new buffered states entry. /// ///Owning entity. public EntityBufferedStates(Entity entity) { Entity = entity; States = new BufferedStatesAccessor(this); InterpolatedStates = new InterpolatedStatesAccessor(this); } /// /// Gets the entity owning this entry. /// public Entity Entity { get; private set; } } } ================================================ FILE: BEPUphysics/EntityStateManagement/EntityStateReadBuffers.cs ================================================ using System; using BEPUphysics.Entities; using BEPUphysics.Threading; using BEPUutilities; namespace BEPUphysics.EntityStateManagement { /// /// Manages the buffered states of entities. /// public class StateReadBuffers : MultithreadedProcessingStage { /// /// Gets or sets whether or not the buffers are active. /// ///Thrown if the read buffers are disabled while the interpolated states are enabled. public override bool Enabled { get { return base.Enabled; } set { if (base.Enabled && !value) { if (!manager.InterpolatedStates.Enabled) throw new InvalidOperationException("Cannot disable read buffers unless the interpolated states are disabled."); Disable(); base.Enabled = false; } else if (!base.Enabled && value) { Enable(); base.Enabled = true; } } } internal void Enable() { //Turn everything on. lock (FlipLocker) { int initialCount = Math.Max(manager.entities.Count, 64); backBuffer = new MotionState[initialCount]; frontBuffer = new MotionState[initialCount]; for (int i = 0; i < manager.entities.Count; i++) { Entity entity = manager.entities[i]; backBuffer[i].Position = entity.position; backBuffer[i].Orientation = entity.orientation; backBuffer[i].LinearVelocity = entity.linearVelocity; backBuffer[i].AngularVelocity = entity.angularVelocity; } Array.Copy(backBuffer, frontBuffer, backBuffer.Length); } } internal void Disable() { //Turn everything off. lock (FlipLocker) { backBuffer = null; frontBuffer = null; } } private BufferedStatesManager manager; /// /// Gets the synchronization object which is locked during internal buffer flips. /// Acquiring a lock on this object will prevent the manager from flipping the buffers /// for the duration of the lock. /// public object FlipLocker { get; private set; } internal MotionState[] backBuffer; internal MotionState[] frontBuffer; /// /// Constructs a read buffer manager. /// ///Owning buffered states manager. public StateReadBuffers(BufferedStatesManager manager) { this.manager = manager; multithreadedStateUpdateDelegate = MultithreadedStateUpdate; FlipLocker = new object(); } /// /// Constructs a read buffer manager. /// ///Owning buffered states manager. ///Thread manager to use. public StateReadBuffers(BufferedStatesManager manager, IThreadManager threadManager) { this.manager = manager; multithreadedStateUpdateDelegate = MultithreadedStateUpdate; FlipLocker = new object(); ThreadManager = threadManager; AllowMultithreading = true; } Action multithreadedStateUpdateDelegate; void MultithreadedStateUpdate(int i) { Entity entity = manager.entities[i]; backBuffer[i].Position = entity.position; backBuffer[i].Orientation = entity.orientation; backBuffer[i].LinearVelocity = entity.linearVelocity; backBuffer[i].AngularVelocity = entity.angularVelocity; } protected override void UpdateMultithreaded() { ThreadManager.ForLoop(0, manager.entities.Count, multithreadedStateUpdateDelegate); FlipBuffers(); } protected override void UpdateSingleThreaded() { for (int i = 0; i < manager.entities.Count; i++) { Entity entity = manager.entities[i]; backBuffer[i].Position = entity.position; backBuffer[i].Orientation = entity.orientation; backBuffer[i].LinearVelocity = entity.linearVelocity; backBuffer[i].AngularVelocity = entity.angularVelocity; } FlipBuffers(); } internal void Add(Entity e) { //Don't need to lock since the parent manager handles it. if (frontBuffer.Length <= e.BufferedStates.motionStateIndex) { var newStates = new MotionState[frontBuffer.Length * 2]; //TODO: shifty frontBuffer.CopyTo(newStates, 0); frontBuffer = newStates; } frontBuffer[e.BufferedStates.motionStateIndex].Position = e.position; frontBuffer[e.BufferedStates.motionStateIndex].Orientation = e.orientation; if (backBuffer.Length <= e.BufferedStates.motionStateIndex) { var newStates = new MotionState[backBuffer.Length * 2]; //TODO: shifty backBuffer.CopyTo(newStates, 0); backBuffer = newStates; } backBuffer[e.BufferedStates.motionStateIndex].Position = e.position; backBuffer[e.BufferedStates.motionStateIndex].Orientation = e.orientation; } internal void Remove(int index, int endIndex) { //Don't need to lock since the parent manager handles it. frontBuffer[index] = frontBuffer[endIndex]; backBuffer[index] = backBuffer[endIndex]; } /// /// Acquires a lock on the FlipLocker and forces the internal buffers to flip. /// public void FlipBuffers() { lock (FlipLocker) { MotionState[] formerFrontBuffer = frontBuffer; frontBuffer = backBuffer; backBuffer = formerFrontBuffer; } } /// /// Gets the state of the entity associated with the given index. /// Does not lock the FlipLocker. /// ///Index of the entity. ///MotionState of the entity at the index. public MotionState GetState(int motionStateIndex) { return frontBuffer[motionStateIndex]; } /// /// Gets the states of all entities atomically. /// ///Entity states. ///Thrown when the array is too small. public void GetStates(MotionState[] states) { lock (FlipLocker) { if (states.Length < manager.entities.Count) { throw new ArgumentException("Array is not large enough to hold the buffer.", "states"); } Array.Copy(frontBuffer, states, manager.entities.Count); } } } } ================================================ FILE: BEPUphysics/EntityStateManagement/EntityStateWriteBuffer.cs ================================================ using System.Runtime.InteropServices; using BEPUphysics.Entities; using BEPUutilities; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUphysics.EntityStateManagement { /// /// Buffer containing pending writes to entity states. /// public class EntityStateWriteBuffer : ProcessingStage { internal enum TargetField : byte { Position, Orientation, LinearVelocity, AngularVelocity, } //TODO: Not a particularly elegant buffering mechanism. Make a better in-order buffering scheme. //There are platform requirements on layout that cause issues with the WINDOWS explicit version. //TODO: There's probably a better way to handle it on the XBOX/WP7 than the "give up" approach taken below. #if WINDOWS [StructLayout(LayoutKind.Explicit)] internal struct EntityStateChange { [FieldOffset(0)] internal Quaternion orientationQuaternion; [FieldOffset(0)] internal Vector3 vector; [FieldOffset(16)] internal TargetField targetField; [FieldOffset(24)] internal Entity target; } #else internal struct EntityStateChange { internal Quaternion orientationQuaternion; internal Vector3 vector; internal TargetField targetField; internal Entity target; } #endif private ConcurrentDeque stateChanges = new ConcurrentDeque(); /// /// Constructs the write buffer. /// public EntityStateWriteBuffer() { Enabled = true; } /// /// Enqueues a change to an entity's position. /// ///Entity to target. ///New position of the entity. public void EnqueuePosition(Entity entity, ref Vector3 newPosition) { stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newPosition, targetField = TargetField.Position }); } /// /// Enqueues a change to an entity's orientation. /// ///Entity to target. ///New orientation of the entity. public void EnqueueOrientation(Entity entity, ref Quaternion newOrientationQuaternion) { stateChanges.Enqueue(new EntityStateChange { target = entity, orientationQuaternion = newOrientationQuaternion, targetField = TargetField.Orientation }); } /// /// Enqueues a change to an entity's linear velocity. /// ///Entity to target. ///New linear velocity of the entity. public void EnqueueLinearVelocity(Entity entity, ref Vector3 newLinearVelocity) { stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newLinearVelocity, targetField = TargetField.LinearVelocity }); } /// /// Enqueues a change to an entity's angular velocity. /// ///Entity to target. ///New angular velocity of the entity. public void EnqueueAngularVelocity(Entity entity, ref Vector3 newAngularVelocity) { stateChanges.Enqueue(new EntityStateChange { target = entity, vector = newAngularVelocity, targetField = TargetField.AngularVelocity }); } protected override void UpdateStage() { EntityStateChange item; while (stateChanges.TryDequeueFirst(out item)) { Entity target = item.target; switch (item.targetField) { case TargetField.Position: target.Position = item.vector; break; case TargetField.Orientation: target.Orientation = item.orientationQuaternion; break; case TargetField.LinearVelocity: target.LinearVelocity = item.vector; break; case TargetField.AngularVelocity: target.AngularVelocity = item.vector; break; } } } } } ================================================ FILE: BEPUphysics/EntityStateManagement/InterpolatedStatesAccessor.cs ================================================ using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.EntityStateManagement { /// /// Accesses an entity's interpolated states. /// Interpolated states are blended states between the previous and current entity states based /// on the time remainder from interal timestepping. /// public class InterpolatedStatesAccessor { internal EntityBufferedStates bufferedStates; /// /// Constructs a new accessor. /// ///Owning entry. public InterpolatedStatesAccessor(EntityBufferedStates bufferedStates) { this.bufferedStates = bufferedStates; } bool IsBufferAccessible() { return bufferedStates.BufferedStatesManager != null && bufferedStates.BufferedStatesManager.Enabled && bufferedStates.BufferedStatesManager.InterpolatedStates.Enabled; } /// /// Gets the interpolated position of the entity. /// public Vector3 Position { get { if (IsBufferAccessible()) return bufferedStates.BufferedStatesManager.InterpolatedStates.GetState(bufferedStates.motionStateIndex).Position; return bufferedStates.Entity.Position; } } /// /// Gets the interpolated orientation of the entity. /// public Quaternion Orientation { get { if (IsBufferAccessible()) return bufferedStates.BufferedStatesManager.InterpolatedStates.GetState(bufferedStates.motionStateIndex).Orientation; return bufferedStates.Entity.Orientation; } } /// /// Gets the interpolated orientation matrix of the entity. /// public Matrix3x3 OrientationMatrix { get { Matrix3x3 toReturn; if (IsBufferAccessible()) { Quaternion o = bufferedStates.BufferedStatesManager.InterpolatedStates.GetState(bufferedStates.motionStateIndex).Orientation; Matrix3x3.CreateFromQuaternion(ref o, out toReturn); } else Matrix3x3.CreateFromQuaternion(ref bufferedStates.Entity.orientation, out toReturn); return toReturn; } } /// /// Gets the interpolated world transform of the entity. /// public Matrix WorldTransform { get { if (IsBufferAccessible()) return bufferedStates.BufferedStatesManager.InterpolatedStates.GetState(bufferedStates.motionStateIndex).Matrix; return bufferedStates.Entity.WorldTransform; } } /// /// Gets the interpolated rigid transform of the entity. /// public RigidTransform RigidTransform { get { if (IsBufferAccessible()) return bufferedStates.BufferedStatesManager.InterpolatedStates.GetState(bufferedStates.motionStateIndex); var toReturn = new RigidTransform { Position = bufferedStates.Entity.position, Orientation = bufferedStates.Entity.orientation }; return toReturn; } } } } ================================================ FILE: BEPUphysics/EntityStateManagement/InterpolatedStatesManager.cs ================================================ using System; using BEPUphysics.Entities; using BEPUphysics.Threading; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.EntityStateManagement { /// /// Manages the interpolated states of entities. Interpolated states are those /// based on the previous entity states and the current entity states, blended together /// using the time remainder from internal time stepping. /// public class InterpolatedStatesManager : MultithreadedProcessingStage { /// /// Gets or sets whether or not the manager is updating. /// ///Thrown when enabling the interpolated manager without having the read buffers active. public override bool Enabled { get { return base.Enabled; } set { if (base.Enabled && !value) { Disable(); base.Enabled = false; } else if (!base.Enabled && value) { if (!manager.ReadBuffers.Enabled) throw new InvalidOperationException("Cannot enable interpolated states unless the read buffers are enabled."); Enable(); base.Enabled = true; } } } internal void Enable() { //Turn everything on. lock (FlipLocker) { int initialCount = Math.Max(manager.entities.Count, 64); backBuffer = new RigidTransform[initialCount]; states = new RigidTransform[initialCount]; for (int i = 0; i < manager.entities.Count; i++) { Entity entity = manager.entities[i]; backBuffer[i].Position = entity.position; backBuffer[i].Orientation = entity.orientation; } Array.Copy(backBuffer, states, backBuffer.Length); } } internal void Disable() { //Turn everything off. lock (FlipLocker) { backBuffer = null; states = null; } } private BufferedStatesManager manager; /// /// Gets the synchronization object locked prior to flipping the internal buffers. /// Acquiring a lock on this object will prevent the internal buffers from flipping for the duration /// of the lock. /// public object FlipLocker { get; private set; } RigidTransform[] backBuffer; RigidTransform[] states = new RigidTransform[64]; /// /// Constructs a new interpolated states manager. /// ///Owning buffered states manager. public InterpolatedStatesManager(BufferedStatesManager manager) { this.manager = manager; multithreadedWithReadBuffersDelegate = UpdateIndex; FlipLocker = new object(); } /// /// Constructs a new interpolated states manager. /// ///Owning buffered states manager. /// Thread manager to use. public InterpolatedStatesManager(BufferedStatesManager manager, IThreadManager threadManager) { this.manager = manager; multithreadedWithReadBuffersDelegate = UpdateIndex; FlipLocker = new object(); ThreadManager = threadManager; AllowMultithreading = true; } float blendAmount; /// /// Gets or sets the blending amount to use. /// This is set automatically when the space is using internal timestepping /// (I.E. when Space.Update(dt) is called). It is a value from 0 to 1 /// that defines the amount of the previous and current frames to include /// in the blended state. A value of 1 means use only the current frame; /// a value of 0 means use only the previous frame. /// public float BlendAmount { get { return blendAmount; } set { blendAmount = MathHelper.Clamp(value, 0, 1); } } Action multithreadedWithReadBuffersDelegate; void UpdateIndex(int i) { Entity entity = manager.entities[i]; //Blend between previous and current states. //Interpolated updates occur after proper updates complete. //That means that the internal positions and the front buffer positions are equivalent. //However, the backbuffer is uncontested and contains the previous frame's data. Vector3.Lerp(ref manager.ReadBuffers.backBuffer[i].Position, ref entity.position, blendAmount, out backBuffer[i].Position); Quaternion.Slerp(ref manager.ReadBuffers.backBuffer[i].Orientation, ref entity.orientation, blendAmount, out backBuffer[i].Orientation); } protected override void UpdateMultithreaded() { ThreadManager.ForLoop(0, manager.entities.Count, multithreadedWithReadBuffersDelegate); FlipBuffers(); } protected override void UpdateSingleThreaded() { for (int i = 0; i < manager.entities.Count; i++) { UpdateIndex(i); } FlipBuffers(); } /// /// Acquires a lock on the FlipLocker and flips the internal buffers. /// public void FlipBuffers() { lock (FlipLocker) { RigidTransform[] formerFrontBuffer = states; states = backBuffer; backBuffer = formerFrontBuffer; } } /// /// Returns an interpolated state associated with an entity with the given index. /// Does not lock the FlipLocker. /// ///Motion state of the entity. ///Interpolated state associated with the entity at the given index. public RigidTransform GetState(int motionStateIndex) { return states[motionStateIndex]; } /// /// Gets the interpolated states of all entities. /// ///Interpolated states of all entities. ///Thrown when the array is too small to hold the states. public void GetStates(RigidTransform[] states) { lock (FlipLocker) { if (states.Length < manager.entities.Count) { throw new ArgumentException("Array is not large enough to hold the buffer.", "states"); } Array.Copy(this.states, states, manager.entities.Count); } } internal void Add(Entity e) { //Don't need to lock since the parent manager handles it. if (states.Length <= e.BufferedStates.motionStateIndex) { var newStates = new RigidTransform[states.Length * 2]; states.CopyTo(newStates, 0); states = newStates; } states[e.BufferedStates.motionStateIndex].Position = e.position; states[e.BufferedStates.motionStateIndex].Orientation = e.orientation; if (backBuffer.Length <= e.BufferedStates.motionStateIndex) { var newStates = new RigidTransform[backBuffer.Length * 2]; backBuffer.CopyTo(newStates, 0); backBuffer = newStates; } backBuffer[e.BufferedStates.motionStateIndex].Position = e.position; backBuffer[e.BufferedStates.motionStateIndex].Orientation = e.orientation; } internal void Remove(int index, int endIndex) { //Don't need to lock since the parent manager handles it. states[index] = states[endIndex]; backBuffer[index] = backBuffer[endIndex]; } } } ================================================ FILE: BEPUphysics/EntityStateManagement/MotionState.cs ================================================ using Microsoft.Xna.Framework; using System; namespace BEPUphysics.EntityStateManagement { /// /// State describing the position, orientation, and velocity of an entity. /// public struct MotionState : IEquatable { /// /// Position of an entity. /// public Vector3 Position; /// /// Orientation of an entity. /// public Quaternion Orientation; /// /// Orientation matrix of an entity. /// public Matrix OrientationMatrix { get { Matrix toReturn; Matrix.CreateFromQuaternion(ref Orientation, out toReturn); return toReturn; } } /// /// World transform of an entity. /// public Matrix WorldTransform { get { Matrix toReturn; Matrix.CreateFromQuaternion(ref Orientation, out toReturn); toReturn.Translation = Position; return toReturn; } } /// /// Linear velocity of an entity. /// public Vector3 LinearVelocity; /// /// Angular velocity of an entity. /// public Vector3 AngularVelocity; public bool Equals(MotionState other) { return other.AngularVelocity == AngularVelocity && other.LinearVelocity == LinearVelocity && other.Position == Position && other.Orientation == Orientation; } } } ================================================ FILE: BEPUphysics/ISpace.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using System.Collections.ObjectModel; using BEPUutilities.DataStructures; namespace BEPUphysics { /// /// Defines the minimum interface required for a Space object which acts as the main simulation class. /// public interface ISpace { /// /// Adds a space object to the simulation. /// ///Space object to add. void Add(ISpaceObject spaceObject); /// /// Removes a space object from the simulation. /// ///Space object to remove. void Remove(ISpaceObject spaceObject); /// /// Performs a single timestep. /// void Update(); /// /// Performs as many timesteps as necessary to get as close to the elapsed time as possible. /// /// Elapsed time from the previous frame. void Update(float dt); //Does as many timesteps as necessary, obeying the timing requirements. /// /// Gets the list of entities in the space. /// ReadOnlyList Entities { get; } /// /// Tests a ray against the space. /// /// Ray to test. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, out RayCastResult result); /// /// Tests a ray against the space. /// /// Ray to test. /// Delegate to prune out hit candidates before performing a ray cast against them. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, Func filter, out RayCastResult result); /// /// Tests a ray against the space. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, float maximumLength, out RayCastResult result); /// /// Tests a ray against the space. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Delegate to prune out hit candidates before performing a ray cast against them. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, float maximumLength, Func filter, out RayCastResult result); /// /// Tests a ray against the space, possibly returning multiple hits. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, float maximumLength, IList outputRayCastResults); /// /// Tests a ray against the space, possibly returning multiple hits. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Delegate to prune out hit candidates before performing a ray cast against them. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. bool RayCast(Ray ray, float maximumLength, Func filter, IList outputRayCastResults); } } ================================================ FILE: BEPUphysics/ISpaceObject.cs ================================================ namespace BEPUphysics { /// /// Defines an object which can be managed by an ISpace. /// public interface ISpaceObject { /// /// Gets the Space to which the object belongs. /// ISpace Space { get; set; } /// /// Called after the object is added to a space. /// /// Space to which the object was added. void OnAdditionToSpace(ISpace newSpace); /// /// Called before an object is removed from its space. /// /// Space from which the object was removed. void OnRemovalFromSpace(ISpace oldSpace); /// /// Gets or sets the user data associated with this object. /// object Tag { get; set; } } } ================================================ FILE: BEPUphysics/Materials/IMaterialOwner.cs ================================================ namespace BEPUphysics.Materials { /// /// Defines an object that has a material. /// public interface IMaterialOwner { /// /// Gets or sets the material of the object. /// Material Material { get; set; } } } ================================================ FILE: BEPUphysics/Materials/InteractionProperties.cs ================================================ namespace BEPUphysics.Materials { /// /// Contains the blended friction and bounciness of a pair of objects. /// public struct InteractionProperties { /// /// Kinetic friction between the pair of objects. /// public float KineticFriction; /// /// Static friction between the pair of objects. /// public float StaticFriction; /// /// Bounciness between the pair of objects. /// public float Bounciness; } } ================================================ FILE: BEPUphysics/Materials/Material.cs ================================================ using System; namespace BEPUphysics.Materials { /// /// Material properties for collidable objects. /// public class Material { internal float kineticFriction = MaterialManager.DefaultKineticFriction; /// /// Gets or sets the friction coefficient used when the object is sliding quickly and /// no special material relationship is defined between the colliding objects. /// public float KineticFriction { get { return kineticFriction; } set { kineticFriction = value; if (MaterialChanged != null) MaterialChanged(this); } } internal float staticFriction = MaterialManager.DefaultStaticFriction; /// /// Gets or sets the friction coefficient used when the object is sliding slowly and /// no special material relationship is defined between the colliding objects. /// public float StaticFriction { get { return staticFriction; } set { staticFriction = value; if (MaterialChanged != null) MaterialChanged(this); } } internal float bounciness = MaterialManager.DefaultBounciness; /// /// Gets or sets the coefficient of restitution between the objects when /// no special material relationship is defined between the colliding objects. /// public float Bounciness { get { return bounciness; } set { bounciness = value; if (MaterialChanged != null) MaterialChanged(this); } } /// /// Gets or sets user data associated with the material. /// public object Tag { get; set; } int hashCode; /// /// Constructs a new material. /// public Material() { hashCode = (int)(((uint)GetHashCode()) * 19999999); } /// /// Constructs a new material. /// ///Static friction to use. ///Kinetic friction to use. ///Bounciness to use. public Material(float staticFriction, float kineticFriction, float bounciness) : this() { this.staticFriction = staticFriction; this.kineticFriction = kineticFriction; this.bounciness = bounciness; } /// /// Serves as a hash function for a particular type. /// /// /// A hash code for the current . /// /// 2 public override int GetHashCode() { return hashCode; } /// /// Fires when the material properties change. /// public event Action MaterialChanged; } } ================================================ FILE: BEPUphysics/Materials/MaterialManager.cs ================================================ using System; using System.Collections.Generic; namespace BEPUphysics.Materials { /// /// A function which takes two materials and computes the interaction properties between them. /// /// First material to blend. /// Second material to blend. /// Interaction properties between the two materials. public delegate void MaterialBlender(Material a, Material b, out InteractionProperties properties); /// /// Manages the relationship between materials. /// public static class MaterialManager { /// /// Delegate used to blend two materials together when there is no special handler defined in the MaterialInteractions dictionary. /// public static MaterialBlender MaterialBlender = DefaultMaterialBlender; /// /// Default coefficient of kinetic friction. /// Defaults to 0.8. /// public static float DefaultKineticFriction = .8f; /// /// Default coefficient of static friction. /// Defaults to 1. /// public static float DefaultStaticFriction = 1f; /// /// Default coefficient of restitution. /// Defaults to 0. /// public static float DefaultBounciness; static MaterialManager() { MaterialInteractions = new Dictionary(); } /// /// Computes the interaction properties between two materials. /// ///First material of the pair. ///Second material of the pair. ///Interaction properties between two materials. public static void GetInteractionProperties(Material materialA, Material materialB, out InteractionProperties properties) { MaterialBlender specialBlender; if (MaterialInteractions.TryGetValue(new MaterialPair(materialA, materialB), out specialBlender)) specialBlender(materialA, materialB, out properties); else MaterialBlender(materialA, materialB, out properties); } /// /// Gets or sets the material interactions dictionary. /// This dictionary contains all the special relationships between specific materials. /// These interaction properties will override properties obtained by normal blending. /// public static Dictionary MaterialInteractions { get; set; } /// /// Blender used to combine materials into a pair's interaction properties. /// /// Material associated with the first object to blend. /// Material associated with the second object to blend. /// Blended material values. public static void DefaultMaterialBlender(Material a, Material b, out InteractionProperties properties) { properties = new InteractionProperties { Bounciness = a.bounciness * b.bounciness, KineticFriction = a.kineticFriction * b.kineticFriction, StaticFriction = a.staticFriction * b.staticFriction }; } } } ================================================ FILE: BEPUphysics/Materials/MaterialPair.cs ================================================ using System; namespace BEPUphysics.Materials { /// /// A pair of materials. /// public struct MaterialPair :IEquatable { /// /// First material in the pair. /// public Material MaterialA; /// /// Second material in the pair. /// public Material MaterialB; /// /// Constructs a new material pair. /// ///First material in the pair. ///Second material in the pair. public MaterialPair(Material a, Material b) { MaterialA = a; MaterialB = b; } /// /// Returns the hash code for this instance. /// /// /// A 32-bit signed integer that is the hash code for this instance. /// /// 2 public override int GetHashCode() { return MaterialA.GetHashCode() + MaterialB.GetHashCode(); } /// /// Determines if the two material pairs have the same materials. /// /// Other pair to compare against. /// Whether or not the two pairs have the same materials. public bool Equals(MaterialPair other) { return (other.MaterialA == MaterialA && other.MaterialB == MaterialB) || (other.MaterialA == MaterialB && other.MaterialB == MaterialA); } } } ================================================ FILE: BEPUphysics/MultithreadedProcessingStage.cs ================================================ using System; using BEPUphysics.Threading; namespace BEPUphysics { /// /// Superclass of processing systems which can use multiple threads. /// public abstract class MultithreadedProcessingStage { /// /// Gets or sets whether or not the processing stage is active. /// public virtual bool Enabled { get; set; } /// /// Gets or sets whether or not the processing stage should allow multithreading. /// public bool AllowMultithreading { get; set; } /// /// Gets or sets the thread manager used by the stage. /// public IThreadManager ThreadManager { get; set; } /// /// Fires when the processing stage begins working. /// public event Action Starting; /// /// Fires when the processing stage finishes working. /// public event Action Finishing; protected bool ShouldUseMultithreading { get { return AllowMultithreading && ThreadManager != null && ThreadManager.ThreadCount > 1; } } #if PROFILE /// /// Gets the time elapsed in the previous execution of this stage, not including any hooked Starting or Finishing events. /// public double Time { get { return (end - start) / (double)Stopwatch.Frequency; } } long start, end; private void StartClock() { start = Stopwatch.GetTimestamp(); } private void StopClock() { end = Stopwatch.GetTimestamp(); } #endif /// /// Runs the processing stage. /// public void Update() { if (!Enabled) return; if (Starting != null) Starting(); #if PROFILE StartClock(); #endif if (ShouldUseMultithreading) { UpdateMultithreaded(); } else { UpdateSingleThreaded(); } #if PROFILE StopClock(); #endif if (Finishing != null) Finishing(); } protected abstract void UpdateMultithreaded(); protected abstract void UpdateSingleThreaded(); } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/NarrowPhase.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.Constraints; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.SolverSystems; using BEPUphysics.Threading; using BEPUutilities; using BEPUphysics.CollisionRuleManagement; using System.Collections.ObjectModel; using System.Diagnostics; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.DataStructures; namespace BEPUphysics.NarrowPhaseSystems { /// /// Pair of types. /// public struct TypePair : IEquatable { //Currently this requires some reflective labor. If perhaps the broad phase entries had some sort of 'id'... and they simply return that int through the interface. //Could be 'faster' assuming the supporting logic that creates the ids to begin with isn't too obtuse. /// /// First type in the pair. /// public Type A; /// /// Second type in the pair. /// public Type B; /// /// Constructs a new type pair. /// ///First type in the pair. ///Second type in the pair. public TypePair(Type a, Type b) { A = a; B = b; } /// /// Returns the hash code for this instance. /// /// /// A 32-bit signed integer that is the hash code for this instance. /// /// 2 public override int GetHashCode() { //TODO: Use old hash code system? return A.GetHashCode() + B.GetHashCode(); } #region IEquatable Members /// /// Indicates whether the current object is equal to another object of the same type. /// /// /// true if the current object is equal to the parameter; otherwise, false. /// /// An object to compare with this object. public bool Equals(TypePair other) { return (other.A == A && other.B == B) || (other.B == A && other.A == B); } #endregion } /// /// Manages and constructs pair handlers from broad phase overlaps. /// public class NarrowPhase : MultithreadedProcessingStage { RawList broadPhaseOverlaps; /// /// Gets or sets the list of broad phase overlaps used by the narrow phase to manage pairs. /// public RawList BroadPhaseOverlaps { get { return broadPhaseOverlaps; } set { broadPhaseOverlaps = value; } } Dictionary overlapMapping = new Dictionary(); RawList narrowPhasePairs = new RawList(); /// /// Gets the list of Pairs managed by the narrow phase. /// public ReadOnlyList Pairs { get { return new ReadOnlyList(narrowPhasePairs); } } /// /// Gets or sets the time step settings used by the narrow phase. /// public TimeStepSettings TimeStepSettings { get; set; } /// /// Gets or sets the solver into which the narrow phase will put solver updateables generated by narrow phase pairs. /// public Solver Solver { get; set; } ConcurrentDeque newNarrowPhasePairs = new ConcurrentDeque(); /// /// Constructs a new narrow phase. /// ///Time step settings used by the narrow phase. public NarrowPhase(TimeStepSettings timeStepSettings) { TimeStepSettings = timeStepSettings; updateBroadPhaseOverlapDelegate = UpdateBroadPhaseOverlap; Enabled = true; } /// /// Constructs a new narrow phase. /// ///Time step settings used by the narrow phase. /// Overlaps list used by the narrow phase to create pairs. public NarrowPhase(TimeStepSettings timeStepSettings, RawList overlaps) : this(timeStepSettings) { broadPhaseOverlaps = overlaps; } /// /// Constructs a new narrow phase. /// ///Time step settings used by the narrow phase. /// Overlaps list used by the narrow phase to create pairs. /// Thread manager used by the narrow phase. public NarrowPhase(TimeStepSettings timeStepSettings, RawList overlaps, IThreadManager threadManager) : this(timeStepSettings, overlaps) { ThreadManager = threadManager; AllowMultithreading = true; } Action updateBroadPhaseOverlapDelegate; void UpdateBroadPhaseOverlap(int i) { BroadPhaseOverlap overlap = broadPhaseOverlaps.Elements[i]; if (overlap.collisionRule < CollisionRule.NoNarrowPhasePair) { NarrowPhasePair pair; //see if the overlap is already present in the narrow phase. if (!overlapMapping.TryGetValue(overlap, out pair)) { //Create/enqueue based on collision table pair = NarrowPhaseHelper.GetPairHandler(ref overlap); if (pair != null) { pair.NarrowPhase = this; //Add the new object to the 'todo' list. //Technically, this doesn't need to be thread-safe when this is called from the sequential context. //It's just bunched together for maintainability despite the slight performance hit. newNarrowPhasePairs.Enqueue(pair); } } if (pair != null) { //Update the collision rule. pair.CollisionRule = overlap.collisionRule; if (pair.BroadPhaseOverlap.collisionRule < CollisionRule.NoNarrowPhaseUpdate) { pair.UpdateCollision(TimeStepSettings.TimeStepDuration); } pair.NeedsUpdate = false; } } } #if PROFILE /// /// Gets the time used in updating the pair handler states. /// public double PairUpdateTime { get { return (endPairs - startPairs) / (double)Stopwatch.Frequency; } } /// /// Gets the time used in flushing new pairs into the simulation. /// public double FlushNewPairsTime { get { return (endFlushNew - endPairs) / (double)Stopwatch.Frequency; } } /// /// Gets the time used in flushing changes to solver updateables managed by the pairs. /// public double FlushSolverUpdateableChangesTime { get { return (endFlushSolverUpdateables - endFlushNew) / (double)Stopwatch.Frequency; } } /// /// Gets the time used in scanning for out of date pairs. /// public double StaleOverlapRemovalTime { get { return (endStale - endFlushSolverUpdateables) / (double)Stopwatch.Frequency; } } private long startPairs; private long endPairs; private long endFlushNew; private long endFlushSolverUpdateables; private long endStale; #endif protected override void UpdateMultithreaded() { #if PROFILE startPairs = Stopwatch.GetTimestamp(); #endif ThreadManager.ForLoop(0, broadPhaseOverlaps.Count, updateBroadPhaseOverlapDelegate); #if PROFILE endPairs = Stopwatch.GetTimestamp(); #endif //The new narrow phase objects should be flushed before we get to the stale overlaps, or else the NeedsUpdate property might get into an invalid state. AddNewNarrowPhaseObjects(); #if PROFILE endFlushNew = Stopwatch.GetTimestamp(); #endif //Flush away every change accumulated since the last flush. FlushGeneratedSolverUpdateables(); #if PROFILE endFlushSolverUpdateables = Stopwatch.GetTimestamp(); #endif //By the time we get here, there's no more pending items in the queue, so the overlaps //can be removed directly by the stale loop. RemoveStaleOverlaps(); #if PROFILE endStale = Stopwatch.GetTimestamp(); #endif } protected override void UpdateSingleThreaded() { #if PROFILE startPairs = Stopwatch.GetTimestamp(); #endif int count = broadPhaseOverlaps.Count; for (int i = 0; i < count; i++) { UpdateBroadPhaseOverlap(i); } #if PROFILE endPairs = Stopwatch.GetTimestamp(); #endif //The new narrow phase objects should be flushed before we get to the stale overlaps, or else the NeedsUpdate property might get into an invalid state. AddNewNarrowPhaseObjects(); #if PROFILE endFlushNew = Stopwatch.GetTimestamp(); #endif //Flush away every change accumulated since the last flush. FlushGeneratedSolverUpdateables(); #if PROFILE endFlushSolverUpdateables = Stopwatch.GetTimestamp(); #endif //By the time we get here, there's no more pending items in the queue, so the overlaps //can be removed directly by the stale loop. RemoveStaleOverlaps(); #if PROFILE endStale = Stopwatch.GetTimestamp(); #endif } void RemoveStaleOverlaps() { //We don't need to do any synchronization or queueing here; just remove everything directly. ApplySolverUpdateableChangesDirectly = true; //Remove stale objects. for (int i = narrowPhasePairs.Count - 1; i >= 0; i--) { var pair = narrowPhasePairs.Elements[i]; //A stale overlap is a pair which has not been updated, but not because of inactivity. //Pairs between two inactive shapes are not updated because the broad phase does not output overlaps //between inactive entries. We need to keep such pairs around, otherwise when they wake up, lots of extra work //will be needed and quality will suffer. //The classic stale overlap is two objects which have moved apart. Because the bounding boxes no longer overlap, //the broad phase does not generate an overlap for them. Obviously, we should get rid of such a pair. //Any such pair will have at least one active member. Having velocity requires activity and teleportation will activate the object. //There's another sneaky kind of stale overlap. Consider a sleeping dynamic object on a Terrain. The Terrain, being a static object, //is considered inactive. The sleeping dynamic object is also inactive. Now, remove the sleeping dynamic object. //Both objects are still considered inactive. But the pair is clearly stale- one of its members doesn't even exist anymore! //This has nasty side effects, like retaining memory. To solve this, also check to see if either member does not belong to the simulation. if (pair.NeedsUpdate && //If we didn't receive an update in the previous narrow phase run and... (pair.broadPhaseOverlap.entryA.IsActive || pair.broadPhaseOverlap.entryB.IsActive || //one of us is active or.. pair.broadPhaseOverlap.entryA.BroadPhase == null || pair.broadPhaseOverlap.entryB.BroadPhase == null)) //one of us doesn't exist anymore... { //Get rid of the pair! if (RemovingPair != null) RemovingPair(pair); narrowPhasePairs.FastRemoveAt(i); overlapMapping.Remove(pair.BroadPhaseOverlap); //The clean up will issue an order to get rid of the solver updateable if it is active. //To avoid a situation where the solver updateable outlives the pair but is available for re-use //because of the factory giveback here, the updateable is removed directly (ApplySolverUpdateableChangesDirectly = true). pair.CleanUp(); pair.Factory.GiveBack(pair); } else { pair.NeedsUpdate = true; } } ApplySolverUpdateableChangesDirectly = false; } void AddNewNarrowPhaseObjects() { //Add new narrow phase objects. This will typically be a very tiny phase. NarrowPhasePair narrowPhaseObject; while (newNarrowPhasePairs.TryUnsafeDequeueFirst(out narrowPhaseObject)) { narrowPhasePairs.Add(narrowPhaseObject); OnCreatePair(narrowPhaseObject); } } /// /// Gets the pair between two broad phase entries, if any. /// ///First entry in the pair. ///Second entry in the pair. ///The pair if it exists, null otherwise. public NarrowPhasePair GetPair(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { NarrowPhasePair toReturn; overlapMapping.TryGetValue(new BroadPhaseOverlap(entryA, entryB), out toReturn); return toReturn; } protected void OnCreatePair(NarrowPhasePair pair) { overlapMapping.Add(pair.BroadPhaseOverlap, pair); pair.OnAddedToNarrowPhase(); if (CreatingPair != null) CreatingPair(pair); } /// /// Fires when the narrow phase creates a pair. /// public event Action CreatingPair; /// /// Fires when the narrow phase removes a pair. /// public event Action RemovingPair; ConcurrentDeque solverUpdateableChanges = new ConcurrentDeque(); /// /// If true, solver updateables added and removed from narrow phase pairs will be added directly to the solver /// without any synchronization or queueing. /// bool ApplySolverUpdateableChangesDirectly { get; set; } /// /// Enqueues a solver updateable created by some pair for flushing into the solver later. /// ///Updateable to add. public void NotifyUpdateableAdded(SolverUpdateable addedItem) { if (ApplySolverUpdateableChangesDirectly) { Solver.Add(addedItem); } else { solverUpdateableChanges.Enqueue(new SolverUpdateableChange(true, addedItem)); } } /// /// Enqueues a solver updateable removed by some pair for flushing into the solver later. /// ///Solver updateable to remove. public void NotifyUpdateableRemoved(SolverUpdateable removedItem) { if (ApplySolverUpdateableChangesDirectly) { Solver.Remove(removedItem); } else { solverUpdateableChanges.Enqueue(new SolverUpdateableChange(false, removedItem)); } } /// /// Flushes the new solver updateables into the solver. /// They are 'flux' updateables, so this uses the solver's flux add method. /// public void FlushGeneratedSolverUpdateables() { SolverUpdateableChange change; while (solverUpdateableChanges.TryUnsafeDequeueFirst(out change)) { if (change.ShouldAdd) { //It is technically possible for a constraint to be added twice, if certain systems interfere. //The character controller is one such system. //We should check the new constraint's solver status before adding it here. if (change.Item.solver == null) { Solver.Add(change.Item); } } else { if (change.Item.solver != null) { Solver.Remove(change.Item); } } } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/NarrowPhaseHelper.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUutilities.DataStructures; using BEPUphysics.UpdateableSystems; namespace BEPUphysics.NarrowPhaseSystems { /// /// Contains the various factories that are used by default in the engine. /// public class Factories { /// /// Gets the factory for the box-box case. /// public NarrowPhasePairFactory BoxBox { get; private set; } /// /// Gets the factory for the box-sphere case. /// public NarrowPhasePairFactory BoxSphere { get; private set; } /// /// Gets the factory for the sphere-sphere case. /// public NarrowPhasePairFactory SphereSphere { get; private set; } /// /// Gets the factory for the convex-convex case. This works for any two convexes, though some other special cases (e.g. box-box) supersede it. /// public NarrowPhasePairFactory ConvexConvex { get; private set; } /// /// Gets the factory for the triangle-convex case. /// public NarrowPhasePairFactory TriangleConvex { get; private set; } /// /// Gets the factory for the compound-convex case. /// public NarrowPhasePairFactory CompoundConvex { get; private set; } /// /// Gets the factory for the compound-compound case. /// public NarrowPhasePairFactory CompoundCompound { get; private set; } /// /// Gets the factory for the compound-static mesh case. /// public NarrowPhasePairFactory CompoundStaticMesh { get; private set; } /// /// Gets the factory for the compound-terrain case. /// public NarrowPhasePairFactory CompoundTerrain { get; private set; } /// /// Gets the factory for the compound-instanced mesh case. /// public NarrowPhasePairFactory CompoundInstancedMesh { get; private set; } /// /// Gets the factory for the compound-mobile mesh case. /// public NarrowPhasePairFactory CompoundMobileMesh { get; private set; } /// /// Gets the factory for the static mesh-convex case. /// public NarrowPhasePairFactory StaticMeshConvex { get; private set; } /// /// Gets the factory for the static mesh-sphere case. /// public NarrowPhasePairFactory StaticMeshSphere { get; private set; } /// /// Gets the factory for the terrain-convex case. /// public NarrowPhasePairFactory TerrainConvex { get; private set; } /// /// Gets the factory for the terrain-sphere case. /// public NarrowPhasePairFactory TerrainSphere { get; private set; } /// /// Gets the factory for the instanced mesh-convex case. /// public NarrowPhasePairFactory InstancedMeshConvex { get; private set; } /// /// Gets the factory for the instanced mesh-sphere case. /// public NarrowPhasePairFactory InstancedMeshSphere { get; private set; } /// /// Gets the factory for the mobile mesh-convex case. /// public NarrowPhasePairFactory MobileMeshConvex { get; private set; } /// /// Gets the factory for the mobile mesh-sphere case. /// public NarrowPhasePairFactory MobileMeshSphere { get; private set; } /// /// Gets the factory for the mobile mesh-triangle case. /// public NarrowPhasePairFactory MobileMeshTriangle { get; private set; } /// /// Gets the factory for the mobile mesh-static mesh case. /// public NarrowPhasePairFactory MobileMeshStaticMesh { get; private set; } /// /// Gets the factory for the mobile mesh-instanced mesh case. /// public NarrowPhasePairFactory MobileMeshInstancedMesh { get; private set; } /// /// Gets the factory for the mobile mesh-terrain case. /// public NarrowPhasePairFactory MobileMeshTerrain { get; private set; } /// /// Gets the factory for the mobile mesh-mobile mesh case. /// public NarrowPhasePairFactory MobileMeshMobileMesh { get; private set; } /// /// Gets the factory for the static group-convex case. /// public NarrowPhasePairFactory StaticGroupConvex { get; private set; } /// /// Gets the factory for the static group-compound case. /// public NarrowPhasePairFactory StaticGroupCompound { get; private set; } /// /// Gets the factory for the static group-mobile mesh case. /// public NarrowPhasePairFactory StaticGroupMobileMesh { get; private set; } /// /// Gets the factory for the detector volume-convex case. /// public NarrowPhasePairFactory DetectorVolumeConvex { get; private set; } /// /// Gets the factory for the detector volume-mobile mesh case. /// public NarrowPhasePairFactory DetectorVolumeMobileMesh { get; private set; } /// /// Gets the factory for the detector volume-compound case. /// public NarrowPhasePairFactory DetectorVolumeCompound { get; private set; } RawList factories = new RawList(); /// /// Gets a collection of all the default factories. /// public ReadOnlyList All { get { return new ReadOnlyList(factories); } } /// /// Constructs all factories. /// public Factories() { factories.Add(BoxBox = new NarrowPhasePairFactory()); factories.Add(BoxSphere = new NarrowPhasePairFactory()); factories.Add(SphereSphere = new NarrowPhasePairFactory()); factories.Add(ConvexConvex = new NarrowPhasePairFactory()); factories.Add(TriangleConvex = new NarrowPhasePairFactory()); factories.Add(CompoundConvex = new NarrowPhasePairFactory()); factories.Add(CompoundCompound = new NarrowPhasePairFactory()); factories.Add(CompoundStaticMesh = new NarrowPhasePairFactory()); factories.Add(CompoundTerrain = new NarrowPhasePairFactory()); factories.Add(CompoundInstancedMesh = new NarrowPhasePairFactory()); factories.Add(CompoundMobileMesh = new NarrowPhasePairFactory()); factories.Add(StaticMeshConvex = new NarrowPhasePairFactory()); factories.Add(StaticMeshSphere = new NarrowPhasePairFactory()); factories.Add(TerrainConvex = new NarrowPhasePairFactory()); factories.Add(TerrainSphere = new NarrowPhasePairFactory()); factories.Add(InstancedMeshConvex = new NarrowPhasePairFactory()); factories.Add(InstancedMeshSphere = new NarrowPhasePairFactory()); factories.Add(MobileMeshConvex = new NarrowPhasePairFactory()); factories.Add(MobileMeshSphere = new NarrowPhasePairFactory()); factories.Add(MobileMeshTriangle = new NarrowPhasePairFactory()); factories.Add(MobileMeshStaticMesh = new NarrowPhasePairFactory()); factories.Add(MobileMeshInstancedMesh = new NarrowPhasePairFactory()); factories.Add(MobileMeshTerrain = new NarrowPhasePairFactory()); factories.Add(MobileMeshMobileMesh = new NarrowPhasePairFactory()); factories.Add(StaticGroupConvex = new NarrowPhasePairFactory()); factories.Add(StaticGroupCompound = new NarrowPhasePairFactory()); factories.Add(StaticGroupMobileMesh = new NarrowPhasePairFactory()); factories.Add(DetectorVolumeConvex = new NarrowPhasePairFactory()); factories.Add(DetectorVolumeMobileMesh = new NarrowPhasePairFactory()); factories.Add(DetectorVolumeCompound = new NarrowPhasePairFactory()); } } /// /// Contains the collision managers dictionary and other helper methods for creating pairs. /// public static class NarrowPhaseHelper { /// /// Gets the factories used by default to construct various pair types in the narrow phase. /// These do not necessarily reflect the state of the narrow phase helper's CollisionManagers dictionary /// if changes are made to its entries. /// public static Factories Factories { get; private set; } static NarrowPhaseHelper() { Factories = new NarrowPhaseSystems.Factories(); collisionManagers = new Dictionary(); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.BoxBox); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.BoxSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.SphereSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(ConvexCollidable)), Factories.TriangleConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(StaticMesh)), Factories.StaticMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(Terrain)), Factories.TerrainConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(InstancedMesh)), Factories.InstancedMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(CompoundCollidable)), Factories.CompoundConvex); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(CompoundCollidable)), Factories.CompoundCompound); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(StaticMesh)), Factories.CompoundStaticMesh); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(Terrain)), Factories.CompoundTerrain); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(InstancedMesh)), Factories.CompoundInstancedMesh); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshSphere); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshTriangle); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshConvex); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(MobileMeshCollidable)), Factories.CompoundMobileMesh); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(StaticMesh)), Factories.MobileMeshStaticMesh); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(InstancedMesh)), Factories.MobileMeshInstancedMesh); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(Terrain)), Factories.MobileMeshTerrain); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(MobileMeshCollidable)), Factories.MobileMeshMobileMesh); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(TriangleCollidable)), Factories.MobileMeshTriangle); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(ConvexCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(TriangleCollidable), typeof(StaticGroup)), Factories.StaticGroupConvex); collisionManagers.Add(new TypePair(typeof(CompoundCollidable), typeof(StaticGroup)), Factories.StaticGroupCompound); collisionManagers.Add(new TypePair(typeof(MobileMeshCollidable), typeof(StaticGroup)), Factories.StaticGroupMobileMesh); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(ConvexCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(TriangleCollidable)), Factories.DetectorVolumeConvex); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(MobileMeshCollidable)), Factories.DetectorVolumeMobileMesh); collisionManagers.Add(new TypePair(typeof(DetectorVolume), typeof(CompoundCollidable)), Factories.DetectorVolumeCompound); } internal static Dictionary collisionManagers; /// /// Gets or sets the dictionary that defines the factory to use for various type pairs. /// public static Dictionary CollisionManagers { get { return collisionManagers; } set { collisionManagers = value; } } /// /// Gets a narrow phase pair for a given broad phase overlap. /// ///Overlap to use to create the pair. ///A INarrowPhasePair for the overlap. public static NarrowPhasePair GetPairHandler(ref BroadPhaseOverlap pair) { NarrowPhasePairFactory factory; if (collisionManagers.TryGetValue(new TypePair(pair.entryA.GetType(), pair.entryB.GetType()), out factory)) { var toReturn = factory.GetNarrowPhasePair(); toReturn.BroadPhaseOverlap = pair; toReturn.Factory = factory; return toReturn; } //Convex-convex collisions are a pretty significant chunk of all tests, so rather than defining them all, just have a fallback. var a = pair.entryA as ConvexCollidable; var b = pair.entryB as ConvexCollidable; if (a != null && b != null) { NarrowPhasePair toReturn = Factories.ConvexConvex.GetNarrowPhasePair(); toReturn.BroadPhaseOverlap = pair; toReturn.Factory = Factories.ConvexConvex; return toReturn; } return null; } /// /// Gets a narrow phase pair for a given pair of entries. /// ///First entry in the pair. /// Second entry in the pair. /// Collision rule governing the pair. ///A NarrowPhasePair for the overlap. public static NarrowPhasePair GetPairHandler(BroadPhaseEntry entryA, BroadPhaseEntry entryB, CollisionRule rule) { var overlap = new BroadPhaseOverlap(entryA, entryB, rule); return GetPairHandler(ref overlap); } /// /// Gets a narrow phase pair for a given pair of entries. /// ///First entry in the pair. /// Second entry in the pair. ///AINarrowPhasePair for the overlap. public static NarrowPhasePair GetPairHandler(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { var overlap = new BroadPhaseOverlap(entryA, entryB); return GetPairHandler(ref overlap); } /// /// Gets a collidable pair handler for a pair of collidables. /// /// Pair of collidables to use to create the pair handler. /// Collision rule governing the pair. /// CollidablePairHandler for the pair. public static CollidablePairHandler GetPairHandler(ref CollidablePair pair, CollisionRule rule) { var overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB, rule); return GetPairHandler(ref overlap) as CollidablePairHandler; } /// /// Gets a collidable pair handler for a pair of collidables. /// /// Pair of collidables to use to create the pair handler. /// CollidablePairHandler for the pair. public static CollidablePairHandler GetPairHandler(ref CollidablePair pair) { var overlap = new BroadPhaseOverlap(pair.collidableA, pair.collidableB); return GetPairHandler(ref overlap) as CollidablePairHandler; } /// /// Tests the pair of collidables for intersection without regard for collision rules. /// /// Pair to test. /// Whether or not the pair is intersecting. public static bool Intersecting(ref CollidablePair pair) { var pairHandler = GetPairHandler(ref pair); if (pairHandler == null) return false; pairHandler.SuppressEvents = true; pairHandler.UpdateCollision(0); //Technically, contacts with negative depth do not count. //The current implementation of collision detection does not generate //negative depths on the first execution of UpdateCollision, though, //so we don't need to worry about that- yet. bool toReturn = pairHandler.ContactCount > 0; pairHandler.SuppressEvents = false; pairHandler.CleanUp(); pairHandler.Factory.GiveBack(pairHandler); return toReturn; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/NarrowPhasePairFactory.cs ================================================ using BEPUphysics.NarrowPhaseSystems.Pairs; using System; using BEPUutilities.ResourceManagement; namespace BEPUphysics.NarrowPhaseSystems { /// /// Superclass of the generic typed NarrowPhasePairFactory. Offers interaction with the factory on a INarrowPhasePair level. /// public abstract class NarrowPhasePairFactory { /// /// Manufactures and returns a narrow phase pair for the given overlap. /// ///Narrow phase pair. public abstract NarrowPhasePair GetNarrowPhasePair(); /// /// Returns a pair to the factory for re-use. /// /// Pair to return. public abstract void GiveBack(NarrowPhasePair pair); /// /// Gets or sets the number of elements in the pair factory that are ready to take. /// If the factory runs out, it will construct new instances to give away (unless AllowOnDemandConstruction is set to false). /// public abstract int Count { get; set; } protected bool allowOnDemandConstruction = true; /// /// Gets or sets whether or not to allow the factory to create additional instances when it runs /// out of its initial set. Defaults to true. /// public bool AllowOnDemandConstruction { get { return allowOnDemandConstruction; } set { allowOnDemandConstruction = value; } } /// /// Ensures that the factory has at least the given number of elements ready to take. /// /// Minimum number of elements to ensure in the factory. public void EnsureCount(int minimumCount) { if (Count < minimumCount) Count = minimumCount; } /// /// Ensures that the factory has at most the given number of elements ready to take. /// /// Maximum number of elements to allow in the factory. public void CapCount(int maximumCount) { if (Count > maximumCount) Count = maximumCount; } /// /// Removes all elements from the factory. /// public abstract void Clear(); } /// /// Manufactures a given type of narrow phase pairs. /// /// Type of the pair to manufacture. public class NarrowPhasePairFactory : NarrowPhasePairFactory where T : NarrowPhasePair, new() { LockingResourcePool pool = new LockingResourcePool(); /// /// Get a resource from the factory. /// /// A pair from the factory. public override NarrowPhasePair GetNarrowPhasePair() { if (!allowOnDemandConstruction && pool.Count == 0) throw new InvalidOperationException("Cannot request additional resources from this factory; it is exhausted. Consider specifying a greater number of initial resources or setting AllowOnDemandConstruction to true."); var pair = pool.Take(); pair.NeedsUpdate = true; return pair; } /// /// Give a resource back to the factory. /// /// Pair to return. public override void GiveBack(NarrowPhasePair pair) { pair.NarrowPhase = null; pool.GiveBack((T)pair); } /// /// Gets or sets the number of elements in the pair factory that are ready to take. /// If the factory runs out, it will construct new instances to give away (unless AllowOnDemandConstruction is set to false). /// public override int Count { get { return pool.Count; } set { pool.Initialize(value); } } /// /// Removes all elements from the factory. /// public override void Clear() { pool.Clear(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/BoxPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Pair handler that manages a pair of two boxes. /// public class BoxPairHandler : ConvexConstraintPairHandler { ConvexCollidable boxA; ConvexCollidable boxB; BoxContactManifold contactManifold = new BoxContactManifold(); public override Collidable CollidableA { get { return boxA; } } public override Collidable CollidableB { get { return boxB; } } public override Entities.Entity EntityA { get { return boxA.entity; } } public override Entities.Entity EntityB { get { return boxB.entity; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return contactManifold; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { boxA = entryA as ConvexCollidable; boxB = entryB as ConvexCollidable; if (boxA == null || boxB == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); boxA = null; boxB = null; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/BoxSpherePairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a box and sphere in a collision. /// public class BoxSpherePairHandler : ConvexPairHandler { ConvexCollidable box; ConvexCollidable sphere; //Using a non-convex one since they have slightly lower overhead than their Convex friends when dealing with a single contact point. BoxSphereContactManifold contactManifold = new BoxSphereContactManifold(); NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return box; } } public override Collidable CollidableB { get { return sphere; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return contactManifold; } } public override Entities.Entity EntityA { get { return box.entity; } } public override Entities.Entity EntityB { get { return sphere.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { box = entryA as ConvexCollidable; sphere = entryB as ConvexCollidable; if (box == null || sphere == null) { box = entryB as ConvexCollidable; sphere = entryA as ConvexCollidable; if (box == null || sphere == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } //Reorder the entries so that the guarantee that the normal points from A to B is satisfied. broadPhaseOverlap.entryA = box; broadPhaseOverlap.entryB = sphere; base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); box = null; sphere = null; } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = ContactManifold.contacts.Elements[index]; //Find the contact's force. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity Vector3 velocity; if (EntityA != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity); Vector3.Cross(ref EntityA.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); if (EntityB != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity); Vector3.Cross(ref EntityB.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityB.linearVelocity, out velocity); Vector3.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CollidablePairHandler.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.Entities; using BEPUphysics.Constraints.Collision; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.CollisionTests; using System; using BEPUphysics.Constraints.SolverGroups; using BEPUphysics.Materials; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of pairs between collidables that generate contact points. /// public abstract class CollidablePairHandler : NarrowPhasePair { /// /// Gets the first collidable associated with the pair. /// public abstract Collidable CollidableA { get; } /// /// Gets the second collidable associated with the pair. /// public abstract Collidable CollidableB { get; } //Entities could be null! /// /// Gets the first entity associated with the pair. This could be null if no entity is associated with CollidableA. /// public abstract Entity EntityA { get; } /// /// Gets the second entity associated with the pair. This could be null if no entity is associated with CollidableB. /// public abstract Entity EntityB { get; } /// /// Index of this pair in CollidableA's pairs list. /// internal int listIndexA = -1; /// /// Index of this pair in CollidableB's pairs list. /// internal int listIndexB = -1; protected internal abstract int ContactCount { get; } protected internal int previousContactCount; protected CollidablePairHandler() { Contacts = new ContactCollection(this); } protected internal float timeOfImpact = 1; /// /// Gets the last computed time of impact of the pair handler. /// This is only computed when one of the members is a continuously /// updated object. /// public float TimeOfImpact { get { return timeOfImpact; } } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public abstract void UpdateTimeOfImpact(Collidable requester, float dt); protected bool suppressEvents; /// /// Gets or sets whether or not to suppress events from this pair handler. /// public bool SuppressEvents { get { return suppressEvents; } set { suppressEvents = value; } } /// /// Gets or sets the parent of this pair handler. /// Pairs with parents report to their parents various /// changes in state. This is mainly used to support /// hierarchies of pairs for compound collisions. /// public IPairHandlerParent Parent { get; set; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Child initialization is responsible for setting up the entries. //Child initialization is responsible for setting up the material. //Child initialization is responsible for setting up the manifold. //Child initialization is responsible for setting up the constraint. if (!suppressEvents) { CollidableA.EventTriggerer.OnPairCreated(CollidableB, this); CollidableB.EventTriggerer.OnPairCreated(CollidableA, this); } } /// /// Called when the pair handler is added to the narrow phase. /// protected internal override void OnAddedToNarrowPhase() { CollidableA.AddPair(this, ref listIndexA); CollidableB.AddPair(this, ref listIndexB); } protected virtual void OnContactAdded(Contact contact) { contact.Validate(); //Children manage the addition of the contact to the constraint, if any. if (!suppressEvents) { CollidableA.EventTriggerer.OnContactCreated(CollidableB, this, contact); CollidableB.EventTriggerer.OnContactCreated(CollidableA, this, contact); } if (Parent != null) Parent.OnContactAdded(contact); } protected virtual void OnContactRemoved(Contact contact) { //Children manage the removal of the contact from the constraint, if any. if (!suppressEvents) { CollidableA.EventTriggerer.OnContactRemoved(CollidableB, this, contact); CollidableB.EventTriggerer.OnContactRemoved(CollidableA, this, contact); } if (Parent != null) Parent.OnContactRemoved(contact); } /// /// Cleans up the pair handler. /// public override void CleanUp() { //Child types remove contacts from the pair handler and call OnContactRemoved. //Child types manage the removal of the constraint from the space, if necessary. //If the contact manifold had any contacts in it on cleanup, then we still need to fire the 'ending' event. if (previousContactCount > 0 && !suppressEvents) { CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this); CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this); } //Remove this pair from each collidable. This can be done safely because the CleanUp is called sequentially. //However, only do it if we have been added to the collidables! This does not happen until this pair is added to the narrow phase. //For pairs which never get added to the broad phase, such as those in queries, we should not attempt to remove something that isn't there! if (listIndexA != -1) { CollidableA.RemovePair(this, ref listIndexA); CollidableB.RemovePair(this, ref listIndexB); } //Notify the colliders that the pair went away. if (!suppressEvents) { CollidableA.EventTriggerer.OnPairRemoved(CollidableB); CollidableB.EventTriggerer.OnPairRemoved(CollidableA); } broadPhaseOverlap = new BroadPhaseOverlap(); suppressEvents = false; timeOfImpact = 1; Parent = null; previousContactCount = 0; //Child cleanup is responsible for cleaning up direct references to the involved collidables. //Child cleanup is responsible for cleaning up contact manifolds. } /// /// Forces an update of the pair's material properties. /// /// Properties to use in the collision. public abstract void UpdateMaterialProperties(InteractionProperties properties); /// /// Forces an update of the pair's material properties. /// /// First material to use. /// Second material to use. public abstract void UpdateMaterialProperties(Material materialA, Material materialB); /// /// Forces an update of the pair's material properties. /// Uses default choices (such as the owning entities' materials). /// public void UpdateMaterialProperties() { UpdateMaterialProperties(null, null); } protected internal abstract void GetContactInformation(int index, out ContactInformation info); /// /// Gets a list of the contacts in the pair and their associated constraint information. /// public ContactCollection Contacts { get; private set; } /// /// Gets whether or not this pair has any contacts in it with nonnegative penetration depths. /// Such a contact would imply the pair of objects is actually colliding. /// public bool Colliding { get { foreach (var contact in Contacts) { if (contact.Contact.PenetrationDepth >= 0) return true; } return false; } } /// /// Forces the pair handler to clean out its contacts. /// public virtual void ClearContacts() { previousContactCount = 0; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundConvexPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound and convex collision pair. /// public class CompoundConvexPairHandler : CompoundGroupPairHandler { ConvexCollidable convexInfo; public override Collidable CollidableB { get { return convexInfo; } } public override Entities.Entity EntityB { get { return convexInfo.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { convexInfo = entryA as ConvexCollidable; if (convexInfo == null) { convexInfo = entryB as ConvexCollidable; if (convexInfo == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); convexInfo = null; } protected override void UpdateContainedPairs() { var overlappedElements = PhysicsResources.GetCompoundChildList(); compoundInfo.hierarchy.Tree.GetOverlaps(convexInfo.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i].CollisionInformation, CollidableB, overlappedElements.Elements[i].Material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundGroupPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound and group collision pair. /// public abstract class CompoundGroupPairHandler : GroupPairHandler { protected CompoundCollidable compoundInfo; public override Collidable CollidableA { get { return compoundInfo; } } public override Entities.Entity EntityA { get { return compoundInfo.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Other member of the pair is initialized by the child. compoundInfo = entryA as CompoundCollidable; if (compoundInfo == null) { compoundInfo = entryB as CompoundCollidable; if (compoundInfo == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); compoundInfo = null; //Child type needs to null out other reference. } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundInstancedMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-instanced mesh collision pair. /// public class CompoundInstancedMeshPairHandler : CompoundGroupPairHandler { InstancedMesh mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return null; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as InstancedMesh; if (mesh == null) { mesh = entryB as InstancedMesh; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs() { //Could go other way; get triangles in mesh that overlap the compound. //Could be faster sometimes depending on the way it's set up. var overlappedElements = PhysicsResources.GetCompoundChildList(); compoundInfo.hierarchy.Tree.GetOverlaps(mesh.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i].CollisionInformation, mesh, overlappedElements.Elements[i].Material, mesh.material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundMobileMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-instanced mesh collision pair. /// public class CompoundMobileMeshPairHandler : CompoundGroupPairHandler { MobileMeshCollidable mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return mesh.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as MobileMeshCollidable; if (mesh == null) { mesh = entryB as MobileMeshCollidable; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs() { //Could go other way; get triangles in mesh that overlap the compound. //Could be faster sometimes depending on the way it's set up. var overlappedElements = PhysicsResources.GetCompoundChildList(); compoundInfo.hierarchy.Tree.GetOverlaps(mesh.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i].CollisionInformation, mesh, overlappedElements.Elements[i].Material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundPairHandler.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Constraints; using BEPUphysics.Constraints.Collision; using BEPUphysics.DataStructures; using BEPUutilities.DataStructures; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.CollisionTests; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-compound collision pair. /// public class CompoundPairHandler : CompoundGroupPairHandler { CompoundCollidable compoundInfoB; public override Collidable CollidableB { get { return compoundInfoB; } } public override Entities.Entity EntityB { get { return compoundInfoB.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { compoundInfoB = entryB as CompoundCollidable; if (compoundInfoB == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); compoundInfoB = null; } //Some danger of unintuitive-to-address allocations here. If these lists get huge, the user will see some RawList<<>> goofiness in the profiler. //They can still address it by clearing out the cached pair factories though. RawList> overlappedElements = new RawList>(); protected override void UpdateContainedPairs() { compoundInfo.hierarchy.Tree.GetOverlaps(compoundInfoB.hierarchy.Tree, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { var element = overlappedElements.Elements[i]; TryToAdd(element.OverlapA.CollisionInformation, element.OverlapB.CollisionInformation, element.OverlapA.Material, element.OverlapB.Material); } overlappedElements.Clear(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundStaticMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-static mesh collision pair. /// public class CompoundStaticMeshPairHandler : CompoundGroupPairHandler { StaticMesh mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return null; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as StaticMesh; if (mesh == null) { mesh = entryB as StaticMesh; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs() { //TODO: Static triangle meshes have a worldspace hierarchy that could be more efficiently traversed with a tree vs tree test. //This is just a lot simpler to manage in the short term. var overlappedElements = PhysicsResources.GetCompoundChildList(); compoundInfo.hierarchy.Tree.GetOverlaps(mesh.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i].CollisionInformation, mesh, overlappedElements.Elements[i].Material, mesh.material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/CompoundTerrainPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-terrain collision pair. /// public class CompoundTerrainPairHandler : CompoundGroupPairHandler { Terrain terrain; public override Collidable CollidableB { get { return terrain; } } public override Entities.Entity EntityB { get { return null; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { terrain = entryA as Terrain; if (terrain == null) { terrain = entryB as Terrain; if (terrain == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); terrain = null; } protected override void UpdateContainedPairs() { //Could go other way; get triangles in mesh that overlap the compound. //Could be faster sometimes depending on the way it's set up. var overlappedElements = PhysicsResources.GetCompoundChildList(); compoundInfo.hierarchy.Tree.GetOverlaps(terrain.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i].CollisionInformation, terrain, overlappedElements.Elements[i].Material, terrain.Material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/ContactCollection.cs ================================================ using System; using System.Collections.Generic; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Convenience collection of contacts and their associated data. /// public class ContactCollection : IList { /// /// Enumerator for the contact collection. /// public struct Enumerator : IEnumerator { ContactCollection contactCollection; int index; int count; internal Enumerator(ContactCollection contactCollection) { this.contactCollection = contactCollection; index = -1; count = contactCollection.Count; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public ContactInformation Current { get { return contactCollection[index]; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. 2 public bool MoveNext() { return ++index < count; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. 2 public void Reset() { index = -1; count = contactCollection.Count; } } CollidablePairHandler pair; internal ContactCollection(CollidablePairHandler pair) { this.pair = pair; } /// /// Gets the number of elements contained in the . /// /// /// The number of elements contained in the . /// public int Count { get { return pair.ContactCount; } } /// /// Gets or sets the element at the specified index. /// /// /// The element at the specified index. /// /// The zero-based index of the element to get or set. is not a valid index in the .The property is set and the is read-only. public ContactInformation this[int index] { get { ContactInformation toReturn; pair.GetContactInformation(index, out toReturn); return toReturn; } set { throw new NotSupportedException(); } } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(this); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(this); } /// /// Gets an enumerator for the collection. /// ///Enumerator for the contact collection. public Enumerator GetEnumerator() { return new Enumerator(this); } /// /// Determines whether the contains a specific value. /// /// /// true if is found in the ; otherwise, false. /// /// The object to locate in the . public bool Contains(ContactInformation item) { int count = Count; for (int i = 0; i < count; i++) { if (this[i].Contact == item.Contact) return true; } return false; } /// /// Copies the elements of the to an , starting at a particular index. /// /// The one-dimensional that is the destination of the elements copied from . The must have zero-based indexing.The zero-based index in at which copying begins. is null. is less than 0. is multidimensional.-or-The number of elements in the source is greater than the available space from to the end of the destination .-or-Type cannot be cast automatically to the type of the destination . public void CopyTo(ContactInformation[] array, int arrayIndex) { int count = Count; for (int i = 0; i < count; i++) { array[arrayIndex + i] = this[i]; } } /// /// Determines the index of a specific item in the . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the . public int IndexOf(ContactInformation item) { int count = Count; for (int i = 0; i < count; i++) { if (this[i].Contact == item.Contact) return i; } return -1; } bool ICollection.IsReadOnly { get { return true; } } bool ICollection.Remove(ContactInformation item) { throw new NotSupportedException(); } void ICollection.Add(ContactInformation item) { throw new NotSupportedException(); } void ICollection.Clear() { throw new NotSupportedException(); } void IList.Insert(int index, ContactInformation item) { throw new NotSupportedException(); } void IList.RemoveAt(int index) { throw new NotSupportedException(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/ContactInformation.cs ================================================ using System; using BEPUphysics.CollisionTests; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Contact and some associated data used by the convenience ContactCollection. /// public struct ContactInformation : IEquatable { /// /// Contact point in the pair. /// public Contact Contact; /// /// Pair that most directly generated the contact. /// The pair may have parents, accessible through the pair's Parent property. /// public CollidablePairHandler Pair; /// /// Normal impulse applied between the objects at the contact point. /// public float NormalImpulse; /// /// Friction impulse applied between the objects at the contact point. /// This is sometimes an approximation due to the varying ways in which /// friction is calculated. /// public float FrictionImpulse; /// /// Relative velocity of the colliding objects at the position of the contact. /// public Vector3 RelativeVelocity; public override string ToString() { return Contact + " NormalImpulse: " + NormalImpulse + " FrictionImpulse: " + FrictionImpulse + " RelativeVelocity: " + RelativeVelocity; } public bool Equals(ContactInformation other) { return other.Contact == Contact; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/ConvexConstraintPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Pair handler that manages a pair of two boxes. /// public abstract class ConvexConstraintPairHandler : ConvexPairHandler { ConvexContactManifoldConstraint contactConstraint = new ConvexContactManifoldConstraint(); //public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) //{ // contactConstraint = new ConvexContactManifoldConstraint(); // base.Initialize(entryA, entryB); //} /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = ContactManifold.contacts.Elements[index]; //Find the contact's normal force. float totalNormalImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.penetrationConstraints.Count; i++) { totalNormalImpulse += contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; if (contactConstraint.penetrationConstraints.Elements[i].contact == info.Contact) { info.NormalImpulse = contactConstraint.penetrationConstraints.Elements[i].accumulatedImpulse; } } //Compute friction force. Since we are using central friction, this is 'faked.' float radius; Vector3.Distance(ref contactConstraint.slidingFriction.manifoldCenter, ref info.Contact.Position, out radius); if (totalNormalImpulse > 0) info.FrictionImpulse = (info.NormalImpulse / totalNormalImpulse) * (contactConstraint.slidingFriction.accumulatedImpulse.Length() + contactConstraint.twistFriction.accumulatedImpulse * radius); else info.FrictionImpulse = 0; //Compute relative velocity Vector3 velocity; //If the pair is handling some type of query and does not actually have supporting entities, then consider the velocity contribution to be zero. if (EntityA != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity); Vector3.Cross(ref EntityA.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); if (EntityB != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity); Vector3.Cross(ref EntityB.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityB.linearVelocity, out velocity); Vector3.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/ConvexPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Pair handler that manages a pair of two boxes. /// public abstract class ConvexPairHandler : StandardPairHandler { public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { UpdateMaterialProperties(); base.Initialize(entryA, entryB); } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { var collidableA = CollidableA as ConvexCollidable; var collidableB = CollidableB as ConvexCollidable; var modeA = collidableA.entity == null ? PositionUpdateMode.Discrete : collidableA.entity.PositionUpdateMode; var modeB = collidableB.entity == null ? PositionUpdateMode.Discrete : collidableB.entity.PositionUpdateMode; var overlap = BroadPhaseOverlap; if ( (overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active. ( ( modeA == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A. modeB == PositionUpdateMode.Continuous && overlap.entryA == requester ) || ( modeA == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it. modeB == PositionUpdateMode.Continuous ) ) ) { //Only perform the test if the minimum radii are small enough relative to the size of the velocity. //Discrete objects have already had their linear motion integrated, so don't use their velocity. Vector3 velocity; if (modeA == PositionUpdateMode.Discrete) { //CollidableA is static for the purposes of this continuous test. velocity = collidableB.entity.linearVelocity; } else if (modeB == PositionUpdateMode.Discrete) { //CollidableB is static for the purposes of this continuous test. Vector3.Negate(ref collidableA.entity.linearVelocity, out velocity); } else { //Both objects are moving. Vector3.Subtract(ref collidableB.entity.linearVelocity, ref collidableA.entity.linearVelocity, out velocity); } Vector3.Multiply(ref velocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadiusA = collidableA.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadiusA * minimumRadiusA < velocitySquared) { //Spherecast A against B. RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(collidableA.worldTransform.Position, -velocity), minimumRadiusA, collidableB.Shape, ref collidableB.worldTransform, timeOfImpact, out rayHit)) timeOfImpact = rayHit.T; } var minimumRadiusB = collidableB.Shape.minimumRadius * MotionSettings.CoreShapeScaling; if (minimumRadiusB * minimumRadiusB < velocitySquared) { //Spherecast B against A. RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(collidableB.worldTransform.Position, velocity), minimumRadiusB, collidableA.Shape, ref collidableA.worldTransform, timeOfImpact, out rayHit)) timeOfImpact = rayHit.T; } //If it's intersecting, throw our hands into the air and give up. //This is generally a perfectly acceptable thing to do, since it's either sitting //inside another object (no ccd makes sense) or we're still in an intersecting case //from a previous frame where CCD took place and a contact should have been created //to deal with interpenetrating velocity. Sometimes that contact isn't sufficient, //but it's good enough. if (timeOfImpact == 0) timeOfImpact = 1; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/DetectorVolumeCompoundPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-static mesh collision pair. /// public class DetectorVolumeCompoundPairHandler : DetectorVolumeGroupPairHandler { private CompoundCollidable compound; /// /// Gets the entity collidable associated with the pair. /// public override EntityCollidable Collidable { get { return compound; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { compound = entryA as CompoundCollidable; if (compound == null) { compound = entryB as CompoundCollidable; if (compound == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); compound = null; } protected override void UpdateContainedPairs() { //TODO: Triangle meshes have a worldspace hierarchy that could be more efficiently traversed with a tree vs tree test. //This is just a lot simpler to manage in the short term. for (int i = 0; i < compound.children.Count; i++) { TryToAdd(compound.children.Elements[i].CollisionInformation); } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/DetectorVolumeConvexPairHandler.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using System; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.DataStructures; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles the tests between a DetectorVolume and a convex collidable. /// public class DetectorVolumeConvexPairHandler : DetectorVolumePairHandler { ConvexCollidable convex; private bool checkContainment = true; /// /// Gets or sets whether or not to check the convex object for total containment within the detector volume. /// public bool CheckContainment { get { return checkContainment; } set { checkContainment = value; } } public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { base.Initialize(entryA, entryB); convex = entryA as ConvexCollidable; if (convex == null) { convex = entryB as ConvexCollidable; if (convex == null) { throw new ArgumentException("Incorrect types passed to pair handler."); } } } public override void CleanUp() { base.CleanUp(); convex = null; checkContainment = true; } public override EntityCollidable Collidable { get { return convex; } } RawList overlaps = new RawList(8); private TriangleShape triangle = new TriangleShape { collisionMargin = 0 }; public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; var transform = new RigidTransform { Orientation = Quaternion.Identity }; DetectorVolume.TriangleMesh.Tree.GetOverlaps(convex.boundingBox, overlaps); for (int i = 0; i < overlaps.Count; i++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); Vector3.Add(ref triangle.vA, ref triangle.vB, out transform.Position); Vector3.Add(ref triangle.vC, ref transform.Position, out transform.Position); Vector3.Multiply(ref transform.Position, 1 / 3f, out transform.Position); Vector3.Subtract(ref triangle.vA, ref transform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref transform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref transform.Position, out triangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker when objects are overlapping than GJK. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(convex.Shape, triangle, ref convex.worldTransform, ref transform)) { Touching = true; //The convex can't be fully contained if it's still touching the surface. Containing = false; overlaps.Clear(); goto events; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //If this is a child pair, the CheckContainment flag may be set to false. This is because the parent has //already determined that it is not contained (another child performed the check and found that it was not contained) //and that it is already touching somehow (either by intersection or by containment). //so further containment tests are unnecessary. if (CheckContainment && DetectorVolume.IsPointContained(ref convex.worldTransform.Position, overlaps)) { Touching = true; Containing = true; goto events; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! Touching = false; Containing = false; events: NotifyDetectorVolumeOfChanges(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/DetectorVolumeGroupPairHandler.cs ================================================ using System.Collections.Generic; using System.Diagnostics; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.CollisionRuleManagement; using BEPUutilities.DataStructures; using BEPUphysics.Materials; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using Microsoft.Xna.Framework.Input; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of pairs between collidables that generate contact points. /// public abstract class DetectorVolumeGroupPairHandler : DetectorVolumePairHandler, IDetectorVolumePairHandlerParent { Dictionary subPairs = new Dictionary(); HashSet containedPairs = new HashSet(); RawList pairsToRemove = new RawList(); /// /// Gets a read-only dictionary of collidables associated with this group pair handler all the subpairs associated with them. /// public ReadOnlyDictionary Pairs { get { return new ReadOnlyDictionary(subPairs); } } /// /// Called when the pair handler is added to the narrow phase. /// protected internal override void OnAddedToNarrowPhase() { DetectorVolume.pairs.Add(Collidable.entity, this); } /// /// Cleans up the pair handler. /// public override void CleanUp() { foreach (var pair in subPairs.Values) { pair.CleanUp(); } subPairs.Clear(); base.CleanUp(); } protected void TryToAdd(EntityCollidable collidable) { CollisionRule rule; if ((rule = CollisionRules.collisionRuleCalculator(DetectorVolume, collidable)) < CollisionRule.NoNarrowPhasePair) { //Clamp the rule to the parent's rule. Always use the more restrictive option. //Don't have to test for NoNarrowPhasePair rule on the parent's rule because then the parent wouldn't exist! if (rule < CollisionRule) rule = CollisionRule; if (!subPairs.ContainsKey(collidable)) { var newPair = NarrowPhaseHelper.GetPairHandler(DetectorVolume, collidable, rule) as DetectorVolumePairHandler; if (newPair != null) { newPair.Parent = this; subPairs.Add(collidable, newPair); } } containedPairs.Add(collidable); } } protected abstract void UpdateContainedPairs(); public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; //Gather current pairs. UpdateContainedPairs(); //Eliminate old pairs. foreach (var other in subPairs.Keys) { if (!containedPairs.Contains(other)) pairsToRemove.Add(other); } for (int i = 0; i < pairsToRemove.Count; i++) { var toReturn = subPairs[pairsToRemove.Elements[i]]; subPairs.Remove(pairsToRemove.Elements[i]); toReturn.CleanUp(); toReturn.Factory.GiveBack(toReturn); } containedPairs.Clear(); pairsToRemove.Clear(); //Scan the pairs in sequence, updating the state as we go. //Touching can be set to true by a single touching subpair. Touching = false; //Containing can be set to false by a single noncontaining or nontouching subpair. Containing = subPairs.Count > 0; foreach (var pair in subPairs.Values) { //For child convex pairs, we don't need to always perform containment checks. //Only check if the containment state has not yet been invalidated or a touching state has not been identified. var convexPair = pair as DetectorVolumeConvexPairHandler; if (convexPair != null) convexPair.CheckContainment = Containing || !Touching; pair.UpdateCollision(dt); if (pair.Touching) Touching = true; //If one child is touching, then we are touching too. else Containing = false; //If one child isn't touching, then we aren't containing. if (!pair.Containing) //If one child isn't containing, then we aren't containing. Containing = false; if (!Containing && Touching) { //If it's touching but not containing, no further pairs will change the state. //Containment has been invalidated by something that either didn't touch or wasn't contained. //Touching has been ensured by at least one object touching. break; } } NotifyDetectorVolumeOfChanges(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/DetectorVolumeMobileMeshPairHandler.cs ================================================ using System; using BEPUphysics.CollisionShapes; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUutilities.DataStructures; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of pairs between collidables that generate contact points. /// public class DetectorVolumeMobileMeshPairHandler : DetectorVolumePairHandler { private MobileMeshCollidable mesh; /// /// Gets the entity collidable associated with the pair. /// public override EntityCollidable Collidable { get { return mesh; } } /// /// Called when the pair handler is added to the narrow phase. /// protected internal override void OnAddedToNarrowPhase() { DetectorVolume.pairs.Add(Collidable.entity, this); } public override void Initialize(BroadPhaseEntries.BroadPhaseEntry entryA, BroadPhaseEntries.BroadPhaseEntry entryB) { base.Initialize(entryA, entryB); mesh = entryA as MobileMeshCollidable; if (mesh == null) { mesh = entryB as MobileMeshCollidable; if (mesh == null) throw new ArgumentException("Invalid types used to initialize pair handler."); } } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } private TriangleShape mobileTriangle = new TriangleShape(); private TriangleShape detectorTriangle = new TriangleShape { collisionMargin = 0 }; RawList overlaps = new RawList(8); public override void UpdateCollision(float dt) { WasContaining = Containing; WasTouching = Touching; mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin; //Scan the pairs in sequence, updating the state as we go. //Touching can be set to true by a single touching subpair. Touching = false; //Containing can be set to false by a single noncontaining or nontouching subpair. Containing = true; var meshData = mesh.Shape.TriangleMesh.Data; RigidTransform mobileTriangleTransform, detectorTriangleTransform; mobileTriangleTransform.Orientation = Quaternion.Identity; detectorTriangleTransform.Orientation = Quaternion.Identity; for (int i = 0; i < meshData.IndexCount; i += 3) { //Grab a triangle associated with the mobile mesh. meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC); RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA); RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB); RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC); Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position); Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position); Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position); Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA); Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB); Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC); //Go through all the detector volume triangles which are near the mobile mesh triangle. bool triangleTouching, triangleContaining; BoundingBox mobileBoundingBox; mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox); DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps); for (int j = 0; j < overlaps.Count; j++) { DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC); Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position); Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position); Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f, out detectorTriangleTransform.Position); Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA); Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB); Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC); //If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.))) //[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.] if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform)) { triangleTouching = true; //The convex can't be fully contained if it's still touching the surface. triangleContaining = false; overlaps.Clear(); goto finishTriangleTest; } } overlaps.Clear(); //If we get here, then there was no shell intersection. //If the convex's center point is contained by the mesh, then the convex is fully contained. //This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established. if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps)) { triangleTouching = true; triangleContaining = true; goto finishTriangleTest; } //If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate! triangleTouching = false; triangleContaining = false; finishTriangleTest: //Analyze the results of the triangle test. if (triangleTouching) Touching = true; //If one child is touching, then we are touching too. else Containing = false; //If one child isn't touching, then we aren't containing. if (!triangleContaining) //If one child isn't containing, then we aren't containing. Containing = false; if (!Containing && Touching) { //If it's touching but not containing, no further pairs will change the state. //Containment has been invalidated by something that either didn't touch or wasn't contained. //Touching has been ensured by at least one object touching. break; } } //There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume. //In this case, we should be Touching, but currently we are not. if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching) { //To determine if the detector volume is fully contained, check if one of the detector mesh's vertices //are in the mobile mesh. //This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids. Vector3 vertex; DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex); Ray ray; ray.Direction = Vector3.Up; RayHit hit; RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position); if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit)) { Touching = true; } } NotifyDetectorVolumeOfChanges(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/DetectorVolumePairHandler.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using System; using BEPUphysics.UpdateableSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of pairs between collidables that generate contact points. /// public abstract class DetectorVolumePairHandler : NarrowPhasePair { /// /// Gets the detector volume associated with the pair. /// public DetectorVolume DetectorVolume { get; private set; } /// /// Gets the entity collidable associated with the pair. /// public abstract EntityCollidable Collidable { get; } /// /// Gets whether or not the collidable was touching the detector volume during the previous frame. /// public bool WasTouching { get; protected set; } /// /// Gets whether or not the collidable was fully contained within the detector volume during the previous frame. /// public bool WasContaining { get; protected set; } /// /// Gets whether or not the collidable is touching the detector volume. /// public bool Touching { get; protected set; } /// /// Gets whether or not the collidable is fully contained within the detector volume. /// public bool Containing { get; protected set; } /// /// Gets the parent of this pair handler, if any. /// public IDetectorVolumePairHandlerParent Parent { get; internal set; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Child initialization is responsible for setting up the collidable. DetectorVolume = entryA as DetectorVolume; if (DetectorVolume == null) { DetectorVolume = entryB as DetectorVolume; if (DetectorVolume == null) throw new ArgumentException("Incorrect types used to initialize detector volume pair."); } } /// /// Called when the pair handler is added to the narrow phase. /// protected internal override void OnAddedToNarrowPhase() { DetectorVolume.pairs.Add(Collidable.entity, this); } /// /// Cleans up the pair handler. /// public override void CleanUp() { //Fire off some events if needed! Note the order; we should stop containing before we stop touching. if (Parent == null) { if (Containing) { DetectorVolume.StoppedContaining(this); } if (Touching) { DetectorVolume.StoppedTouching(this); } } Containing = false; Touching = false; WasContaining = false; WasTouching = false; DetectorVolume.pairs.Remove(Collidable.entity); broadPhaseOverlap = new BroadPhaseOverlap(); DetectorVolume = null; Parent = null; //Child cleanup is responsible for cleaning up direct references to the involved collidables. } protected void NotifyDetectorVolumeOfChanges() { //Don't notify the detector volume if we have a parent. The parent will analyze our state. if (Parent == null) { //Beware the order! //Starts touching -> starts containing if (!WasTouching && Touching) { DetectorVolume.BeganTouching(this); } if (!WasContaining && Containing) { DetectorVolume.BeganContaining(this); } //Stops containing -> stops touching if (WasContaining && !Containing) { DetectorVolume.StoppedContaining(this); } if (WasTouching && !Touching) { DetectorVolume.StoppedTouching(this); } } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/GeneralConvexPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a convex-convex collision pair. /// public class GeneralConvexPairHandler : ConvexConstraintPairHandler { ConvexCollidable convexA; ConvexCollidable convexB; GeneralConvexContactManifold contactManifold = new GeneralConvexContactManifold(); public override Collidable CollidableA { get { return convexA; } } public override Collidable CollidableB { get { return convexB; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return contactManifold; } } public override Entities.Entity EntityA { get { return convexA.entity; } } public override Entities.Entity EntityB { get { return convexB.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { convexA = entryA as ConvexCollidable; convexB = entryB as ConvexCollidable; if (convexA == null || convexB == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); convexA = null; convexB = null; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/GroupPairHandler.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Constraints; using BEPUphysics.Constraints.Collision; using BEPUutilities.DataStructures; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.CollisionTests; using BEPUphysics.Materials; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of pairs which manage multiple sub-collidable pairs. /// public abstract class GroupPairHandler : CollidablePairHandler, IPairHandlerParent { ContactManifoldConstraintGroup manifoldConstraintGroup; Dictionary subPairs = new Dictionary(); HashSet containedPairs = new HashSet(); RawList pairsToRemove = new RawList(); /// /// Gets a list of the pairs associated with children. /// public ReadOnlyDictionary ChildPairs { get { return new ReadOnlyDictionary(subPairs); } } /// /// Constructs a new compound-convex pair handler. /// protected GroupPairHandler() { manifoldConstraintGroup = new ContactManifoldConstraintGroup(); } /// /// Forces an update of the pair's material properties. /// ///Material of the first member of the pair. ///Material of the second member of the pair. public override void UpdateMaterialProperties(Material a, Material b) { foreach (var pairHandler in subPairs.Values) { pairHandler.UpdateMaterialProperties(a, b); } } /// /// Updates the material interaction properties of the pair handler's constraint. /// /// Properties to use. public override void UpdateMaterialProperties(InteractionProperties properties) { foreach (var pairHandler in subPairs.Values) { pairHandler.UpdateMaterialProperties(properties); } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Child initialization is responsible for setting up the entries. //Child initialization is responsible for setting up the manifold, if any. manifoldConstraintGroup.Initialize(EntityA, EntityB); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { //The pair handler cleanup will get rid of contacts. foreach (var pairHandler in subPairs.Values) { pairHandler.CleanUp(); //Don't forget to give the pair back to the factory! //There'd be a lot of leaks otherwise. pairHandler.Factory.GiveBack(pairHandler); } subPairs.Clear(); //don't need to remove constraints directly from our group, since cleaning up our children should get rid of them. base.CleanUp(); //Child type needs to null out the references. } //TODO: At the time of writing this, the default project configuration for the Xbox360 didn't allow optional parameters. Could compress this. protected void TryToAdd(Collidable a, Collidable b) { TryToAdd(a, b, null, null); } protected void TryToAdd(Collidable a, Collidable b, Material materialA) { TryToAdd(a, b, materialA, null); } protected void TryToAdd(Collidable a, Collidable b, Material materialA, Material materialB) { CollisionRule rule; if ((rule = CollisionRules.collisionRuleCalculator(a, b)) < CollisionRule.NoNarrowPhasePair) { //Clamp the rule to the parent's rule. Always use the more restrictive option. //Don't have to test for NoNarrowPhasePair rule on the parent's rule because then the parent wouldn't exist! if (rule < CollisionRule) rule = CollisionRule; var pair = new CollidablePair(a, b); if (!subPairs.ContainsKey(pair)) { var newPair = NarrowPhaseHelper.GetPairHandler(ref pair, rule); if (newPair != null) { newPair.UpdateMaterialProperties(materialA, materialB); //Override the materials, if necessary. newPair.Parent = this; subPairs.Add(pair, newPair); } } containedPairs.Add(pair); } } protected abstract void UpdateContainedPairs(); /// /// Updates the pair handler's contacts. /// ///Timestep duration. protected virtual void UpdateContacts(float dt) { UpdateContainedPairs(); //Eliminate old pairs. foreach (CollidablePair pair in subPairs.Keys) { if (!containedPairs.Contains(pair)) pairsToRemove.Add(pair); } for (int i = 0; i < pairsToRemove.Count; i++) { CollidablePairHandler toReturn = subPairs[pairsToRemove.Elements[i]]; subPairs.Remove(pairsToRemove.Elements[i]); toReturn.CleanUp(); toReturn.Factory.GiveBack(toReturn); } containedPairs.Clear(); pairsToRemove.Clear(); foreach (CollidablePairHandler pair in subPairs.Values) { if (pair.BroadPhaseOverlap.collisionRule < CollisionRule.NoNarrowPhaseUpdate) //Don't test if the collision rules say don't. pair.UpdateCollision(dt); } } /// /// Updates the pair handler. /// ///Timestep duration. public override void UpdateCollision(float dt) { if (!suppressEvents) { CollidableA.EventTriggerer.OnPairUpdated(CollidableB, this); CollidableB.EventTriggerer.OnPairUpdated(CollidableA, this); } UpdateContacts(dt); if (contactCount > 0) { if (!suppressEvents) { CollidableA.EventTriggerer.OnPairTouching(CollidableB, this); CollidableB.EventTriggerer.OnPairTouching(CollidableA, this); } if (previousContactCount == 0) { //collision started! CollidableA.EventTriggerer.OnInitialCollisionDetected(CollidableB, this); CollidableB.EventTriggerer.OnInitialCollisionDetected(CollidableA, this); //No solver updateable addition in this method since it's handled by the "AddSolverUpdateable" method. } } else if (previousContactCount > 0 && !suppressEvents) { //collision ended! CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this); CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this); //No solver updateable removal in this method since it's handled by the "RemoveSolverUpdateable" method. } previousContactCount = contactCount; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { timeOfImpact = 1; foreach (CollidablePairHandler pair in subPairs.Values) { //The system uses the identity of the requester to determine if it needs to do handle the TOI calculation. //Use the child pair's own entries as a proxy. if (BroadPhaseOverlap.entryA == requester) pair.UpdateTimeOfImpact((Collidable)pair.BroadPhaseOverlap.entryA, dt); else pair.UpdateTimeOfImpact((Collidable)pair.BroadPhaseOverlap.entryB, dt); if (pair.timeOfImpact < timeOfImpact) timeOfImpact = pair.timeOfImpact; } } protected internal override void GetContactInformation(int index, out ContactInformation info) { foreach (CollidablePairHandler pair in subPairs.Values) { int count = pair.Contacts.Count; if (index - count < 0) { pair.GetContactInformation(index, out info); return; } index -= count; } throw new IndexOutOfRangeException("Contact index is not present in the pair."); } void IPairHandlerParent.AddSolverUpdateable(EntitySolverUpdateable addedItem) { manifoldConstraintGroup.Add(addedItem); //If this is the first child solver item to be added, we need to add ourselves to our parent too. if (manifoldConstraintGroup.SolverUpdateables.Count == 1) { if (Parent != null) Parent.AddSolverUpdateable(manifoldConstraintGroup); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableAdded(manifoldConstraintGroup); } } void IPairHandlerParent.RemoveSolverUpdateable(EntitySolverUpdateable removedItem) { manifoldConstraintGroup.Remove(removedItem); //If this is the last child solver item, we need to remove ourselves from our parent too. if (manifoldConstraintGroup.SolverUpdateables.Count == 0) { if (Parent != null) Parent.RemoveSolverUpdateable(manifoldConstraintGroup); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableRemoved(manifoldConstraintGroup); } } void IPairHandlerParent.OnContactAdded(Contact contact) { contactCount++; OnContactAdded(contact); } void IPairHandlerParent.OnContactRemoved(Contact contact) { contactCount--; OnContactRemoved(contact); } int contactCount; /// /// Gets the number of contacts in the pair. /// protected internal override int ContactCount { get { return contactCount; } } /// /// Clears the pair's contacts. /// public override void ClearContacts() { foreach (var pair in subPairs.Values) { pair.ClearContacts(); } base.ClearContacts(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/IDetectorVolumePairHandlerParent.cs ================================================ namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Implemented by detector volume pair handlers with children. /// public interface IDetectorVolumePairHandlerParent { } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/IPairHandlerParent.cs ================================================ using BEPUphysics.Constraints; using BEPUphysics.CollisionTests; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Defines a pair handler which can have children. /// public interface IPairHandlerParent { /// /// Called when a child adds a contact. /// ///Contact added. void OnContactAdded(Contact contact); /// /// Called when a child removes a contact. /// /// Contact removed. void OnContactRemoved(Contact contact); /// /// Called when a child attempts to add a solver updateable to the solver. /// ///Item to add. void AddSolverUpdateable(EntitySolverUpdateable addedItem); /// /// Called when a child attempts to remove a solver updateable from the solver. /// ///Item to remove. void RemoveSolverUpdateable(EntitySolverUpdateable removedItem); } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/InstancedMeshConvexPairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a instanced mesh-convex collision pair. /// public class InstancedMeshConvexPairHandler : InstancedMeshPairHandler { InstancedMeshConvexContactManifold contactManifold = new InstancedMeshConvexContactManifold(); protected override InstancedMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/InstancedMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.DataStructures; using BEPUutilities.DataStructures; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a instanced mesh-convex collision pair. /// public abstract class InstancedMeshPairHandler : StandardPairHandler { InstancedMesh instancedMesh; ConvexCollidable convex; NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return convex; } } public override Collidable CollidableB { get { return instancedMesh; } } public override Entities.Entity EntityA { get { return convex.entity; } } public override Entities.Entity EntityB { get { return null; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return MeshManifold; } } protected abstract InstancedMeshContactManifold MeshManifold { get; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { instancedMesh = entryA as InstancedMesh; convex = entryB as ConvexCollidable; if (instancedMesh == null || convex == null) { instancedMesh = entryB as InstancedMesh; convex = entryA as ConvexCollidable; if (instancedMesh == null || convex == null) throw new ArgumentException("Inappropriate types used to initialize pair."); } //Contact normal goes from A to B. broadPhaseOverlap.entryA = convex; broadPhaseOverlap.entryB = instancedMesh; UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, instancedMesh.material); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); instancedMesh = null; convex = null; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { MeshBoundingBoxTreeData data = instancedMesh.Shape.TriangleMesh.Data; int triangleIndex = MeshManifold.overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC); AffineTransform.Transform(ref triangle.vA, ref instancedMesh.worldTransform, out triangle.vA); AffineTransform.Transform(ref triangle.vB, ref instancedMesh.worldTransform, out triangle.vB); AffineTransform.Transform(ref triangle.vC, ref instancedMesh.worldTransform, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (instancedMesh.sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (instancedMesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || instancedMesh.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsResources.GiveBack(triangle); } } } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = MeshManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity if (convex.entity != null) { Vector3 velocity; Vector3.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/InstancedMeshSpherePairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a instanced mesh-convex collision pair. /// public class InstancedMeshSpherePairHandler : InstancedMeshPairHandler { InstancedMeshSphereContactManifold contactManifold = new InstancedMeshSphereContactManifold(); protected override InstancedMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MeshGroupPairHandler.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Constraints; using BEPUphysics.Constraints.Collision; using BEPUutilities.DataStructures; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.CollisionTests; using BEPUphysics.Materials; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Contains a triangle collidable and its index. Used by mobile mesh-mesh collisions. /// public struct TriangleEntry : IEquatable { /// /// Index of the triangle that was the source of this entry. /// public int Index; /// /// Collidable for the triangle. /// public TriangleCollidable Collidable; /// /// Gets the hash code of the object. /// /// Hash code of the object. public override int GetHashCode() { return Index; } /// /// Determines if two colliders refer to the same triangle. /// /// Object to compare. /// Whether or not the objects are equal. public bool Equals(TriangleEntry other) { return other.Index == Index; } } //TODO: Lots of overlap with the GroupPairHandler. /// /// Superclass of pair handlers which have multiple index-based collidable child pairs. /// public abstract class MeshGroupPairHandler : CollidablePairHandler, IPairHandlerParent { ContactManifoldConstraintGroup manifoldConstraintGroup; Dictionary subPairs = new Dictionary(); HashSet containedPairs = new HashSet(); RawList pairsToRemove = new RawList(); /// /// Gets a list of the pairs associated with children. /// public ReadOnlyDictionary ChildPairs { get { return new ReadOnlyDictionary(subPairs); } } /// /// Material of the first collidable. /// protected abstract Material MaterialA { get; } /// /// Material of the second collidable. /// protected abstract Material MaterialB { get; } /// /// Constructs a new compound-convex pair handler. /// protected MeshGroupPairHandler() { manifoldConstraintGroup = new ContactManifoldConstraintGroup(); } /// /// Forces an update of the pair's material properties. /// ///Material of the first member of the pair. ///Material of the second member of the pair. public override void UpdateMaterialProperties(Material a, Material b) { foreach (CollidablePairHandler pairHandler in subPairs.Values) { pairHandler.UpdateMaterialProperties(a, b); } } /// /// Updates the material interaction properties of the pair handler's constraint. /// /// Properties to use. public override void UpdateMaterialProperties(InteractionProperties properties) { foreach (CollidablePairHandler pairHandler in subPairs.Values) { pairHandler.UpdateMaterialProperties(properties); } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Child initialization is responsible for setting up the entries. //Child initialization is responsible for setting up the manifold, if any. manifoldConstraintGroup.Initialize(EntityA, EntityB); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { //The pair handler cleanup will get rid of contacts. foreach (CollidablePairHandler pairHandler in subPairs.Values) { pairHandler.CleanUp(); //Don't forget to give the pair back to the factory! //There'd be a lot of leaks otherwise. pairHandler.Factory.GiveBack(pairHandler); } subPairs.Clear(); //don't need to remove constraints directly from our group, since cleaning up our children should get rid of them. base.CleanUp(); //Child type needs to null out the references. } protected void TryToAdd(int index) { var entry = new TriangleEntry { Index = index }; if (!subPairs.ContainsKey(entry)) { var collidablePair = new CollidablePair(CollidableA, entry.Collidable = GetOpposingCollidable(index)); var newPair = (MobileMeshPairHandler)NarrowPhaseHelper.GetPairHandler(ref collidablePair); if (newPair != null) { newPair.CollisionRule = CollisionRule; newPair.UpdateMaterialProperties(MaterialA, MaterialB); //Override the materials, if necessary. Meshes don't currently support custom materials but.. newPair.Parent = this; subPairs.Add(entry, newPair); } } containedPairs.Add(entry); } /// /// Get a collidable from CollidableB to represent the object at the given index. /// /// Index to create a collidable for. /// Collidable for the object at the given index. protected abstract TriangleCollidable GetOpposingCollidable(int index); /// /// Configure a triangle from CollidableB to represent the object at the given index. /// /// Entry to configure. /// Time step duration. protected abstract void ConfigureCollidable(TriangleEntry entry, float dt); /// /// Cleans up the collidable. /// /// Collidable to clean up. protected virtual void CleanUpCollidable(TriangleCollidable collidable) { PhysicsResources.GiveBack(collidable); } protected abstract void UpdateContainedPairs(float dt); /// /// Updates the pair handler's contacts. /// ///Timestep duration. protected virtual void UpdateContacts(float dt) { UpdateContainedPairs(dt); //Eliminate old pairs. foreach (var pair in subPairs.Keys) { if (!containedPairs.Contains(pair)) pairsToRemove.Add(pair); } for (int i = 0; i < pairsToRemove.Count; i++) { var toReturn = subPairs[pairsToRemove.Elements[i]]; subPairs.Remove(pairsToRemove.Elements[i]); toReturn.CleanUp(); toReturn.Factory.GiveBack(toReturn); } containedPairs.Clear(); pairsToRemove.Clear(); foreach (var pair in subPairs) { if (pair.Value.BroadPhaseOverlap.collisionRule < CollisionRule.NoNarrowPhaseUpdate) //Don't test if the collision rules say don't. { ConfigureCollidable(pair.Key, dt); //Update the contact count using our (the parent) contact count so that the child can avoid costly solidity testing. pair.Value.MeshManifold.parentContactCount = contactCount; pair.Value.UpdateCollision(dt); } } } /// /// Updates the pair handler. /// ///Timestep duration. public override void UpdateCollision(float dt) { if (!suppressEvents) { CollidableA.EventTriggerer.OnPairUpdated(CollidableB, this); CollidableB.EventTriggerer.OnPairUpdated(CollidableA, this); } UpdateContacts(dt); if (contactCount > 0) { if (!suppressEvents) { CollidableA.EventTriggerer.OnPairTouching(CollidableB, this); CollidableB.EventTriggerer.OnPairTouching(CollidableA, this); } if (previousContactCount == 0) { //collision started! CollidableA.EventTriggerer.OnInitialCollisionDetected(CollidableB, this); CollidableB.EventTriggerer.OnInitialCollisionDetected(CollidableA, this); //No solver updateable addition in this method since it's handled by the "AddSolverUpdateable" method. } } else if (previousContactCount > 0 && !suppressEvents) { //collision ended! CollidableA.EventTriggerer.OnCollisionEnded(CollidableB, this); CollidableB.EventTriggerer.OnCollisionEnded(CollidableA, this); //No solver updateable removal in this method since it's handled by the "RemoveSolverUpdateable" method. } previousContactCount = contactCount; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { timeOfImpact = 1; foreach (var pair in subPairs.Values) { //The system uses the identity of the requester to determine if it needs to do handle the TOI calculation. //Use the child pair's own entries as a proxy. if (BroadPhaseOverlap.entryA == requester) pair.UpdateTimeOfImpact((Collidable)pair.BroadPhaseOverlap.entryA, dt); else pair.UpdateTimeOfImpact((Collidable)pair.BroadPhaseOverlap.entryB, dt); if (pair.timeOfImpact < timeOfImpact) timeOfImpact = pair.timeOfImpact; } } protected internal override void GetContactInformation(int index, out ContactInformation info) { foreach (CollidablePairHandler pair in subPairs.Values) { int count = pair.Contacts.Count; if (index - count < 0) { pair.GetContactInformation(index, out info); return; } index -= count; } throw new IndexOutOfRangeException("Contact index is not present in the pair."); } void IPairHandlerParent.AddSolverUpdateable(EntitySolverUpdateable addedItem) { manifoldConstraintGroup.Add(addedItem); //If this is the first child solver item to be added, we need to add ourselves to our parent too. if (manifoldConstraintGroup.SolverUpdateables.Count == 1) { if (Parent != null) Parent.AddSolverUpdateable(manifoldConstraintGroup); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableAdded(manifoldConstraintGroup); } } void IPairHandlerParent.RemoveSolverUpdateable(EntitySolverUpdateable removedItem) { manifoldConstraintGroup.Remove(removedItem); //If this is the last child solver item, we need to remove ourselves from our parent too. if (manifoldConstraintGroup.SolverUpdateables.Count == 0) { if (Parent != null) Parent.RemoveSolverUpdateable(manifoldConstraintGroup); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableRemoved(manifoldConstraintGroup); } } void IPairHandlerParent.OnContactAdded(Contact contact) { contactCount++; OnContactAdded(contact); } void IPairHandlerParent.OnContactRemoved(Contact contact) { contactCount--; OnContactRemoved(contact); } int contactCount; /// /// Gets the number of contacts in the pair. /// protected internal override int ContactCount { get { return contactCount; } } /// /// Clears the pair's contacts. /// public override void ClearContacts() { foreach (var pair in subPairs.Values) { pair.ClearContacts(); } base.ClearContacts(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshConvexPairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-convex collision pair. /// public class MobileMeshConvexPairHandler : MobileMeshPairHandler { MobileMeshConvexContactManifold contactManifold = new MobileMeshConvexContactManifold(); protected internal override MobileMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshInstancedMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-mobile mesh collision pair. /// public class MobileMeshInstancedMeshPairHandler : MobileMeshMeshPairHandler { InstancedMesh mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return null; } } protected override Materials.Material MaterialB { get { return mesh.material; } } protected override TriangleCollidable GetOpposingCollidable(int index) { //Construct a TriangleCollidable from the static mesh. var toReturn = PhysicsResources.GetTriangleCollidable(); var shape = toReturn.Shape; mesh.Shape.TriangleMesh.Data.GetTriangle(index, out shape.vA, out shape.vB, out shape.vC); Matrix3x3.Transform(ref shape.vA, ref mesh.worldTransform.LinearTransform, out shape.vA); Matrix3x3.Transform(ref shape.vB, ref mesh.worldTransform.LinearTransform, out shape.vB); Matrix3x3.Transform(ref shape.vC, ref mesh.worldTransform.LinearTransform, out shape.vC); Vector3 center; Vector3.Add(ref shape.vA, ref shape.vB, out center); Vector3.Add(ref center, ref shape.vC, out center); Vector3.Multiply(ref center, 1 / 3f, out center); Vector3.Subtract(ref shape.vA, ref center, out shape.vA); Vector3.Subtract(ref shape.vB, ref center, out shape.vB); Vector3.Subtract(ref shape.vC, ref center, out shape.vC); Vector3.Add(ref center, ref mesh.worldTransform.Translation, out center); //The bounding box doesn't update by itself. toReturn.worldTransform.Position = center; toReturn.worldTransform.Orientation = Quaternion.Identity; toReturn.UpdateBoundingBoxInternal(0); shape.sidedness = mesh.Sidedness; shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin; return toReturn; } protected override void ConfigureCollidable(TriangleEntry entry, float dt) { } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as InstancedMesh; if (mesh == null) { mesh = entryB as InstancedMesh; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs(float dt) { var overlappedElements = CommonResources.GetIntList(); BoundingBox localBoundingBox; Vector3 sweep; Vector3.Multiply(ref mobileMesh.entity.linearVelocity, dt, out sweep); mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref mesh.worldTransform, ref sweep, out localBoundingBox); mesh.Shape.TriangleMesh.Tree.GetOverlaps(localBoundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i]); } CommonResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh and mesh collision pair. /// public abstract class MobileMeshMeshPairHandler : MeshGroupPairHandler { public MobileMeshCollidable mobileMesh; public override Collidable CollidableA { get { return mobileMesh; } } public override Entities.Entity EntityA { get { return mobileMesh.entity; } } protected override Materials.Material MaterialA { get { return mobileMesh.entity.material; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Other member of the pair is initialized by the child. mobileMesh = entryA as MobileMeshCollidable; if (mobileMesh == null) { mobileMesh = entryB as MobileMeshCollidable; if (mobileMesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mobileMesh = null; //Child type needs to null out other reference. } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshMobileMeshPairHandler.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-mobile mesh collision pair. /// public class MobileMeshMobileMeshPairHandler : MobileMeshMeshPairHandler { MobileMeshCollidable mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return mesh.entity; } } protected override Materials.Material MaterialB { get { return mesh.entity.material; } } protected override TriangleCollidable GetOpposingCollidable(int index) { //Construct a TriangleCollidable from the static mesh. var toReturn = PhysicsResources.GetTriangleCollidable(); toReturn.Shape.sidedness = mesh.Shape.Sidedness; toReturn.Shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin; toReturn.Entity = mesh.entity; return toReturn; } protected override void CleanUpCollidable(TriangleCollidable collidable) { collidable.Entity = null; base.CleanUpCollidable(collidable); } protected override void ConfigureCollidable(TriangleEntry entry, float dt) { var shape = entry.Collidable.Shape; mesh.Shape.TriangleMesh.Data.GetTriangle(entry.Index, out shape.vA, out shape.vB, out shape.vC); Matrix3x3 o; Matrix3x3.CreateFromQuaternion(ref mesh.worldTransform.Orientation, out o); Matrix3x3.Transform(ref shape.vA, ref o, out shape.vA); Matrix3x3.Transform(ref shape.vB, ref o, out shape.vB); Matrix3x3.Transform(ref shape.vC, ref o, out shape.vC); Vector3 center; Vector3.Add(ref shape.vA, ref shape.vB, out center); Vector3.Add(ref center, ref shape.vC, out center); Vector3.Multiply(ref center, 1 / 3f, out center); Vector3.Subtract(ref shape.vA, ref center, out shape.vA); Vector3.Subtract(ref shape.vB, ref center, out shape.vB); Vector3.Subtract(ref shape.vC, ref center, out shape.vC); Vector3.Add(ref center, ref mesh.worldTransform.Position, out center); //The bounding box doesn't update by itself. entry.Collidable.worldTransform.Position = center; entry.Collidable.worldTransform.Orientation = Quaternion.Identity; entry.Collidable.UpdateBoundingBoxInternal(dt); } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = (MobileMeshCollidable)entryB; base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs(float dt) { var overlappedElements = CommonResources.GetIntList(); BoundingBox localBoundingBox; AffineTransform meshTransform; AffineTransform.CreateFromRigidTransform(ref mesh.worldTransform, out meshTransform); Vector3 sweep; Vector3.Subtract(ref mobileMesh.entity.linearVelocity, ref mesh.entity.linearVelocity, out sweep); Vector3.Multiply(ref sweep, dt, out sweep); mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref meshTransform, ref sweep, out localBoundingBox); mesh.Shape.TriangleMesh.Tree.GetOverlaps(localBoundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i]); } CommonResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.DataStructures; using BEPUutilities.DataStructures; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-convex collision pair. /// public abstract class MobileMeshPairHandler : StandardPairHandler { MobileMeshCollidable mobileMesh; ConvexCollidable convex; NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return convex; } } public override Collidable CollidableB { get { return mobileMesh; } } public override Entities.Entity EntityA { get { return convex.entity; } } public override Entities.Entity EntityB { get { return mobileMesh.entity; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return MeshManifold; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } protected internal abstract MobileMeshContactManifold MeshManifold { get; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mobileMesh = entryA as MobileMeshCollidable; convex = entryB as ConvexCollidable; if (mobileMesh == null || convex == null) { mobileMesh = entryB as MobileMeshCollidable; convex = entryA as ConvexCollidable; if (mobileMesh == null || convex == null) throw new ArgumentException("Inappropriate types used to initialize pair."); } //Contact normal goes from A to B. broadPhaseOverlap.entryA = convex; broadPhaseOverlap.entryB = mobileMesh; //It's possible that the convex does not have an entity if it is a proxy for a non-entity collidable. //Similarly, the mesh could be a query object. UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, mobileMesh.entity != null ? mobileMesh.entity.material : null); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mobileMesh = null; convex = null; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { var overlap = BroadPhaseOverlap; var meshMode = mobileMesh.entity == null ? PositionUpdateMode.Discrete : mobileMesh.entity.PositionUpdateMode; var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode; if ( (mobileMesh.IsActive || convex.IsActive) && //At least one has to be active. ( ( convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A. meshMode == PositionUpdateMode.Continuous && overlap.entryA == requester ) || ( convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it. meshMode == PositionUpdateMode.Continuous ) ) ) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; if (convexMode == PositionUpdateMode.Discrete) { //Convex is static for the purposes of CCD. Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity); } else if (meshMode == PositionUpdateMode.Discrete) { //Mesh is static for the purposes of CCD. velocity = convex.entity.linearVelocity; } else { //Both objects can move. Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity); } Vector3.Multiply(ref velocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { TriangleSidedness sidedness = mobileMesh.Shape.Sidedness; Matrix3x3 orientation; Matrix3x3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation); var triangle = PhysicsResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data; int triangleIndex = MeshManifold.overlappedTriangles.Elements[i]; data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC); Matrix3x3.Transform(ref triangle.vA, ref orientation, out triangle.vA); Matrix3x3.Transform(ref triangle.vB, ref orientation, out triangle.vB); Matrix3x3.Transform(ref triangle.vC, ref orientation, out triangle.vC); Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA); Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB); Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 || sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsResources.GiveBack(triangle); } } } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = MeshManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity Vector3 velocity; if (convex.entity != null) { Vector3.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); if (mobileMesh.entity != null) { Vector3.Subtract(ref info.Contact.Position, ref mobileMesh.entity.position, out velocity); Vector3.Cross(ref mobileMesh.entity.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref mobileMesh.entity.linearVelocity, out velocity); Vector3.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshSpherePairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-sphere collision pair. /// public class MobileMeshSpherePairHandler : MobileMeshPairHandler { MobileMeshSphereContactManifold contactManifold = new MobileMeshSphereContactManifold(); protected internal override MobileMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshStaticMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-static mesh collision pair. /// public class MobileMeshStaticMeshPairHandler : MobileMeshMeshPairHandler { StaticMesh mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return null; } } protected override Materials.Material MaterialB { get { return mesh.material; } } protected override TriangleCollidable GetOpposingCollidable(int index) { //Construct a TriangleCollidable from the static mesh. var toReturn = PhysicsResources.GetTriangleCollidable(); var shape = toReturn.Shape; mesh.Mesh.Data.GetTriangle(index, out shape.vA, out shape.vB, out shape.vC); Vector3 center; Vector3.Add(ref shape.vA, ref shape.vB, out center); Vector3.Add(ref center, ref shape.vC, out center); Vector3.Multiply(ref center, 1 / 3f, out center); Vector3.Subtract(ref shape.vA, ref center, out shape.vA); Vector3.Subtract(ref shape.vB, ref center, out shape.vB); Vector3.Subtract(ref shape.vC, ref center, out shape.vC); //The bounding box doesn't update by itself. toReturn.worldTransform.Position = center; toReturn.worldTransform.Orientation = Quaternion.Identity; toReturn.UpdateBoundingBoxInternal(0); shape.sidedness = mesh.sidedness; shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin; return toReturn; } protected override void ConfigureCollidable(TriangleEntry entry, float dt) { } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as StaticMesh; if (mesh == null) { mesh = entryB as StaticMesh; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs(float dt) { var overlappedElements = CommonResources.GetIntList(); mesh.Mesh.Tree.GetOverlaps(mobileMesh.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i]); } CommonResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshTerrainPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-mobile mesh collision pair. /// public class MobileMeshTerrainPairHandler : MobileMeshMeshPairHandler { Terrain mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return null; } } protected override Materials.Material MaterialB { get { return mesh.material; } } protected override TriangleCollidable GetOpposingCollidable(int index) { //Construct a TriangleCollidable from the static mesh. var toReturn = PhysicsResources.GetTriangleCollidable(); Vector3 terrainUp = new Vector3(mesh.worldTransform.LinearTransform.M21, mesh.worldTransform.LinearTransform.M22, mesh.worldTransform.LinearTransform.M23); float dot; Vector3 AB, AC, normal; var shape = toReturn.Shape; mesh.Shape.GetTriangle(index, ref mesh.worldTransform, out shape.vA, out shape.vB, out shape.vC); Vector3 center; Vector3.Add(ref shape.vA, ref shape.vB, out center); Vector3.Add(ref center, ref shape.vC, out center); Vector3.Multiply(ref center, 1 / 3f, out center); Vector3.Subtract(ref shape.vA, ref center, out shape.vA); Vector3.Subtract(ref shape.vB, ref center, out shape.vB); Vector3.Subtract(ref shape.vC, ref center, out shape.vC); //The bounding box doesn't update by itself. toReturn.worldTransform.Position = center; toReturn.worldTransform.Orientation = Quaternion.Identity; toReturn.UpdateBoundingBoxInternal(0); Vector3.Subtract(ref shape.vB, ref shape.vA, out AB); Vector3.Subtract(ref shape.vC, ref shape.vA, out AC); Vector3.Cross(ref AB, ref AC, out normal); Vector3.Dot(ref terrainUp, ref normal, out dot); if (dot > 0) { shape.sidedness = TriangleSidedness.Clockwise; } else { shape.sidedness = TriangleSidedness.Counterclockwise; } shape.collisionMargin = mobileMesh.Shape.MeshCollisionMargin; return toReturn; } protected override void ConfigureCollidable(TriangleEntry entry, float dt) { } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as Terrain; if (mesh == null) { mesh = entryB as Terrain; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs(float dt) { var overlappedElements = CommonResources.GetIntList(); BoundingBox localBoundingBox; Vector3 sweep; Vector3.Multiply(ref mobileMesh.entity.linearVelocity, dt, out sweep); mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref mesh.worldTransform, ref sweep, out localBoundingBox); mesh.Shape.GetOverlaps(localBoundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { TryToAdd(overlappedElements.Elements[i]); } CommonResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/MobileMeshTrianglePairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a mobile mesh-convex collision pair. /// public class MobileMeshTrianglePairHandler : MobileMeshPairHandler { MobileMeshTriangleContactManifold contactManifold = new MobileMeshTriangleContactManifold(); protected internal override MobileMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/NarrowPhasePair.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.CollisionRuleManagement; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Superclass of objects which handle a collision between two broad phase entries. /// public abstract class NarrowPhasePair { /// /// Updates the collision between the broad phase entries. /// ///Timestep duration. public abstract void UpdateCollision(float dt); /// /// Gets or sets whether or not the pair needs to be updated. /// Used by the NarrowPhase to manage pairs. /// public bool NeedsUpdate { get; set; } /// /// Gets or sets the collision rule governing this pair. /// public CollisionRule CollisionRule { get { return broadPhaseOverlap.collisionRule; } set { broadPhaseOverlap.collisionRule = value; } } internal BroadPhaseOverlap broadPhaseOverlap; /// /// Gets the overlap used to create the pair. /// public BroadPhaseOverlap BroadPhaseOverlap { get { return broadPhaseOverlap; } set { broadPhaseOverlap = value; Initialize(value.entryA, value.entryB); } } /// /// Gets the factory that created the pair. /// public NarrowPhasePairFactory Factory { get; internal set; } /// /// Gets the narrow phase that owns this pair. /// public NarrowPhase NarrowPhase { get; internal set; } /// /// Initializes the pair. /// ///First entry in the pair. ///Second entry in the pair. public abstract void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB); /// /// Called when the pair is added to the narrow phase. /// protected internal abstract void OnAddedToNarrowPhase(); /// /// Cleans up the pair, preparing it to go inactive. /// public abstract void CleanUp(); } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/SpherePairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using Microsoft.Xna.Framework; using BEPUphysics.CollisionTests.CollisionAlgorithms; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a sphere-sphere collision pair. /// public class SpherePairHandler : ConvexPairHandler { ConvexCollidable sphereA; ConvexCollidable sphereB; //Using a non-convex one since they have slightly lower overhead than their Convex friends when dealing with a single contact point. SphereContactManifold contactManifold = new SphereContactManifold(); NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return sphereA; } } public override Collidable CollidableB { get { return sphereB; } } public override Entities.Entity EntityA { get { return sphereA.entity; } } public override Entities.Entity EntityB { get { return sphereB.entity; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return contactManifold; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { sphereA = entryA as ConvexCollidable; sphereB = entryB as ConvexCollidable; if (sphereA == null || sphereB == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); sphereA = null; sphereB = null; } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = ContactManifold.contacts.Elements[index]; //Find the contact's force. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity Vector3 velocity; if (EntityA != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityA.position, out velocity); Vector3.Cross(ref EntityA.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityA.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); if (EntityB != null) { Vector3.Subtract(ref info.Contact.Position, ref EntityB.position, out velocity); Vector3.Cross(ref EntityB.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref EntityB.linearVelocity, out velocity); Vector3.Subtract(ref info.RelativeVelocity, ref velocity, out info.RelativeVelocity); } info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StandardPairHandler.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.Materials; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a standard pair handler that has a direct manifold and constraint. /// public abstract class StandardPairHandler : CollidablePairHandler { /// /// Gets the contact manifold used by the pair handler. /// public abstract ContactManifold ContactManifold { get; } /// /// Gets the contact constraint usd by the pair handler. /// public abstract ContactManifoldConstraint ContactConstraint { get; } /// /// Constructs a pair handler. /// protected StandardPairHandler() { //Child type constructors construct manifold and constraint first. ContactManifold.ContactAdded += OnContactAdded; ContactManifold.ContactRemoved += OnContactRemoved; } protected override void OnContactAdded(Contact contact) { ContactConstraint.AddContact(contact); base.OnContactAdded(contact); } protected override void OnContactRemoved(Contact contact) { ContactConstraint.RemoveContact(contact); base.OnContactRemoved(contact); } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Child initialization is responsible for setting up the entries. ContactManifold.Initialize(CollidableA, CollidableB); ContactConstraint.Initialize(EntityA, EntityB, this); base.Initialize(entryA, entryB); } /// /// Forces an update of the pair's material properties. /// public override void UpdateMaterialProperties(Material a, Material b) { ContactConstraint.UpdateMaterialProperties( a ?? (EntityA == null ? null : EntityA.material), b ?? (EntityB == null ? null : EntityB.material)); } /// /// Updates the material interaction properties of the pair handler's constraint. /// /// Properties to use. public override void UpdateMaterialProperties(InteractionProperties properties) { ContactConstraint.MaterialInteraction = properties; } /// /// Cleans up the pair handler. /// public override void CleanUp() { //Deal with the remaining contacts. for (int i = ContactManifold.contacts.Count - 1; i >= 0; i--) { OnContactRemoved(ContactManifold.contacts[i]); } //If the constraint is still in the solver, then request to have it removed. if (ContactConstraint.solver != null) { ContactConstraint.pair = null; //Setting the pair to null tells the constraint that it's going to be orphaned. It will be cleaned up on removal. if (Parent != null) Parent.RemoveSolverUpdateable(ContactConstraint); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableRemoved(ContactConstraint); } else { ContactConstraint.CleanUpReferences();//The constraint isn't in the solver, so we can safely clean it up directly. //Even though it's not in the solver, we still may need to notify the parent to remove it. if (Parent != null && ContactConstraint.SolverGroup != null) Parent.RemoveSolverUpdateable(ContactConstraint); } ContactConstraint.CleanUp(); base.CleanUp(); ContactManifold.CleanUp(); //Child cleanup is responsible for cleaning up direct references to the involved collidables. } /// /// Updates the pair handler. /// ///Timestep duration. public override void UpdateCollision(float dt) { //Cache some properties. var a = CollidableA; var b = CollidableB; var triggerA = a.EventTriggerer; var triggerB = b.EventTriggerer; if (!suppressEvents) { triggerA.OnPairUpdated(b, this); triggerB.OnPairUpdated(a, this); } ContactManifold.Update(dt); if (ContactManifold.contacts.Count > 0) { if (!suppressEvents) { triggerA.OnPairTouching(b, this); triggerB.OnPairTouching(a, this); } if (previousContactCount == 0) { //New collision. //Add a solver item. if (Parent != null) Parent.AddSolverUpdateable(ContactConstraint); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableAdded(ContactConstraint); //And notify the pair members. if (!suppressEvents) { triggerA.OnInitialCollisionDetected(b, this); triggerB.OnInitialCollisionDetected(a, this); } } } else if (previousContactCount > 0) { //Just exited collision. //Remove the solver item. if (Parent != null) Parent.RemoveSolverUpdateable(ContactConstraint); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableRemoved(ContactConstraint); if (!suppressEvents) { triggerA.OnCollisionEnded(b, this); triggerB.OnCollisionEnded(a, this); } } previousContactCount = ContactManifold.contacts.Count; } /// /// Gets the number of contacts associated with this pair handler. /// protected internal override int ContactCount { get { return ContactManifold.contacts.Count; } } /// /// Clears the contacts associated with this pair handler. /// public override void ClearContacts() { if (previousContactCount > 0) { //Just exited collision. //Remove the solver item. if (Parent != null) Parent.RemoveSolverUpdateable(ContactConstraint); else if (NarrowPhase != null) NarrowPhase.NotifyUpdateableRemoved(ContactConstraint); if (!suppressEvents) { var a = CollidableA; var b = CollidableB; a.EventTriggerer.OnCollisionEnded(b, this); b.EventTriggerer.OnCollisionEnded(a, this); } } ContactManifold.ClearContacts(); base.ClearContacts(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticGroupCompoundPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.DataStructures; using BEPUutilities.DataStructures; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-compound collision pair. /// public class StaticGroupCompoundPairHandler : StaticGroupPairHandler { CompoundCollidable compoundInfo; public override Collidable CollidableB { get { return compoundInfo; } } public override Entities.Entity EntityB { get { return compoundInfo.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { compoundInfo = entryA as CompoundCollidable; if (compoundInfo == null) { compoundInfo = entryB as CompoundCollidable; if (compoundInfo == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); compoundInfo = null; } //Some danger of unintuitive-to-address allocations here. If these lists get huge, the user will see some RawList<<>> goofiness in the profiler. //They can still address it by clearing out the cached pair factories though. RawList> overlappedElements = new RawList>(); protected override void UpdateContainedPairs() { staticGroup.Shape.CollidableTree.GetOverlaps(compoundInfo.hierarchy.Tree, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { var element = overlappedElements.Elements[i]; var staticCollidable = element.OverlapA as StaticCollidable; TryToAdd(element.OverlapA, element.OverlapB.CollisionInformation, staticCollidable != null ? staticCollidable.Material : staticGroup.Material, element.OverlapB.Material); } overlappedElements.Clear(); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticGroupConvexPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound and convex collision pair. /// public class StaticGroupConvexPairHandler : StaticGroupPairHandler { ConvexCollidable convexInfo; public override Collidable CollidableB { get { return convexInfo; } } public override Entities.Entity EntityB { get { return convexInfo.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { convexInfo = entryA as ConvexCollidable; if (convexInfo == null) { convexInfo = entryB as ConvexCollidable; if (convexInfo == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); convexInfo = null; } protected override void UpdateContainedPairs() { var overlappedElements = PhysicsResources.GetCollidableList(); staticGroup.Shape.CollidableTree.GetOverlaps(convexInfo.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { var staticCollidable = overlappedElements.Elements[i] as StaticCollidable; TryToAdd(overlappedElements.Elements[i], CollidableB, staticCollidable != null ? staticCollidable.Material : staticGroup.Material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticGroupMobileMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound-instanced mesh collision pair. /// public class StaticGroupMobileMeshPairHandler : StaticGroupPairHandler { MobileMeshCollidable mesh; public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityB { get { return mesh.entity; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as MobileMeshCollidable; if (mesh == null) { mesh = entryB as MobileMeshCollidable; if (mesh == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; } protected override void UpdateContainedPairs() { var overlappedElements = PhysicsResources.GetCollidableList(); staticGroup.Shape.CollidableTree.GetOverlaps(mesh.boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; i++) { var staticCollidable = overlappedElements.Elements[i] as StaticCollidable; TryToAdd(overlappedElements.Elements[i], mesh, staticCollidable != null ? staticCollidable.Material : staticGroup.Material); } PhysicsResources.GiveBack(overlappedElements); } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticGroupPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a compound and group collision pair. /// public abstract class StaticGroupPairHandler : GroupPairHandler { protected StaticGroup staticGroup; public override Collidable CollidableA { get { return staticGroup; } } public override Entities.Entity EntityA { get { return null; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { //Other member of the pair is initialized by the child. staticGroup = entryA as StaticGroup; if (staticGroup == null) { staticGroup = entryB as StaticGroup; if (staticGroup == null) { throw new ArgumentException("Inappropriate types used to initialize pair."); } } base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); staticGroup = null; //Child type needs to null out other reference. } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticMeshConvexPairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a static mesh-convex collision pair. /// public class StaticMeshConvexPairHandler : StaticMeshPairHandler { StaticMeshConvexContactManifold contactManifold = new StaticMeshConvexContactManifold(); protected override StaticMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticMeshPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a static mesh-convex collision pair. /// public abstract class StaticMeshPairHandler : StandardPairHandler { StaticMesh mesh; ConvexCollidable convex; NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return convex; } } public override Collidable CollidableB { get { return mesh; } } public override Entities.Entity EntityA { get { return convex.entity; } } public override Entities.Entity EntityB { get { return null; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return MeshManifold; } } protected abstract StaticMeshContactManifold MeshManifold { get; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { mesh = entryA as StaticMesh; convex = entryB as ConvexCollidable; if (mesh == null || convex == null) { mesh = entryB as StaticMesh; convex = entryA as ConvexCollidable; if (mesh == null || convex == null) throw new ArgumentException("Inappropriate types used to initialize pair."); } //Contact normal goes from A to B. broadPhaseOverlap.entryA = convex; broadPhaseOverlap.entryB = mesh; UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, mesh.material); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); mesh = null; convex = null; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsResources.GetTriangle(); triangle.collisionMargin = 0; //Spherecast against all triangles to find the earliest time. for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++) { mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { if (mesh.sidedness != TriangleSidedness.DoubleSided) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref normal, ref rayHit.Normal, out dot); //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || mesh.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } PhysicsResources.GiveBack(triangle); } } } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = MeshManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity if (convex.entity != null) { Vector3 velocity; Vector3.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/StaticMeshSpherePairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a static mesh-sphere collision pair. /// public class StaticMeshSpherePairHandler : StaticMeshPairHandler { StaticMeshSphereContactManifold contactManifold = new StaticMeshSphereContactManifold(); protected override StaticMeshContactManifold MeshManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/TerrainConvexPairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a terrain-convex collision pair. /// public sealed class TerrainConvexPairHandler : TerrainPairHandler { protected TerrainConvexContactManifold contactManifold = new TerrainConvexContactManifold(); protected override TerrainContactManifold TerrainManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/TerrainPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a terrain-convex collision pair. /// public abstract class TerrainPairHandler : StandardPairHandler { Terrain terrain; ConvexCollidable convex; NonConvexContactManifoldConstraint contactConstraint = new NonConvexContactManifoldConstraint(); public override Collidable CollidableA { get { return convex; } } public override Collidable CollidableB { get { return terrain; } } public override Entities.Entity EntityA { get { return convex.entity; } } public override Entities.Entity EntityB { get { return null; } } /// /// Gets the contact constraint used by the pair handler. /// public override ContactManifoldConstraint ContactConstraint { get { return contactConstraint; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return TerrainManifold; } } protected abstract TerrainContactManifold TerrainManifold { get; } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { terrain = entryA as Terrain; convex = entryB as ConvexCollidable; if (terrain == null || convex == null) { terrain = entryB as Terrain; convex = entryA as ConvexCollidable; if (terrain == null || convex == null) throw new ArgumentException("Inappropriate types used to initialize pair."); } //Contact normal goes from A to B. broadPhaseOverlap.entryA = convex; broadPhaseOverlap.entryB = terrain; UpdateMaterialProperties(convex.entity != null ? convex.entity.material : null, terrain.material); base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); terrain = null; convex = null; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { //Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us. if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous) { //TODO: This system could be made more robust by using a similar region-based rejection of edges. //CCD events are awfully rare under normal circumstances, so this isn't usually an issue. //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadius * minimumRadius < velocitySquared) { var triangle = PhysicsResources.GetTriangle(); triangle.collisionMargin = 0; Vector3 terrainUp = new Vector3(terrain.worldTransform.LinearTransform.M21, terrain.worldTransform.LinearTransform.M22, terrain.worldTransform.LinearTransform.M23); //Spherecast against all triangles to find the earliest time. for (int i = 0; i < TerrainManifold.overlappedTriangles.Count; i++) { terrain.Shape.GetTriangle(ref TerrainManifold.overlappedTriangles.Elements[i], ref terrain.worldTransform, out triangle.vA, out triangle.vB, out triangle.vC); //Put the triangle into 'localish' space of the convex. Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA); Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB); Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC); RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) && rayHit.T > Toolbox.BigEpsilon) { Vector3 AB, AC; Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB); Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC); Vector3 normal; Vector3.Cross(ref AC, ref AB, out normal); float dot; Vector3.Dot(ref normal, ref terrainUp, out dot); if (dot < 0) Vector3.Dot(ref normal, ref rayHit.Normal, out dot); else { Vector3.Dot(ref normal, ref rayHit.Normal, out dot); dot = -dot; } //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. if (dot < 0) { timeOfImpact = rayHit.T; } } } PhysicsResources.GiveBack(triangle); } } } protected internal override void GetContactInformation(int index, out ContactInformation info) { info.Contact = TerrainManifold.contacts.Elements[index]; //Find the contact's normal and friction forces. info.FrictionImpulse = 0; info.NormalImpulse = 0; for (int i = 0; i < contactConstraint.frictionConstraints.Count; i++) { if (contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.contact == info.Contact) { info.FrictionImpulse = contactConstraint.frictionConstraints.Elements[i].accumulatedImpulse; info.NormalImpulse = contactConstraint.frictionConstraints.Elements[i].PenetrationConstraint.accumulatedImpulse; break; } } //Compute relative velocity if (convex.entity != null) { Vector3 velocity; Vector3.Subtract(ref info.Contact.Position, ref convex.entity.position, out velocity); Vector3.Cross(ref convex.entity.angularVelocity, ref velocity, out velocity); Vector3.Add(ref velocity, ref convex.entity.linearVelocity, out info.RelativeVelocity); } else info.RelativeVelocity = new Vector3(); info.Pair = this; } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/TerrainSpherePairHandler.cs ================================================ using BEPUphysics.CollisionTests.Manifolds; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a terrain-sphere collision pair. /// public sealed class TerrainSpherePairHandler : TerrainPairHandler { protected TerrainSphereContactManifold contactManifold = new TerrainSphereContactManifold(); protected override TerrainContactManifold TerrainManifold { get { return contactManifold; } } } } ================================================ FILE: BEPUphysics/NarrowPhaseSystems/Pairs/TriangleConvexPairHandler.cs ================================================ using System; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests; using BEPUphysics.CollisionTests.CollisionAlgorithms.GJK; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Constraints.Collision; using BEPUphysics.PositionUpdating; using BEPUphysics.Settings; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.CollisionShapes.ConvexShapes; namespace BEPUphysics.NarrowPhaseSystems.Pairs { /// /// Handles a triangle-convex collision pair. /// public class TriangleConvexPairHandler : ConvexConstraintPairHandler { ConvexCollidable triangle; ConvexCollidable convex; TriangleConvexContactManifold contactManifold = new TriangleConvexContactManifold(); public override Collidable CollidableA { get { return convex; } } public override Collidable CollidableB { get { return triangle; } } public override Entities.Entity EntityA { get { return convex.entity; } } public override Entities.Entity EntityB { get { return triangle.entity; } } /// /// Gets the contact manifold used by the pair handler. /// public override ContactManifold ContactManifold { get { return contactManifold; } } /// /// Initializes the pair handler. /// ///First entry in the pair. ///Second entry in the pair. public override void Initialize(BroadPhaseEntry entryA, BroadPhaseEntry entryB) { triangle = entryA as ConvexCollidable; convex = entryB as ConvexCollidable; if (triangle == null || convex == null) { triangle = entryB as ConvexCollidable; convex = entryA as ConvexCollidable; if (triangle == null || convex == null) throw new ArgumentException("Inappropriate types used to initialize pair."); } //Contact normal goes from A to B. broadPhaseOverlap.entryA = convex; broadPhaseOverlap.entryB = triangle; base.Initialize(entryA, entryB); } /// /// Cleans up the pair handler. /// public override void CleanUp() { base.CleanUp(); triangle = null; convex = null; } /// /// Updates the time of impact for the pair. /// ///Collidable requesting the update. ///Timestep duration. public override void UpdateTimeOfImpact(Collidable requester, float dt) { var overlap = BroadPhaseOverlap; var triangleMode = triangle.entity == null ? PositionUpdateMode.Discrete : triangle.entity.PositionUpdateMode; var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode; if ( (overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active. ( ( convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A. triangleMode == PositionUpdateMode.Continuous && overlap.entryA == requester ) || ( convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it. triangleMode == PositionUpdateMode.Continuous ) ) ) { //Only perform the test if the minimum radii are small enough relative to the size of the velocity. Vector3 velocity; if (convexMode == PositionUpdateMode.Discrete) { //Triangle is static for the purposes of this continuous test. velocity = triangle.entity.linearVelocity; } else if (triangleMode == PositionUpdateMode.Discrete) { //Convex is static for the purposes of this continuous test. Vector3.Negate(ref convex.entity.linearVelocity, out velocity); } else { //Both objects are moving. Vector3.Subtract(ref triangle.entity.linearVelocity, ref convex.entity.linearVelocity, out velocity); } Vector3.Multiply(ref velocity, dt, out velocity); float velocitySquared = velocity.LengthSquared(); var minimumRadiusA = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling; timeOfImpact = 1; if (minimumRadiusA * minimumRadiusA < velocitySquared) { //Spherecast A against B. RayHit rayHit; if (GJKToolbox.CCDSphereCast(new Ray(convex.worldTransform.Position, -velocity), minimumRadiusA, triangle.Shape, ref triangle.worldTransform, timeOfImpact, out rayHit)) { if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided) { //Only perform sweep if the object is in danger of hitting the object. //Triangles can be one sided, so check the impact normal against the triangle normal. Vector3 AB, AC; Vector3.Subtract(ref triangle.Shape.vB, ref triangle.Shape.vA, out AB); Vector3.Subtract(ref triangle.Shape.vC, ref triangle.Shape.vA, out AC); Vector3 normal; Vector3.Cross(ref AB, ref AC, out normal); float dot; Vector3.Dot(ref rayHit.Normal, ref normal, out dot); if (triangle.Shape.sidedness == TriangleSidedness.Counterclockwise && dot < 0 || triangle.Shape.sidedness == TriangleSidedness.Clockwise && dot > 0) { timeOfImpact = rayHit.T; } } else { timeOfImpact = rayHit.T; } } } //TECHNICALLY, the triangle should be casted too. But, given the way triangles are usually used and their tiny minimum radius, ignoring it is usually just fine. //var minimumRadiusB = triangle.minimumRadius * MotionSettings.CoreShapeScaling; //if (minimumRadiusB * minimumRadiusB < velocitySquared) //{ // //Spherecast B against A. // RayHit rayHit; // if (GJKToolbox.SphereCast(new Ray(triangle.entity.position, velocity), minimumRadiusB, convex.Shape, ref convex.worldTransform, 1, out rayHit) && // rayHit.T < timeOfImpact) // { // if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided) // { // float dot; // Vector3.Dot(ref rayHit.Normal, ref normal, out dot); // if (dot > 0) // { // timeOfImpact = rayHit.T; // } // } // else // { // timeOfImpact = rayHit.T; // } // } //} //If it's intersecting, throw our hands into the air and give up. //This is generally a perfectly acceptable thing to do, since it's either sitting //inside another object (no ccd makes sense) or we're still in an intersecting case //from a previous frame where CCD took place and a contact should have been created //to deal with interpenetrating velocity. Sometimes that contact isn't sufficient, //but it's good enough. if (timeOfImpact == 0) timeOfImpact = 1; } } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/BoundingBoxUpdater.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Threading; using BEPUutilities; using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.OtherSpaceStages { /// /// Updates the bounding box of managed objects. /// public class BoundingBoxUpdater : MultithreadedProcessingStage { //TODO: should the Entries field be publicly accessible since there's not any custom add/remove logic? RawList entries = new RawList(); TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the updater. /// public TimeStepSettings TimeStepSettings { get; set; } /// /// Constructs the bounding box updater. /// ///Time step setttings to be used by the updater. public BoundingBoxUpdater(TimeStepSettings timeStepSettings) { multithreadedLoopBodyDelegate = LoopBody; Enabled = true; this.timeStepSettings = timeStepSettings; } /// /// Constructs the bounding box updater. /// ///Time step setttings to be used by the updater. /// Thread manager to be used by the updater. public BoundingBoxUpdater(TimeStepSettings timeStepSettings, IThreadManager threadManager) : this(timeStepSettings) { ThreadManager = threadManager; AllowMultithreading = true; } Action multithreadedLoopBodyDelegate; void LoopBody(int i) { var entry = entries.Elements[i]; if (entry.IsActive) { entry.UpdateBoundingBox(timeStepSettings.TimeStepDuration); entry.boundingBox.Validate(); } } /// /// Adds an entry to the updater. /// ///Entry to add. public void Add(MobileCollidable entry) { //TODO: Contains check? entries.Add(entry); } /// /// Removes an entry from the updater. /// ///Entry to remove. public void Remove(MobileCollidable entry) { entries.Remove(entry); } protected override void UpdateMultithreaded() { ThreadManager.ForLoop(0, entries.Count, multithreadedLoopBodyDelegate); } protected override void UpdateSingleThreaded() { for (int i = 0; i < entries.Count; ++i) { LoopBody(i); } } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/DeferredEventDispatcher.cs ================================================ using System; using System.Collections.Generic; namespace BEPUphysics.OtherSpaceStages { /// /// Manages the deferred events spawned by IDeferredEventCreators and dispatches them on update. /// public class DeferredEventDispatcher : ProcessingStage { private List activeEventCreators = new List(); /// /// Constructs the dispatcher. /// public DeferredEventDispatcher() { Enabled = true; } /// /// Adds an event creator. /// ///Creator to add. ///Thrown when the creator is already managed by a dispatcher. public void AddEventCreator(IDeferredEventCreator creator) { if (creator.DeferredEventDispatcher == null) { creator.DeferredEventDispatcher = this; //If it already has events attached, add it to the active event creators list. //Otherwise, don't bother adding it until it has some. //It is up to the creator to notify the dispatcher of the change. if (creator.IsActive) activeEventCreators.Add(creator); } else throw new ArgumentException("The event creator is already managed by a dispatcher; it cannot be added.", "creator"); } /// /// Removes an event creator. /// /// Creator to remove. public void RemoveEventCreator(IDeferredEventCreator creator) { if (creator.DeferredEventDispatcher == this) { creator.DeferredEventDispatcher = null; if (creator.IsActive) activeEventCreators.Remove(creator); } else throw new ArgumentException("The event creator is managed by a different dispatcher; it cannot be removed.", "creator"); } /// /// Notifies the dispatcher that the event activity of a creator has changed. /// ///Cretor whose activity has changed. ///Thrown when the event creator's state hasn't changed. public void CreatorActivityChanged(IDeferredEventCreator creator) { //This is a pretty rarely called method. It's okay to do a little extra verification at the cost of performance. if (creator.IsActive) if (!activeEventCreators.Contains(creator)) activeEventCreators.Add(creator); else throw new ArgumentException("The event creator was already active in the dispatcher; make sure the CreatorActivityChanged function is only called when the state actually changes.", "creator"); else if (!activeEventCreators.Remove(creator)) throw new ArgumentException("The event creator not active in the dispatcher; make sure the CreatorActivityChanged function is only called when the state actually changes.", "creator"); } protected override void UpdateStage() { for (int i = activeEventCreators.Count - 1; i >= 0; i--) { activeEventCreators[i].DispatchEvents(); // Since it can attempt to remove or add any number of creators during the handler, all we can do is make sure // that our current index is still valid. We may re-dispatch some event creators, but assuming they follow the // proper pattern for deferred event creators, it will be okay. The first execution cleans out all of the // deferred events that have been queued up. The second execution will try, but the queues will be empty. // This is a fairly acceptable result for a rare case. if (i > activeEventCreators.Count) i = activeEventCreators.Count; } } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/ForceUpdater.cs ================================================ using System; using BEPUphysics.Threading; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; using BEPUutilities; namespace BEPUphysics.OtherSpaceStages { /// /// Applies forces to managed objects. /// public class ForceUpdater : MultithreadedProcessingStage { RawList dynamicObjects = new RawList(); protected internal Vector3 gravity; /// /// Gets or sets the gravity applied by the force updater. /// public Vector3 Gravity { get { return gravity; } set { gravity = value; } } internal Vector3 gravityDt; protected TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the force updater. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } set { timeStepSettings = value; } } /// /// Constructs the force updater. /// ///Time step settings to use. public ForceUpdater(TimeStepSettings timeStepSettings) { TimeStepSettings = timeStepSettings; Enabled = true; multithreadedLoopBodyDelegate = UpdateObject; } /// /// Constructs the force updater. /// ///Time step settings to use. /// Thread manager to use. public ForceUpdater(TimeStepSettings timeStepSettings, IThreadManager threadManager) : this(timeStepSettings) { ThreadManager = threadManager; AllowMultithreading = true; } private Action multithreadedLoopBodyDelegate; void UpdateObject(int i) { if (dynamicObjects.Elements[i].IsActive) dynamicObjects.Elements[i].UpdateForForces(timeStepSettings.TimeStepDuration); } protected override void UpdateMultithreaded() { Vector3.Multiply(ref gravity, timeStepSettings.TimeStepDuration, out gravityDt); ThreadManager.ForLoop(0, dynamicObjects.Count, multithreadedLoopBodyDelegate); } protected override void UpdateSingleThreaded() { Vector3.Multiply(ref gravity, timeStepSettings.TimeStepDuration, out gravityDt); for (int i = 0; i < dynamicObjects.Count; i++) { UpdateObject(i); } } /// /// Adds a force updateable to the force updater. /// ///Item to add. ///Thrown when the item already belongs to a force updater. public void Add(IForceUpdateable forceUpdateable) { if (forceUpdateable.ForceUpdater == null) { forceUpdateable.ForceUpdater = this; if (forceUpdateable.IsDynamic) dynamicObjects.Add(forceUpdateable); } else throw new ArgumentException("Cannot add updateable; it already belongs to another manager."); } /// /// Removes a force updateable from the force updater. /// ///Item to remove. ///Thrown when the item does not belong to this force updater or its state is corrupted. public void Remove(IForceUpdateable forceUpdateable) { if (forceUpdateable.ForceUpdater == this) { if (forceUpdateable.IsDynamic && !dynamicObjects.Remove(forceUpdateable)) throw new InvalidOperationException("Dynamic object not present in dynamic objects list; ensure that the IForceUpdateable was never removed from the list improperly by using ForceUpdateableBecomingKinematic."); forceUpdateable.ForceUpdater = null; } else throw new ArgumentException("Cannot remove updateable; it does not belong to this manager."); } /// /// Notifies the system that a force updateable is becoming dynamic. /// /// Updateable changing state. public void ForceUpdateableBecomingDynamic(IForceUpdateable updateable) { //This does not verify that it used to be kinematic. Small room for unsafety. if (updateable.ForceUpdater == this) { dynamicObjects.Add(updateable); } else throw new ArgumentException("Updateable does not belong to this manager."); } /// /// Notifies the system that a force updateable is becoming kinematic. /// /// Updateable changing state. public void ForceUpdateableBecomingKinematic(IForceUpdateable updateable) { //This does not verify that it used to be dynamic. Small room for unsafety. if (updateable.ForceUpdater == this) { if (!dynamicObjects.Remove(updateable)) throw new InvalidOperationException("Dynamic object not present in dynamic objects list; ensure that the IVelocityUpdateable was never removed from the list improperly by using VelocityUpdateableBecomingKinematic."); } else throw new ArgumentException("Updateable does not belong to this manager."); } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/IDeferredEventCreator.cs ================================================ namespace BEPUphysics.OtherSpaceStages { /// /// Defines an object which can create deferred events. /// public interface IDeferredEventCreator { //Needs backreference in order to add/remove itself to the dispatcher when deferred event handlers are added/removed. /// /// Gets or sets the deferred event dispatcher that owns this creator. /// DeferredEventDispatcher DeferredEventDispatcher { get; set; } /// /// Gets or sets the activity state of this creator. /// bool IsActive { get; set; } /// /// Dispatches the events created by this creator. /// void DispatchEvents(); /// /// Gets or sets the number of child deferred event creators. /// int ChildDeferredEventCreators { get; set; } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/IDeferredEventCreatorOwner.cs ================================================ namespace BEPUphysics.OtherSpaceStages { /// /// Defines an object that owns a deferred event creator. /// public interface IDeferredEventCreatorOwner { /// /// Gets the event creator owned by the object. /// IDeferredEventCreator EventCreator { get; } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/IForceUpdateable.cs ================================================ namespace BEPUphysics.OtherSpaceStages { /// /// Defines an object which can be updated using forces by the ForceUpdater. /// public interface IForceUpdateable { /// /// Applies forces to the object. /// ///Time step duration. void UpdateForForces(float dt); /// /// Force updater that owns this object. /// ForceUpdater ForceUpdater { get; set; } /// /// Gets whether or not this object is dynamic. /// Only dynamic objects are updated by the force updater. /// bool IsDynamic { get; } /// /// Gets whether or not this object is active. Only active objects are updated by the force updater. /// bool IsActive { get; } } } ================================================ FILE: BEPUphysics/OtherSpaceStages/SpaceObjectBuffer.cs ================================================ using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.OtherSpaceStages { /// /// Thead-safely buffers up space objects for addition and removal. /// public class SpaceObjectBuffer : ProcessingStage { private struct SpaceObjectChange { public readonly ISpaceObject SpaceObject; //Could change to enumeration, or more generally, buffered 'action' to perform on the space object. public readonly bool ShouldAdd; public SpaceObjectChange(ISpaceObject spaceObject, bool shouldAdd) { SpaceObject = spaceObject; ShouldAdd = shouldAdd; } } private ConcurrentDeque objectsToChange = new ConcurrentDeque(); private ISpace space; /// /// Gets the space which owns this buffer. /// public ISpace Space { get { return space; } } /// /// Constructs the buffer. /// ///Space that owns the buffer. public SpaceObjectBuffer(ISpace space) { Enabled = true; this.space = space; } /// /// Adds a space object to the buffer. /// It will be added to the space the next time the buffer is flushed. /// ///Space object to add. public void Add(ISpaceObject spaceObject) { objectsToChange.Enqueue(new SpaceObjectChange(spaceObject, true)); } /// /// Enqueues a removal request to the buffer. /// It will be processed the next time the buffer is flushed. /// /// Space object to remove. public void Remove(ISpaceObject spaceObject) { objectsToChange.Enqueue(new SpaceObjectChange(spaceObject, false)); } protected override void UpdateStage() { SpaceObjectChange change; while (objectsToChange.TryDequeueFirst(out change)) { if (change.ShouldAdd) space.Add(change.SpaceObject); else space.Remove(change.SpaceObject); } } } } ================================================ FILE: BEPUphysics/Paths/CardinalSpline3D.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Cardinal spline implementation of the 3D hermite curve. Uses a tension parameter to control /// the tightness of the curve. When tension is zero, a cardinal spline acts like a Catmull-Rom spline. /// public class CardinalSpline3D : HermiteCurve3D { private float tension; /// /// Gets or sets the tension parameter of the cardinal spline. /// A value of 0 acts like a Catmull-Rom spline, while a /// value of 1 produces 0-length tangents. /// public float Tension { get { return tension; } set { tension = MathHelper.Clamp(value, 0, 1); } } /// /// Gets the curve's bounding index information. /// /// Index of the minimum control point in the active curve segment. /// Index of the maximum control point in the active curve segment. public override void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex) { if (ControlPoints.Count > 3) { minIndex = 1; maxIndex = ControlPoints.Count - 2; } else { minIndex = -1; maxIndex = -1; } } protected override void ComputeTangents() { tangents.Add(Vector3.Zero); for (int i = 1; i < ControlPoints.Count - 1; i++) { Vector3 tangent; Vector3 previous = ControlPoints[i - 1].Value; Vector3 next = ControlPoints[i + 1].Value; Vector3.Subtract(ref next, ref previous, out tangent); Vector3.Multiply(ref tangent, (float)((1 - tension) / (ControlPoints[i + 1].Time - ControlPoints[i - 1].Time)), out tangent); tangents.Add(tangent); } tangents.Add(Vector3.Zero); } } } ================================================ FILE: BEPUphysics/Paths/ConstantAngularSpeedCurve.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Wrapper around an orientation curve that specifies a specific velocity at which to travel. /// public class ConstantAngularSpeedCurve : ConstantSpeedCurve { /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. public ConstantAngularSpeedCurve(float speed, Curve curve) : base(speed, curve) { } /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. public ConstantAngularSpeedCurve(float speed, Curve curve, int sampleCount) : base(speed, curve, sampleCount) { } protected override float GetDistance(Quaternion start, Quaternion end) { Quaternion.Conjugate(ref end, out end); Quaternion.Multiply(ref end, ref start, out end); return Toolbox.GetAngleFromQuaternion(ref end); } } } ================================================ FILE: BEPUphysics/Paths/ConstantLinearSpeedCurve.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Wrapper around a 3d position curve that specifies a specific velocity at which to travel. /// public class ConstantLinearSpeedCurve : ConstantSpeedCurve { /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. public ConstantLinearSpeedCurve(float speed, Curve curve) : base(speed, curve) { } /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. public ConstantLinearSpeedCurve(float speed, Curve curve, int sampleCount) : base(speed, curve, sampleCount) { } protected override float GetDistance(Vector3 start, Vector3 end) { float distance; Vector3.Distance(ref start, ref end, out distance); return distance; } } } ================================================ FILE: BEPUphysics/Paths/ConstantSpeedCurve.cs ================================================ namespace BEPUphysics.Paths { /// /// Superclass of speed-controlled curves that have a constant speed. /// /// Type of the values in the curve. public abstract class ConstantSpeedCurve : SpeedControlledCurve { /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. protected ConstantSpeedCurve(float speed, Curve curve) : base(curve) { Speed = speed; ResampleCurve(); } /// /// Constructs a new constant speed curve. /// /// Speed to maintain while traveling around a curve. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. protected ConstantSpeedCurve(float speed, Curve curve, int sampleCount) : base(curve, sampleCount) { Speed = speed; ResampleCurve(); } /// /// Gets or sets the speed of the curve. /// public float Speed { get; set; } /// /// Gets the desired speed at a given time. /// /// Time to check for speed. /// Speed at the given time. public override float GetSpeedAtCurveTime(double time) { return Speed; } } } ================================================ FILE: BEPUphysics/Paths/Curve.cs ================================================ using System; using BEPUutilities; namespace BEPUphysics.Paths { /// /// Manages a curve in 3D space that supports interpolation. /// /// Type of values in the curve. public abstract class Curve : Path { /// /// Constructs a new 3D curve. /// protected Curve() { ControlPoints = new CurveControlPointList(this); } /// /// Gets the list of control points in the curve. /// public CurveControlPointList ControlPoints { get; private set; } /// /// Defines how the curve is sampled when the evaluation time exceeds the final control point. /// public CurveEndpointBehavior PostLoop { get; set; } /// /// Defines how the curve is sampled when the evaluation time exceeds the beginning control point. /// public CurveEndpointBehavior PreLoop { get; set; } /// /// Converts an unbounded time to a time within the curve's interval using the /// endpoint behavior. /// /// Time to convert. /// Beginning of the curve's interval. /// End of the curve's interval. /// Looping behavior of the curve before the first endpoint's time. /// Looping behavior of the curve after the last endpoint's time. /// Time within the curve's interval. public static double ModifyTime(double time, double intervalBegin, double intervalEnd, CurveEndpointBehavior preLoop, CurveEndpointBehavior postLoop) { if (time < intervalBegin) { switch (preLoop) { case CurveEndpointBehavior.Wrap: double modifiedTime = time - intervalBegin; double intervalLength = intervalEnd - intervalBegin; modifiedTime %= intervalLength; return intervalEnd + modifiedTime; case CurveEndpointBehavior.Clamp: return Math.Max(intervalBegin, time); case CurveEndpointBehavior.Mirror: modifiedTime = time - intervalBegin; intervalLength = intervalEnd - intervalBegin; var numFlips = (int) (modifiedTime / intervalLength); if (numFlips % 2 == 0) return intervalBegin - modifiedTime % intervalLength; return intervalEnd + modifiedTime % intervalLength; } } else if (time >= intervalEnd) { switch (postLoop) { case CurveEndpointBehavior.Wrap: double modifiedTime = time - intervalEnd; double intervalLength = intervalEnd - intervalBegin; modifiedTime %= intervalLength; return intervalBegin + modifiedTime; case CurveEndpointBehavior.Clamp: return Math.Min(intervalEnd, time); case CurveEndpointBehavior.Mirror: modifiedTime = time - intervalEnd; intervalLength = intervalEnd - intervalBegin; var numFlips = (int) (modifiedTime / intervalLength); if (numFlips % 2 == 0) return intervalEnd - modifiedTime % intervalLength; return intervalBegin + modifiedTime % intervalLength; } } return time; } /// /// Evaluates the curve section starting at the control point index using /// the weight value. /// /// Index of the starting control point of the subinterval. /// Location to evaluate on the subinterval from 0 to 1. /// Value at the given location. public abstract void Evaluate(int controlPointIndex, float weight, out TValue value); /// /// Gets the curve's bounding index information. /// /// Index of the minimum control point in the active curve segment. /// Index of the maximum control point in the active curve segment. public abstract void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex); /// /// Computes the value of the curve at a given time. /// /// Time at which to evaluate the curve. /// Curve value at the given time. public override void Evaluate(double time, out TValue value) { double firstTime, lastTime; int minIndex, maxIndex; GetCurveBoundsInformation(out firstTime, out lastTime, out minIndex, out maxIndex); if (minIndex < 0 || maxIndex < 0) { //Invalid bounds, quit with default value = default(TValue); return; } if (minIndex == maxIndex) { //1-length curve; asking the system to evaluate //this will be a waste of time AND //crash since +1 will be outside scope value = ControlPoints[minIndex].Value; return; } time = ModifyTime(time, firstTime, lastTime, PreLoop, PostLoop); int index = GetPreviousIndex(time); if (index == maxIndex) { //Somehow the index is the very last index, so next index would be invalid. //Just 'clamp' it. //This generally implies a bug, but it might also just be some very close floating point issue. value = ControlPoints[maxIndex].Value; } else { var denominator = ControlPoints[index + 1].Time - ControlPoints[index].Time; float intervalTime; if (denominator < Toolbox.Epsilon) intervalTime = 0; else intervalTime = (float) ((time - ControlPoints[index].Time) / denominator); Evaluate(index, intervalTime, out value); } } /// /// Gets the starting and ending times of the path. /// /// Beginning time of the path. /// Ending time of the path. public override void GetPathBoundsInformation(out double startingTime, out double endingTime) { int index; GetCurveBoundsInformation(out startingTime, out endingTime, out index, out index); } /// /// Gets information about the curve's total active interval. /// These are not always the first and last endpoints in a curve. /// /// Time of the first index. /// Time of the last index. /// First index in the reachable curve. /// Last index in the reachable curve. public void GetCurveBoundsInformation(out double firstIndexTime, out double lastIndexTime, out int minIndex, out int maxIndex) { GetCurveIndexBoundsInformation(out minIndex, out maxIndex); if (minIndex >= 0 && maxIndex < ControlPoints.Count && minIndex <= maxIndex) { firstIndexTime = ControlPoints[minIndex].Time; lastIndexTime = ControlPoints[maxIndex].Time; } else { firstIndexTime = 0; lastIndexTime = 0; } } /// /// Computes the indices of control points surrounding the time. /// If the time is equal to a control point's time, indexA will /// be that control point's index. /// /// Time to index. /// Index prior to or equal to the given time. public int GetPreviousIndex(double time) { int indexMin = 0; int indexMax = ControlPoints.Count; if (indexMax == 0) return -1; //If time < controlpoints.mintime, should be... 0 or -1? while (indexMax - indexMin > 1) //if time belongs to min { int midIndex = (indexMin + indexMax) / 2; if (time > ControlPoints[midIndex].Time) { indexMin = midIndex; } else { indexMax = midIndex; } } return indexMin; } internal void InternalControlPointTimeChanged(CurveControlPoint controlPoint) { int oldIndex = ControlPoints.list.IndexOf(controlPoint); ControlPoints.list.RemoveAt(oldIndex); int index = GetPreviousIndex(controlPoint.Time) + 1; ControlPoints.list.Insert(index, controlPoint); ControlPointTimeChanged(controlPoint, oldIndex, index); } /// /// Called when a control point is added. /// /// New control point. /// Index of the control point. protected internal abstract void ControlPointAdded(CurveControlPoint curveControlPoint, int index); /// /// Called when a control point is removed. /// /// Removed control point. /// Index of the control point before it was removed. protected internal abstract void ControlPointRemoved(CurveControlPoint curveControlPoint, int oldIndex); /// /// Called when a control point belonging to the curve has its time changed. /// /// Changed control point. /// Old index of the control point. /// New index of the control point. protected internal abstract void ControlPointTimeChanged(CurveControlPoint curveControlPoint, int oldIndex, int newIndex); /// /// Called when a control point belonging to the curve has its value changed. /// /// Changed control point. protected internal abstract void ControlPointValueChanged(CurveControlPoint curveControlPoint); } } ================================================ FILE: BEPUphysics/Paths/CurveControlPoint.cs ================================================ using System; using System.Linq; namespace BEPUphysics.Paths { /// /// Point defining the shape of a 3D curve. /// /// Type of values in the curve. public class CurveControlPoint : IComparable> { private double time; private TValue value; /// /// Constructs a new 3D curve control point. /// /// Time at which the point is positioned. /// Value of the control point. /// Curve associated with the control point. public CurveControlPoint(double time, TValue value, Curve curve) { Curve = curve; Time = time; Value = value; } /// /// Gets the curve associated with this control point. /// public Curve Curve { get; private set; } /// /// Gets or sets the time at which this control point is positioned. /// public double Time { get { return time; } set { time = value; if (Curve.ControlPoints.Contains(this)) { Curve.InternalControlPointTimeChanged(this); } } } /// /// Gets or sets the value of this control point. /// public TValue Value { get { return value; } set { this.value = value; if (Curve.ControlPoints.Contains(this)) { Curve.ControlPointValueChanged(this); } } } #region IComparable> Members /// /// Compares the two control points based on their time. /// /// Control point to compare. /// -1 if the current instance has a smaller time, 0 if equal, and 1 if the current instance has a larger time. public int CompareTo(CurveControlPoint other) { if (other.Time < Time) return 1; if (other.Time > Time) return -1; return 0; } #endregion } } ================================================ FILE: BEPUphysics/Paths/CurveControlPointList.cs ================================================ using System.Collections; using System.Collections.Generic; namespace BEPUphysics.Paths { /// /// Collection of control points in a curve. /// /// Type of values in the curve. public class CurveControlPointList : IEnumerable> { internal List> list = new List>(); internal CurveControlPointList(Curve curve) { Curve = curve; } /// /// Gets the control point at the given index. /// /// Index into the list. /// Control point at the index. public CurveControlPoint this[int index] { get { return list[index]; } } /// /// Gets the number of elements in the list. /// public int Count { get { return list.Count; } } /// /// Gets the curve associated with this control point list. /// public Curve Curve { get; private set; } #region IEnumerable> Members /// /// Gets an enumerator for the list. /// ///Enumerator for the list. public List>.Enumerator GetEnumerator() { return list.GetEnumerator(); } IEnumerator> IEnumerable>.GetEnumerator() { return list.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return list.GetEnumerator(); } #endregion /// /// Adds a control point to the curve. /// /// New control point to add to the curve. public void Add(CurveControlPoint point) { int index = Curve.GetPreviousIndex(point.Time) + 1; //TODO: Test for time-wise duplicate? //IndexA would be the one that's possibly duplicated. //Even with duplicate, this is still technically sorted. list.Insert(index, point); Curve.ControlPointAdded(point, index); } /// /// Adds a new control point to the curve. /// /// Time of the new control point. /// Value of the new control point. /// Newly created control point. public CurveControlPoint Add(float time, TValue value) { var toAdd = new CurveControlPoint(time, value, Curve); Add(toAdd); return toAdd; } /// /// Removes the control point from the curve. /// /// Control point to remove. public void Remove(CurveControlPoint controlPoint) { RemoveAt(list.IndexOf(controlPoint)); } /// /// Removes the control point from the curve. /// /// Index to remove at. public void RemoveAt(int index) { CurveControlPoint removed = list[index]; list.RemoveAt(index); Curve.ControlPointRemoved(removed, index); } } } ================================================ FILE: BEPUphysics/Paths/CurveEndpointBehavior.cs ================================================ namespace BEPUphysics.Paths { /// /// Defines how a curve behaves beyond an endpoint. /// public enum CurveEndpointBehavior { /// /// When the time exceeds the endpoint, it wraps around to the other end of the curve. /// Wrap, /// /// Times exceeding the endpoint are clamped to the endpoint's value. /// Clamp, /// /// Times exceeding the endpoint will reverse direction and sample backwards. /// Mirror } } ================================================ FILE: BEPUphysics/Paths/FiniteDifferenceSpline3D.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// 3D hermite curve that uses the finite difference method to compute tangents. /// public class FiniteDifferenceSpline3D : HermiteCurve3D { /// /// Gets the curve's bounding index information. /// /// Index of the minimum control point in the active curve segment. /// Index of the maximum control point in the active curve segment. public override void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex) { if (ControlPoints.Count > 0) { minIndex = 0; maxIndex = ControlPoints.Count - 1; } else { minIndex = -1; maxIndex = -1; } } protected override void ComputeTangents() { if (ControlPoints.Count == 1) { tangents.Add(Vector3.Zero); return; } if (ControlPoints.Count == 2) { Vector3 tangent = ControlPoints[1].Value - ControlPoints[0].Value; tangents.Add(tangent); tangents.Add(tangent); return; } Vector3 tangentA, tangentB; Vector3 previous, current, next; //First endpoint current = ControlPoints[0].Value; next = ControlPoints[1].Value; Vector3.Subtract(ref next, ref current, out tangentA); Vector3.Multiply(ref tangentA, (float)(.5 / (ControlPoints[1].Time - ControlPoints[0].Time)), out tangentA); //Vector3.Multiply(ref current, .5f / (controlPoints[0].time), out tangentB); //Vector3.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentA); for (int i = 1; i < ControlPoints.Count - 1; i++) { previous = current; current = next; next = ControlPoints[i + 1].Value; Vector3.Subtract(ref next, ref current, out tangentA); Vector3.Subtract(ref current, ref previous, out tangentB); Vector3.Multiply(ref tangentA, (float)(.5 / (ControlPoints[i + 1].Time - ControlPoints[i].Time)), out tangentA); Vector3.Multiply(ref tangentB, (float)(.5 / (ControlPoints[i].Time - ControlPoints[i - 1].Time)), out tangentB); Vector3.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentA); } previous = current; current = next; Vector3.Negate(ref current, out tangentA); Vector3.Subtract(ref current, ref previous, out tangentB); int currentIndex = ControlPoints.Count - 1; int previousIndex = currentIndex - 1; //Vector3.Multiply(ref tangentA, .5f / (-controlPoints[currentIndex].time), out tangentA); Vector3.Multiply(ref tangentB, (float)(.5 / (ControlPoints[currentIndex].Time - ControlPoints[previousIndex].Time)), out tangentB); //Vector3.Add(ref tangentA, ref tangentB, out tangentA); tangents.Add(tangentB); } } } ================================================ FILE: BEPUphysics/Paths/HermiteCurve3D.cs ================================================ using System.Collections.Generic; using System.Collections.ObjectModel; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.Paths { /// /// Defines a 3D curve using hermite interpolation. /// public abstract class HermiteCurve3D : Curve { /// /// Internal list of curve tangents. /// protected List tangents = new List(); /// /// Gets the tangents used by the curve per control point. /// public ReadOnlyList Tangents { get { return new ReadOnlyList(tangents); } } /// /// Evaluates the curve section starting at the control point index using /// the weight value. /// /// Index of the starting control point of the subinterval. /// Location to evaluate on the subinterval from 0 to 1. /// Value at the given location. public override void Evaluate(int controlPointIndex, float weight, out Vector3 value) { value = Vector3.Hermite( ControlPoints[controlPointIndex].Value, tangents[controlPointIndex], ControlPoints[controlPointIndex + 1].Value, tangents[controlPointIndex + 1], weight); } /// /// Called when a control point is added. /// /// New control point. /// Index of the control point. protected internal override void ControlPointAdded(CurveControlPoint curveControlPoint, int index) { tangents.Clear(); ComputeTangents(); } /// /// Called when a control point is removed. /// /// Removed control point. /// Index of the control point before it was removed. protected internal override void ControlPointRemoved(CurveControlPoint curveControlPoint, int oldIndex) { tangents.Clear(); ComputeTangents(); } /// /// Called when a control point belonging to the curve has its time changed. /// /// Changed control point. /// Old index of the control point. /// New index of the control point. protected internal override void ControlPointTimeChanged(CurveControlPoint curveControlPoint, int oldIndex, int newIndex) { tangents.Clear(); ComputeTangents(); } /// /// Called when a control point belonging to the curve has its value changed. /// /// Changed control point. protected internal override void ControlPointValueChanged(CurveControlPoint curveControlPoint) { tangents.Clear(); ComputeTangents(); } /// /// Computes the tangent entries in the curve according to some type of hermite curve. /// protected abstract void ComputeTangents(); } } ================================================ FILE: BEPUphysics/Paths/LinearInterpolationCurve3D.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Defines a 3D curve using linear interpolation. /// public class LinearInterpolationCurve3D : Curve { /// /// Evaluates the curve section starting at the control point index using /// the weight value. /// /// Index of the starting control point of the subinterval. /// Location to evaluate on the subinterval from 0 to 1. /// Value at the given location. public override void Evaluate(int controlPointIndex, float weight, out Vector3 value) { value = Vector3.Lerp(ControlPoints[controlPointIndex].Value, ControlPoints[controlPointIndex + 1].Value, weight); } /// /// Gets the curve's bounding index information. /// /// Index of the minimum control point in the active curve segment. /// Index of the maximum control point in the active curve segment. public override void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex) { maxIndex = ControlPoints.Count - 1; if (maxIndex < 0) minIndex = -1; else minIndex = 0; } /// /// Called when a control point is added. /// /// New control point. /// Index of the control point. protected internal override void ControlPointAdded(CurveControlPoint curveControlPoint, int index) { } /// /// Called when a control point is removed. /// /// Removed control point. /// Index of the control point before it was removed. protected internal override void ControlPointRemoved(CurveControlPoint curveControlPoint, int oldIndex) { } /// /// Called when a control point belonging to the curve has its time changed. /// /// Changed control point. /// Old index of the control point. /// New index of the control point. protected internal override void ControlPointTimeChanged(CurveControlPoint curveControlPoint, int oldIndex, int newIndex) { } /// /// Called when a control point belonging to the curve has its value changed. /// /// Changed control point. protected internal override void ControlPointValueChanged(CurveControlPoint curveControlPoint) { } ///// ///// Evaluates the curve at a certain time. ///// ///// Time to evaluate. ///// Value at evaluated time. //public override void evaluate(double time, out Vector3 value) //{ // int maxIndex = controlPoints.count - 1; // if (maxIndex == -1) // { // value = Vector3.Zero; // return; // } // time = modifyTime(time, controlPoints[0].time, controlPoints[maxIndex].time); // int index = getPreviousIndex(time); // int nextIndex = Math.Min(index + 1, maxIndex); // float denominator = controlPoints[nextIndex].time - controlPoints[index].time; // float intervalTime; // if (denominator < Toolbox.epsilon) // intervalTime = 0; // else // intervalTime = (float)(time - controlPoints[index].time) / denominator; // value = Vector3.Lerp(controlPoints[index].value, controlPoints[nextIndex].value, intervalTime); //} } } ================================================ FILE: BEPUphysics/Paths/Path following/EntityMover.cs ================================================ using System; using BEPUphysics.Constraints.SingleEntity; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUphysics.UpdateableSystems; using Microsoft.Xna.Framework; namespace BEPUphysics.Paths.PathFollowing { /// /// Pushes an entity around according to goal positions and orientations. /// public class EntityMover : Updateable, IDuringForcesUpdateable { private Entity entity; /// /// Constructs a new EntityMover. /// /// Entity to move. public EntityMover(Entity e) { IsUpdatedSequentially = false; LinearMotor = new SingleEntityLinearMotor(e, e.Position); Entity = e; LinearMotor.Settings.Mode = MotorMode.Servomechanism; TargetPosition = e.Position; } /// /// Constructs a new EntityMover. /// /// Entity to move. /// Motor to use for linear motion if the entity is dynamic. public EntityMover(Entity e, SingleEntityLinearMotor linearMotor) { IsUpdatedSequentially = false; LinearMotor = linearMotor; Entity = e; linearMotor.Entity = Entity; linearMotor.Settings.Mode = MotorMode.Servomechanism; TargetPosition = e.Position; } /// /// Gets or sets the entity being pushed by the entity mover. /// public Entity Entity { get { return entity; } set { entity = value; LinearMotor.Entity = value; } } /// /// Gets the linear motor used by the entity mover. /// When the affected entity is dynamic, it is pushed by motors. /// This ensures that its interactions and collisions with /// other entities remain stable. /// public SingleEntityLinearMotor LinearMotor { get; private set; } /// /// Gets or sets the point in the entity's local space that will be moved towards the target position. /// public Vector3 LocalOffset { get { return LinearMotor.LocalPoint; } set { LinearMotor.LocalPoint = value; } } /// /// Gets or sets the point attached to the entity in world space that will be moved towards the target position. /// public Vector3 Offset { get { return LinearMotor.Point; } set { LinearMotor.Point = value; } } /// /// Gets or sets the target location of the entity mover. /// public Vector3 TargetPosition { get; set; } /// /// Gets the angular velocity necessary to change an entity's orientation from /// the starting quaternion to the ending quaternion over time dt. /// /// Initial position. /// Final position. /// Time over which the angular velocity is to be applied. /// Angular velocity to reach the goal in time. public static Vector3 GetLinearVelocity(Vector3 start, Vector3 end, float dt) { Vector3 offset; Vector3.Subtract(ref end, ref start, out offset); Vector3.Divide(ref offset, dt, out offset); return offset; } /// /// Adds the motors to the space. Called automatically. /// public override void OnAdditionToSpace(ISpace newSpace) { newSpace.Add(LinearMotor); } /// /// Removes the motors from the space. Called automatically. /// public override void OnRemovalFromSpace(ISpace oldSpace) { oldSpace.Remove(LinearMotor); } /// /// Called automatically by the space. /// /// Simulation timestep. void IDuringForcesUpdateable.Update(float dt) { if (Entity != LinearMotor.Entity) throw new InvalidOperationException( "EntityMover's entity differs from EntityMover's motors' entities. Ensure that the moved entity is only changed by setting the EntityMover's entity property."); if (Entity.IsDynamic) { LinearMotor.IsActive = true; LinearMotor.Settings.Servo.Goal = TargetPosition; } else { LinearMotor.IsActive = false; Vector3 worldMovedPoint = Vector3.Transform(LocalOffset, entity.WorldTransform); Entity.LinearVelocity = GetLinearVelocity(worldMovedPoint, TargetPosition, dt); } } } } ================================================ FILE: BEPUphysics/Paths/Path following/EntityRotator.cs ================================================ using System; using BEPUphysics.Constraints.SingleEntity; using BEPUphysics.Constraints.TwoEntity.Motors; using BEPUphysics.Entities; using BEPUphysics.UpdateableSystems; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Paths.PathFollowing { /// /// Pushes an entity around according to goal positions and orientations. /// public class EntityRotator : Updateable, IDuringForcesUpdateable { private Entity entity; /// /// Constructs a new EntityRotator. /// /// Entity to move. public EntityRotator(Entity e) { IsUpdatedSequentially = false; AngularMotor = new SingleEntityAngularMotor(e); Entity = e; AngularMotor.Settings.Mode = MotorMode.Servomechanism; TargetOrientation = e.Orientation; } /// /// Constructs a new EntityRotator. /// /// Entity to move. /// Motor to use for angular motion if the entity is dynamic. public EntityRotator(Entity e, SingleEntityAngularMotor angularMotor) { IsUpdatedSequentially = false; AngularMotor = angularMotor; Entity = e; angularMotor.Entity = Entity; angularMotor.Settings.Mode = MotorMode.Servomechanism; TargetOrientation = e.Orientation; } /// /// Gets the angular motor used by the entity rotator. /// When the affected entity is dynamic, it is pushed by motors. /// This ensures that its interactions and collisions with /// other entities remain stable. /// public SingleEntityAngularMotor AngularMotor { get; private set; } /// /// Gets or sets the entity being pushed by the entity rotator. /// public Entity Entity { get { return entity; } set { entity = value; AngularMotor.Entity = value; } } /// /// Gets or sets the target orientation of the entity rotator. /// public Quaternion TargetOrientation { get; set; } /// /// Gets the angular velocity necessary to change an entity's orientation from /// the starting quaternion to the ending quaternion over time dt. /// /// Initial orientation. /// Final orientation. /// Time over which the angular velocity is to be applied. /// Angular velocity to reach the goal in time. public static Vector3 GetAngularVelocity(Quaternion start, Quaternion end, float dt) { //Compute the relative orientation R' between R and the target relative orientation. Quaternion errorOrientation; Quaternion.Conjugate(ref start, out errorOrientation); Quaternion.Multiply(ref end, ref errorOrientation, out errorOrientation); Vector3 axis; float angle; //Turn this into an axis-angle representation. Toolbox.GetAxisAngleFromQuaternion(ref errorOrientation, out axis, out angle); Vector3.Multiply(ref axis, angle / dt, out axis); return axis; } /// /// Adds the motors to the solver. Called automatically. /// public override void OnAdditionToSpace(ISpace newSpace) { newSpace.Add(AngularMotor); } /// /// Removes the motors from the solver. Called automatically. /// public override void OnRemovalFromSpace(ISpace oldSpace) { oldSpace.Remove(AngularMotor); } /// /// Called automatically by the space. /// /// Simulation timestep. void IDuringForcesUpdateable.Update(float dt) { if (Entity != AngularMotor.Entity) throw new InvalidOperationException( "EntityRotator's entity differs from EntityRotator's motor's entities. Ensure that the moved entity is only changed by setting the EntityRotator's entity property."); if (Entity.IsDynamic) { AngularMotor.IsActive = true; AngularMotor.Settings.Servo.Goal = TargetOrientation; } else { AngularMotor.IsActive = false; Entity.AngularVelocity = GetAngularVelocity(Entity.Orientation, TargetOrientation, dt); } } } } ================================================ FILE: BEPUphysics/Paths/Path.cs ================================================ namespace BEPUphysics.Paths { /// /// Superclass of a variety of classes that can be evaluated at a time to retrieve a value associated with that time. /// /// Type of the value of the path. public abstract class Path { /// /// Computes the value of the path at a given time. /// /// Time at which to evaluate the path. /// Path value at the given time. public abstract void Evaluate(double time, out TValue value); /// /// Gets the starting and ending times of the path. /// /// Beginning time of the path. /// Ending time of the path. public abstract void GetPathBoundsInformation(out double startingTime, out double endingTime); /// /// Computes the value of the path at a given time. /// /// Time at which to evaluate the path. /// Path value at the given time. public TValue Evaluate(double time) { TValue toReturn; Evaluate(time, out toReturn); return toReturn; } } } ================================================ FILE: BEPUphysics/Paths/QuaternionSlerpCurve.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Defines a quaternion curve using spherical linear interpolation. /// public class QuaternionSlerpCurve : Curve { /// /// Evaluates the curve section starting at the control point index using /// the weight value. /// /// Index of the starting control point of the subinterval. /// Location to evaluate on the subinterval from 0 to 1. /// Value at the given location. public override void Evaluate(int controlPointIndex, float weight, out Quaternion value) { value = Quaternion.Slerp(ControlPoints[controlPointIndex].Value, ControlPoints[controlPointIndex + 1].Value, weight); } /// /// Gets the curve's bounding index information. /// /// Index of the minimum control point in the active curve segment. /// Index of the maximum control point in the active curve segment. public override void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex) { maxIndex = ControlPoints.Count - 1; if (maxIndex < 0) minIndex = -1; else minIndex = 0; } /// /// Called when a control point is added. /// /// New control point. /// Index of the control point. protected internal override void ControlPointAdded(CurveControlPoint curveControlPoint, int index) { } /// /// Called when a control point is removed. /// /// Removed control point. /// Index of the control point before it was removed. protected internal override void ControlPointRemoved(CurveControlPoint curveControlPoint, int oldIndex) { } /// /// Called when a control point belonging to the curve has its time changed. /// /// Changed control point. /// Old index of the control point. /// New index of the control point. protected internal override void ControlPointTimeChanged(CurveControlPoint curveControlPoint, int oldIndex, int newIndex) { } /// /// Called when a control point belonging to the curve has its value changed. /// /// Changed control point. protected internal override void ControlPointValueChanged(CurveControlPoint curveControlPoint) { } } } ================================================ FILE: BEPUphysics/Paths/SpeedControlledCurve.cs ================================================ using System; using System.Collections.Generic; using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { internal struct SpeedControlledCurveSample { public double Wrapped; public double SpeedControlled; } /// /// Wrapper that controls the speed at which a curve is traversed. /// /// /// /// Even if a curve is evaluated at linearly increasing positions, /// the distance between consecutive values can be different. This /// has the effect of a curve-following object having variable velocity. /// /// /// To counteract the variable velocity, this wrapper samples the curve /// and produces a reparameterized, distance-based curve. Changing the /// evaluated curve position will linearly change the value. /// /// public abstract class SpeedControlledCurve : Path { private readonly List samples = new List(); // X is wrapped view, Y is associated curve view private Curve curve; private int samplesPerInterval; /// /// Constructs a new speed controlled curve. /// protected SpeedControlledCurve() { } /// /// Constructs a new speed-controlled curve. /// /// Curve to wrap. protected SpeedControlledCurve(Curve curve) { samplesPerInterval = 10; this.curve = curve; } /// /// Constructs a new speed-controlled curve. /// /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. protected SpeedControlledCurve(Curve curve, int samplesPerInterval) { this.curve = curve; this.samplesPerInterval = samplesPerInterval; } /// /// Gets or sets the curve wrapped by this instance. /// public Curve Curve { get { return curve; } set { curve = value; if (Curve != null) ResampleCurve(); } } /// /// Defines how the curve is sampled when the evaluation time exceeds the final control point. /// public CurveEndpointBehavior PostLoop { get; set; } /// /// Defines how the curve is sampled when the evaluation time exceeds the beginning control point. /// public CurveEndpointBehavior PreLoop { get; set; } /// /// Gets or sets the number of samples to use per interval in the curve. /// public int SamplesPerInterval { get { return samplesPerInterval; } set { samplesPerInterval = value; if (Curve != null) ResampleCurve(); } } /// /// Gets the desired speed at a given time. /// /// Time to check for speed. /// Speed at the given time. public abstract float GetSpeedAtCurveTime(double time); /// /// Gets the time at which the internal curve would be evaluated at the given time. /// /// Time to evaluate the speed-controlled curve. /// Time at which the internal curve would be evaluated. public double GetInnerTime(double time) { if (Curve == null) throw new InvalidOperationException("SpeedControlledCurve's internal curve is null; ensure that its curve property is set prior to evaluation."); double firstTime, lastTime; GetPathBoundsInformation(out firstTime, out lastTime); time = Curve.ModifyTime(time, firstTime, lastTime, Curve.PreLoop, Curve.PostLoop); int indexMin = 0; int indexMax = samples.Count; if (indexMax == 1) { //1-length curve; asking the system to evaluate //this will be a waste of time AND //crash since +1 will be outside scope return samples[0].SpeedControlled; } if (indexMax == 0) { return 0; } //If time < controlpoints.mintime, should be... 0 or -1? while (indexMax - indexMin > 1) //if time belongs to min { int midIndex = (indexMin + indexMax) / 2; if (time > samples[midIndex].Wrapped) { indexMin = midIndex; } else { indexMax = midIndex; } } double curveTime = (time - samples[indexMin].Wrapped) / (samples[indexMin + 1].Wrapped - samples[indexMin].Wrapped); return (1 - curveTime) * samples[indexMin].SpeedControlled + (curveTime) * samples[indexMin + 1].SpeedControlled; } /// /// Computes the value of the curve at a given time. /// /// Time to evaluate the curve at. /// Value of the curve at the given time. /// Time at which the internal curve was evaluated to get the value. public void Evaluate(double time, out TValue value, out double innerTime) { Curve.Evaluate(innerTime = GetInnerTime(time), out value); } /// /// Computes the value of the curve at a given time. /// /// Time to evaluate the curve at. /// Value of the curve at the given time. public override void Evaluate(double time, out TValue value) { Curve.Evaluate(GetInnerTime(time), out value); } /// /// Gets the starting and ending times of the path. /// /// Beginning time of the path. /// Ending time of the path. public override void GetPathBoundsInformation(out double startingTime, out double endingTime) { if (samples.Count > 0) { startingTime = 0; endingTime = samples[samples.Count - 1].Wrapped; } else { startingTime = 0; endingTime = 0; } } /// /// Forces a recalculation of curve samples. /// This needs to be called if the wrapped curve /// is changed. /// public void ResampleCurve() { //TODO: Call this from curve if add/remove/timechange/valuechange happens //Could hide it then. samples.Clear(); double firstTime, lastTime; int minIndex, maxIndex; curve.GetCurveBoundsInformation(out firstTime, out lastTime, out minIndex, out maxIndex); //Curve isn't valid. if (minIndex < 0 || maxIndex < 0) return; float timeElapsed = 0; //TODO: useless calculation due to this TValue currentValue = Curve.ControlPoints[minIndex].Value; TValue previousValue = currentValue; float inverseSampleCount = 1f / (SamplesPerInterval + 1); float speed = GetSpeedAtCurveTime(Curve.ControlPoints[minIndex].Time); float previousSpeed = speed; for (int i = minIndex; i < maxIndex; i++) { previousValue = currentValue; currentValue = Curve.ControlPoints[i].Value; if (speed != 0) timeElapsed += GetDistance(previousValue, currentValue) / speed; previousSpeed = speed; speed = GetSpeedAtCurveTime(Curve.ControlPoints[i].Time); samples.Add(new SpeedControlledCurveSample { Wrapped = timeElapsed, SpeedControlled = Curve.ControlPoints[i].Time }); var curveTime = Curve.ControlPoints[i].Time; var intervalLength = Curve.ControlPoints[i + 1].Time - curveTime; var curveTimePerSample = intervalLength / (SamplesPerInterval + 1); for (int j = 1; j <= SamplesPerInterval; j++) { previousValue = currentValue; Curve.Evaluate(i, j * inverseSampleCount, out currentValue); curveTime += curveTimePerSample; if (speed != 0) timeElapsed += GetDistance(previousValue, currentValue) / speed; previousSpeed = speed; speed = GetSpeedAtCurveTime(curveTime); samples.Add(new SpeedControlledCurveSample { Wrapped = timeElapsed, SpeedControlled = curveTime }); } } timeElapsed += GetDistance(previousValue, currentValue) / previousSpeed; samples.Add(new SpeedControlledCurveSample { Wrapped = timeElapsed, SpeedControlled = Curve.ControlPoints[maxIndex].Time }); } /// /// Computes the distance between the two values. /// /// Starting value. /// Ending value. /// Distance between the values. protected abstract float GetDistance(TValue start, TValue end); } } ================================================ FILE: BEPUphysics/Paths/StepCurve1D.cs ================================================ namespace BEPUphysics.Paths { /// /// One dimensional-valued curve that does not interpolate values. /// Instead, it just picks the value from the previous control point. /// public class StepCurve1D : Curve { /// /// Evaluates the curve at a given time using linear interpolation. /// /// Index of the control point at the beginning of the evaluation interval. /// Value of 0 to 1 representing how far along the interval to sample. /// Value of the curve at the given location. public override void Evaluate(int controlPointIndex, float weight, out float value) { value = ControlPoints[controlPointIndex].Value; } /// /// Computes the bounds of the curve. /// /// Minimum index of the curve. /// Maximum index of the curve. public override void GetCurveIndexBoundsInformation(out int minIndex, out int maxIndex) { maxIndex = ControlPoints.Count - 1; if (maxIndex < 0) minIndex = -1; else minIndex = 0; } protected internal override void ControlPointAdded(CurveControlPoint curveControlPoint, int index) { } protected internal override void ControlPointRemoved(CurveControlPoint curveControlPoint, int oldIndex) { } protected internal override void ControlPointTimeChanged(CurveControlPoint curveControlPoint, int oldIndex, int newIndex) { } protected internal override void ControlPointValueChanged(CurveControlPoint curveControlPoint) { } } } ================================================ FILE: BEPUphysics/Paths/VariableAngularSpeedCurve.cs ================================================ using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Wraps a curve that is traveled along with arbitrary defined angular speed. /// /// /// The speed curve should be designed with the wrapped curve's times in mind. /// Speeds will be sampled based on the wrapped curve's interval. public class VariableAngularSpeedCurve : VariableSpeedCurve { /// /// Constructs a new variable speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. public VariableAngularSpeedCurve(Path speedCurve, Curve curve) : base(speedCurve, curve) { } /// /// Constructs a new variable speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. public VariableAngularSpeedCurve(Path speedCurve, Curve curve, int sampleCount) : base(speedCurve, curve, sampleCount) { } protected override float GetDistance(Quaternion start, Quaternion end) { Quaternion.Conjugate(ref end, out end); Quaternion.Multiply(ref end, ref start, out end); return Toolbox.GetAngleFromQuaternion(ref end); } } } ================================================ FILE: BEPUphysics/Paths/VariableLinearSpeedCurve.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUphysics.Paths { /// /// Wraps a curve that is traveled along with arbitrary defined linear speed. /// /// /// The speed curve should be designed with the wrapped curve's times in mind. /// Speeds will be sampled based on the wrapped curve's interval. public class VariableLinearSpeedCurve : VariableSpeedCurve { /// /// Constructs a new variable speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. public VariableLinearSpeedCurve(Path speedCurve, Curve curve) : base(speedCurve, curve) { } /// /// Constructs a new variable speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. public VariableLinearSpeedCurve(Path speedCurve, Curve curve, int sampleCount) : base(speedCurve, curve, sampleCount) { } protected override float GetDistance(Vector3 start, Vector3 end) { float distance; Vector3.Distance(ref start, ref end, out distance); return distance; } } } ================================================ FILE: BEPUphysics/Paths/VariableSpeedCurve.cs ================================================ namespace BEPUphysics.Paths { /// /// Curve that wraps another curve and travels along it with specified speeds. /// /// Type of the value of the wrapped curve. public abstract class VariableSpeedCurve : SpeedControlledCurve { /// /// Constructs a new constant speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. protected VariableSpeedCurve(Path speedCurve, Curve curve) : base(curve) { SpeedCurve = speedCurve; ResampleCurve(); } /// /// Constructs a new constant speed curve. /// /// Curve defining speeds to use. /// Curve to wrap. /// Number of samples to use when constructing the wrapper curve. /// More samples increases the accuracy of the speed requirement at the cost of performance. protected VariableSpeedCurve(Path speedCurve, Curve curve, int sampleCount) : base(curve, sampleCount) { SpeedCurve = speedCurve; ResampleCurve(); } /// /// Gets or sets the path that defines the speeds at given locations. /// The speed curve will be sampled at times associated with the wrapped curve. /// public Path SpeedCurve { get; set; } /// /// Gets the speed at a given time on the wrapped curve. /// /// Time to evaluate. /// Speed at the given time. public override float GetSpeedAtCurveTime(double time) { return SpeedCurve.Evaluate(time); } } } ================================================ FILE: BEPUphysics/PhysicsChecker.cs ================================================ using System; using System.Diagnostics; using BEPUphysics.CollisionTests; using BEPUutilities; using Microsoft.Xna.Framework; namespace BEPUphysics { /// /// Contains conditional extensions to check for bad values in various structures. /// public static class PhysicsChecker { /// /// Checks the contact to see if it contains NaNs or infinities. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Contact contact) { contact.Normal.Validate(); if (contact.Normal.LengthSquared() < 0.9f) throw new ArithmeticException("Invalid contact normal."); contact.Position.Validate(); contact.PenetrationDepth.Validate(); } /// /// Checks the contact to see if it contains NaNs or infinities. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this ContactData contact) { contact.Normal.Validate(); contact.Position.Validate(); contact.PenetrationDepth.Validate(); } } } ================================================ FILE: BEPUphysics/PhysicsResources.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionTests.Manifolds; using BEPUphysics.Entities; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.DeactivationManagement; using BEPUutilities; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUphysics { /// /// Handles allocation and management of commonly used resources. /// public static class PhysicsResources { static PhysicsResources() { ResetPools(); } public static void ResetPools() { SubPoolRayCastResultList = new LockingResourcePool>(); SubPoolBroadPhaseEntryList = new LockingResourcePool>(); SubPoolCollidableList = new LockingResourcePool>(); SubPoolCompoundChildList = new LockingResourcePool>(); SubPoolEntityRawList = new LockingResourcePool>(16); SubPoolTriangleShape = new LockingResourcePool(); SubPoolTriangleCollidables = new LockingResourcePool(); SubPoolTriangleIndicesList = new LockingResourcePool>(); SimulationIslandConnections = new LockingResourcePool(); } static ResourcePool> SubPoolRayCastResultList; static ResourcePool> SubPoolBroadPhaseEntryList; static ResourcePool> SubPoolCollidableList; static ResourcePool> SubPoolEntityRawList; static ResourcePool SubPoolTriangleShape; static ResourcePool> SubPoolCompoundChildList; static ResourcePool SubPoolTriangleCollidables; static ResourcePool> SubPoolTriangleIndicesList; static ResourcePool SimulationIslandConnections; //#endif /// /// Retrieves a ray cast result list from the resource pool. /// /// Empty ray cast result list. public static RawList GetRayCastResultList() { return SubPoolRayCastResultList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolRayCastResultList.GiveBack(list); } /// /// Retrieves an BroadPhaseEntry list from the resource pool. /// /// Empty BroadPhaseEntry list. public static RawList GetBroadPhaseEntryList() { return SubPoolBroadPhaseEntryList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolBroadPhaseEntryList.GiveBack(list); } /// /// Retrieves a Collidable list from the resource pool. /// /// Empty Collidable list. public static RawList GetCollidableList() { return SubPoolCollidableList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolCollidableList.GiveBack(list); } /// /// Retrieves an CompoundChild list from the resource pool. /// /// Empty information list. public static RawList GetCompoundChildList() { return SubPoolCompoundChildList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolCompoundChildList.GiveBack(list); } /// /// Retrieves an Entity RawList from the resource pool. /// /// Empty Entity raw list. public static RawList GetEntityRawList() { return SubPoolEntityRawList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolEntityRawList.GiveBack(list); } /// /// Retrieves a Triangle shape from the resource pool. /// /// Position of the first vertex. /// Position of the second vertex. /// Position of the third vertex. /// Initialized TriangleShape. public static TriangleShape GetTriangle(ref Vector3 v1, ref Vector3 v2, ref Vector3 v3) { TriangleShape toReturn = SubPoolTriangleShape.Take(); toReturn.vA = v1; toReturn.vB = v2; toReturn.vC = v3; return toReturn; } /// /// Retrieves a Triangle shape from the resource pool. /// /// Initialized TriangleShape. public static TriangleShape GetTriangle() { return SubPoolTriangleShape.Take(); } /// /// Returns a resource to the pool. /// /// Triangle to return. public static void GiveBack(TriangleShape triangle) { triangle.collisionMargin = 0; triangle.sidedness = TriangleSidedness.DoubleSided; SubPoolTriangleShape.GiveBack(triangle); } /// /// Retrieves a TriangleCollidable from the resource pool. /// /// First vertex in the triangle. /// Second vertex in the triangle. /// Third vertex in the triangle. /// Initialized TriangleCollidable. public static TriangleCollidable GetTriangleCollidable(ref Vector3 a, ref Vector3 b, ref Vector3 c) { var tri = SubPoolTriangleCollidables.Take(); var shape = tri.Shape; shape.vA = a; shape.vB = b; shape.vC = c; var identity = RigidTransform.Identity; tri.UpdateBoundingBoxForTransform(ref identity); return tri; } /// /// Retrieves a TriangleCollidable from the resource pool. /// /// Initialized TriangleCollidable. public static TriangleCollidable GetTriangleCollidable() { return SubPoolTriangleCollidables.Take(); } /// /// Returns a resource to the pool. /// /// Triangle collidable to return. public static void GiveBack(TriangleCollidable triangle) { triangle.CleanUp(); SubPoolTriangleCollidables.GiveBack(triangle); } /// /// Retrieves a TriangleIndices list from the resource pool. /// /// TriangleIndices list. public static RawList GetTriangleIndicesList() { return SubPoolTriangleIndicesList.Take(); } /// /// Returns a resource to the pool. /// /// TriangleIndices list to return. public static void GiveBack(RawList triangleIndices) { triangleIndices.Clear(); SubPoolTriangleIndicesList.GiveBack(triangleIndices); } /// /// Retrieves a simulation island connection from the resource pool. /// /// Uninitialized simulation island connection. public static SimulationIslandConnection GetSimulationIslandConnection() { return SimulationIslandConnections.Take(); } /// /// Returns a resource to the pool. /// /// Connection to return. public static void GiveBack(SimulationIslandConnection connection) { connection.CleanUp(); SimulationIslandConnections.GiveBack(connection); } } } ================================================ FILE: BEPUphysics/PositionUpdating/ContinuousPositionUpdater.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.Settings; using BEPUphysics.Threading; using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.PositionUpdating { /// /// Updates objects according to the position update mode. /// This allows continuous objects to avoid missing collisions. /// public class ContinuousPositionUpdater : PositionUpdater { RawList discreteUpdateables = new RawList(); RawList passiveUpdateables = new RawList(); RawList continuousUpdateables = new RawList(); /// /// Number of objects in a list required to use multithreading. /// public static int MultithreadingThreshold = 100; /// /// Constructs the position updater. /// ///Time step settings to use. public ContinuousPositionUpdater(TimeStepSettings timeStepSettings) : base(timeStepSettings) { preUpdate = PreUpdate; updateTimeOfImpact = UpdateTimeOfImpact; updateContinuous = UpdateContinuousItem; } /// /// Constructs the position updater. /// ///Time step settings to use. /// Thread manager to use. public ContinuousPositionUpdater(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { preUpdate = PreUpdate; updateTimeOfImpact = UpdateTimeOfImpact; updateContinuous = UpdateContinuousItem; } Action preUpdate; void PreUpdate(int i) { if (i >= discreteUpdateables.Count) { i -= discreteUpdateables.Count; if (i >= passiveUpdateables.Count) { i -= passiveUpdateables.Count; //It's a continuous updateable. if (continuousUpdateables.Elements[i].IsActive) continuousUpdateables.Elements[i].PreUpdatePosition(timeStepSettings.TimeStepDuration); } else { //It's a passive updateable. if (passiveUpdateables.Elements[i].IsActive) passiveUpdateables.Elements[i].PreUpdatePosition(timeStepSettings.TimeStepDuration); } } else { //It's a discrete updateable. if (discreteUpdateables.Elements[i].IsActive) discreteUpdateables.Elements[i].PreUpdatePosition(timeStepSettings.TimeStepDuration); } } Action updateTimeOfImpact; void UpdateTimeOfImpact(int i) { //This should always execute, even if the updateable is not active. This is because //a CCD object may be in a pair where the CCD object is resting, and another incoming object //is awake. continuousUpdateables.Elements[i].UpdateTimesOfImpact(timeStepSettings.TimeStepDuration); } Action updateContinuous; void UpdateContinuousItem(int i) { if (i < passiveUpdateables.Count) { if (passiveUpdateables.Elements[i].IsActive) passiveUpdateables.Elements[i].UpdatePositionContinuously(timeStepSettings.TimeStepDuration); } else { if (continuousUpdateables.Elements[i - passiveUpdateables.Count].IsActive) continuousUpdateables.Elements[i - passiveUpdateables.Count].UpdatePositionContinuously(timeStepSettings.TimeStepDuration); } } protected override void UpdateMultithreaded() { //Go through the list of all updateables which do not permit motion clamping. //Since these do not care about CCD, just update them as if they were discrete. //In addition, go through the remaining non-discrete objects and perform their prestep. //This usually involves updating their angular motion, but not their linear motion. int count = discreteUpdateables.Count + passiveUpdateables.Count + continuousUpdateables.Count; ThreadManager.ForLoop(0, count, preUpdate); //Now go through the list of all full CCD objects. These are responsible //for determining the TOI of collision pairs, if existent. if (continuousUpdateables.Count > MultithreadingThreshold) ThreadManager.ForLoop(0, continuousUpdateables.Count, updateTimeOfImpact); else for (int i = 0; i < continuousUpdateables.Count; i++) UpdateTimeOfImpact(i); //The TOI's are now computed, so we can integrate all of the CCD or allow-motionclampers //to their new positions. count = passiveUpdateables.Count + continuousUpdateables.Count; if (count > MultithreadingThreshold) ThreadManager.ForLoop(0, count, updateContinuous); else for (int i = 0; i < count; i++) UpdateContinuousItem(i); //The above process is the same as the UpdateSingleThreaded version, but //it doesn't always use multithreading. Sometimes, a simulation can have //very few continuous objects. In this case, there's no point in having the //multithreaded overhead. } protected override void UpdateSingleThreaded() { //Go through the list of all updateables which do not permit motion clamping. //Since these do not care about CCD, just update them as if they were discrete. //In addition, go through the remaining non-discrete objects and perform their prestep. //This usually involves updating their angular motion, but not their linear motion. int count = discreteUpdateables.Count + passiveUpdateables.Count + continuousUpdateables.Count; for (int i = 0; i < count; i++) PreUpdate(i); //Now go through the list of all full CCD objects. These are responsible //for determining the TOI of collision pairs, if existent. for (int i = 0; i < continuousUpdateables.Count; i++) UpdateTimeOfImpact(i); //The TOI's are now computed, so we can integrate all of the CCD or allow-motionclampers //to their new positions. count = passiveUpdateables.Count + continuousUpdateables.Count; for (int i = 0; i < count; i++) UpdateContinuousItem(i); } /// /// Notifies the position updater that an updateable has changed state. /// ///Updateable with changed state. ///Previous state the updateable was in. public void UpdateableModeChanged(ICCDPositionUpdateable updateable, PositionUpdateMode previousMode) { switch (previousMode) { case PositionUpdateMode.Discrete: discreteUpdateables.Remove(updateable); break; case PositionUpdateMode.Passive: passiveUpdateables.Remove(updateable); break; case PositionUpdateMode.Continuous: continuousUpdateables.Remove(updateable); break; } switch (updateable.PositionUpdateMode) { case PositionUpdateMode.Discrete: discreteUpdateables.Add(updateable); break; case PositionUpdateMode.Passive: passiveUpdateables.Add(updateable); break; case PositionUpdateMode.Continuous: continuousUpdateables.Add(updateable); break; } } /// /// Adds an object to the position updater. /// ///Updateable to add. ///Thrown if the updateable already belongs to a position updater. public override void Add(IPositionUpdateable updateable) { if (updateable.PositionUpdater == null) { updateable.PositionUpdater = this; var ccdUpdateable = updateable as ICCDPositionUpdateable; if (ccdUpdateable != null) { switch (ccdUpdateable.PositionUpdateMode) { case PositionUpdateMode.Discrete: discreteUpdateables.Add(updateable); break; case PositionUpdateMode.Passive: passiveUpdateables.Add(ccdUpdateable); break; case PositionUpdateMode.Continuous: continuousUpdateables.Add(ccdUpdateable); break; } } else discreteUpdateables.Add(updateable); } else { throw new ArgumentException("Cannot add object to Integrator; it already belongs to one."); } } /// /// Removes an updateable from the updater. /// ///Item to remove. ///Thrown if the updater does not own the updateable. public override void Remove(IPositionUpdateable updateable) { if (updateable.PositionUpdater == this) { updateable.PositionUpdater = null; var ccdUpdateable = updateable as ICCDPositionUpdateable; if (ccdUpdateable != null) { switch (ccdUpdateable.PositionUpdateMode) { case PositionUpdateMode.Discrete: discreteUpdateables.Remove(updateable); break; case PositionUpdateMode.Passive: passiveUpdateables.Remove(ccdUpdateable); break; case PositionUpdateMode.Continuous: continuousUpdateables.Remove(ccdUpdateable); break; } } else discreteUpdateables.Remove(updateable); } else throw new ArgumentException("Cannot remove object from this Integrator. The object doesn't belong to it."); } } } ================================================ FILE: BEPUphysics/PositionUpdating/ICCDPositionUpdateable.cs ================================================ namespace BEPUphysics.PositionUpdating { /// /// Update modes for position updateables. /// public enum PositionUpdateMode : byte { /// /// Updates position discretely regardless of its collision pairs. /// Discrete, /// /// Updates position discretely in isolation; when a Continuous object collides with it, /// its position update will be bounded by the time of impact. /// Passive, /// /// Updates position continuously. Continuous objects will integrate up to their earliest collision time. /// Continuous } /// /// A position updateable that can be updated continuously. /// public interface ICCDPositionUpdateable : IPositionUpdateable { /// /// Updates the time of impacts associated with the updateable. /// ///Time step duration. void UpdateTimesOfImpact(float dt); /// /// Updates the updateable using its continuous nature. /// /// Time step duration. void UpdatePositionContinuously(float dt); /// /// Gets or sets the position update mode of the object. /// The position update mode defines the way the object /// interacts with continuous collision detection. /// PositionUpdateMode PositionUpdateMode { get; set; } /// /// Resets the times of impact for pairs associated with this position updateable. /// void ResetTimesOfImpact(); } } ================================================ FILE: BEPUphysics/PositionUpdating/IPositionUpdateable.cs ================================================ namespace BEPUphysics.PositionUpdating { /// /// Defines an object capable of a position update. /// public interface IPositionUpdateable { /// /// Gets whether or not the object is active. /// Only active objects will be updated. /// bool IsActive { get; } /// /// Gets or sets the position updater that owns this updateable. /// PositionUpdater PositionUpdater { get; set; } /// /// Updates the position state of the object. /// ///Time step duration. void PreUpdatePosition(float dt); } } ================================================ FILE: BEPUphysics/PositionUpdating/PositionUpdater.cs ================================================ using System; using BEPUphysics.Threading; namespace BEPUphysics.PositionUpdating { /// /// Superclass of updaters which manage the position of objects. /// public abstract class PositionUpdater : MultithreadedProcessingStage { protected TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the updater. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } set { timeStepSettings = value; } } protected PositionUpdater(TimeStepSettings timeStepSettings, IThreadManager threadManager) :this(timeStepSettings) { ThreadManager = threadManager; AllowMultithreading = true; } protected PositionUpdater(TimeStepSettings timeStepSettings) { this.timeStepSettings = timeStepSettings; Enabled = true; } /// /// Adds an object to the position updater. /// ///Updateable to add. ///Thrown if the updateable already belongs to a position updater. public abstract void Add(IPositionUpdateable updateable); /// /// Removes an updateable from the updater. /// ///Item to remove. ///Thrown if the updater does not own the updateable. public abstract void Remove(IPositionUpdateable updateable); } } ================================================ FILE: BEPUphysics/ProcessingStage.cs ================================================ using System; using System.Diagnostics; namespace BEPUphysics { /// /// Superclass of singlethreaded update systems. /// public abstract class ProcessingStage { /// /// Gets or sets whether or not the stage should update. /// public virtual bool Enabled { get; set; } /// /// Fires when the stage starts working. /// public event Action Starting; /// /// Fires when the stage finishes working. /// public event Action Finishing; #if PROFILE /// /// Gets the time elapsed in the previous execution of this stage, not including any hooked Starting or Finishing events. /// public double Time { get { return (end - start) / (double)Stopwatch.Frequency; } } long start, end; #endif /// /// Updates the stage. /// public void Update() { if (!Enabled) return; if (Starting != null) Starting(); #if PROFILE start = Stopwatch.GetTimestamp(); #endif UpdateStage(); #if PROFILE end = Stopwatch.GetTimestamp(); #endif if (Finishing != null) Finishing(); } protected abstract void UpdateStage(); } } ================================================ FILE: BEPUphysics/Properties/AppManifest.xml ================================================  ================================================ FILE: BEPUphysics/Properties/AssemblyInfo.cs ================================================ using System.Reflection; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; // General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. [assembly: AssemblyTitle("BEPUphysics")] [assembly: AssemblyProduct("BEPUphysics")] [assembly: AssemblyDescription("Real time physics simulation library")] [assembly: AssemblyCompany("Bepu Entertainment LLC")] [assembly: AssemblyCopyright("Copyright © Bepu Entertainment LLC")] [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] // Setting ComVisible to false makes the types in this assembly not visible // to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. Only Windows // assemblies support COM. [assembly: ComVisible(false)] // On Windows, the following GUID is for the ID of the typelib if this // project is exposed to COM. On other platforms, it unique identifies the // title storage container when deploying this assembly to the device. [assembly: Guid("ab0c58ea-ef42-46d7-b180-2baedc61ce9b")] // Version information for an assembly consists of the following four values: // // Major Version // Minor Version // Build Number // Revision // [assembly: AssemblyVersion("1.3.0.0")] #if WINDOWS_PHONE [assembly: CodeGeneration(CodeGenerationFlags.EnableFPIntrinsicsUsingSIMD)] #endif ================================================ FILE: BEPUphysics/Properties/WMAppManifest.xml ================================================  0 ================================================ FILE: BEPUphysics/RayCastResult.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUutilities; namespace BEPUphysics { /// /// Contains information about a ray cast hit. /// public struct RayCastResult { /// /// Position, normal, and t paramater of the hit. /// public RayHit HitData; /// /// Object hit by the ray. /// public BroadPhaseEntry HitObject; /// /// Constructs a new ray cast result. /// ///Ray cast hit data. ///Object hit by the ray. public RayCastResult(RayHit hitData, BroadPhaseEntry hitObject) { HitData = hitData; HitObject = hitObject; } } } ================================================ FILE: BEPUphysics/Settings/CollisionDetectionSettings.cs ================================================ using System; namespace BEPUphysics.Settings { /// /// Settings class containing global information about collision detection. /// public static class CollisionDetectionSettings { internal static float ContactInvalidationLengthSquared = .01f; /// /// For persistent manifolds, contacts are represented by an offset in local space of two colliding bodies. /// The distance between these offsets transformed into world space and projected onto a plane defined by the contact normal squared is compared against this value. /// If this value is exceeded, the contact is removed from the contact manifold. /// /// If the world is smaller or larger than 'normal' for the engine, adjusting this value proportionally can improve contact caching behavior. /// The default value of .1f works well for worlds that operate on the order of 1 unit. /// public static float ContactInvalidationLength { get { return (float)Math.Sqrt(ContactInvalidationLengthSquared); } set { ContactInvalidationLengthSquared = value * value; } } internal static float ContactMinimumSeparationDistanceSquared = .0009f; /// /// In persistent manifolds, if two contacts are too close together, then /// the system will not use one of them. This avoids redundant constraints. /// Defaults to .03f. /// public static float ContactMinimumSeparationDistance { get { return (float)Math.Sqrt(ContactMinimumSeparationDistanceSquared); } set { ContactMinimumSeparationDistanceSquared = value * value; } } internal static float nonconvexNormalDotMinimum = .99f; /// /// In regular convex manifolds, two contacts are considered redundant if their positions are too close together. /// In nonconvex manifolds, the normal must also be tested, since a contact in the same location could have a different normal. /// This property is the minimum angle in radians between normals below which contacts are considered redundant. /// public static float NonconvexNormalAngleDifferenceMinimum { get { return (float)Math.Acos(nonconvexNormalDotMinimum); } set { nonconvexNormalDotMinimum = (float)Math.Cos(value); } } /// /// The default amount of allowed penetration into the margin before position correcting impulses will be applied. /// Defaults to .01f. /// public static float AllowedPenetration = .01f; /// /// Default collision margin around objects. Margins help prevent objects from interpenetrating and improve stability. /// Defaults to .04f. /// public static float DefaultMargin = .04f; internal static float maximumContactDistance = .1f; /// /// Maximum distance between the surfaces defining a contact point allowed before removing the contact. /// Defaults to .1f. /// public static float MaximumContactDistance { get { return maximumContactDistance; } set { if (value >= 0) maximumContactDistance = value; else throw new ArgumentException("Distance must be nonnegative."); } } } } ================================================ FILE: BEPUphysics/Settings/CollisionResponseSettings.cs ================================================ namespace BEPUphysics.Settings { /// /// Contains global settings relating to the collision response system. /// public static class CollisionResponseSettings { /// /// Impact velocity above which the bouciness of the object pair is taken into account. Below the threshold, no extra energy is added. /// Defaults to 1. /// public static float BouncinessVelocityThreshold = 1; /// /// Maximum speed at which interpenetrating objects will attempt to undo any overlap. /// Defaults to 2. /// public static float MaximumPenetrationCorrectionSpeed = 2; /// /// Fraction of position error to convert into corrective momentum. /// Defaults to 0.2. /// public static float PenetrationRecoveryStiffness = 0.2f; /// /// Magnitude of relative velocity at a contact point below which staticFriction is used. /// dynamicFriction is used when velocity exceeds this threshold. /// Defaults to 0.2. /// public static float StaticFrictionVelocityThreshold = 0.2f; /// /// Value by which a collision pair's friction coefficient will be multiplied to get the twist friction coefficient. /// Defaults to 1. /// public static float TwistFrictionFactor = 1f; /// /// Softness multiplier used by collision penetration constraints. Higher softness values allow more velocity error and make things look 'squishier'. Defaults to 0.05. /// Note that this value is not used directly by constraints; it is first scaled by the raw inverse effective mass. This allows consistent behavior across objects with different masses. /// public static float Softness = 0.05f; } } ================================================ FILE: BEPUphysics/Settings/MotionSettings.cs ================================================ using BEPUphysics.PositionUpdating; using System; using Microsoft.Xna.Framework; using BEPUphysics.NarrowPhaseSystems.Pairs; using BEPUphysics.Entities; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.Settings { /// /// Contains global settings about motion updating. /// public static class MotionSettings { /// /// Whether or not to use RK4 angular integration. This can improve simulation quality sometimes, but not always. /// It has a slight performance impact. Enabling this when ConserveAngularMomentum is set to true may be helpful. /// Defaults to false. /// public static bool UseRk4AngularIntegration; /// /// Whether or not to conserve angular momentum. /// This produces slightly more realistic angular behavior, but can reduce stability. /// Consider using a smaller timestep, enabling RK4 angular integration, or both. /// Do not use singular inertia tensors while momentum conservation is enabled. /// Defaults to false. /// public static bool ConserveAngularMomentum; /// /// The scaling to apply to the core shapes used for continuous collision detection tests. /// Values should be between 0 and 0.99f. The smaller the value, the smaller the shapes used /// to perform CCD are, and more collisions are missed. /// Defaults to .8f. /// public static float CoreShapeScaling { get { return coreShapeScaling; } set { //The reason why it doesn't allow up to 1.0 is there exist systems that require a small margin between the full minimum radius and the core shape. coreShapeScaling = MathHelper.Clamp(value, 0, .99f); } } static float coreShapeScaling = .8f; /// /// The default position updating mode used by position updateables. /// Defaults to Discrete. /// public static PositionUpdateMode DefaultPositionUpdateMode = PositionUpdateMode.Discrete; /// /// It is possible for an object in danger of being hit by a moving object to have a bounding box which /// does not contain the resulting motion, and CCD will fail to detect a secondary collision. /// Setting this to true will take into account nearby objects' velocities and use them to enlarge the /// bounding box so that secondary collisions are not missed. /// The larger size of bounding boxes can cause an increase in collision pairs during stressful situations, /// which can harm performance. /// Defaults to false. /// public static bool UseExtraExpansionForContinuousBoundingBoxes; /// /// Delegate which determines if a given pair should be allowed to run continuous collision detection. /// This is only called for entities which are continuous and colliding with other objects. /// By default, this prevents CCD from being used in any pair where the pair's CollisionRule stops collision response. /// public static CCDFilter CCDFilter; internal static bool PairAllowsCCD(Entity entity, CollidablePairHandler pair) { var other = (pair.broadPhaseOverlap.entryA == entity.CollisionInformation ? pair.broadPhaseOverlap.entryB : pair.broadPhaseOverlap.entryA) as Collidable; return CCDFilter(entity, other, pair); } static bool DefaultCCDFilter(Entity entity, Collidable other, CollidablePairHandler pair) { return pair.broadPhaseOverlap.collisionRule < CollisionRule.NoSolver; } static MotionSettings() { CCDFilter = DefaultCCDFilter; } } /// /// Delegate which determines if a given pair should be allowed to run continuous collision detection. /// This is only called for entities which are continuous and colliding with other objects. /// public delegate bool CCDFilter(Entity entity, Collidable other, CollidablePairHandler pair); } ================================================ FILE: BEPUphysics/SolverSystems/Solver.cs ================================================ using System; using BEPUphysics.DeactivationManagement; using BEPUphysics.Threading; using BEPUphysics.Constraints; using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.SolverSystems { /// /// Iteratively solves solver updateables, converging to a solution for simulated joints and collision pair contact constraints. /// public class Solver : MultithreadedProcessingStage { RawList solverUpdateables = new RawList(); internal int iterationLimit = 10; /// /// Gets or sets the maximum number of iterations the solver will attempt to use to solve the simulation's constraints. /// public int IterationLimit { get { return iterationLimit; } set { iterationLimit = Math.Max(value, 0); } } /// /// Gets the list of solver updateables in the solver. /// public ReadOnlyList SolverUpdateables { get { return new ReadOnlyList(solverUpdateables); } } protected internal TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the solver. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } set { timeStepSettings = value; } } /// /// Gets or sets the deactivation manager used by the solver. /// When constraints are added and removed, the deactivation manager /// gains and loses simulation island connections that affect simulation islands /// and activity states. /// public DeactivationManager DeactivationManager { get; set; } /// /// Gets the permutation mapper used by the solver. /// public PermutationMapper PermutationMapper { get; private set; } /// /// Constructs a Solver. /// ///Time step settings used by the solver. ///Deactivation manager used by the solver. public Solver(TimeStepSettings timeStepSettings, DeactivationManager deactivationManager) { TimeStepSettings = timeStepSettings; DeactivationManager = deactivationManager; multithreadedPrestepDelegate = MultithreadedPrestep; multithreadedIterationDelegate = MultithreadedIteration; Enabled = true; PermutationMapper = new PermutationMapper(); } /// /// Constructs a Solver. /// ///Time step settings used by the solver. ///Deactivation manager used by the solver. /// Thread manager used by the solver. public Solver(TimeStepSettings timeStepSettings, DeactivationManager deactivationManager, IThreadManager threadManager) : this(timeStepSettings, deactivationManager) { ThreadManager = threadManager; AllowMultithreading = true; } /// /// Adds a solver updateable to the solver. /// ///Updateable to add. ///Thrown when the item already belongs to a solver. public void Add(SolverUpdateable item) { if (item.Solver == null) { item.Solver = this; item.solverIndex = solverUpdateables.Count; solverUpdateables.Add(item); DeactivationManager.Add(item.simulationIslandConnection); item.OnAdditionToSolver(this); } else throw new ArgumentException("Solver updateable already belongs to something; it can't be added.", "item"); } /// /// Removes a solver updateable from the solver. /// ///Updateable to remove. ///Thrown when the item does not belong to the solver. public void Remove(SolverUpdateable item) { if (item.Solver == this) { item.Solver = null; solverUpdateables.Count--; if (item.solverIndex < solverUpdateables.Count) { //The solver updateable isn't the last element, so put the last element in its place. solverUpdateables.Elements[item.solverIndex] = solverUpdateables.Elements[solverUpdateables.Count]; //Update the replacement's solver index to its new location. solverUpdateables.Elements[item.solverIndex].solverIndex = item.solverIndex; } solverUpdateables.Elements[solverUpdateables.Count] = null; DeactivationManager.Remove(item.simulationIslandConnection); item.OnRemovalFromSolver(this); } else throw new ArgumentException("Solver updateable doesn't belong to this solver; it can't be removed.", "item"); } Action multithreadedPrestepDelegate; void MultithreadedPrestep(int i) { var updateable = solverUpdateables.Elements[i]; updateable.UpdateSolverActivity(); if (updateable.isActiveInSolver) { updateable.SolverSettings.currentIterations = 0; updateable.SolverSettings.iterationsAtZeroImpulse = 0; updateable.Update(timeStepSettings.TimeStepDuration); updateable.EnterLock(); try { updateable.ExclusiveUpdate(); } finally { updateable.ExitLock(); } } } Action multithreadedIterationDelegate; void MultithreadedIteration(int i) { //'i' is currently an index into an implicit array of solver updateables that goes from 0 to solverUpdateables.count * iterationLimit. //It includes iterationLimit copies of each updateable. //Permute the entire set with duplicates. var updateable = solverUpdateables.Elements[PermutationMapper.GetMappedIndex(i, solverUpdateables.Count)]; SolverSettings solverSettings = updateable.solverSettings; //Updateables only ever go from active to inactive during iterations, //so it's safe to check for activity before we do hard (synchronized) work. if (updateable.isActiveInSolver) { int incrementedIterations = -1; updateable.EnterLock(); //This duplicate test protects against the possibility that the updateable went inactive between the first check and the lock. if (updateable.isActiveInSolver) { if (updateable.SolveIteration() < solverSettings.minimumImpulse) { solverSettings.iterationsAtZeroImpulse++; if (solverSettings.iterationsAtZeroImpulse > solverSettings.minimumIterationCount) updateable.isActiveInSolver = false; } else { solverSettings.iterationsAtZeroImpulse = 0; } //Increment the iteration count. incrementedIterations = solverSettings.currentIterations++; } updateable.ExitLock(); //Since the updateables only ever go from active to inactive, it's safe to check outside of the lock. //Keeping this if statement out of the lock allows other waiters to get to work a few nanoseconds faster. if (incrementedIterations > iterationLimit || incrementedIterations > solverSettings.maximumIterationCount) { updateable.isActiveInSolver = false; } } } protected override void UpdateMultithreaded() { ThreadManager.ForLoop(0, solverUpdateables.Count, multithreadedPrestepDelegate); ++PermutationMapper.PermutationIndex; ThreadManager.ForLoop(0, iterationLimit * solverUpdateables.Count, multithreadedIterationDelegate); } protected override void UpdateSingleThreaded() { int totalUpdateableCount = solverUpdateables.Count; for (int i = 0; i < totalUpdateableCount; i++) { UnsafePrestep(solverUpdateables.Elements[i]); } int totalCount = iterationLimit * totalUpdateableCount; ++PermutationMapper.PermutationIndex; for (int i = 0; i < totalCount; i++) { UnsafeSolveIteration(solverUpdateables.Elements[PermutationMapper.GetMappedIndex(i, totalUpdateableCount)]); } } protected internal void UnsafePrestep(SolverUpdateable updateable) { updateable.UpdateSolverActivity(); if (updateable.isActiveInSolver) { SolverSettings solverSettings = updateable.solverSettings; solverSettings.currentIterations = 0; solverSettings.iterationsAtZeroImpulse = 0; updateable.Update(timeStepSettings.TimeStepDuration); updateable.ExclusiveUpdate(); } } protected internal void UnsafeSolveIteration(SolverUpdateable updateable) { if (updateable.isActiveInSolver) { SolverSettings solverSettings = updateable.solverSettings; solverSettings.currentIterations++; if (solverSettings.currentIterations <= iterationLimit && solverSettings.currentIterations <= solverSettings.maximumIterationCount) { if (updateable.SolveIteration() < solverSettings.minimumImpulse) { solverSettings.iterationsAtZeroImpulse++; if (solverSettings.iterationsAtZeroImpulse > solverSettings.minimumIterationCount) updateable.isActiveInSolver = false; } else { solverSettings.iterationsAtZeroImpulse = 0; } } else { updateable.isActiveInSolver = false; } } } } } ================================================ FILE: BEPUphysics/SolverSystems/SolverUpdateable.cs ================================================ using BEPUphysics.Constraints; using BEPUphysics.DeactivationManagement; namespace BEPUphysics.SolverSystems { /// /// Superclass of all objects that live in the solver. /// public abstract class SolverUpdateable : ISimulationIslandConnectionOwner, ISpaceObject { internal int solverIndex; protected internal Solver solver; /// /// Gets the solver to which the solver updateable belongs. /// public virtual Solver Solver { get { return solver; } internal set { solver = value; } } protected SolverUpdateable() { //Initialize the connection. //It will usually be overridden and end up floating on back to the resource pool. simulationIslandConnection = PhysicsResources.GetSimulationIslandConnection(); simulationIslandConnection.Owner = this; } /// /// Performs the frame's configuration step. /// ///Timestep duration. public abstract void Update(float dt); //Will be locked if necessary by the solver. /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public abstract void ExclusiveUpdate(); /// /// Computes one iteration of the constraint to meet the solver updateable's goal. /// /// The rough applied impulse magnitude. public abstract float SolveIteration(); protected internal SolverSettings solverSettings = new SolverSettings(); /// /// Gets the solver settings that manage how the solver updates. /// public SolverSettings SolverSettings { get { return solverSettings; } } protected internal bool isActive = true; /// /// Gets or sets whether or not this solver updateable is active. /// /// When set to false, this solver updateable will be idle and its /// isActiveInSolver field will always be false. /// /// When set to true, the solver updateable will run normally and update if /// the type's activity conditions allow it. /// public bool IsActive { get { return isActive; } set { isActive = value; } } protected internal bool isActiveInSolver = true; /// /// Gets whether or not the space's solver should try to solve this object. /// Depends on conditions specific to each solver updateable type and whether or not /// it has completed its computations early. Recomputed each frame. /// public bool IsActiveInSolver { get { return isActiveInSolver; } } /// /// Attempts to acquire a lock on the solver updateable. /// This allows operations that need exclusive access to the solver updateable's members. /// If it is contested, it aborts the attempt. /// /// Whether or not the lock could be acquired. public abstract bool TryEnterLock(); /// /// Acquires a lock on the solver updateable. /// This allows operations that need exclusive access to the solver updateable's members. /// public abstract void EnterLock(); /// /// Releases the lock on the solver updateable. /// public abstract void ExitLock(); /// /// Updates the activity state of the solver updateable based on its members. /// public virtual void UpdateSolverActivity() { if (isActive) { //This is a simulation island connection. We already know that all connected objects share the //same simulation island (or don't have one, in the case of kinematics). All we have to do is test to see if that island is active! for (int i = 0; i < simulationIslandConnection.entries.Count; i++) { var island = simulationIslandConnection.entries.Elements[i].Member.SimulationIsland; if (island != null && island.isActive) { isActiveInSolver = true; return; } } } isActiveInSolver = false; } protected internal ISpace space; ISpace ISpaceObject.Space { get { return space; } set { space = value; } } /// /// Called after the object is added to a space. /// /// public virtual void OnAdditionToSpace(ISpace newSpace) { } /// /// Called before an object is removed from its space. /// public virtual void OnRemovalFromSpace(ISpace oldSpace) { } /// /// Called when the updateable is added to a solver. /// ///Solver to which the updateable was added. public virtual void OnAdditionToSolver(Solver newSolver) { } /// /// Called when the updateable is removed from its solver. /// /// Solver from which the updateable was removed. public virtual void OnRemovalFromSolver(Solver oldSolver) { } /// /// Gets or sets the user data associated with this object. /// public object Tag { get; set; } protected internal SimulationIslandConnection simulationIslandConnection; /// /// Gets the simulation island connection associated with this updateable. /// public SimulationIslandConnection SimulationIslandConnection { get { return simulationIslandConnection; } } } } ================================================ FILE: BEPUphysics/SolverSystems/SolverUpdateableChange.cs ================================================ namespace BEPUphysics.SolverSystems { /// /// Stores an enqueued solver updateable addition or removal. /// public struct SolverUpdateableChange { /// /// Whether the item is going to be added or removed. /// public bool ShouldAdd; /// /// Item being added or removed. /// public SolverUpdateable Item; /// /// Constructs a new solver updateable change. /// ///Whether the item is going to be added or removed. ///Item to add or remove. public SolverUpdateableChange(bool shouldAdd, SolverUpdateable item) { ShouldAdd = shouldAdd; Item = item; } } } ================================================ FILE: BEPUphysics/Space.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseSystems.Hierarchies; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.DeactivationManagement; using BEPUphysics.Entities; using BEPUphysics.EntityStateManagement; using BEPUphysics.OtherSpaceStages; using BEPUphysics.PositionUpdating; using BEPUphysics.SolverSystems; using BEPUphysics.Threading; using BEPUutilities; using BEPUphysics.NarrowPhaseSystems; using BEPUphysics.UpdateableSystems; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics { /// /// Main simulation class of BEPUphysics. Contains various updating stages addition/removal methods for getting objects into the simulation. /// public class Space : ISpace, IDisposable { private TimeStepSettings timeStepSettings; /// /// Gets or sets the time step settings used by the space. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } set { timeStepSettings = value; DeactivationManager.TimeStepSettings = value; ForceUpdater.TimeStepSettings = value; BoundingBoxUpdater.TimeStepSettings = value; Solver.TimeStepSettings = value; PositionUpdater.TimeStepSettings = value; } } IThreadManager threadManager; /// /// Gets or sets the thread manager used by the space. /// public IThreadManager ThreadManager { get { return threadManager; } set { threadManager = value; DeactivationManager.ThreadManager = value; ForceUpdater.ThreadManager = value; BoundingBoxUpdater.ThreadManager = value; BroadPhase.ThreadManager = value; NarrowPhase.ThreadManager = value; Solver.ThreadManager = value; PositionUpdater.ThreadManager = value; DuringForcesUpdateables.ThreadManager = value; BeforeNarrowPhaseUpdateables.ThreadManager = value; EndOfTimeStepUpdateables.ThreadManager = value; EndOfFrameUpdateables.ThreadManager = value; } } /// /// Gets or sets the space object buffer used by the space. /// The space object buffer allows objects to be safely asynchronously /// added to and removed from the space. /// public SpaceObjectBuffer SpaceObjectBuffer { get; set; } /// /// Gets or sets the entity state write buffer used by the space. /// The write buffer contains buffered writes to entity states that are /// flushed each frame when the buffer is updated. /// public EntityStateWriteBuffer EntityStateWriteBuffer { get; set; } /// /// Gets or sets the deactivation manager used by the space. /// The deactivation manager controls the activity state objects, putting them /// to sleep and managing the connections between objects and simulation islands. /// public DeactivationManager DeactivationManager { get; set; } /// /// Gets or sets the force updater used by the space. /// The force updater applies forces to all dynamic objects in the space each frame. /// public ForceUpdater ForceUpdater { get; set; } /// /// Gets or sets the bounding box updater used by the space. /// The bounding box updater updates the bounding box of mobile collidables each frame. /// public BoundingBoxUpdater BoundingBoxUpdater { get; set; } private BroadPhase broadPhase; /// /// Gets or sets the broad phase used by the space. /// The broad phase finds overlaps between broad phase entries and passes /// them off to the narrow phase for processing. /// public BroadPhase BroadPhase { get { return broadPhase; } set { broadPhase = value; if (NarrowPhase != null) if (value != null) { NarrowPhase.BroadPhaseOverlaps = broadPhase.Overlaps; } else { NarrowPhase.BroadPhaseOverlaps = null; } } } /// /// Gets or sets the narrow phase used by the space. /// The narrow phase uses overlaps found by the broad phase /// to create pair handlers. Those pair handlers can go on to /// create things like contacts and constraints. /// public NarrowPhase NarrowPhase { get; set; } /// /// Gets or sets the solver used by the space. /// The solver iteratively finds a solution to the constraints in the simulation. /// public Solver Solver { get; set; } /// /// Gets or sets the position updater used by the space. /// The position updater moves everything around each frame. /// public PositionUpdater PositionUpdater { get; set; } /// /// Gets or sets the buffered states manager used by the space. /// The buffered states manager keeps track of read buffered entity states /// and also interpolated states based on the time remaining from internal /// time steps. /// public BufferedStatesManager BufferedStates { get; set; } /// /// Gets or sets the deferred event dispatcher used by the space. /// The event dispatcher gathers up deferred events created /// over the course of a timestep and dispatches them sequentially at the end. /// public DeferredEventDispatcher DeferredEventDispatcher { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update during force application. /// public DuringForcesUpdateableManager DuringForcesUpdateables { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update before the narrow phase. /// public BeforeNarrowPhaseUpdateableManager BeforeNarrowPhaseUpdateables { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update before the solver /// public BeforeSolverUpdateableManager BeforeSolverUpdateables { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update right before the position update phase. /// public BeforePositionUpdateUpdateableManager BeforePositionUpdateUpdateables { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update at the end of a timestep. /// public EndOfTimeStepUpdateableManager EndOfTimeStepUpdateables { get; set; } /// /// Gets or sets the updateable manager that handles updateables that update at the end of a frame. /// public EndOfFrameUpdateableManager EndOfFrameUpdateables { get; set; } /// /// Gets the list of entities in the space. /// public ReadOnlyList Entities { get { return BufferedStates.Entities; } } /// /// Constructs a new space for things to live in. /// Uses the SpecializedThreadManager. /// public Space() : this(new SpecializedThreadManager()) { } /// /// Constructs a new space for things to live in. /// ///Thread manager to use with the space. public Space(IThreadManager threadManager) { timeStepSettings = new TimeStepSettings(); this.threadManager = threadManager; SpaceObjectBuffer = new SpaceObjectBuffer(this); EntityStateWriteBuffer = new EntityStateWriteBuffer(); DeactivationManager = new DeactivationManager(TimeStepSettings, ThreadManager); ForceUpdater = new ForceUpdater(TimeStepSettings, ThreadManager); BoundingBoxUpdater = new BoundingBoxUpdater(TimeStepSettings, ThreadManager); BroadPhase = new DynamicHierarchy(ThreadManager); NarrowPhase = new NarrowPhase(TimeStepSettings, BroadPhase.Overlaps, ThreadManager); Solver = new Solver(TimeStepSettings, DeactivationManager, ThreadManager); NarrowPhase.Solver = Solver; PositionUpdater = new ContinuousPositionUpdater(TimeStepSettings, ThreadManager); BufferedStates = new BufferedStatesManager(ThreadManager); DeferredEventDispatcher = new DeferredEventDispatcher(); DuringForcesUpdateables = new DuringForcesUpdateableManager(timeStepSettings, ThreadManager); BeforeNarrowPhaseUpdateables = new BeforeNarrowPhaseUpdateableManager(timeStepSettings, ThreadManager); BeforeSolverUpdateables = new BeforeSolverUpdateableManager(timeStepSettings, ThreadManager); BeforePositionUpdateUpdateables = new BeforePositionUpdateUpdateableManager(timeStepSettings, ThreadManager); EndOfTimeStepUpdateables = new EndOfTimeStepUpdateableManager(timeStepSettings, ThreadManager); EndOfFrameUpdateables = new EndOfFrameUpdateableManager(timeStepSettings, ThreadManager); } /// /// Adds a space object to the simulation. /// ///Space object to add. public void Add(ISpaceObject spaceObject) { if (spaceObject.Space != null) throw new ArgumentException("The object belongs to some Space already; cannot add it again."); spaceObject.Space = this; SimulationIslandMember simulationIslandMember = spaceObject as SimulationIslandMember; if (simulationIslandMember != null) { DeactivationManager.Add(simulationIslandMember); } ISimulationIslandMemberOwner simulationIslandMemberOwner = spaceObject as ISimulationIslandMemberOwner; if (simulationIslandMemberOwner != null) { DeactivationManager.Add(simulationIslandMemberOwner.ActivityInformation); } //Go through each stage, adding the space object to it if necessary. IForceUpdateable velocityUpdateable = spaceObject as IForceUpdateable; if (velocityUpdateable != null) { ForceUpdater.Add(velocityUpdateable); } MobileCollidable boundingBoxUpdateable = spaceObject as MobileCollidable; if (boundingBoxUpdateable != null) { BoundingBoxUpdater.Add(boundingBoxUpdateable); } BroadPhaseEntry broadPhaseEntry = spaceObject as BroadPhaseEntry; if (broadPhaseEntry != null) { BroadPhase.Add(broadPhaseEntry); } //Entites own collision proxies, but are not entries themselves. IBroadPhaseEntryOwner broadPhaseEntryOwner = spaceObject as IBroadPhaseEntryOwner; if (broadPhaseEntryOwner != null) { BroadPhase.Add(broadPhaseEntryOwner.Entry); boundingBoxUpdateable = broadPhaseEntryOwner.Entry as MobileCollidable; if (boundingBoxUpdateable != null) { BoundingBoxUpdater.Add(boundingBoxUpdateable); } } SolverUpdateable solverUpdateable = spaceObject as SolverUpdateable; if (solverUpdateable != null) { Solver.Add(solverUpdateable); } IPositionUpdateable integrable = spaceObject as IPositionUpdateable; if (integrable != null) { PositionUpdater.Add(integrable); } Entity entity = spaceObject as Entity; if (entity != null) { BufferedStates.Add(entity); } IDeferredEventCreator deferredEventCreator = spaceObject as IDeferredEventCreator; if (deferredEventCreator != null) { DeferredEventDispatcher.AddEventCreator(deferredEventCreator); } IDeferredEventCreatorOwner deferredEventCreatorOwner = spaceObject as IDeferredEventCreatorOwner; if (deferredEventCreatorOwner != null) { DeferredEventDispatcher.AddEventCreator(deferredEventCreatorOwner.EventCreator); } //Updateable stages. IDuringForcesUpdateable duringForcesUpdateable = spaceObject as IDuringForcesUpdateable; if (duringForcesUpdateable != null) { DuringForcesUpdateables.Add(duringForcesUpdateable); } IBeforeNarrowPhaseUpdateable beforeNarrowPhaseUpdateable = spaceObject as IBeforeNarrowPhaseUpdateable; if (beforeNarrowPhaseUpdateable != null) { BeforeNarrowPhaseUpdateables.Add(beforeNarrowPhaseUpdateable); } IBeforeSolverUpdateable beforeSolverUpdateable = spaceObject as IBeforeSolverUpdateable; if (beforeSolverUpdateable != null) { BeforeSolverUpdateables.Add(beforeSolverUpdateable); } IBeforePositionUpdateUpdateable beforePositionUpdateUpdateable = spaceObject as IBeforePositionUpdateUpdateable; if (beforePositionUpdateUpdateable != null) { BeforePositionUpdateUpdateables.Add(beforePositionUpdateUpdateable); } IEndOfTimeStepUpdateable endOfStepUpdateable = spaceObject as IEndOfTimeStepUpdateable; if (endOfStepUpdateable != null) { EndOfTimeStepUpdateables.Add(endOfStepUpdateable); } IEndOfFrameUpdateable endOfFrameUpdateable = spaceObject as IEndOfFrameUpdateable; if (endOfFrameUpdateable != null) { EndOfFrameUpdateables.Add(endOfFrameUpdateable); } spaceObject.OnAdditionToSpace(this); } /// /// Removes a space object from the simulation. /// ///Space object to remove. public void Remove(ISpaceObject spaceObject) { if (spaceObject.Space != this) // HACK return; SimulationIslandMember simulationIslandMember = spaceObject as SimulationIslandMember; if (simulationIslandMember != null) { DeactivationManager.Remove(simulationIslandMember); } ISimulationIslandMemberOwner simulationIslandMemberOwner = spaceObject as ISimulationIslandMemberOwner; if (simulationIslandMemberOwner != null) { DeactivationManager.Remove(simulationIslandMemberOwner.ActivityInformation); } //Go through each stage, removing the space object from it if necessary. IForceUpdateable velocityUpdateable = spaceObject as IForceUpdateable; if (velocityUpdateable != null) { ForceUpdater.Remove(velocityUpdateable); } MobileCollidable boundingBoxUpdateable = spaceObject as MobileCollidable; if (boundingBoxUpdateable != null) { BoundingBoxUpdater.Remove(boundingBoxUpdateable); } BroadPhaseEntry broadPhaseEntry = spaceObject as BroadPhaseEntry; if (broadPhaseEntry != null) { BroadPhase.Remove(broadPhaseEntry); } //Entites own collision proxies, but are not entries themselves. IBroadPhaseEntryOwner broadPhaseEntryOwner = spaceObject as IBroadPhaseEntryOwner; if (broadPhaseEntryOwner != null) { BroadPhase.Remove(broadPhaseEntryOwner.Entry); boundingBoxUpdateable = broadPhaseEntryOwner.Entry as MobileCollidable; if (boundingBoxUpdateable != null) { BoundingBoxUpdater.Remove(boundingBoxUpdateable); } } SolverUpdateable solverUpdateable = spaceObject as SolverUpdateable; if (solverUpdateable != null) { Solver.Remove(solverUpdateable); } IPositionUpdateable integrable = spaceObject as IPositionUpdateable; if (integrable != null) { PositionUpdater.Remove(integrable); } Entity entity = spaceObject as Entity; if (entity != null) { BufferedStates.Remove(entity); } IDeferredEventCreator deferredEventCreator = spaceObject as IDeferredEventCreator; if (deferredEventCreator != null) { DeferredEventDispatcher.RemoveEventCreator(deferredEventCreator); } IDeferredEventCreatorOwner deferredEventCreatorOwner = spaceObject as IDeferredEventCreatorOwner; if (deferredEventCreatorOwner != null) { DeferredEventDispatcher.RemoveEventCreator(deferredEventCreatorOwner.EventCreator); } //Updateable stages. IDuringForcesUpdateable duringForcesUpdateable = spaceObject as IDuringForcesUpdateable; if (duringForcesUpdateable != null) { DuringForcesUpdateables.Remove(duringForcesUpdateable); } IBeforeNarrowPhaseUpdateable beforeNarrowPhaseUpdateable = spaceObject as IBeforeNarrowPhaseUpdateable; if (beforeNarrowPhaseUpdateable != null) { BeforeNarrowPhaseUpdateables.Remove(beforeNarrowPhaseUpdateable); } IBeforeSolverUpdateable beforeSolverUpdateable = spaceObject as IBeforeSolverUpdateable; if (beforeSolverUpdateable != null) { BeforeSolverUpdateables.Remove(beforeSolverUpdateable); } IBeforePositionUpdateUpdateable beforePositionUpdateUpdateable = spaceObject as IBeforePositionUpdateUpdateable; if (beforePositionUpdateUpdateable != null) { BeforePositionUpdateUpdateables.Remove(beforePositionUpdateUpdateable); } IEndOfTimeStepUpdateable endOfStepUpdateable = spaceObject as IEndOfTimeStepUpdateable; if (endOfStepUpdateable != null) { EndOfTimeStepUpdateables.Remove(endOfStepUpdateable); } IEndOfFrameUpdateable endOfFrameUpdateable = spaceObject as IEndOfFrameUpdateable; if (endOfFrameUpdateable != null) { EndOfFrameUpdateables.Remove(endOfFrameUpdateable); } spaceObject.Space = null; spaceObject.OnRemovalFromSpace(this); } #if PROFILE /// /// Gets the time it took to perform the previous time step. /// public double Time { get { return (end - start) / (double)Stopwatch.Frequency; } } private long start, end; #endif void DoTimeStep() { #if PROFILE start = Stopwatch.GetTimestamp(); #endif SpaceObjectBuffer.Update(); EntityStateWriteBuffer.Update(); DeactivationManager.Update(); ForceUpdater.Update(); DuringForcesUpdateables.Update(); BoundingBoxUpdater.Update(); BroadPhase.Update(); BeforeNarrowPhaseUpdateables.Update(); NarrowPhase.Update(); BeforeSolverUpdateables.Update(); Solver.Update(); BeforePositionUpdateUpdateables.Update(); PositionUpdater.Update(); BufferedStates.ReadBuffers.Update(); DeferredEventDispatcher.Update(); EndOfTimeStepUpdateables.Update(); #if PROFILE end = Stopwatch.GetTimestamp(); #endif } /// /// Performs a single timestep. /// public void Update() { DoTimeStep(); EndOfFrameUpdateables.Update(); } /// /// Performs as many timesteps as necessary to get as close to the elapsed time as possible. /// /// Elapsed time from the previous frame. public void Update(float dt) { TimeStepSettings.AccumulatedTime += dt; for (int i = 0; i < TimeStepSettings.MaximumTimeStepsPerFrame; i++) { if (TimeStepSettings.AccumulatedTime >= TimeStepSettings.TimeStepDuration) { TimeStepSettings.AccumulatedTime -= TimeStepSettings.TimeStepDuration; DoTimeStep(); } else { break; } } BufferedStates.InterpolatedStates.BlendAmount = TimeStepSettings.AccumulatedTime / TimeStepSettings.TimeStepDuration; BufferedStates.InterpolatedStates.Update(); EndOfFrameUpdateables.Update(); } /// /// Tests a ray against the space. /// /// Ray to test. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, out RayCastResult result) { return RayCast(ray, float.MaxValue, out result); } /// /// Tests a ray against the space. /// /// Ray to test. /// Delegate to prune out hit candidates before performing a ray cast against them. Return true from the filter to process an entry or false to ignore the entry. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, Func filter, out RayCastResult result) { return RayCast(ray, float.MaxValue, filter, out result); } /// /// Tests a ray against the space. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, float maximumLength, out RayCastResult result) { var resultsList = PhysicsResources.GetRayCastResultList(); bool didHit = RayCast(ray, maximumLength, resultsList); result = resultsList.Elements[0]; for (int i = 1; i < resultsList.Count; i++) { RayCastResult candidate = resultsList.Elements[i]; if (candidate.HitData.T < result.HitData.T) result = candidate; } PhysicsResources.GiveBack(resultsList); return didHit; } /// /// Tests a ray against the space. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Delegate to prune out hit candidates before performing a ray cast against them. Return true from the filter to process an entry or false to ignore the entry. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, float maximumLength, Func filter, out RayCastResult result) { var resultsList = PhysicsResources.GetRayCastResultList(); bool didHit = RayCast(ray, maximumLength, filter, resultsList); result = resultsList.Elements[0]; for (int i = 1; i < resultsList.Count; i++) { RayCastResult candidate = resultsList.Elements[i]; if (candidate.HitData.T < result.HitData.T) result = candidate; } PhysicsResources.GiveBack(resultsList); return didHit; } /// /// Tests a ray against the space, possibly returning multiple hits. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, float maximumLength, IList outputRayCastResults) { var outputIntersections = PhysicsResources.GetBroadPhaseEntryList(); if (BroadPhase.QueryAccelerator.RayCast(ray, maximumLength, outputIntersections)) { for (int i = 0; i < outputIntersections.Count; i++) { RayHit rayHit; BroadPhaseEntry candidate = outputIntersections.Elements[i]; if (candidate.RayCast(ray, maximumLength, out rayHit)) { outputRayCastResults.Add(new RayCastResult(rayHit, candidate)); } } } PhysicsResources.GiveBack(outputIntersections); return outputRayCastResults.Count > 0; } /// /// Tests a ray against the space, possibly returning multiple hits. /// /// Ray to test. /// Maximum length of the ray in units of the ray direction's length. /// Delegate to prune out hit candidates before performing a cast against them. Return true from the filter to process an entry or false to ignore the entry. /// Hit data of the ray, if any. /// Whether or not the ray hit anything. public bool RayCast(Ray ray, float maximumLength, Func filter, IList outputRayCastResults) { var outputIntersections = PhysicsResources.GetBroadPhaseEntryList(); if (BroadPhase.QueryAccelerator.RayCast(ray, maximumLength, outputIntersections)) { for (int i = 0; i < outputIntersections.Count; i++) { RayHit rayHit; BroadPhaseEntry candidate = outputIntersections.Elements[i]; if (candidate.RayCast(ray, maximumLength, filter, out rayHit)) { outputRayCastResults.Add(new RayCastResult(rayHit, candidate)); } } } PhysicsResources.GiveBack(outputIntersections); return outputRayCastResults.Count > 0; } /// /// Casts a convex shape against the space. /// Convex casts are sensitive to length; avoid extremely long convex casts for better stability and performance. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. Avoid extremely long convex casts for better stability and performance. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayCastResult castResult) { var castResults = PhysicsResources.GetRayCastResultList(); bool didHit = ConvexCast(castShape, ref startingTransform, ref sweep, castResults); castResult = castResults.Elements[0]; for (int i = 1; i < castResults.Count; i++) { RayCastResult candidate = castResults.Elements[i]; if (candidate.HitData.T < castResult.HitData.T) castResult = candidate; } PhysicsResources.GiveBack(castResults); return didHit; } /// /// Casts a convex shape against the space. /// Convex casts are sensitive to length; avoid extremely long convex casts for better stability and performance. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. Avoid extremely long convex casts for better stability and performance. /// Delegate to prune out hit candidates before performing a cast against them. Return true from the filter to process an entry or false to ignore the entry. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, out RayCastResult castResult) { var castResults = PhysicsResources.GetRayCastResultList(); bool didHit = ConvexCast(castShape, ref startingTransform, ref sweep, filter, castResults); castResult = castResults.Elements[0]; for (int i = 1; i < castResults.Count; i++) { RayCastResult candidate = castResults.Elements[i]; if (candidate.HitData.T < castResult.HitData.T) castResult = candidate; } PhysicsResources.GiveBack(castResults); return didHit; } /// /// Casts a convex shape against the space. /// Convex casts are sensitive to length; avoid extremely long convex casts for better stability and performance. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. Avoid extremely long convex casts for better stability and performance. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, IList outputCastResults) { var overlappedElements = PhysicsResources.GetBroadPhaseEntryList(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); BroadPhase.QueryAccelerator.GetEntries(boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; ++i) { RayHit hit; if (overlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, out hit)) { outputCastResults.Add(new RayCastResult { HitData = hit, HitObject = overlappedElements.Elements[i] }); } } PhysicsResources.GiveBack(overlappedElements); return outputCastResults.Count > 0; } /// /// Casts a convex shape against the space. /// Convex casts are sensitive to length; avoid extremely long convex casts for better stability and performance. /// /// Shape to cast. /// Initial transform of the shape. /// Sweep to apply to the shape. Avoid extremely long convex casts for better stability and performance. /// Delegate to prune out hit candidates before performing a cast against them. Return true from the filter to process an entry or false to ignore the entry. /// Hit data, if any. /// Whether or not the cast hit anything. public bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, Func filter, IList outputCastResults) { var overlappedElements = PhysicsResources.GetBroadPhaseEntryList(); BoundingBox boundingBox; castShape.GetSweptBoundingBox(ref startingTransform, ref sweep, out boundingBox); BroadPhase.QueryAccelerator.GetEntries(boundingBox, overlappedElements); for (int i = 0; i < overlappedElements.Count; ++i) { RayHit hit; if (overlappedElements.Elements[i].ConvexCast(castShape, ref startingTransform, ref sweep, filter, out hit)) { outputCastResults.Add(new RayCastResult { HitData = hit, HitObject = overlappedElements.Elements[i] }); } } PhysicsResources.GiveBack(overlappedElements); return outputCastResults.Count > 0; } bool disposed; /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { if (!disposed) { disposed = true; ThreadManager.Dispose(); } } } } ================================================ FILE: BEPUphysics/StyleCop.Cache ================================================ 10 06/30/2010 11:43:56 0 10/22/2010 20:31:19 604 The file has no header, the header Xml is invalid, or the header is not located at the top of the file. 1 False All using directives must be placed inside of the namespace. 1 False All using directives must be placed inside of the namespace. 2 False Using directives must be sorted alphabetically by the namespaces. 2 False All using directives must be placed inside of the namespace. 3 False Using directives must be sorted alphabetically by the namespaces. 3 False All using directives must be placed inside of the namespace. 4 False Using directives must be sorted alphabetically by the namespaces. 4 False All using directives must be placed inside of the namespace. 5 False All using directives must be placed inside of the namespace. 6 False System using directives must be placed before all other using directives. 6 False All using directives must be placed inside of the namespace. 7 False The class must have a documentation header. 11 False The field must have a documentation header. 13 False The field must have an access modifier. 13 False The constructor must have a documentation header. 15 False The call to Reconstruct must begin with the 'this.' prefix to indicate that the item is a member of the class. 17 False The method must have a documentation header. 20 False The body of the if statement must be wrapped in opening and closing curly brackets. 28 False The body of the if statement must be wrapped in opening and closing curly brackets. 30 False The body of the if statement must be wrapped in opening and closing curly brackets. 32 False The body of the if statement must be wrapped in opening and closing curly brackets. 35 False The body of the if statement must be wrapped in opening and closing curly brackets. 37 False The body of the if statement must be wrapped in opening and closing curly brackets. 39 False The method must have an access modifier. 20 False All private methods must be placed after all public methods. 43 False All private methods must be placed after all public methods. 66 False The method must have a documentation header. 43 False Insert parenthesis within the arithmetic expression to declare the operator precedence. 46 False The call to nodeArray must begin with the 'this.' prefix to indicate that the item is a member of the class. 56 False The call to nodeArray must begin with the 'this.' prefix to indicate that the item is a member of the class. 54 False The call to ReconstructNode must begin with the 'this.' prefix to indicate that the item is a member of the class. 59 False The comment must start with a single space. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 45 False The method must have a documentation header. 62 False The method must have an access modifier. 62 False The method must have a documentation header. 66 False The call to Refit must begin with the 'this.' prefix to indicate that the item is a member of the class. 68 False The method must have a documentation header. 71 False A single-line comment must be preceded by a blank line or another single-line comment, or must be the first item in its scope. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 77 False The method must have an access modifier. 71 False The call to nodeArray must begin with the 'this.' prefix to indicate that the item is a member of the class. 76 False The call to nodeArray must begin with the 'this.' prefix to indicate that the item is a member of the class. 74 False The call to Refit must begin with the 'this.' prefix to indicate that the item is a member of the class. 85 False The call to Refit must begin with the 'this.' prefix to indicate that the item is a member of the class. 87 False The call to nodeArray must begin with the 'this.' prefix to indicate that the item is a member of the class. 88 False The comment must start with a single space. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 77 False The comment must start with a single space. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 81 False The comment must start with a single space. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 82 False The comment must start with a single space. To ignore this error when commenting out a line of code, begin the comment with '////' rather than '//'. 83 False The struct must have a documentation header. 92 False The code must not contain multiple blank lines in a row. 97 False A closing curly bracket must not be preceded by a blank line. 98 False The struct must have an access modifier. 92 False The field must have a documentation header. 94 False The field must have a documentation header. 95 False Public and internal fields must start with an upper-case letter: value. 95 False TRACE;WINDOWS 06/30/2010 11:43:56 0 10/30/2010 18:41:28 690 The file has no header, the header Xml is invalid, or the header is not located at the top of the file. 1 False All using directives must be placed inside of the namespace. 1 False All using directives must be placed inside of the namespace. 2 False All using directives must be placed inside of the namespace. 3 False All using directives must be placed inside of the namespace. 4 False All using directives must be placed inside of the namespace. 5 False Using directives must be sorted alphabetically by the namespaces. 5 False All using directives must be placed inside of the namespace. 6 False Using directives must be sorted alphabetically by the namespaces. 6 False All using directives must be placed inside of the namespace. 7 False Using directives must be sorted alphabetically by the namespaces. 7 False All using directives must be placed inside of the namespace. 8 False A closing curly bracket must not be preceded by a blank line. 30 False The class must have a documentation header. 12 False A closing curly bracket must not be preceded by a blank line. 28 False The constructor must have a documentation header. 16 False All constructors must be placed after all fields. 16 False The field must have a documentation header. 22 False Fields must be declared with private access. Use properties to expose fields. 22 False Public and internal fields must start with an upper-case letter: shapeA. 22 False The field must have a documentation header. 23 False Fields must be declared with private access. Use properties to expose fields. 23 False Public and internal fields must start with an upper-case letter: shapeB. 23 False The property must have a documentation header. 25 False The property must have a documentation header. 26 False Adjacent elements must be separated by a blank line. 26 False ================================================ FILE: BEPUphysics/Threading/IThreadManager.cs ================================================ using System; namespace BEPUphysics.Threading { /// /// Manages the engine's threads. /// /// /// The thread manager is constructed with certain access assumptions in mind. /// When implementing custom thread managers, ensure that the requirements are met /// or exceeded with regard to concurrent access. /// public interface IThreadManager : IDisposable { /// /// Gets the number of threads currently managed by the thread manager. /// int ThreadCount { get; } /// /// Adds a new worker thread to the engine. /// void AddThread(); /// /// Adds a new worker thread to the engine. /// /// Function that the new thread will call before entering its work loop. /// Data to give the initializer. void AddThread(Action initialization, object initializationInformation); /// /// Enqueues a task to the thread manager. /// This should be safe to call from multiple threads and from other tasks. /// /// Method to run. /// Data to give to the task. void EnqueueTask(Action taskBody, object taskInformation); /// /// Loops from the starting index (inclusive) to the ending index (exclusive), calling the loopBody at each iteration. /// The forLoop function will not return until all iterations are complete. /// This is meant to be used in a 'fork-join' model; only a single thread should be running a forLoop /// at any time. /// /// Inclusive starting index. /// Exclusive ending index. /// Function that handles an individual iteration of the loop. void ForLoop(int startIndex, int endIndex, Action loopBody); /// /// Removes a worker thread from the engine. /// void RemoveThread(); /// /// Waits until all tasks enqueued using enqueueTask are complete. /// void WaitForTaskCompletion(); //optional; if non-setting enqueue is added, doTasks is needed. //non-setting enqueue //doTasks } } ================================================ FILE: BEPUphysics/Threading/Modified Pool/ParallelLoopManager.cs ================================================ using System; using System.Collections.Generic; using System.Threading; namespace BEPUphysics.Threading { /// /// Manages parallel for loops. /// Cannot handle general task-based parallelism. /// public class ParallelLoopManager : IDisposable { private readonly AutoResetEvent loopFinished; private int workerCount; internal List workers = new List(); //internal SemaphoreSlim workerWaker; internal int currentBeginIndex, currentEndIndex; internal Action currentLoopBody; internal int iterationsPerSteal; /// /// Gets or sets the minimum number of tasks to be allocated to each thread /// per loop. /// public int MinimumTasksPerThread { get { return minimumTasksPerThread; } set { minimumTasksPerThread = value; } } /// /// Gets or sets the maximum number of loop iterations /// per individual task. /// public int MaximumIterationsPerTask { get { return maximumIterationsPerTask; } set { maximumIterationsPerTask = value; } } #if WINDOWS private int minimumTasksPerThread = 3; private int maximumIterationsPerTask = 80; #else int minimumTasksPerThread = 3; int maximumIterationsPerTask = 80; #endif internal int jobIndex; internal int maxJobIndex; /// /// Constructs a new parallel loop manager. /// public ParallelLoopManager() { loopFinished = new AutoResetEvent(false); //workerWaker = new SemaphoreSlim(0); } internal void AddThread() { AddThread(null, null); } internal void AddThread(Action threadStart, object threadStartInformation) { workers.Add(new ParallelLoopWorker(this, threadStart, threadStartInformation)); } internal void RemoveThread() { if (workers.Count > 0) { lock (workers[0].disposedLocker) { if (!workers[0].disposed) { currentLoopBody = null; workerCount = 1; workers[0].getToWork.Set(); loopFinished.WaitOne(); workers[0].Dispose(); } } workers.RemoveAt(0); } } /// /// Iterates over the interval. /// /// Starting index of the iteration. /// Ending index of the iteration. /// Function to call on each iteration. public void ForLoop(int beginIndex, int endIndex, Action loopBody) { //CANNOT CALL THIS WHILE BUSY!!!! ASSUME THAT IS GUARANTEED. //Compute intervals for each worker. workerCount = workers.Count; //TODO: The job splitting could be tuned possibly. int iterationCount = endIndex - beginIndex; int tasksPerThread = Math.Max(minimumTasksPerThread, iterationCount / maximumIterationsPerTask); int taskSubdivisions = workerCount * tasksPerThread; currentBeginIndex = beginIndex; currentEndIndex = endIndex; currentLoopBody = loopBody; iterationsPerSteal = Math.Max(1, iterationCount / taskSubdivisions); jobIndex = 0; float maxJobs = iterationCount / (float) iterationsPerSteal; if (maxJobs % 1 == 0) maxJobIndex = (int) maxJobs; else maxJobIndex = 1 + (int) maxJobs; for (int i = 0; i < workers.Count; i++) { workers[i].finalIndex = endIndex; workers[i].iterationsPerSteal = iterationsPerSteal; workers[i].getToWork.Set(); } loopFinished.WaitOne(); } internal void OnWorkerFinish() { if (Interlocked.Decrement(ref workerCount) == 0) loopFinished.Set(); } #region IDisposable Members private bool disposed; private readonly object disposedLocker = new object(); /// /// Releases resources used by the object. /// public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; while (workers.Count > 0) { RemoveThread(); } loopFinished.Close(); GC.SuppressFinalize(this); } } } /// /// Releases resources used by the object. /// ~ParallelLoopManager() { Dispose(); } #endregion } } ================================================ FILE: BEPUphysics/Threading/Modified Pool/ParallelLoopWorker.cs ================================================ using System; using System.Threading; namespace BEPUphysics.Threading { internal class ParallelLoopWorker : IDisposable { private readonly ParallelLoopManager manager; internal bool disposed; internal object disposedLocker = new object(); internal int finalIndex; internal AutoResetEvent getToWork; private object initializationInformation; internal int iterationsPerSteal; private Thread thread; private Action threadStart; internal ParallelLoopWorker(ParallelLoopManager manager, Action threadStart, object initializationInformation) { this.manager = manager; this.threadStart = threadStart; this.initializationInformation = initializationInformation; getToWork = new AutoResetEvent(false); thread = new Thread(Work) {IsBackground = true}; thread.Start(); } /// /// Releases resources used by the object. /// ~ParallelLoopWorker() { Dispose(); } #region IDisposable Members /// /// Disposes the worker. /// public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; getToWork.Close(); getToWork = null; thread = null; GC.SuppressFinalize(this); } } } #endregion internal void Work() { if (threadStart != null) { threadStart(initializationInformation); } threadStart = null; initializationInformation = null; while (true) { //When ThreadManager sees a loop available, it set it up and then wake me up. getToWork.WaitOne(); if (manager.currentLoopBody == null) { //Woops, looks like it's time for me to die. manager.OnWorkerFinish(); return; } while (manager.jobIndex <= manager.maxJobIndex) { //Claim a piece of job. int jobIndex = Interlocked.Increment(ref manager.jobIndex); //The job interval. int endIndex = jobIndex * iterationsPerSteal; int beginIndex = endIndex - iterationsPerSteal; //Do the job piece. Make sure you don't do more than exists in the list itself. for (int i = beginIndex; i < endIndex && i < finalIndex; i++) { manager.currentLoopBody(i); } } //this is not 'thread safe' but the result of the unsafety is a quick fail in the worst case. manager.OnWorkerFinish(); } } } } ================================================ FILE: BEPUphysics/Threading/Modified Pool/SpecializedThreadManager.cs ================================================ using System; namespace BEPUphysics.Threading { /// /// Manages the engine's threads. /// /// /// Separates the management of ThreadTasks and loops /// into specialized systems. Should have generally higher /// performance than the SimpleThreadManager. /// public class SpecializedThreadManager : IThreadManager { private readonly object disposedLocker = new object(); private bool disposed; private ParallelLoopManager loopManager; private ThreadTaskManager taskManager; /// /// Constructs a new specialized thread manager /// that manages loops and tasks separately. /// public SpecializedThreadManager() { taskManager = new ThreadTaskManager(); loopManager = new ParallelLoopManager(); } /// /// Releases resources used by the object. /// ~SpecializedThreadManager() { Dispose(); } /// /// Gets or sets the loop manager used by this threading system. /// The loop manager is used to specifically parallelize forloops. /// public ParallelLoopManager LoopManager { get { return loopManager; } set { loopManager = value; } } /// /// Gets or sets the task manager used by this threading system. /// The task manager is used for anything that isn't strictly a /// for loop. /// public ThreadTaskManager TaskManager { get { return taskManager; } set { taskManager = value; } } #region IThreadManager Members /// /// Gets the number of threads in use by the manager. /// public int ThreadCount { get { return taskManager.ThreadCount; } } /// /// Adds a new worker thread to the engine. /// public void AddThread() { taskManager.AddThread(); loopManager.AddThread(); } /// /// Adds a new worker thread to the engine. /// /// Function that each of the new threads will call before entering its work loop. Note that this type of thread manager spawns two worker threads for each given thread; /// the initializer will run twice. /// Data to give the initializer. public void AddThread(Action initialization, object initializationInformation) { taskManager.AddThread(initialization, initializationInformation); loopManager.AddThread(initialization, initializationInformation); } /// /// Removes a worker thread from the engine. /// public void RemoveThread() { taskManager.RemoveThread(); loopManager.RemoveThread(); } /// /// Enqueues a task to the thread manager. /// This should be safe to call from multiple threads and from other tasks. /// /// Method to run. /// Data to give to the task. public void EnqueueTask(Action taskBody, object taskInformation) { taskManager.EnqueueTask(taskBody, taskInformation); } /// /// Loops from the starting index (inclusive) to the ending index (exclusive), calling the loopBody at each iteration. /// The forLoop function will not return until all iterations are complete. /// This is meant to be used in a 'fork-join' model; only a single thread should be running a forLoop /// at any time. /// /// Inclusive starting index. /// Exclusive ending index. /// Function that handles an individual iteration of the loop. public void ForLoop(int startIndex, int endIndex, Action loopBody) { loopManager.ForLoop(startIndex, endIndex, loopBody); } /// /// Waits until all tasks enqueued using enqueueTask are complete. /// public void WaitForTaskCompletion() { taskManager.WaitForTaskCompletion(); } /// /// Releases resources used by the object. /// public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; taskManager.Dispose(); loopManager.Dispose(); GC.SuppressFinalize(this); } } } #endregion } } ================================================ FILE: BEPUphysics/Threading/Modified Pool/ThreadTaskManager.cs ================================================ using System; using System.Collections.Generic; using System.Threading; using BEPUutilities.DataStructures; namespace BEPUphysics.Threading { /// /// Keeps track of the threads currently available to the physics engine. /// public class ThreadTaskManager : IThreadManager { private readonly object disposedLocker = new object(); private readonly Action doLoopSectionDelegate; private readonly List taskInfos = new List(); private readonly List workers = new List(); private ManualResetEvent allThreadsIdleNotifier = new ManualResetEvent(false); /// /// Index into the thread loop lists, incremented after each task allocation. /// private int currentTaskAllocationIndex; private bool disposed; private int loopTasksPerThread; private int tasksRemaining = 1; /// /// Constructs a new thread task manager. /// public ThreadTaskManager() { LoopTasksPerThread = 1; doLoopSectionDelegate = new Action(DoLoopSection); } /// /// Releases resources used by the object. /// ~ThreadTaskManager() { Dispose(); } /// /// Gets or sets the number of tasks to create per thread when doing forLoops. /// public int LoopTasksPerThread { get { return loopTasksPerThread; } set { loopTasksPerThread = value; RemakeLoopSections(); } } #region IThreadManager Members /// /// Gets the number of threads currently handled by the manager. /// public int ThreadCount { get { return workers.Count; } } /// /// Blocks the current thread until all tasks have been completed. /// public void WaitForTaskCompletion() { //TODO: Try a WAITALL version of this if (Interlocked.Decrement(ref tasksRemaining) == 0) { allThreadsIdleNotifier.Set(); } allThreadsIdleNotifier.WaitOne(); //When it gets here, it means things are successfully idle'd. tasksRemaining = 1; allThreadsIdleNotifier.Reset(); } /// /// Adds a thread to the manager. /// public void AddThread() { AddThread(null, null); } /// /// Adds a thread to the manager. /// /// A function to run to perform any initialization on the new thread. /// Data to give the ParameterizedThreadStart for initialization. public void AddThread(Action initialization, object initializationInformation) { lock (workers) { var worker = new WorkerThread(workers.Count, this, initialization, initializationInformation); workers.Add(worker); RemakeLoopSections(); } } /// /// Removes a thread from the manager. /// public void RemoveThread() { if (workers.Count > 0) { workers[0].EnqueueTask(null, null); WaitForTaskCompletion(); workers[0].Dispose(); } } /// /// Gives the thread manager a new task to run. /// /// Task to run. /// Information to be used by the task. public void EnqueueTask(Action task, object taskInformation) { lock (workers) { currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; workers[currentTaskAllocationIndex].EnqueueTask(task, taskInformation); } } /// /// Loops from the starting index (inclusive) to the ending index (exclusive), calling the loopBody at each iteration. /// The forLoop function will not return until all iterations are complete. /// This is meant to be used in a 'fork-join' model; only a single thread should be running a forLoop /// at any time. /// /// Inclusive starting index. /// Exclusive ending index. /// Function that handles an individual iteration of the loop. public void ForLoop(int startIndex, int endIndex, Action loopBody) { int subdivisions = workers.Count * loopTasksPerThread; int iterationCount = endIndex - startIndex; for (int b = 0; b < subdivisions; b++) { taskInfos[b].loopBody = loopBody; taskInfos[b].iterationCount = iterationCount; EnqueueTaskSequentially(doLoopSectionDelegate, taskInfos[b]); } WaitForTaskCompletion(); } /// /// Releases resources used by the object. /// public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; while (workers.Count > 0) { RemoveThread(); } allThreadsIdleNotifier.Close(); allThreadsIdleNotifier = null; } } } #endregion /// /// Enqueues a task. /// This method also does not perform any locking; it should only be called when all worker threads of the thread pool are idle and all calls to this method are from the same thread. /// /// Task to enqueue. /// Information for the task. public void EnqueueTaskSequentially(Action task, object taskInformation) { //enqueueTask(task, taskInformation); workers[currentTaskAllocationIndex].EnqueueTask(task, taskInformation); currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; //workers[currentTaskAllocationIndex].enqueueTaskSequentially(task, taskInformation); //currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; } private static void DoLoopSection(object o) { var data = o as LoopSection; int finalIndex = (data.iterationCount * (data.Index + 1)) / data.Subdivisions; for (int i = (data.iterationCount * data.Index) / data.Subdivisions; i < finalIndex; i++) { //do stuff data.loopBody(i); } } private void RemakeLoopSections() { taskInfos.Clear(); int workerCount = workers.Count; int subdivisions = workerCount * loopTasksPerThread; for (int i = 0; i < workerCount; i++) { for (int j = 0; j < loopTasksPerThread; j++) { taskInfos.Add(new LoopSection(i * loopTasksPerThread + j, subdivisions)); } } } private struct TaskEntry { internal readonly object Info; internal readonly Action Task; internal TaskEntry(Action task, object info) { Task = task; Info = info; } } private class LoopSection { internal readonly int Index; internal readonly int Subdivisions; internal int iterationCount; internal Action loopBody; internal LoopSection(int index, int subdivisions) { Index = index; Subdivisions = subdivisions; } } private class WorkerThread : IDisposable { private readonly object disposedLocker = new object(); private readonly object initializationInformation; private readonly ThreadTaskManager manager; private readonly ConcurrentDeque taskData; private readonly Thread thread; private readonly Action threadStart; private bool disposed; private int index; private AutoResetEvent resetEvent = new AutoResetEvent(false); internal WorkerThread(int index, ThreadTaskManager manager, Action threadStart, object initializationInformation) { this.manager = manager; thread = new Thread(ThreadExecutionLoop); thread.IsBackground = true; taskData = new ConcurrentDeque(); this.threadStart = threadStart; this.initializationInformation = initializationInformation; UpdateIndex(index); //#if WINDOWS // ResourcePool.addThreadID(thread.ManagedThreadId); //#endif thread.Start(); } ~WorkerThread() { Dispose(); } #region IDisposable Members public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; resetEvent.Close(); resetEvent = null; manager.workers.Remove(this); GC.SuppressFinalize(this); } } } #endregion internal void EnqueueTask(Action task, object taskInformation) { Interlocked.Increment(ref manager.tasksRemaining); taskData.Enqueue(new TaskEntry(task, taskInformation)); resetEvent.Set(); } private bool TrySteal(int victim, out TaskEntry task) { return manager.workers[victim].taskData.TryDequeueLast(out task); } /// Thrown when the thread encounters an invalid state; generally propagated float.NaN's. private void ThreadExecutionLoop() { //Perform any initialization requested. if (threadStart != null) threadStart(initializationInformation); TaskEntry task; while (true) { resetEvent.WaitOne(); while (true) { if (!taskData.TryDequeueFirst(out task)) { bool gotSomething = false; for (int i = 1; i < manager.workers.Count; i++) { if (TrySteal((index + i) % manager.workers.Count, out task)) { gotSomething = true; break; } } if (!gotSomething) break; //Nothing to steal and I'm broke! Guess I'll mosey on out } try { if (task.Task != null) task.Task(task.Info); if (Interlocked.Decrement(ref manager.tasksRemaining) == 0) { manager.allThreadsIdleNotifier.Set(); } if (task.Task == null) return; } catch (ArithmeticException arithmeticException) { throw new ArithmeticException( "Some internal multithreaded arithmetic has encountered an invalid state. Check for invalid entity momentums, velocities, and positions; propagating NaN's will generally trigger this exception in the getExtremePoint function.", arithmeticException); } } } } private void UpdateIndex(int newIndex) { index = newIndex; } } } } ================================================ FILE: BEPUphysics/Threading/SimpleThreadManager.cs ================================================ using System; using System.Collections.Generic; using System.Threading; namespace BEPUphysics.Threading { /// /// Manages the engine's threads. /// /// /// Uses a simple round-robin threadpool. /// It is recommended that other thread managers are used instead of this one; /// it is kept for compatability and a fallback in case of problems. /// public class SimpleThreadManager : IThreadManager { private readonly ManualResetEvent allThreadsIdleNotifier = new ManualResetEvent(false); private readonly object disposedLocker = new object(); private readonly Action doLoopSectionDelegate; private readonly List taskInfos = new List(); private readonly List workers = new List(); /// /// Index into the thread loop lists, incremented after each task allocation. /// private int currentTaskAllocationIndex; private bool disposed; private int loopTasksPerThread; private int tasksRemaining = 1; /// /// Constructs the thread manager. /// public SimpleThreadManager() { LoopTasksPerThread = 1; doLoopSectionDelegate = new Action(DoLoopSection); } /// /// Releases resources used by the object. /// ~SimpleThreadManager() { Dispose(); } /// /// Gets or sets the number of tasks to create per thread when doing forLoops. /// public int LoopTasksPerThread { get { return loopTasksPerThread; } set { loopTasksPerThread = value; RemakeLoopSections(); } } #region IThreadManager Members /// /// Gets the number of threads currently handled by the manager. /// public int ThreadCount { get { return workers.Count; } } /// /// Blocks the current thread until all tasks have been completed. /// public void WaitForTaskCompletion() { if (Interlocked.Decrement(ref tasksRemaining) == 0) { allThreadsIdleNotifier.Set(); } allThreadsIdleNotifier.WaitOne(); //When it gets here, it means things are successfully idle'd. tasksRemaining = 1; allThreadsIdleNotifier.Reset(); } /// /// Adds a thread to the manager. /// public void AddThread() { AddThread(null, null); } /// /// Adds a thread to the manager. /// /// A function to run to perform any initialization on the new thread. /// Data to give the ParameterizedThreadStart for initialization. public void AddThread(Action initialization, object initializationInformation) { lock (workers) { var worker = new WorkerThread(this, initialization, initializationInformation); workers.Add(worker); RemakeLoopSections(); } } /// /// Removes a thread and blocks until success. /// public void RemoveThread() { EnqueueTask(null, null); WaitForTaskCompletion(); RemakeLoopSections(); } /// /// Gives the thread manager a new task to run. /// /// Task to run. /// Information to be used by the task. public void EnqueueTask(Action task, object taskInformation) { lock (workers) { workers[currentTaskAllocationIndex].EnqueueTask(task, taskInformation); currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; } } /// /// Loops from the starting index (inclusive) to the ending index (exclusive), calling the loopBody at each iteration. /// The forLoop function will not return until all iterations are complete. /// This is meant to be used in a 'fork-join' model; only a single thread should be running a forLoop /// at any time. /// /// Inclusive starting index. /// Exclusive ending index. /// Function that handles an individual iteration of the loop. public void ForLoop(int startIndex, int endIndex, Action loopBody) { int subdivisions = workers.Count * loopTasksPerThread; int iterationCount = endIndex - startIndex; for (int b = 0; b < subdivisions; b++) { taskInfos[b].loopBody = loopBody; taskInfos[b].iterationCount = iterationCount; EnqueueTaskSequentially(doLoopSectionDelegate, taskInfos[b]); } WaitForTaskCompletion(); } /// /// Releases threads and resources used by the thread manager. /// public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; ShutDown(); allThreadsIdleNotifier.Close(); GC.SuppressFinalize(this); } } } #endregion /// /// Enqueues a task. /// This method also does not perform any locking; it should only be called when all worker threads of the thread pool are idle and all calls to this method are from the same thread. /// /// Task to enqueue. /// Information for the task. public void EnqueueTaskSequentially(Action task, object taskInformation) { //enqueueTask(task, taskInformation); workers[currentTaskAllocationIndex].EnqueueTask(task, taskInformation); currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; //workers[currentTaskAllocationIndex].enqueueTaskSequentially(task, taskInformation); //currentTaskAllocationIndex = (currentTaskAllocationIndex + 1) % workers.Count; } /// /// Tells every thread in the thread manager to shut down and waits until completion. /// public void ShutDown() { var toJoin = new Queue(); for (int i = workers.Count - 1; i >= 0; i--) { lock (workers) { toJoin.Enqueue(workers[i].Thread); workers[i].EnqueueTask(null, null); } } while (toJoin.Count > 0) { toJoin.Dequeue().Join(); } } private static void DoLoopSection(object o) { var data = o as LoopSection; int finalIndex = (data.iterationCount * (data.Index + 1)) / data.Subdivisions; for (int i = (data.iterationCount * data.Index) / data.Subdivisions; i < finalIndex; i++) { //do stuff data.loopBody(i); } } private void RemakeLoopSections() { taskInfos.Clear(); int workerCount = workers.Count; int subdivisions = workerCount * loopTasksPerThread; for (int i = 0; i < workerCount; i++) { for (int j = 0; j < loopTasksPerThread; j++) { taskInfos.Add(new LoopSection(i * loopTasksPerThread + j, subdivisions)); } } } private class LoopSection { internal readonly int Index; internal readonly int Subdivisions; internal int iterationCount; internal Action loopBody; internal LoopSection(int index, int subdivisions) { Index = index; Subdivisions = subdivisions; } } private class WorkerThread : IDisposable { private readonly object disposedLocker = new object(); private readonly object initializationInformation; private readonly SimpleThreadManager manager; private readonly AutoResetEvent resetEvent = new AutoResetEvent(false); private readonly Queue taskInformationQueue; private readonly Queue> taskQueue; internal readonly Thread Thread; private readonly Action threadStart; private bool disposed; internal WorkerThread(SimpleThreadManager manager, Action threadStart, object initializationInformation) { this.manager = manager; Thread = new Thread(ThreadExecutionLoop); Thread.IsBackground = true; taskQueue = new Queue>(); taskInformationQueue = new Queue(); this.threadStart = threadStart; this.initializationInformation = initializationInformation; Thread.Start(); } /// /// Shuts down any still living threads. /// ~WorkerThread() { Dispose(); } #region IDisposable Members public void Dispose() { lock (disposedLocker) { if (!disposed) { disposed = true; ShutDownThread(); resetEvent.Close(); GC.SuppressFinalize(this); } } } #endregion internal void EnqueueTask(Action task, object taskInformation) { lock (taskQueue) { Interlocked.Increment(ref manager.tasksRemaining); taskQueue.Enqueue(task); taskInformationQueue.Enqueue(taskInformation); resetEvent.Set(); } } private void ShutDownThread() { //Let the manager know that it is done with its 'task'! if (Interlocked.Decrement(ref manager.tasksRemaining) == 0) { if (!manager.disposed) //Don't mess with the handle if it's already disposed. manager.allThreadsIdleNotifier.Set(); } //Dump out any remaining tasks in the queue. for (int i = 0; i < taskQueue.Count; i++) //This is still safe since shutDownThread is called from within a lock(taskQueue) block. { taskQueue.Dequeue(); if (Interlocked.Decrement(ref manager.tasksRemaining) == 0) { if (!manager.disposed) //Don't mess with the handle if it's already disposed. manager.allThreadsIdleNotifier.Set(); } } lock (manager.workers) manager.workers.Remove(this); } /// Thrown when the thread encounters an invalid state; generally propagated float.NaN's. private void ThreadExecutionLoop() { //Perform any initialization requested. if (threadStart != null) threadStart(initializationInformation); object information = null; while (true) { Action task = null; lock (taskQueue) { if (taskQueue.Count > 0) { task = taskQueue.Dequeue(); if (task == null) { Dispose(); return; } information = taskInformationQueue.Dequeue(); } } if (task != null) { //Perform the task! try { task(information); } catch (ArithmeticException arithmeticException) { throw new ArithmeticException( "Some internal multithreaded arithmetic has encountered an invalid state. Check for invalid entity momentums, velocities, and positions; propagating NaN's will generally trigger this exception in the getExtremePoint function.", arithmeticException); } if (Interlocked.Decrement(ref manager.tasksRemaining) == 0) { manager.allThreadsIdleNotifier.Set(); resetEvent.WaitOne(); } } else resetEvent.WaitOne(); } } } } } ================================================ FILE: BEPUphysics/Threading/ThreadManagerTPL.cs ================================================ #if WINDOWS using System; using System.Threading.Tasks; namespace BEPUphysics.Threading { /// /// Uses the .NET Task Parallel library to manage the engine's threads. /// public class ThreadManagerTPL : IThreadManager { /// /// Gets or sets the task manager used to supplement the TPL. /// public IThreadManager TaskManager { get; set; } /// /// Gets the number of threads that the threadpool is targeting. /// public int ThreadCount { get { return TaskManager.ThreadCount; } } /// /// Constructs the TPL thread manager. /// public ThreadManagerTPL() { TaskManager = new ThreadTaskManager(); } /// /// Notifies the thread manager that it should use another thread. /// public void AddThread() { TaskManager.AddThread(); } /// /// Notifies the thread manager that it should use another thread. /// /// Function to use to initialize the thread. /// Information to provide to the initializer. public void AddThread(Action initialization, object initializationInformation) { TaskManager.AddThread(initialization, initializationInformation); } /// /// Notifies the thread manager that it should decrease the number of threads used. /// public void RemoveThread() { TaskManager.RemoveThread(); } /// /// Enqueues a task to the thread manager. /// This should be safe to call from multiple threads and from other tasks. /// /// Method to run. /// Data to give to the task. public void EnqueueTask(Action taskBody, object taskInformation) { TaskManager.EnqueueTask(taskBody, taskInformation); } /// /// Loops from the starting index (inclusive) to the ending index (exclusive), calling the loopBody at each iteration. /// The forLoop function will not return until all iterations are complete. /// This is meant to be used in a 'fork-join' model; only a single thread should be running a forLoop /// at any time. /// /// Inclusive starting index. /// Exclusive ending index. /// Function that handles an individual iteration of the loop. public void ForLoop(int startIndex, int endIndex, Action loopBody) { Parallel.For(startIndex, endIndex, loopBody); } /// /// Waits until all tasks enqueued using enqueueTask are complete. /// public void WaitForTaskCompletion() { TaskManager.WaitForTaskCompletion(); } #region IDisposable Members /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { TaskManager.Dispose(); } #endregion } } #endif ================================================ FILE: BEPUphysics/TimeStepSettings.cs ================================================ namespace BEPUphysics { /// /// Contains settings for the instance's time step. /// public class TimeStepSettings { /// /// Maximum number of timesteps to perform during a given frame when Space.Update(float) is used. The unsimulated time will be accumulated for subsequent calls to Space.Update(float). /// Defaults to 3. /// public int MaximumTimeStepsPerFrame = 3; /// /// Length of each integration step. Calling a Space's Update() method moves time forward this much. /// The other method, Space.Update(float), will try to move time forward by the amount specified in the parameter by taking steps of TimeStepDuration size. /// Defaults to 1/60. /// public float TimeStepDuration = 1f / 60; /// /// Amount of time accumulated by previous calls to Space.Update(float) that has not yet been simulated. /// public float AccumulatedTime; } } ================================================ FILE: BEPUphysics/UpdateableSystems/CombinedUpdateable.cs ================================================ using System.Collections.Generic; using BEPUphysics.Constraints; namespace BEPUphysics.UpdateableSystems { /// /// A class which is both a space updateable and a Solver Updateable. /// public abstract class CombinedUpdateable : EntitySolverUpdateable, ISpaceUpdateable { private bool isSequentiallyUpdated = true; protected CombinedUpdateable() { IsUpdating = true; } #region ISpaceUpdateable Members List managers = new List(); List ISpaceUpdateable.Managers { get { return managers; } } /// /// Gets and sets whether or not the updateable should be updated sequentially even in a multithreaded space. /// If this is true, the updateable can make use of the space's ThreadManager for internal multithreading. /// public bool IsUpdatedSequentially { get { return isSequentiallyUpdated; } set { bool oldValue = isSequentiallyUpdated; isSequentiallyUpdated = value; if (value != oldValue) for (int i = 0; i < managers.Count; i++) { managers[i].SequentialUpdatingStateChanged(this); } } } /// /// Gets and sets whether or not the updateable should be updated by the space. /// public bool IsUpdating { get; set; } ISpace ISpaceObject.Space { get; set; } /// /// Gets or sets the user data associated with this object. /// public new object Tag { get; set; } #endregion } } ================================================ FILE: BEPUphysics/UpdateableSystems/FluidVolume.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.Entities; using BEPUphysics.Threading; using BEPUutilities; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUphysics.UpdateableSystems { /// /// Volume in which physically simulated objects have a buoyancy force applied to them based on their density and volume. /// public class FluidVolume : Updateable, IDuringForcesUpdateable, ICollisionRulesOwner { //TODO: The current FluidVolume implementation is awfully awful. //It would be really nice if it was a bit more flexible and less clunktastic. //(A mesh volume, maybe?) private RigidTransform surfaceTransform; private Matrix3x3 toSurfaceRotationMatrix; Vector3 upVector; /// /// Gets or sets the up vector of the fluid volume. /// public Vector3 UpVector { get { return upVector; } set { value.Normalize(); upVector = value; RecalculateBoundingBox(); } } /// /// Gets or sets the dictionary storing density multipliers for the fluid volume. If a value is specified for an entity, the density of the object is effectively scaled to match. /// Higher values make entities sink more, lower values make entities float more. /// public Dictionary DensityMultipliers { get; set; } BoundingBox boundingBox; /// /// Bounding box surrounding the surface triangles and entire depth of the object. /// public BoundingBox BoundingBox { get { return boundingBox; } } float maxDepth; /// /// Maximum depth of the fluid from the surface. /// public float MaxDepth { get { return maxDepth; } set { maxDepth = value; RecalculateBoundingBox(); } } /// /// Density of the fluid represented in the volume. /// public float Density { get; set; } int samplePointsPerDimension = 8; /// /// Number of locations along each of the horizontal axes from which to sample the shape. /// Defaults to 8. /// public int SamplePointsPerDimension { get { return samplePointsPerDimension; } set { samplePointsPerDimension = value; } } /// /// Fraction by which to reduce the linear momentum of floating objects each update. /// public float LinearDamping { get; set; } /// /// Fraction by which to reduce the angular momentum of floating objects each update. /// public float AngularDamping { get; set; } private Vector3 flowDirection; /// /// Direction in which to exert force on objects within the fluid. /// flowForce and maxFlowSpeed must have valid values as well for the flow to work. /// public Vector3 FlowDirection { get { return flowDirection; } set { float length = value.Length(); if (length > 0) { flowDirection = value / length; } else flowDirection = Vector3.Zero; //TODO: Activate bodies in water } } private float flowForce; /// /// Magnitude of the flow's force, in units of flow direction. /// flowDirection and maxFlowSpeed must have valid values as well for the flow to work. /// public float FlowForce { get { return flowForce; } set { flowForce = value; //TODO: Activate bodies in water } } float maxFlowSpeed; /// /// Maximum speed of the flow; objects will not be accelerated by the flow force beyond this speed. /// flowForce and flowDirection must have valid values as well for the flow to work. /// public float MaxFlowSpeed { get { return maxFlowSpeed; } set { maxFlowSpeed = value; } } IQueryAccelerator QueryAccelerator { get; set; } /// /// Gets or sets the thread manager used by the fluid volume. /// public IThreadManager ThreadManager { get; set; } private List surfaceTriangles; /// /// List of coplanar triangles composing the surface of the fluid. /// public List SurfaceTriangles { get { return surfaceTriangles; } set { surfaceTriangles = value; RecalculateBoundingBox(); } } float gravity; /// /// Gets or sets the gravity used by the fluid volume. /// public float Gravity { get { return gravity; } set { gravity = value; } } /// /// Creates a fluid volume. /// /// Up vector of the fluid volume. /// Strength of gravity for the purposes of the fluid volume. /// List of triangles composing the surface of the fluid. Set up as a list of length 3 arrays of Vector3's. /// Depth of the fluid back along the surface normal. /// Density of the fluid represented in the volume. /// Fraction by which to reduce the linear momentum of floating objects each update, in addition to any of the body's own damping. /// Fraction by which to reduce the angular momentum of floating objects each update, in addition to any of the body's own damping. /// System to accelerate queries to find nearby entities. /// Thread manager used by the fluid volume. public FluidVolume(Vector3 upVector, float gravity, List surfaceTriangles, float depth, float fluidDensity, float linearDamping, float angularDamping) { Gravity = gravity; SurfaceTriangles = surfaceTriangles; MaxDepth = depth; Density = fluidDensity; LinearDamping = linearDamping; AngularDamping = angularDamping; UpVector = upVector; analyzeCollisionEntryDelegate = AnalyzeEntry; DensityMultipliers = new Dictionary(); } /// /// Recalculates the bounding box of the fluid based on its depth, surface normal, and surface triangles. /// public void RecalculateBoundingBox() { var points = CommonResources.GetVectorList(); foreach (var tri in SurfaceTriangles) { points.Add(tri[0]); points.Add(tri[1]); points.Add(tri[2]); points.Add(tri[0] - upVector * MaxDepth); points.Add(tri[1] - upVector * MaxDepth); points.Add(tri[2] - upVector * MaxDepth); } boundingBox = BoundingBox.CreateFromPoints(points); CommonResources.GiveBack(points); //Compute the transforms used to pull objects into fluid local space. Toolbox.GetQuaternionBetweenNormalizedVectors(ref Toolbox.UpVector, ref upVector, out surfaceTransform.Orientation); Matrix3x3.CreateFromQuaternion(ref surfaceTransform.Orientation, out toSurfaceRotationMatrix); surfaceTransform.Position = surfaceTriangles[0][0]; } List broadPhaseEntries = new List(); /// /// Applies buoyancy forces to appropriate objects. /// Called automatically when needed by the owning Space. /// /// Time since last frame in physical logic. void IDuringForcesUpdateable.Update(float dt) { QueryAccelerator.GetEntries(boundingBox, broadPhaseEntries); //TODO: Could integrate the entire thing into the collision detection pipeline. Applying forces //in the collision detection pipeline isn't allowed, so there'd still need to be an Updateable involved. //However, the broadphase query would be eliminated and the raycasting work would be automatically multithreaded. this.dt = dt; //Don't always multithread. For small numbers of objects, the overhead of using multithreading isn't worth it. //Could tune this value depending on platform for better performance. if (broadPhaseEntries.Count > 30 && ThreadManager.ThreadCount > 1) ThreadManager.ForLoop(0, broadPhaseEntries.Count, analyzeCollisionEntryDelegate); else for (int i = 0; i < broadPhaseEntries.Count; i++) { AnalyzeEntry(i); } broadPhaseEntries.Clear(); } float dt; Action analyzeCollisionEntryDelegate; void AnalyzeEntry(int i) { var entityCollidable = broadPhaseEntries[i] as EntityCollidable; if (entityCollidable != null && entityCollidable.IsActive && entityCollidable.entity.isDynamic && CollisionRules.collisionRuleCalculator(this, entityCollidable) <= CollisionRule.Normal) { bool keepGoing = false; foreach (var tri in surfaceTriangles) { //Don't need to do anything if the entity is outside of the water. if (Toolbox.IsPointInsideTriangle(ref tri[0], ref tri[1], ref tri[2], ref entityCollidable.worldTransform.Position)) { keepGoing = true; break; } } if (!keepGoing) return; //The entity is submerged, apply buoyancy forces. float submergedVolume; Vector3 submergedCenter; GetBuoyancyInformation(entityCollidable, out submergedVolume, out submergedCenter); if (submergedVolume > 0) { float fractionSubmerged = submergedVolume / entityCollidable.entity.volume; //Divide the volume by the density multiplier if present. float densityMultiplier; if (DensityMultipliers.TryGetValue(entityCollidable.entity, out densityMultiplier)) { submergedVolume /= densityMultiplier; } Vector3 force; Vector3.Multiply(ref upVector, -gravity * Density * dt * submergedVolume, out force); entityCollidable.entity.ApplyImpulse(ref submergedCenter, ref force); //Flow if (FlowForce != 0) { float dot = Math.Max(Vector3.Dot(entityCollidable.entity.linearVelocity, flowDirection), 0); if (dot < MaxFlowSpeed) { force = Math.Min(FlowForce, (MaxFlowSpeed - dot) * entityCollidable.entity.mass) * dt * fractionSubmerged * FlowDirection; entityCollidable.entity.ApplyLinearImpulse(ref force); } } //Damping entityCollidable.entity.ModifyLinearDamping(fractionSubmerged * LinearDamping); entityCollidable.entity.ModifyAngularDamping(fractionSubmerged * AngularDamping); } } } void GetBuoyancyInformation(EntityCollidable collidable, out float submergedVolume, out Vector3 submergedCenter) { BoundingBox entityBoundingBox; RigidTransform localTransform; RigidTransform.TransformByInverse(ref collidable.worldTransform, ref surfaceTransform, out localTransform); collidable.Shape.GetBoundingBox(ref localTransform, out entityBoundingBox); if (entityBoundingBox.Min.Y > 0) { //Fish out of the water. Don't need to do raycast tests on objects not at the boundary. submergedVolume = 0; submergedCenter = collidable.worldTransform.Position; return; } if (entityBoundingBox.Max.Y < 0) { submergedVolume = collidable.entity.volume; submergedCenter = collidable.worldTransform.Position; return; } Vector3 origin, xSpacing, zSpacing; float perColumnArea; GetSamplingOrigin(ref entityBoundingBox, out xSpacing, out zSpacing, out perColumnArea, out origin); float boundingBoxHeight = entityBoundingBox.Max.Y - entityBoundingBox.Min.Y; float maxLength = -entityBoundingBox.Min.Y; submergedCenter = new Vector3(); submergedVolume = 0; for (int i = 0; i < samplePointsPerDimension; i++) { for (int j = 0; j < samplePointsPerDimension; j++) { Vector3 columnVolumeCenter; float submergedHeight; if ((submergedHeight = GetSubmergedHeight(collidable, maxLength, boundingBoxHeight, ref origin, ref xSpacing, ref zSpacing, i, j, out columnVolumeCenter)) > 0) { float columnVolume = submergedHeight * perColumnArea; Vector3.Multiply(ref columnVolumeCenter, columnVolume, out columnVolumeCenter); Vector3.Add(ref columnVolumeCenter, ref submergedCenter, out submergedCenter); submergedVolume += columnVolume; } } } Vector3.Divide(ref submergedCenter, submergedVolume, out submergedCenter); //Pull the submerged center into world space before applying the force. RigidTransform.Transform(ref submergedCenter, ref surfaceTransform, out submergedCenter); } void GetSamplingOrigin(ref BoundingBox entityBoundingBox, out Vector3 xSpacing, out Vector3 zSpacing, out float perColumnArea, out Vector3 origin) { //Compute spacing and increment informaiton. float widthIncrement = (entityBoundingBox.Max.X - entityBoundingBox.Min.X) / samplePointsPerDimension; float lengthIncrement = (entityBoundingBox.Max.Z - entityBoundingBox.Min.Z) / samplePointsPerDimension; xSpacing = new Vector3(widthIncrement, 0, 0); zSpacing = new Vector3(0, 0, lengthIncrement); Vector3.Transform(ref xSpacing, ref surfaceTransform.Orientation, out xSpacing); Vector3.Transform(ref zSpacing, ref surfaceTransform.Orientation, out zSpacing); perColumnArea = widthIncrement * lengthIncrement; //Compute the origin. Vector3 minimum; RigidTransform.Transform(ref entityBoundingBox.Min, ref surfaceTransform, out minimum); //Matrix3X3.TransformTranspose(ref entityBoundingBox.Min, ref surfaceOrientationTranspose, out minimum); Vector3 offset; Vector3.Multiply(ref xSpacing, .5f, out offset); Vector3.Add(ref minimum, ref offset, out origin); Vector3.Multiply(ref zSpacing, .5f, out offset); Vector3.Add(ref origin, ref offset, out origin); //TODO: Could adjust the grid origin such that a ray always hits the deepest point. //The below code is a prototype of the idea, but has bugs. //var convexInfo = collidable as ConvexCollisionInformation; //if (convexInfo != null) //{ // Vector3 dir; // Vector3.Negate(ref upVector, out dir); // Vector3 extremePoint; // convexInfo.Shape.GetExtremePoint(dir, ref convexInfo.worldTransform, out extremePoint); // //Use extreme point to snap to grid. // Vector3.Subtract(ref extremePoint, ref origin, out offset); // float offsetX, offsetZ; // Vector3.Dot(ref offset, ref right, out offsetX); // Vector3.Dot(ref offset, ref backward, out offsetZ); // offsetX %= widthIncrement; // offsetZ %= lengthIncrement; // if (offsetX > .5f * widthIncrement) // { // Vector3.Multiply(ref right, 1 - offsetX, out offset); // } // else // { // Vector3.Multiply(ref right, -offsetX, out offset); // } // if (offsetZ > .5f * lengthIncrement) // { // Vector3 temp; // Vector3.Multiply(ref right, 1 - offsetZ, out temp); // Vector3.Add(ref temp, ref offset, out offset); // } // else // { // Vector3 temp; // Vector3.Multiply(ref right, -offsetZ, out temp); // Vector3.Add(ref temp, ref offset, out offset); // } // Vector3.Add(ref origin, ref offset, out origin); //} } float GetSubmergedHeight(EntityCollidable collidable, float maxLength, float boundingBoxHeight, ref Vector3 rayOrigin, ref Vector3 xSpacing, ref Vector3 zSpacing, int i, int j, out Vector3 volumeCenter) { Ray ray; Vector3.Multiply(ref xSpacing, i, out ray.Position); Vector3.Multiply(ref zSpacing, j, out ray.Direction); Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position); Vector3.Add(ref ray.Position, ref rayOrigin, out ray.Position); ray.Direction = upVector; //do a bottom-up raycast. RayHit rayHit; //Only go up to maxLength. If it's further away than maxLength, then it's above the water and it doesn't contribute anything. if (collidable.RayCast(ray, maxLength, out rayHit)) { //Position the ray to point from the other side. Vector3.Multiply(ref ray.Direction, boundingBoxHeight, out ray.Direction); Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position); Vector3.Negate(ref upVector, out ray.Direction); //Transform the hit into local space. RigidTransform.TransformByInverse(ref rayHit.Location, ref surfaceTransform, out rayHit.Location); float bottomY = rayHit.Location.Y; float bottom = rayHit.T; Vector3 bottomPosition = rayHit.Location; if (collidable.RayCast(ray, boundingBoxHeight - rayHit.T, out rayHit)) { //Transform the hit into local space. RigidTransform.TransformByInverse(ref rayHit.Location, ref surfaceTransform, out rayHit.Location); Vector3.Add(ref rayHit.Location, ref bottomPosition, out volumeCenter); Vector3.Multiply(ref volumeCenter, .5f, out volumeCenter); return Math.Min(-bottomY, boundingBoxHeight - rayHit.T - bottom); } //This inner raycast should always hit, but just in case it doesn't due to some numerical problem, give it a graceful way out. volumeCenter = Vector3.Zero; return 0; } volumeCenter = Vector3.Zero; return 0; } public override void OnAdditionToSpace(ISpace newSpace) { base.OnAdditionToSpace(newSpace); Space space = newSpace as Space; ThreadManager = space.ThreadManager; QueryAccelerator = space.BroadPhase.QueryAccelerator; } public override void OnRemovalFromSpace(ISpace oldSpace) { base.OnRemovalFromSpace(oldSpace); ThreadManager = null; QueryAccelerator = null; } private CollisionRules collisionRules = new CollisionRules(); /// /// Gets or sets the collision rules associated with the fluid volume. /// public CollisionRules CollisionRules { get { return collisionRules; } set { collisionRules = value; } } } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/BoundingBoxForceFieldShape.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Defines the area in which a force field works using an entity's shape. /// public class BoundingBoxForceFieldShape : ForceFieldShape { private readonly List affectedEntities = new List(); private readonly RawList affectedEntries = new RawList(); /// /// Constructs a new force field shape using a bounding box. /// /// Bounding box to use. public BoundingBoxForceFieldShape(BoundingBox box) { BoundingBox = box; } /// /// Gets or sets the bounding box used by the shape. /// public BoundingBox BoundingBox { get; set; } /// /// Determines the possibly involved entities. /// /// Possibly involved entities. public override IList GetPossiblyAffectedEntities() { affectedEntities.Clear(); ForceField.QueryAccelerator.GetEntries(BoundingBox, affectedEntries); for (int i = 0; i < affectedEntries.Count; i++) { var EntityCollidable = affectedEntries[i] as EntityCollidable; if (EntityCollidable != null) affectedEntities.Add(EntityCollidable.Entity); } affectedEntries.Clear(); return affectedEntities; } /// /// Determines if the entity is affected by the force field. /// /// Entity to test. /// Whether the entity is affected. public override bool IsEntityAffected(Entity testEntity) { return true; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/BoundingSphereForceFieldShape.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Defines the area in which a force field works using an entity's shape. /// public class BoundingSphereForceFieldShape : ForceFieldShape { private readonly List affectedEntities = new List(); private readonly RawList affectedEntries = new RawList(); /// /// Constructs a new force field shape using a bounding sphere. /// /// Bounding sphere to use. public BoundingSphereForceFieldShape(BoundingSphere sphere) { BoundingSphere = sphere; } /// /// Gets or sets the bounding box used by the shape. /// public BoundingSphere BoundingSphere { get; set; } /// /// Determines the possibly involved entities. /// /// Possibly involved entities. public override IList GetPossiblyAffectedEntities() { affectedEntities.Clear(); ForceField.QueryAccelerator.GetEntries(BoundingSphere, affectedEntries); for (int i = 0; i < affectedEntries.Count; i++) { var EntityCollidable = affectedEntries[i] as EntityCollidable; if (EntityCollidable != null) affectedEntities.Add(EntityCollidable.Entity); } affectedEntries.Clear(); return affectedEntities; } /// /// Determines if the entity is affected by the force field. /// /// Entity to test. /// Whether the entity is affected. public override bool IsEntityAffected(Entity testEntity) { return true; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/ForceField.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.BroadPhaseSystems; using BEPUphysics.Entities; using BEPUphysics.Threading; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Superclass of objects which apply forces to entities in some field. /// public abstract class ForceField : Updateable, IDuringForcesUpdateable { private readonly Action subfunction; private IList affectedEntities; private float currentTimestep; private ForceFieldShape shape; /// /// Gets or sets whether or not threading is allowed. /// public bool AllowMultithreading { get; set; } /// /// Gets or sets the query accelerator used by the force field to find entities. /// public IQueryAccelerator QueryAccelerator { get; set; } /// /// Gets or sets the thread manager used by the force field. /// public IThreadManager ThreadManager { get; set; } /// /// Constructs a force field. /// /// Shape to use for the force field. protected ForceField(ForceFieldShape shape) { Shape = shape; subfunction = new Action(CalculateImpulsesSubfunction); AllowMultithreading = true; } /// /// Gets or sets whether the the force field will force affected entities to wake up. /// public bool ForceWakeUp { get; set; } /// /// Gets or sets the shape of the force field used to determine which entities to apply forces to. /// public ForceFieldShape Shape { get { return shape; } set { if (value != null && value.ForceField != null) throw new ArgumentException("The force field shape already belongs to another force field."); //Get rid of my old shape. if (shape != null) { shape.ForceField = null; } //Get my new shape! shape = value; if (shape != null) { shape.ForceField = this; } } } /// /// Performs any custom logic desired prior to the force application. /// protected virtual void PreUpdate() { } /// /// Applies forces specified by the given calculation delegate to bodies in the volume. /// Called automatically when needed by the owning Space. /// /// Time since the last frame in simulation seconds. void IDuringForcesUpdateable.Update(float dt) { PreUpdate(); affectedEntities = Shape.GetPossiblyAffectedEntities(); if (AllowMultithreading && ThreadManager.ThreadCount > 1) { currentTimestep = dt; ThreadManager.ForLoop(0, affectedEntities.Count, subfunction); } else { currentTimestep = dt; //No multithreading, so do it directly. int count = affectedEntities.Count; for (int i = 0; i < count; i++) { CalculateImpulsesSubfunction(i); } } } /// /// Calculates the impulse to apply to the entity. /// /// Affected entity. /// Duration between simulation updates. /// Impulse to apply to the entity. protected abstract void CalculateImpulse(Entity e, float dt, out Vector3 impulse); private void CalculateImpulsesSubfunction(int index) { Entity e = affectedEntities[index]; if (e.isDynamic && (e.activityInformation.IsActive || ForceWakeUp) && Shape.IsEntityAffected(e)) { if (ForceWakeUp) e.activityInformation.Activate(); Vector3 impulse; CalculateImpulse(e, currentTimestep, out impulse); e.ApplyLinearImpulse(ref impulse); } } /// /// Called after the object is added to a space. /// /// Space to which the field has been added. public override void OnAdditionToSpace(ISpace newSpace) { base.OnAdditionToSpace(newSpace); var space = newSpace as Space; if(space != null) { ThreadManager = space.ThreadManager; QueryAccelerator = space.BroadPhase.QueryAccelerator; } } /// /// Called before an object is removed from its space. /// /// Space from which the object has been removed. public override void OnRemovalFromSpace(ISpace oldSpace) { base.OnRemovalFromSpace(oldSpace); ThreadManager = null; QueryAccelerator = null; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/ForceFieldShape.cs ================================================ using System.Collections.Generic; using BEPUphysics.Entities; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Superclass of force field shapes that test whether or not entities are affected by a forcefield. /// public abstract class ForceFieldShape { /// /// Force field associated with this shape. /// public ForceField ForceField { get; internal set; } /// /// Uses an efficient query to see what entities may be affected. /// Usually uses a broadphase bounding box query. /// /// Possibly affected entities. public abstract IList GetPossiblyAffectedEntities(); /// /// Performs a narrow-phase test to see if an entity is affected by the force field. /// /// Entity to test. /// Whether or not the entity is affected. public abstract bool IsEntityAffected(Entity entity); } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/InfiniteForceFieldShape.cs ================================================ using System.Collections.Generic; using BEPUphysics.Entities; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Defines the area in which a force field works using an entity's shape. /// public class InfiniteForceFieldShape : ForceFieldShape { private IList boxedList; /// /// Determines the possibly involved entities. /// /// Possibly involved entities. public override IList GetPossiblyAffectedEntities() { return boxedList ?? (boxedList = (ForceField as ISpaceObject).Space.Entities); } /// /// Determines if the entity is affected by the force field. /// /// Entity to test. /// Whether the entity is affected. public override bool IsEntityAffected(Entity testEntity) { return true; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/ForceFields/VolumeForceFieldShape.cs ================================================ using System.Collections.Generic; using BEPUphysics.BroadPhaseEntries; using BEPUphysics.Entities; using BEPUutilities.DataStructures; namespace BEPUphysics.UpdateableSystems.ForceFields { /// /// Defines the area in which a force field works using an entity's shape. /// public class VolumeForceFieldShape : ForceFieldShape { private readonly RawList affectedEntities = new RawList(); /// /// Constructs a new force field shape using a detector volume. /// /// Volume to use. public VolumeForceFieldShape(DetectorVolume volume) { Volume = volume; } /// /// Gets or sets the volume used by the shape. /// public DetectorVolume Volume { get; set; } /// /// Determines the possibly involved entities. /// /// Possibly involved entities. public override IList GetPossiblyAffectedEntities() { affectedEntities.Clear(); foreach (var entity in Volume.pairs.Keys) { affectedEntities.Add(entity); } return affectedEntities; } /// /// Determines if the entity is affected by the force field. /// /// Entity to test. /// Whether the entity is affected. public override bool IsEntityAffected(Entity testEntity) { return Volume.pairs[testEntity].Touching; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/IBeforeNarrowPhaseUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space before the narrow phase runs. /// public interface IBeforeNarrowPhaseUpdateable : ISpaceUpdateable { /// /// Updates the updateable before the narrow phase. /// ///Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/IBeforePositionUpdateUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space at the end of a time step. /// public interface IBeforePositionUpdateUpdateable : ISpaceUpdateable { /// /// Updates the object at the end of a time step. /// ///Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/IBeforeSolverUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space before the solver runs. /// public interface IBeforeSolverUpdateable : ISpaceUpdateable { /// /// Updates the updateable before the solver. /// ///Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/IDuringForcesUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by a space during force application. /// public interface IDuringForcesUpdateable : ISpaceUpdateable { /// /// Updates the object during force application. /// ///Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/IEndOfFrameUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space at the end of the frame. /// public interface IEndOfFrameUpdateable : ISpaceUpdateable { /// /// Updates the object at the end of the frame. /// /// Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/IEndOfTimeStepUpdateable.cs ================================================ namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space at the end of a time step. /// public interface IEndOfTimeStepUpdateable : ISpaceUpdateable { /// /// Updates the object at the end of a time step. /// ///Time step duration. void Update(float dt); } } ================================================ FILE: BEPUphysics/UpdateableSystems/ISpaceUpdateable.cs ================================================ using System.Collections.Generic; namespace BEPUphysics.UpdateableSystems { /// /// Defines an object which is updated by the space. /// These refer to the special Updateable types which /// allow for easier integration into the update flow of the space. /// public interface ISpaceUpdateable : ISpaceObject { /// /// Gets and sets whether or not the updateable should be updated sequentially even in a multithreaded space. /// If this is true, the updateable can make use of the space's ThreadManager for internal multithreading. /// bool IsUpdatedSequentially { get; set; } /// /// Gets and sets whether or not the updateable should be updated by the space. /// bool IsUpdating { get; set; } /// /// List of managers owning the updateable. /// List Managers { get; } } } ================================================ FILE: BEPUphysics/UpdateableSystems/Updateable.cs ================================================ using System.Collections.Generic; namespace BEPUphysics.UpdateableSystems { /// /// Convenience superclass of Updateables. /// Updateables are updated by the Space at various /// points during the execution of the engine /// to support easy extensions. /// public abstract class Updateable : ISpaceUpdateable { protected Updateable() { IsUpdating = true; } #region ISpaceUpdateable Members List managers = new List(); List ISpaceUpdateable.Managers { get { return managers; } } private bool isUpdatedSequentially = true; /// /// Gets and sets whether or not the updateable should be updated sequentially even in a multithreaded space. /// If this is true, the updateable can make use of the space's ThreadManager for internal multithreading. /// public bool IsUpdatedSequentially { get { return isUpdatedSequentially; } set { bool oldValue = isUpdatedSequentially; isUpdatedSequentially = value; if (value != oldValue) for (int i = 0; i < managers.Count; i++) { managers[i].SequentialUpdatingStateChanged(this); } } } /// /// Gets and sets whether or not the updateable should be updated by its manager. /// public bool IsUpdating { get; set; } /// /// Called after the object is added to a space. /// /// Space to which the object was added. public virtual void OnAdditionToSpace(ISpace newSpace) { } /// /// Called before an object is removed from its space. /// /// Space from which the object was removed. public virtual void OnRemovalFromSpace(ISpace oldSpace) { } private ISpace space; ISpace ISpaceObject.Space { get { return space; } set { space = value; } } /// /// Space that owns the updateable. /// public ISpace Space { get { return space; } } /// /// Gets or sets the user data associated with this object. /// public object Tag { get; set; } #endregion } } ================================================ FILE: BEPUphysics/UpdateableSystems/UpdateableManager.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.Threading; namespace BEPUphysics.UpdateableSystems { /// /// Superclass of updateable managers. /// public abstract class UpdateableManager : MultithreadedProcessingStage { protected Action multithreadedUpdateDelegate; protected TimeStepSettings timeStepSettings; /// /// Gets the time step settings used by the updateable manager. /// public TimeStepSettings TimeStepSettings { get { return timeStepSettings; } } protected UpdateableManager(TimeStepSettings timeStepSettings) { this.timeStepSettings = timeStepSettings; Enabled = true; } protected UpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : this(timeStepSettings) { ThreadManager = threadManager; AllowMultithreading = true; } /// /// Notifies the manager that the updateable has changed sequential updating state. /// ///Updateable with changed state. public abstract void SequentialUpdatingStateChanged(ISpaceUpdateable updateable); /// /// Gets or sets the owning space. /// public ISpace Space { get; set; } } /// /// Superclass of updateable managers with a specific type. /// ///Type of Updateable being managed. public abstract class UpdateableManager : UpdateableManager where T : class, ISpaceUpdateable { protected List sequentiallyUpdatedUpdateables = new List(); protected List simultaneouslyUpdatedUpdateables = new List(); protected UpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { multithreadedUpdateDelegate = MultithreadedUpdate; } protected UpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { multithreadedUpdateDelegate = MultithreadedUpdate; } protected abstract void MultithreadedUpdate(int i); protected abstract void SequentialUpdate(int i); public override void SequentialUpdatingStateChanged(ISpaceUpdateable updateable) { if (updateable.Managers.Contains(this)) { T u = updateable as T; if (updateable.IsUpdatedSequentially) { if (simultaneouslyUpdatedUpdateables.Remove(u)) sequentiallyUpdatedUpdateables.Add(u); } else { if (sequentiallyUpdatedUpdateables.Remove(u)) simultaneouslyUpdatedUpdateables.Add(u); } } else { throw new ArgumentException("Updateable does not belong to this manager."); } } /// /// Adds an updateable to the manager. /// ///Updateable to add. ///Thrown if the manager already contains the updateable. public void Add(T updateable) { if (!updateable.Managers.Contains(this)) { if (updateable.IsUpdatedSequentially) sequentiallyUpdatedUpdateables.Add(updateable); else simultaneouslyUpdatedUpdateables.Add(updateable); updateable.Managers.Add(this); } else { throw new ArgumentException("Updateable already belongs to the manager, cannot re-add."); } } /// /// Removes an updateable from the manager. /// ///Updateable to remove. ///Thrown if the manager does not contain the updateable. public void Remove(T updateable) { if (updateable.Managers.Contains(this)) { if (updateable.IsUpdatedSequentially) sequentiallyUpdatedUpdateables.Remove(updateable); else simultaneouslyUpdatedUpdateables.Remove(updateable); updateable.Managers.Remove(this); } else { throw new ArgumentException("Updateable does not belong to this manager; cannot remove."); } } protected override void UpdateMultithreaded() { for (int i = 0; i < sequentiallyUpdatedUpdateables.Count; i++) { SequentialUpdate(i); } ThreadManager.ForLoop(0, simultaneouslyUpdatedUpdateables.Count, multithreadedUpdateDelegate); } protected override void UpdateSingleThreaded() { for (int i = 0; i < sequentiallyUpdatedUpdateables.Count; i++) { SequentialUpdate(i); } for (int i = 0; i < simultaneouslyUpdatedUpdateables.Count; i++) { MultithreadedUpdate(i); } } } } ================================================ FILE: BEPUphysics/UpdateableSystems/UpdateableManagers.cs ================================================ using BEPUphysics.Threading; using BEPUutilities; namespace BEPUphysics.UpdateableSystems { /// /// Manages updateables that update during the forces stage. /// public class DuringForcesUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public DuringForcesUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public DuringForcesUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } /// /// Manages updateables that update before the narrow phase. /// public class BeforeNarrowPhaseUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public BeforeNarrowPhaseUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public BeforeNarrowPhaseUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } /// /// Manages updateables that update before the solver. /// public class BeforeSolverUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public BeforeSolverUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public BeforeSolverUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } /// /// Manages updateables that update at the end of a time step. /// public class BeforePositionUpdateUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public BeforePositionUpdateUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public BeforePositionUpdateUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } /// /// Manages updateables that update at the end of a time step. /// public class EndOfTimeStepUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public EndOfTimeStepUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public EndOfTimeStepUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } /// /// Manages updateables that update at the end of a frame. /// public class EndOfFrameUpdateableManager : UpdateableManager { /// /// Constructs a manager. /// ///Time step settings to use. public EndOfFrameUpdateableManager(TimeStepSettings timeStepSettings) : base(timeStepSettings) { } /// /// Constructs a manager. /// ///Time step settings to use. /// Thread manager to use. public EndOfFrameUpdateableManager(TimeStepSettings timeStepSettings, IThreadManager threadManager) : base(timeStepSettings, threadManager) { } protected override void MultithreadedUpdate(int i) { if (simultaneouslyUpdatedUpdateables[i].IsUpdating) simultaneouslyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } protected override void SequentialUpdate(int i) { if (sequentiallyUpdatedUpdateables[i].IsUpdating) sequentiallyUpdatedUpdateables[i].Update(timeStepSettings.TimeStepDuration); } } } ================================================ FILE: BEPUphysics/Vehicle/CylinderCastWheelShape.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.CollisionShapes.ConvexShapes; using BEPUphysics.Entities; using BEPUphysics.NarrowPhaseSystems.Pairs; using Microsoft.Xna.Framework; using BEPUphysics.CollisionRuleManagement; using BEPUutilities; using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Uses a cylinder cast as the shape of a wheel. /// public class CylinderCastWheelShape : WheelShape { private CylinderShape shape; /// /// Gets or sets the unsteered orientation of the wheel in the vehicle's local space. /// public Quaternion LocalWheelOrientation { get; set; } /// /// Creates a new cylinder cast based wheel shape. /// /// Radius of the wheel. /// Width of the wheel. /// Unsteered orientation of the wheel in the vehicle's local space. /// Local graphic transform of the wheel shape. /// This transform is applied first when creating the shape's worldTransform. /// Whether or not to include the steering transform in the wheel shape cast. If false, the casted wheel shape will always point straight forward. /// If true, it will rotate with steering. Sometimes, setting this to false is helpful when the cast shape would otherwise become exposed when steering. public CylinderCastWheelShape(float radius, float width, Quaternion localWheelOrientation, Matrix localGraphicTransform, bool includeSteeringTransformInCast) { shape = new CylinderShape(width, radius); this.LocalWheelOrientation = localWheelOrientation; LocalGraphicTransform = localGraphicTransform; this.IncludeSteeringTransformInCast = includeSteeringTransformInCast; } /// /// Gets or sets the radius of the wheel. /// public override sealed float Radius { get { return shape.Radius; } set { shape.Radius = MathHelper.Max(value, 0); } } /// /// Gets or sets the width of the wheel. /// public float Width { get { return shape.Height; } set { shape.Height = MathHelper.Max(value, 0); } } /// /// Gets or sets whether or not to include the rotation from steering in the cast. If false, the casted wheel shape will always point straight forward. /// If true, it will rotate with steering. Sometimes, setting this to false is helpful when the cast shape would otherwise become exposed when steering. /// public bool IncludeSteeringTransformInCast { get; set; } /// /// Updates the wheel's world transform for graphics. /// Called automatically by the owning wheel at the end of each frame. /// If the engine is updating asynchronously, you can call this inside of a space read buffer lock /// and update the wheel transforms safely. /// public override void UpdateWorldTransform() { #if !WINDOWS Vector3 newPosition = new Vector3(); #else Vector3 newPosition; #endif Vector3 worldAttachmentPoint; Vector3 localAttach; Vector3.Add(ref wheel.suspension.localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.localPosition, out localAttach); worldTransform = Matrix3x3.ToMatrix4X4(wheel.vehicle.Body.BufferedStates.InterpolatedStates.OrientationMatrix); Vector3.TransformNormal(ref localAttach, ref worldTransform, out worldAttachmentPoint); worldAttachmentPoint += wheel.vehicle.Body.BufferedStates.InterpolatedStates.Position; Vector3 worldDirection; Vector3.Transform(ref wheel.suspension.localDirection, ref worldTransform, out worldDirection); float length = wheel.suspension.currentLength; newPosition.X = worldAttachmentPoint.X + worldDirection.X * length; newPosition.Y = worldAttachmentPoint.Y + worldDirection.Y * length; newPosition.Z = worldAttachmentPoint.Z + worldDirection.Z * length; Matrix spinTransform; Vector3 localSpinAxis; Vector3.Cross(ref wheel.localForwardDirection, ref wheel.suspension.localDirection, out localSpinAxis); Matrix.CreateFromAxisAngle(ref localSpinAxis, spinAngle, out spinTransform); Matrix localTurnTransform; Matrix.Multiply(ref localGraphicTransform, ref spinTransform, out localTurnTransform); Matrix.Multiply(ref localTurnTransform, ref steeringTransform, out localTurnTransform); //Matrix.Multiply(ref localTurnTransform, ref spinTransform, out localTurnTransform); Matrix.Multiply(ref localTurnTransform, ref worldTransform, out worldTransform); worldTransform.Translation = newPosition; } /// /// Finds a supporting entity, the contact location, and the contact normal. /// /// Contact point between the wheel and the support. /// Contact normal between the wheel and the support. /// Length of the suspension at the contact. /// Collidable supporting the wheel, if any. /// Supporting object. /// Material of the wheel. /// Whether or not any support was found. protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material) { suspensionLength = float.MaxValue; location = Toolbox.NoVector; supportingCollidable = null; entity = null; normal = Toolbox.NoVector; material = null; Collidable testCollidable; RayHit rayHit; bool hit = false; Quaternion localSteeringTransform; Quaternion.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform); var startingTransform = new RigidTransform { Position = wheel.suspension.worldAttachmentPoint, Orientation = Quaternion.Concatenate(Quaternion.Concatenate(LocalWheelOrientation, IncludeSteeringTransformInCast ? localSteeringTransform : Quaternion.Identity), wheel.vehicle.Body.orientation) }; Vector3 sweep; Vector3.Multiply(ref wheel.suspension.worldDirection, wheel.suspension.restLength, out sweep); for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++) { var pair = detector.CollisionInformation.pairs[i]; testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable; if (testCollidable != null) { if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal && testCollidable.ConvexCast(shape, ref startingTransform, ref sweep, out rayHit) && rayHit.T * wheel.suspension.restLength < suspensionLength) { suspensionLength = rayHit.T * wheel.suspension.restLength; EntityCollidable entityCollidable; if ((entityCollidable = testCollidable as EntityCollidable) != null) { entity = entityCollidable.Entity; material = entityCollidable.Entity.Material; } else { entity = null; supportingCollidable = testCollidable; var materialOwner = testCollidable as IMaterialOwner; if (materialOwner != null) material = materialOwner.Material; } location = rayHit.Location; normal = rayHit.Normal; hit = true; } } } if (hit) { if (suspensionLength > 0) { float dot; Vector3.Dot(ref normal, ref wheel.suspension.worldDirection, out dot); if (dot > 0) { //The cylinder cast produced a normal which is opposite of what we expect. Vector3.Negate(ref normal, out normal); } normal.Normalize(); } else Vector3.Negate(ref wheel.suspension.worldDirection, out normal); return true; } return false; } /// /// Initializes the detector entity and any other necessary logic. /// protected internal override void Initialize() { //Setup the dimensions of the detector. var initialTransform = new RigidTransform { Orientation = LocalWheelOrientation }; BoundingBox boundingBox; shape.GetBoundingBox(ref initialTransform, out boundingBox); var expansion = wheel.suspension.localDirection * wheel.suspension.restLength; if (expansion.X > 0) boundingBox.Max.X += expansion.X; else if (expansion.X < 0) boundingBox.Min.X += expansion.X; if (expansion.Y > 0) boundingBox.Max.Y += expansion.Y; else if (expansion.Y < 0) boundingBox.Min.Y += expansion.Y; if (expansion.Z > 0) boundingBox.Max.Z += expansion.Z; else if (expansion.Z < 0) boundingBox.Min.Z += expansion.Z; detector.Width = boundingBox.Max.X - boundingBox.Min.X; detector.Height = boundingBox.Max.Y - boundingBox.Min.Y; detector.Length = boundingBox.Max.Z - boundingBox.Min.Z; } /// /// Updates the position of the detector before each step. /// protected internal override void UpdateDetectorPosition() { #if !WINDOWS Vector3 newPosition = new Vector3(); #else Vector3 newPosition; #endif newPosition.X = wheel.suspension.worldAttachmentPoint.X + wheel.suspension.worldDirection.X * wheel.suspension.restLength * .5f; newPosition.Y = wheel.suspension.worldAttachmentPoint.Y + wheel.suspension.worldDirection.Y * wheel.suspension.restLength * .5f; newPosition.Z = wheel.suspension.worldAttachmentPoint.Z + wheel.suspension.worldDirection.Z * wheel.suspension.restLength * .5f; detector.Position = newPosition; if (IncludeSteeringTransformInCast) { Quaternion localSteeringTransform; Quaternion.CreateFromAxisAngle(ref wheel.suspension.localDirection, steeringAngle, out localSteeringTransform); detector.Orientation = Quaternion.Concatenate(localSteeringTransform, wheel.Vehicle.Body.orientation); } else { detector.Orientation = wheel.Vehicle.Body.orientation; } Vector3 linearVelocity; Vector3.Subtract(ref newPosition, ref wheel.vehicle.Body.position, out linearVelocity); Vector3.Cross(ref linearVelocity, ref wheel.vehicle.Body.angularVelocity, out linearVelocity); Vector3.Add(ref linearVelocity, ref wheel.vehicle.Body.linearVelocity, out linearVelocity); detector.LinearVelocity = linearVelocity; detector.AngularVelocity = wheel.vehicle.Body.angularVelocity; } } } ================================================ FILE: BEPUphysics/Vehicle/RaycastWheelShape.cs ================================================ using BEPUphysics.BroadPhaseEntries; using BEPUphysics.BroadPhaseEntries.MobileCollidables; using BEPUphysics.Entities; using BEPUphysics.NarrowPhaseSystems.Pairs; using Microsoft.Xna.Framework; using BEPUphysics.CollisionRuleManagement; using BEPUutilities; using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Uses a raycast as the shape of a wheel. /// public class RaycastWheelShape : WheelShape { private float graphicalRadius; /// /// Creates a new raycast based wheel shape. /// /// Graphical radius of the wheel. /// This is not used for simulation. It is only used in /// determining aesthetic properties of a vehicle wheel, /// like position and orientation. /// Local graphic transform of the wheel shape. /// This transform is applied first when creating the shape's worldTransform. public RaycastWheelShape(float graphicalRadius, Matrix localGraphicTransform) { Radius = graphicalRadius; LocalGraphicTransform = localGraphicTransform; } /// /// Gets or sets the graphical radius of the wheel. /// This is not used for simulation. It is only used in /// determining aesthetic properties of a vehicle wheel, /// like position and orientation. /// public override sealed float Radius { get { return graphicalRadius; } set { graphicalRadius = MathHelper.Max(value, 0); } } /// /// Updates the wheel's world transform for graphics. /// Called automatically by the owning wheel at the end of each frame. /// If the engine is updating asynchronously, you can call this inside of a space read buffer lock /// and update the wheel transforms safely. /// public override void UpdateWorldTransform() { #if !WINDOWS Vector3 newPosition = new Vector3(); #else Vector3 newPosition; #endif Vector3 worldAttachmentPoint; Vector3 localAttach; Vector3.Add(ref wheel.suspension.localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.localPosition, out localAttach); worldTransform = Matrix3x3.ToMatrix4X4(wheel.vehicle.Body.BufferedStates.InterpolatedStates.OrientationMatrix); Vector3.TransformNormal(ref localAttach, ref worldTransform, out worldAttachmentPoint); worldAttachmentPoint += wheel.vehicle.Body.BufferedStates.InterpolatedStates.Position; Vector3 worldDirection; Vector3.Transform(ref wheel.suspension.localDirection, ref worldTransform, out worldDirection); float length = wheel.suspension.currentLength - graphicalRadius; newPosition.X = worldAttachmentPoint.X + worldDirection.X * length; newPosition.Y = worldAttachmentPoint.Y + worldDirection.Y * length; newPosition.Z = worldAttachmentPoint.Z + worldDirection.Z * length; Matrix spinTransform; Vector3 localSpinAxis; Vector3.Cross(ref wheel.localForwardDirection, ref wheel.suspension.localDirection, out localSpinAxis); Matrix.CreateFromAxisAngle(ref localSpinAxis, spinAngle, out spinTransform); Matrix localTurnTransform; Matrix.Multiply(ref localGraphicTransform, ref spinTransform, out localTurnTransform); Matrix.Multiply(ref localTurnTransform, ref steeringTransform, out localTurnTransform); //Matrix.Multiply(ref localTurnTransform, ref spinTransform, out localTurnTransform); Matrix.Multiply(ref localTurnTransform, ref worldTransform, out worldTransform); worldTransform.Translation = newPosition; } /// /// Finds a supporting entity, the contact location, and the contact normal. /// /// Contact point between the wheel and the support. /// Contact normal between the wheel and the support. /// Length of the suspension at the contact. /// Collidable supporting the wheel, if any. /// Supporting object. /// Material of the wheel. /// Whether or not any support was found. protected internal override bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportingCollidable, out Entity entity, out Material material) { suspensionLength = float.MaxValue; location = Toolbox.NoVector; supportingCollidable = null; entity = null; normal = Toolbox.NoVector; material = null; Collidable testCollidable; RayHit rayHit; bool hit = false; for (int i = 0; i < detector.CollisionInformation.pairs.Count; i++) { var pair = detector.CollisionInformation.pairs[i]; testCollidable = (pair.BroadPhaseOverlap.entryA == detector.CollisionInformation ? pair.BroadPhaseOverlap.entryB : pair.BroadPhaseOverlap.entryA) as Collidable; if (testCollidable != null) { if (CollisionRules.CollisionRuleCalculator(this, testCollidable) == CollisionRule.Normal && testCollidable.RayCast(new Ray(wheel.suspension.worldAttachmentPoint, wheel.suspension.worldDirection), wheel.suspension.restLength, out rayHit) && rayHit.T < suspensionLength) { suspensionLength = rayHit.T; EntityCollidable entityCollidable; if ((entityCollidable = testCollidable as EntityCollidable) != null) { entity = entityCollidable.Entity; material = entityCollidable.Entity.Material; } else { entity = null; supportingCollidable = testCollidable; var materialOwner = testCollidable as IMaterialOwner; if (materialOwner != null) material = materialOwner.Material; } location = rayHit.Location; normal = rayHit.Normal; hit = true; } } } if (hit) { if (suspensionLength > 0) normal.Normalize(); else Vector3.Negate(ref wheel.suspension.worldDirection, out normal); return true; } return false; } /// /// Initializes the detector entity and any other necessary logic. /// protected internal override void Initialize() { //Setup the dimensions of the detector. Vector3 startpoint = wheel.suspension.localAttachmentPoint; Vector3 endpoint = startpoint + wheel.suspension.localDirection * wheel.suspension.restLength; Vector3 min, max; Vector3.Min(ref startpoint, ref endpoint, out min); Vector3.Max(ref startpoint, ref endpoint, out max); detector.Width = max.X - min.X; detector.Height = max.Y - min.Y; detector.Length = max.Z - min.Z; } /// /// Updates the position of the detector before each step. /// protected internal override void UpdateDetectorPosition() { #if !WINDOWS Vector3 newPosition = new Vector3(); #else Vector3 newPosition; #endif newPosition.X = wheel.suspension.worldAttachmentPoint.X + wheel.suspension.worldDirection.X * wheel.suspension.restLength * .5f; newPosition.Y = wheel.suspension.worldAttachmentPoint.Y + wheel.suspension.worldDirection.Y * wheel.suspension.restLength * .5f; newPosition.Z = wheel.suspension.worldAttachmentPoint.Z + wheel.suspension.worldDirection.Z * wheel.suspension.restLength * .5f; detector.Position = newPosition; detector.OrientationMatrix = wheel.Vehicle.Body.orientationMatrix; Vector3 linearVelocity; Vector3.Subtract(ref newPosition, ref wheel.vehicle.Body.position, out linearVelocity); Vector3.Cross(ref linearVelocity, ref wheel.vehicle.Body.angularVelocity, out linearVelocity); Vector3.Add(ref linearVelocity, ref wheel.vehicle.Body.linearVelocity, out linearVelocity); detector.LinearVelocity = linearVelocity; detector.AngularVelocity = wheel.vehicle.Body.angularVelocity; } } } ================================================ FILE: BEPUphysics/Vehicle/Vehicle.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.Entities; using BEPUutilities.DataStructures; using BEPUphysics.UpdateableSystems; namespace BEPUphysics.Vehicle { /// /// Simulates wheeled vehicles using a variety of constraints and shape casts. /// public class Vehicle : CombinedUpdateable, IDuringForcesUpdateable, IBeforeNarrowPhaseUpdateable, IEndOfTimeStepUpdateable, IEndOfFrameUpdateable { //TODO: The vehicle uses wheel 'fake constraints' that were made prior to the changes to the constraint system that allow for customizable solver settings. //It would be convenient to use a SolverGroup to handle the wheel constraints, since the functionality is nearly the same. private readonly List wheels = new List(); private Entity body; internal List previousSupports = new List(); /// /// Constructs a vehicle. /// /// Body of the vehicle. public Vehicle(Entity shape) { IsUpdatedSequentially = false; Body = shape; Body.activityInformation.IsAlwaysActive = true; //The body is always active, so don't bother with stabilization either. //Stabilization can introduce artifacts as well. body.activityInformation.AllowStabilization = false; } /// /// Constructs a vehicle. /// /// Body of the vehicle. /// List of wheels of the vehicle. public Vehicle(Entity shape, IEnumerable wheelList) { IsUpdatedSequentially = false; Body = shape; Body.activityInformation.IsAlwaysActive = true; //The body is always active, so don't bother with stabilization either. //Stabilization can introduce artifacts as well. body.activityInformation.AllowStabilization = false; foreach (Wheel wheel in wheelList) { AddWheel(wheel); } } /// /// Gets or sets the entity representing the shape of the car. /// public Entity Body { get { return body; } set { body = value; OnInvolvedEntitiesChanged(); } } /// /// Number of wheels with supports. /// public int SupportedWheelCount { get { int toReturn = 0; foreach (Wheel wheel in Wheels) { if (wheel.HasSupport) toReturn++; } return toReturn; } } /// /// Gets the list of wheels supporting the vehicle. /// public List Wheels { get { return wheels; } } /// /// Sets up the vehicle's information when being added to the space. /// Called automatically when the space adds the vehicle. /// /// New owning space. public override void OnAdditionToSpace(ISpace newSpace) { newSpace.Add(body); foreach (Wheel wheel in Wheels) { wheel.OnAdditionToSpace(newSpace); } } /// /// Sets up the vehicle's information when being added to the space. /// Called automatically when the space adds the vehicle. /// public override void OnRemovalFromSpace(ISpace oldSpace) { foreach (Wheel wheel in Wheels) { wheel.OnRemovalFromSpace(oldSpace); } oldSpace.Remove(Body); } /// /// Performs the end-of-frame update component. /// /// Time since last frame in simulation seconds. void IEndOfFrameUpdateable.Update(float dt) { //Graphics should be updated at the end of each frame. foreach (Wheel wheel in Wheels) { wheel.UpdateAtEndOfFrame(dt); } } /// /// Performs the end-of-update update component. /// /// Time since last frame in simulation seconds. void IEndOfTimeStepUpdateable.Update(float dt) { //Graphics should be updated at the end of each frame. foreach (Wheel wheel in Wheels) { wheel.UpdateAtEndOfUpdate(dt); } } void IBeforeNarrowPhaseUpdateable.Update(float dt) { //After broadphase, test for supports. foreach (Wheel wheel in wheels) { wheel.FindSupport(); } OnInvolvedEntitiesChanged(); } void IDuringForcesUpdateable.Update(float dt) { foreach (Wheel wheel in wheels) { wheel.UpdateDuringForces(dt); } } /// /// Adds a wheel to the vehicle. /// /// WheelTest to add. public void AddWheel(Wheel wheel) { if (wheel.vehicle == null) { Wheels.Add(wheel); wheel.OnAddedToVehicle(this); } else throw new InvalidOperationException("Can't add a wheel to a vehicle if it already belongs to a vehicle."); } /// /// Removes a wheel from the vehicle. /// /// WheelTest to remove. public void RemoveWheel(Wheel wheel) { if (wheel.vehicle == this) { wheel.OnRemovedFromVehicle(); Wheels.Remove(wheel); } else throw new InvalidOperationException("Can't remove a wheel from a vehicle that does not own it."); } /// /// Updates the vehicle. /// Called automatically when needed by the owning Space. /// public override float SolveIteration() { int numActive = 0; foreach (Wheel wheel in Wheels) { if (wheel.isActiveInSolver) if (!wheel.ApplyImpulse()) wheel.isActiveInSolver = false; else numActive++; } if (numActive == 0) isActiveInSolver = false; return solverSettings.minimumImpulse + 1; //We take care of ourselves. } /// /// Adds entities associated with the solver item to the involved entities list. /// Ensure that sortInvolvedEntities() is called at the end of the function. /// This allows the non-batched multithreading system to lock properly. /// protected internal override void CollectInvolvedEntities(RawList outputInvolvedEntities) { outputInvolvedEntities.Add(Body); foreach (Wheel wheel in Wheels) { if (wheel.supportingEntity != null && !outputInvolvedEntities.Contains(wheel.supportingEntity)) outputInvolvedEntities.Add(wheel.supportingEntity); } } /// /// Computes information required during the later update. /// Called once before the iteration loop. /// /// Time since previous frame in simulation seconds. public override void Update(float dt) { //TODO: to help balance multithreading, what if each wheel were its own SolverUpdateable //(no more CombinedUpdateable, basically) //This might be okay, but chances are if each was totally isolated, the 'enter exit' //of the monitor would be more expensive than just going in all at once and leaving at once. //Maybe a SolverGroup instead of CombinedUpdateable, though. //Update the wheel 'constraints.' foreach (Wheel wheel in Wheels) { if (wheel.isActiveInSolver) wheel.PreStep(dt); } } /// /// Performs any pre-solve iteration work that needs exclusive /// access to the members of the solver updateable. /// Usually, this is used for applying warmstarting impulses. /// public override void ExclusiveUpdate() { foreach (Wheel wheel in Wheels) { if (wheel.isActiveInSolver) wheel.ExclusiveUpdate(); } } /// /// Updates the activity state of the wheel constraints. /// public override void UpdateSolverActivity() { if (isActive) { isActiveInSolver = false; if (body.activityInformation.IsActive) { foreach (Wheel wheel in Wheels) { wheel.UpdateSolverActivity(); isActiveInSolver = isActiveInSolver || wheel.isActiveInSolver; } } } else isActiveInSolver = false; } } } ================================================ FILE: BEPUphysics/Vehicle/Wheel.cs ================================================ using System; using BEPUphysics.Entities; using BEPUphysics.UpdateableSystems; using Microsoft.Xna.Framework; using BEPUutilities; using BEPUphysics.Materials; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.Vehicle { /// /// Supports a Vehicle. /// public class Wheel { internal Vector3 ra, rb; /// /// Used for solver early outing. /// internal bool isActiveInSolver = true; internal bool isSupported; internal WheelBrake brake; internal WheelDrivingMotor drivingMotor; internal Vector3 localForwardDirection = new Vector3(0, 0, -1); internal Vector3 normal; internal WheelShape shape; internal WheelSlidingFriction slidingFriction; internal Collidable supportingCollidable; internal Material supportMaterial; internal Vector3 supportLocation; internal Entity supportingEntity; internal WheelSuspension suspension; internal Vehicle vehicle; internal Vector3 worldForwardDirection; /// /// Constructs a new wheel. The WheelSlidingFriction, WheelBrake, WheelSuspension, and WheelDrivingMotor should be configured prior to using this wheel. /// /// Shape of the wheel. public Wheel(WheelShape shape) { slidingFriction = new WheelSlidingFriction(this); brake = new WheelBrake(this); suspension = new WheelSuspension(this); drivingMotor = new WheelDrivingMotor(this); Shape = shape; } /// /// Constructs a new wheel. /// /// Shape of the wheel. /// Springy support of the vehicle. /// Driving force for the wheel. /// Friction force resisting the forward and backward motion of the wheel. /// Friction force resisting the side to side motion of the wheel. public Wheel(WheelShape shape, WheelSuspension suspension, WheelDrivingMotor motor, WheelBrake rollingFriction, WheelSlidingFriction slidingFriction) { Suspension = suspension; DrivingMotor = motor; Brake = rollingFriction; this.SlidingFriction = slidingFriction; Shape = shape; } internal void UpdateSolverActivity() { isActiveInSolver = true; } /// /// Gets the brake for this wheel. /// public WheelBrake Brake { get { return brake; } set { if (brake != null) brake.Wheel = null; if (value != null) { if (value.Wheel == null) { value.Wheel = this; } else throw new InvalidOperationException("Can't use a rolling friction object that already belongs to another wheel."); } brake = value; } } /// /// Gets the motor that turns the wheel. /// public WheelDrivingMotor DrivingMotor { get { return drivingMotor; } set { if (drivingMotor != null) drivingMotor.Wheel = null; if (value != null) { if (value.Wheel == null) { value.Wheel = this; } else throw new InvalidOperationException("Can't use a motor object that already belongs to another wheel."); } drivingMotor = value; } } /// /// Gets whether or not this wheel is sitting on anything. /// public bool HasSupport { get { return isSupported; } } /// /// Gets or sets the local space forward direction of the wheel. /// public Vector3 LocalForwardDirection { get { return localForwardDirection; } set { localForwardDirection = Vector3.Normalize(value); if (vehicle != null) Matrix3x3.Transform(ref localForwardDirection, ref Vehicle.Body.orientationMatrix, out worldForwardDirection); else worldForwardDirection = localForwardDirection; } } /// /// Gets or sets the shape of this wheel used to find collisions with the ground. /// public WheelShape Shape { get { return shape; } set { if (shape != null) shape.Wheel = null; if (value != null) { if (value.Wheel == null) { value.Wheel = this; value.Initialize(); } else throw new InvalidOperationException("Can't use a wheel shape object that already belongs to another wheel."); } shape = value; } } /// /// Gets the sliding friction settings for this wheel. /// public WheelSlidingFriction SlidingFriction { get { return slidingFriction; } set { if (slidingFriction != null) slidingFriction.Wheel = null; if (value != null) { if (value.Wheel == null) { value.Wheel = this; } else throw new InvalidOperationException("Can't use a sliding friction object that already belongs to another wheel."); } slidingFriction = value; } } /// /// Gets the current support location of this wheel. /// public Vector3 SupportLocation { get { return supportLocation; } } /// /// Gets the normal /// public Vector3 SupportNormal { get { return normal; } } /// /// Gets the entity supporting the wheel, if any. /// public Entity SupportingEntity { get { return supportingEntity; } } /// /// Gets the collidable supporting the wheel, if any. /// public Collidable SupportingCollidable { get { return supportingCollidable; } } /// /// Gets the material associated with the support, if any. /// public Material SupportMaterial { get { return supportMaterial; } } /// /// Gets the suspension supporting this wheel. /// public WheelSuspension Suspension { get { return suspension; } set { if (suspension != null) suspension.Wheel = null; if (value != null) { if (value.Wheel == null) { value.Wheel = this; } else throw new InvalidOperationException("Can't use a suspension object that already belongs to another wheel."); } suspension = value; } } /// /// Gets the vehicle this wheel is attached to. /// public Vehicle Vehicle { get { return vehicle; } } /// /// Gets or sets the world space forward direction of the wheel. /// public Vector3 WorldForwardDirection { get { return worldForwardDirection; } set { worldForwardDirection = Vector3.Normalize(value); if (vehicle != null) { Quaternion conjugate; Quaternion.Conjugate(ref Vehicle.Body.orientation, out conjugate); Vector3.Transform(ref worldForwardDirection, ref conjugate, out localForwardDirection); } else localForwardDirection = worldForwardDirection; } } internal void PreStep(float dt) { Matrix.CreateFromAxisAngle(ref suspension.localDirection, shape.steeringAngle, out shape.steeringTransform); Vector3.TransformNormal(ref localForwardDirection, ref shape.steeringTransform, out worldForwardDirection); Matrix3x3.Transform(ref worldForwardDirection, ref Vehicle.Body.orientationMatrix, out worldForwardDirection); if (HasSupport) { Vector3.Subtract(ref supportLocation, ref Vehicle.Body.position, out ra); if (supportingEntity != null) Vector3.Subtract(ref supportLocation, ref SupportingEntity.position, out rb); //Mind the order of updating! sliding friction must come before driving force or rolling friction //because it computes the sliding direction. suspension.isActive = true; suspension.numIterationsAtZeroImpulse = 0; suspension.solverSettings.currentIterations = 0; slidingFriction.isActive = true; slidingFriction.numIterationsAtZeroImpulse = 0; slidingFriction.solverSettings.currentIterations = 0; drivingMotor.isActive = true; drivingMotor.numIterationsAtZeroImpulse = 0; drivingMotor.solverSettings.currentIterations = 0; brake.isActive = true; brake.numIterationsAtZeroImpulse = 0; brake.solverSettings.currentIterations = 0; suspension.PreStep(dt); slidingFriction.PreStep(dt); drivingMotor.PreStep(dt); brake.PreStep(dt); } else { //No support, don't need any solving. suspension.isActive = false; slidingFriction.isActive = false; drivingMotor.isActive = false; brake.isActive = false; suspension.accumulatedImpulse = 0; slidingFriction.accumulatedImpulse = 0; drivingMotor.accumulatedImpulse = 0; brake.accumulatedImpulse = 0; } } internal void ExclusiveUpdate() { if (HasSupport) { suspension.ExclusiveUpdate(); slidingFriction.ExclusiveUpdate(); drivingMotor.ExclusiveUpdate(); brake.ExclusiveUpdate(); } } /// /// Applies impulses and returns whether or not this wheel should be updated more. /// /// Whether not the wheel is done updating for the frame. internal bool ApplyImpulse() { int numActiveConstraints = 0; if (suspension.isActive) { if (++suspension.solverSettings.currentIterations <= suspension.solverSettings.maximumIterationCount) if (Math.Abs(suspension.ApplyImpulse()) < suspension.solverSettings.minimumImpulse) { suspension.numIterationsAtZeroImpulse++; if (suspension.numIterationsAtZeroImpulse > suspension.solverSettings.minimumIterationCount) suspension.isActive = false; else { numActiveConstraints++; suspension.numIterationsAtZeroImpulse = 0; } } else { numActiveConstraints++; suspension.numIterationsAtZeroImpulse = 0; } else suspension.isActive = false; } if (slidingFriction.isActive) { if (++slidingFriction.solverSettings.currentIterations <= suspension.solverSettings.maximumIterationCount) if (Math.Abs(slidingFriction.ApplyImpulse()) < slidingFriction.solverSettings.minimumImpulse) { slidingFriction.numIterationsAtZeroImpulse++; if (slidingFriction.numIterationsAtZeroImpulse > slidingFriction.solverSettings.minimumIterationCount) slidingFriction.isActive = false; else { numActiveConstraints++; slidingFriction.numIterationsAtZeroImpulse = 0; } } else { numActiveConstraints++; slidingFriction.numIterationsAtZeroImpulse = 0; } else slidingFriction.isActive = false; } if (drivingMotor.isActive) { if (++drivingMotor.solverSettings.currentIterations <= suspension.solverSettings.maximumIterationCount) if (Math.Abs(drivingMotor.ApplyImpulse()) < drivingMotor.solverSettings.minimumImpulse) { drivingMotor.numIterationsAtZeroImpulse++; if (drivingMotor.numIterationsAtZeroImpulse > drivingMotor.solverSettings.minimumIterationCount) drivingMotor.isActive = false; else { numActiveConstraints++; drivingMotor.numIterationsAtZeroImpulse = 0; } } else { numActiveConstraints++; drivingMotor.numIterationsAtZeroImpulse = 0; } else drivingMotor.isActive = false; } if (brake.isActive) { if (++brake.solverSettings.currentIterations <= suspension.solverSettings.maximumIterationCount) if (Math.Abs(brake.ApplyImpulse()) < brake.solverSettings.minimumImpulse) { brake.numIterationsAtZeroImpulse++; if (brake.numIterationsAtZeroImpulse > brake.solverSettings.minimumIterationCount) brake.isActive = false; else { numActiveConstraints++; brake.numIterationsAtZeroImpulse = 0; } } else { numActiveConstraints++; brake.numIterationsAtZeroImpulse = 0; } else brake.isActive = false; } return numActiveConstraints > 0; } internal void FindSupport() { if (!(isSupported = shape.FindSupport(out supportLocation, out normal, out suspension.currentLength, out supportingCollidable, out supportingEntity, out supportMaterial))) suspension.currentLength = suspension.restLength; } internal void OnAdditionToSpace(ISpace space) { //Make sure it doesn't collide with anything. shape.OnAdditionToSpace(space); shape.UpdateDetectorPosition(); //Need to put the detectors in appropriate locations before adding since otherwise overloads the broadphase space.Add(shape.detector); } internal void OnRemovalFromSpace(ISpace space) { space.Remove(shape.detector); shape.OnRemovalFromSpace(space); } internal void OnAddedToVehicle(Vehicle vehicle) { this.vehicle = vehicle; ISpace space = (vehicle as ISpaceUpdateable).Space; if (space != null) { OnAdditionToSpace(space); } LocalForwardDirection = LocalForwardDirection; suspension.OnAdditionToVehicle(); } internal void OnRemovedFromVehicle() { ISpace space = (vehicle as ISpaceUpdateable).Space; if (space != null) { OnRemovalFromSpace(space); } vehicle = null; } internal void UpdateAtEndOfFrame(float dt) { shape.UpdateWorldTransform(); } internal void UpdateAtEndOfUpdate(float dt) { shape.UpdateSpin(dt); } internal void UpdateDuringForces(float dt) { suspension.ComputeWorldSpaceData(); shape.UpdateDetectorPosition(); } } } ================================================ FILE: BEPUphysics/Vehicle/WheelBrake.cs ================================================ using System; using BEPUphysics.Constraints; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Attempts to resist rolling motion of a vehicle. /// public class WheelBrake : ISolverSettings { #region Static Stuff /// /// Default blender used by WheelRollingFriction constraints. /// public static WheelFrictionBlender DefaultRollingFrictionBlender; static WheelBrake() { DefaultRollingFrictionBlender = BlendFriction; } /// /// Function which takes the friction values from a wheel and a supporting material and computes the blended friction. /// /// Friction coefficient associated with the wheel. /// Friction coefficient associated with the support material. /// True if the friction coefficients passed into the blender are kinetic coefficients, false otherwise. /// Wheel being blended. /// Blended friction coefficient. public static float BlendFriction(float wheelFriction, float materialFriction, bool usingKinematicFriction, Wheel wheel) { return wheelFriction * materialFriction; } #endregion internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; private float angularAX, angularAY, angularAZ; private float angularBX, angularBY, angularBZ; internal bool isActive = true; private float linearAX, linearAY, linearAZ; private float blendedCoefficient; private float kineticBrakingFrictionCoefficient; private WheelFrictionBlender frictionBlender = DefaultRollingFrictionBlender; private bool isBraking; private float rollingFrictionCoefficient; internal SolverSettings solverSettings = new SolverSettings(); private float staticBrakingFrictionCoefficient; private float staticFrictionVelocityThreshold = 5f; private Wheel wheel; internal int numIterationsAtZeroImpulse; private Entity vehicleEntity, supportEntity; //Inverse effective mass matrix private float velocityToImpulse; private bool supportIsDynamic; /// /// Constructs a new rolling friction object for a wheel. /// /// Coefficient of dynamic friction of the wheel for friction when the brake is active. /// Coefficient of static friction of the wheel for friction when the brake is active. /// Coefficient of friction of the wheel for rolling friction when the brake isn't active. public WheelBrake(float dynamicBrakingFrictionCoefficient, float staticBrakingFrictionCoefficient, float rollingFrictionCoefficient) { KineticBrakingFrictionCoefficient = dynamicBrakingFrictionCoefficient; StaticBrakingFrictionCoefficient = staticBrakingFrictionCoefficient; RollingFrictionCoefficient = rollingFrictionCoefficient; } internal WheelBrake(Wheel wheel) { Wheel = wheel; } /// /// Gets the coefficient of rolling friction between the wheel and support. /// This coefficient is the blended result of the supporting entity's friction and the wheel's friction. /// public float BlendedCoefficient { get { return blendedCoefficient; } } /// /// Gets or sets the coefficient of braking dynamic friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// This coefficient is used instead of the rollingFrictionCoefficient when /// isBraking is true. /// public float KineticBrakingFrictionCoefficient { get { return kineticBrakingFrictionCoefficient; } set { kineticBrakingFrictionCoefficient = MathHelper.Max(value, 0); } } /// /// Gets the axis along which rolling friction is applied. /// public Vector3 FrictionAxis { get { return wheel.drivingMotor.ForceAxis; } } /// /// Gets or sets the function used to blend the supporting entity's friction and the wheel's friction. /// public WheelFrictionBlender FrictionBlender { get { return frictionBlender; } set { frictionBlender = value; } } /// /// Gets or sets whether or not the wheel is braking. /// When set to true, the brakingFrictionCoefficient is used. /// When false, the rollingFrictionCoefficient is used. /// public bool IsBraking { get { return isBraking; } set { isBraking = value; } } /// /// Gets or sets the coefficient of rolling friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// This coefficient is used instead of the brakingFrictionCoefficient when /// isBraking is false. /// public float RollingFrictionCoefficient { get { return rollingFrictionCoefficient; } set { rollingFrictionCoefficient = MathHelper.Max(value, 0); } } /// /// Gets or sets the coefficient of static dynamic friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// This coefficient is used instead of the rollingFrictionCoefficient when /// isBraking is true. /// public float StaticBrakingFrictionCoefficient { get { return staticBrakingFrictionCoefficient; } set { staticBrakingFrictionCoefficient = MathHelper.Max(value, 0); } } /// /// Gets or sets the velocity under which the coefficient of static friction will be used instead of the dynamic one. /// public float StaticFrictionVelocityThreshold { get { return staticFrictionVelocityThreshold; } set { staticFrictionVelocityThreshold = Math.Abs(value); } } /// /// Gets the force /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the wheel that this rolling friction applies to. /// public Wheel Wheel { get { return wheel; } internal set { wheel = value; } } #region ISolverSettings Members /// /// Gets the solver settings used by this wheel constraint. /// public SolverSettings SolverSettings { get { return solverSettings; } } #endregion /// /// Gets the relative velocity along the braking direction at the wheel contact. /// public float RelativeVelocity { get { float velocity = vehicleEntity.linearVelocity.X * linearAX + vehicleEntity.linearVelocity.Y * linearAY + vehicleEntity.linearVelocity.Z * linearAZ + vehicleEntity.angularVelocity.X * angularAX + vehicleEntity.angularVelocity.Y * angularAY + vehicleEntity.angularVelocity.Z * angularAZ; if (supportEntity != null) velocity += -supportEntity.linearVelocity.X * linearAX - supportEntity.linearVelocity.Y * linearAY - supportEntity.linearVelocity.Z * linearAZ + supportEntity.angularVelocity.X * angularBX + supportEntity.angularVelocity.Y * angularBY + supportEntity.angularVelocity.Z * angularBZ; return velocity; } } internal float ApplyImpulse() { //Compute relative velocity and convert to impulse float lambda = RelativeVelocity * velocityToImpulse; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; float maxForce = -blendedCoefficient * wheel.suspension.accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForce, maxForce); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (vehicleEntity.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } return lambda; } internal void PreStep(float dt) { vehicleEntity = wheel.Vehicle.Body; supportEntity = wheel.SupportingEntity; supportIsDynamic = supportEntity != null && supportEntity.isDynamic; //Grab jacobian and mass matrix from the driving motor! linearAX = wheel.drivingMotor.linearAX; linearAY = wheel.drivingMotor.linearAY; linearAZ = wheel.drivingMotor.linearAZ; angularAX = wheel.drivingMotor.angularAX; angularAY = wheel.drivingMotor.angularAY; angularAZ = wheel.drivingMotor.angularAZ; angularBX = wheel.drivingMotor.angularBX; angularBY = wheel.drivingMotor.angularBY; angularBZ = wheel.drivingMotor.angularBZ; velocityToImpulse = wheel.drivingMotor.velocityToImpulse; //Friction //Which coefficient? Check velocity. if (isBraking) if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold) blendedCoefficient = frictionBlender(staticBrakingFrictionCoefficient, wheel.supportMaterial.staticFriction, false, wheel); else blendedCoefficient = frictionBlender(kineticBrakingFrictionCoefficient, wheel.supportMaterial.kineticFriction, true, wheel); else blendedCoefficient = rollingFrictionCoefficient; } internal void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (vehicleEntity.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Vehicle/WheelDrivingMotor.cs ================================================ using BEPUphysics.Constraints; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Handles a wheel's driving force for a vehicle. /// public class WheelDrivingMotor : ISolverSettings { #region Static Stuff /// /// Default blender used by WheelSlidingFriction constraints. /// public static WheelFrictionBlender DefaultGripFrictionBlender; static WheelDrivingMotor() { DefaultGripFrictionBlender = BlendFriction; } /// /// Function which takes the friction values from a wheel and a supporting material and computes the blended friction. /// /// Friction coefficient associated with the wheel. /// Friction coefficient associated with the support material. /// True if the friction coefficients passed into the blender are kinetic coefficients, false otherwise. /// Wheel being blended. /// Blended friction coefficient. public static float BlendFriction(float wheelFriction, float materialFriction, bool usingKinematicFriction, Wheel wheel) { return wheelFriction * materialFriction; } #endregion internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; internal float angularAX, angularAY, angularAZ; internal float angularBX, angularBY, angularBZ; internal bool isActive = true; internal float linearAX, linearAY, linearAZ; private float currentFrictionCoefficient; internal Vector3 forceAxis; private float gripFriction; private WheelFrictionBlender gripFrictionBlender = DefaultGripFrictionBlender; private float maxMotorForceDt; private float maximumBackwardForce = float.MaxValue; private float maximumForwardForce = float.MaxValue; internal SolverSettings solverSettings = new SolverSettings(); private float targetSpeed; private Wheel wheel; internal int numIterationsAtZeroImpulse; private Entity vehicleEntity, supportEntity; //Inverse effective mass matrix internal float velocityToImpulse; private bool supportIsDynamic; /// /// Constructs a new wheel motor. /// /// Friction coefficient of the wheel. Blended with the ground's friction coefficient and normal force to determine a maximum force. /// Maximum force that the wheel motor can apply when driving forward (a target speed greater than zero). /// Maximum force that the wheel motor can apply when driving backward (a target speed less than zero). public WheelDrivingMotor(float gripFriction, float maximumForwardForce, float maximumBackwardForce) { GripFriction = gripFriction; MaximumForwardForce = maximumForwardForce; MaximumBackwardForce = maximumBackwardForce; } internal WheelDrivingMotor(Wheel wheel) { Wheel = wheel; } /// /// Gets the coefficient of grip friction between the wheel and support. /// This coefficient is the blended result of the supporting entity's friction and the wheel's friction. /// public float BlendedCoefficient { get { return currentFrictionCoefficient; } } /// /// Gets the axis along which the driving forces are applied. /// public Vector3 ForceAxis { get { return ForceAxis; } } /// /// Gets or sets the coefficient of forward-backward gripping friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// public float GripFriction { get { return gripFriction; } set { gripFriction = MathHelper.Max(value, 0); } } /// /// Gets or sets the function used to blend the supporting entity's friction and the wheel's friction. /// public WheelFrictionBlender GripFrictionBlender { get { return gripFrictionBlender; } set { gripFrictionBlender = value; } } /// /// Gets or sets the maximum force that the wheel motor can apply when driving backward (a target speed less than zero). /// public float MaximumBackwardForce { get { return maximumBackwardForce; } set { maximumBackwardForce = value; } } /// /// Gets or sets the maximum force that the wheel motor can apply when driving forward (a target speed greater than zero). /// public float MaximumForwardForce { get { return maximumForwardForce; } set { maximumForwardForce = value; } } /// /// Gets or sets the target speed of this wheel. /// public float TargetSpeed { get { return targetSpeed; } set { targetSpeed = value; } } /// /// Gets the force this wheel's motor is applying. /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the wheel that this motor applies to. /// public Wheel Wheel { get { return wheel; } internal set { wheel = value; } } #region ISolverSettings Members /// /// Gets the solver settings used by this wheel constraint. /// public SolverSettings SolverSettings { get { return solverSettings; } } #endregion /// /// Gets the relative velocity between the ground and wheel. /// /// Relative velocity between the ground and wheel. public float RelativeVelocity { get { float velocity = 0; if (vehicleEntity != null) velocity += vehicleEntity.linearVelocity.X * linearAX + vehicleEntity.linearVelocity.Y * linearAY + vehicleEntity.linearVelocity.Z * linearAZ + vehicleEntity.angularVelocity.X * angularAX + vehicleEntity.angularVelocity.Y * angularAY + vehicleEntity.angularVelocity.Z * angularAZ; if (supportEntity != null) velocity += -supportEntity.linearVelocity.X * linearAX - supportEntity.linearVelocity.Y * linearAY - supportEntity.linearVelocity.Z * linearAZ + supportEntity.angularVelocity.X * angularBX + supportEntity.angularVelocity.Y * angularBY + supportEntity.angularVelocity.Z * angularBZ; return velocity; } } internal float ApplyImpulse() { //Compute relative velocity float lambda = (RelativeVelocity - targetSpeed) //Add in the extra goal speed * velocityToImpulse; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse += lambda; //Don't brake, and take into account the motor's maximum force. if (targetSpeed > 0) accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse, 0, maxMotorForceDt); //MathHelper.Min(MathHelper.Max(accumulatedImpulse, 0), myMaxMotorForceDt); else if (targetSpeed < 0) accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse, maxMotorForceDt, 0); //MathHelper.Max(MathHelper.Min(accumulatedImpulse, 0), myMaxMotorForceDt); else accumulatedImpulse = 0; //Friction float maxForce = currentFrictionCoefficient * wheel.suspension.accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse, maxForce, -maxForce); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (vehicleEntity.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } return lambda; } internal void PreStep(float dt) { vehicleEntity = wheel.Vehicle.Body; supportEntity = wheel.SupportingEntity; supportIsDynamic = supportEntity != null && supportEntity.isDynamic; Vector3.Cross(ref wheel.normal, ref wheel.slidingFriction.slidingFrictionAxis, out forceAxis); forceAxis.Normalize(); //Do not need to check for normalize safety because normal and sliding friction axis must be perpendicular. linearAX = forceAxis.X; linearAY = forceAxis.Y; linearAZ = forceAxis.Z; //angular A = Ra x N angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY); angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ); angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y); angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z); angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (vehicleEntity.isDynamic) { tX = angularAX * vehicleEntity.inertiaTensorInverse.M11 + angularAY * vehicleEntity.inertiaTensorInverse.M21 + angularAZ * vehicleEntity.inertiaTensorInverse.M31; tY = angularAX * vehicleEntity.inertiaTensorInverse.M12 + angularAY * vehicleEntity.inertiaTensorInverse.M22 + angularAZ * vehicleEntity.inertiaTensorInverse.M32; tZ = angularAX * vehicleEntity.inertiaTensorInverse.M13 + angularAY * vehicleEntity.inertiaTensorInverse.M23 + angularAZ * vehicleEntity.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEntity.inverseMass; } else entryA = 0; if (supportIsDynamic) { tX = angularBX * supportEntity.inertiaTensorInverse.M11 + angularBY * supportEntity.inertiaTensorInverse.M21 + angularBZ * supportEntity.inertiaTensorInverse.M31; tY = angularBX * supportEntity.inertiaTensorInverse.M12 + angularBY * supportEntity.inertiaTensorInverse.M22 + angularBZ * supportEntity.inertiaTensorInverse.M32; tZ = angularBX * supportEntity.inertiaTensorInverse.M13 + angularBY * supportEntity.inertiaTensorInverse.M23 + angularBZ * supportEntity.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEntity.inverseMass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Softness? currentFrictionCoefficient = gripFrictionBlender(gripFriction, wheel.supportMaterial.kineticFriction, true, wheel); //Compute the maximum force if (targetSpeed > 0) maxMotorForceDt = maximumForwardForce * dt; else maxMotorForceDt = -maximumBackwardForce * dt; } internal void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (vehicleEntity.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Vehicle/WheelFrictionBlender.cs ================================================ using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Function which takes the friction values from a wheel and a supporting material and computes the blended friction. /// /// Friction coefficient associated with the wheel. /// Friction coefficient associated with the support material. /// True if the friction coefficients passed into the blender are kinetic coefficients, false otherwise. /// Wheel being blended. /// Blended friction coefficient. public delegate float WheelFrictionBlender(float wheelFriction, float materialFriction, bool usingKineticFriction, Wheel wheel); } ================================================ FILE: BEPUphysics/Vehicle/WheelShape.cs ================================================ using System; using BEPUphysics.Entities; using BEPUphysics.Entities.Prefabs; using Microsoft.Xna.Framework; using BEPUphysics.CollisionRuleManagement; using BEPUphysics.Materials; using BEPUphysics.BroadPhaseEntries; namespace BEPUphysics.Vehicle { /// /// Superclass for the shape of the tires of a vehicle. /// Responsible for figuring out where the wheel touches the ground and /// managing graphical properties. /// public abstract class WheelShape : ICollisionRulesOwner { private float airborneWheelAcceleration = 40; private float airborneWheelDeceleration = 4; private float brakeFreezeWheelDeceleration = 40; /// /// Collects collision pairs from the environment. /// protected internal Box detector = new Box(Vector3.Zero, 0, 0, 0); protected internal Matrix localGraphicTransform; protected float spinAngle; protected float spinVelocity; internal float steeringAngle; internal Matrix steeringTransform; protected internal Wheel wheel; protected internal Matrix worldTransform; CollisionRules collisionRules = new CollisionRules() { Group = CollisionRules.DefaultDynamicCollisionGroup}; /// /// Gets or sets the collision rules used by the wheel. /// public CollisionRules CollisionRules { get { return collisionRules; } set { collisionRules = value; } } /// /// Gets or sets the graphical radius of the wheel. /// public abstract float Radius { get; set; } /// /// Gets or sets the rate at which the wheel's spinning velocity increases when accelerating and airborne. /// This is a purely graphical effect. /// public float AirborneWheelAcceleration { get { return airborneWheelAcceleration; } set { airborneWheelAcceleration = Math.Abs(value); } } /// /// Gets or sets the rate at which the wheel's spinning velocity decreases when the wheel is airborne and its motor is idle. /// This is a purely graphical effect. /// public float AirborneWheelDeceleration { get { return airborneWheelDeceleration; } set { airborneWheelDeceleration = Math.Abs(value); } } /// /// Gets or sets the rate at which the wheel's spinning velocity decreases when braking. /// This is a purely graphical effect. /// public float BrakeFreezeWheelDeceleration { get { return brakeFreezeWheelDeceleration; } set { brakeFreezeWheelDeceleration = Math.Abs(value); } } /// /// Gets the detector entity used by the wheelshape to collect collision pairs. /// public Box Detector { get { return detector; } } /// /// Gets or sets whether or not to halt the wheel spin while the WheelBrake is active. /// public bool FreezeWheelsWhileBraking { get; set; } /// /// Gets or sets the local graphic transform of the wheel shape. /// This transform is applied first when creating the shape's worldTransform. /// public Matrix LocalGraphicTransform { get { return localGraphicTransform; } set { localGraphicTransform = value; } } /// /// Gets or sets the current spin angle of this wheel. /// This changes each frame based on the relative velocity between the /// support and the wheel. /// public float SpinAngle { get { return spinAngle; } set { spinAngle = value; } } /// /// Gets or sets the graphical spin velocity of the wheel based on the relative velocity /// between the support and the wheel. Whenever the wheel is in contact with /// the ground, the spin velocity will be each frame. /// public float SpinVelocity { get { return spinVelocity; } set { spinVelocity = value; } } /// /// Gets or sets the current steering angle of this wheel. /// public float SteeringAngle { get { return steeringAngle; } set { steeringAngle = value; } } /// /// Gets the wheel object associated with this shape. /// public Wheel Wheel { get { return wheel; } internal set { wheel = value; } } /// /// Gets the world matrix of the wheel for positioning a graphic. /// public Matrix WorldTransform { get { return worldTransform; } } /// /// Updates the wheel's world transform for graphics. /// Called automatically by the owning wheel at the end of each frame. /// If the engine is updating asynchronously, you can call this inside of a space read buffer lock /// and update the wheel transforms safely. /// public abstract void UpdateWorldTransform(); internal void OnAdditionToSpace(ISpace space) { detector.CollisionInformation.collisionRules.Specific.Add(wheel.vehicle.Body.CollisionInformation.collisionRules, CollisionRule.NoBroadPhase); detector.CollisionInformation.collisionRules.Personal = CollisionRule.NoNarrowPhaseUpdate; detector.CollisionInformation.collisionRules.group = CollisionRules.DefaultDynamicCollisionGroup; } internal void OnRemovalFromSpace(ISpace space) { detector.CollisionInformation.CollisionRules.Specific.Remove(wheel.vehicle.Body.CollisionInformation.collisionRules); } /// /// Updates the spin velocity and spin angle for the shape. /// /// Simulation timestep. internal void UpdateSpin(float dt) { if (wheel.HasSupport && !(wheel.brake.IsBraking && FreezeWheelsWhileBraking)) { //On the ground, not braking. spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius; } else if (wheel.HasSupport && wheel.brake.IsBraking && FreezeWheelsWhileBraking) { //On the ground, braking float deceleratedValue = 0; if (spinVelocity > 0) deceleratedValue = Math.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, 0); else if (spinVelocity < 0) deceleratedValue = Math.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, 0); spinVelocity = wheel.drivingMotor.RelativeVelocity / Radius; if (Math.Abs(deceleratedValue) < Math.Abs(spinVelocity)) spinVelocity = deceleratedValue; } else if (!wheel.HasSupport && wheel.drivingMotor.TargetSpeed != 0) { //Airborne and accelerating, increase spin velocity. float maxSpeed = Math.Abs(wheel.drivingMotor.TargetSpeed) / Radius; spinVelocity = MathHelper.Clamp(spinVelocity + Math.Sign(wheel.drivingMotor.TargetSpeed) * airborneWheelAcceleration * dt, -maxSpeed, maxSpeed); } else if (!wheel.HasSupport && wheel.Brake.IsBraking) { //Airborne and braking if (spinVelocity > 0) spinVelocity = Math.Max(spinVelocity - brakeFreezeWheelDeceleration * dt, 0); else if (spinVelocity < 0) spinVelocity = Math.Min(spinVelocity + brakeFreezeWheelDeceleration * dt, 0); } else if (!wheel.HasSupport) { //Just idly slowing down. if (spinVelocity > 0) spinVelocity = Math.Max(spinVelocity - airborneWheelDeceleration * dt, 0); else if (spinVelocity < 0) spinVelocity = Math.Min(spinVelocity + airborneWheelDeceleration * dt, 0); } spinAngle += spinVelocity * dt; } /// /// Finds a supporting entity, the contact location, and the contact normal. /// /// Contact point between the wheel and the support. /// Contact normal between the wheel and the support. /// Length of the suspension at the contact. /// Collidable supporting the wheel, if any. /// Entity supporting the wheel, if any. /// Material of the support. /// Whether or not any support was found. protected internal abstract bool FindSupport(out Vector3 location, out Vector3 normal, out float suspensionLength, out Collidable supportCollidable, out Entity entity, out Material material); /// /// Initializes the detector entity and any other necessary logic. /// protected internal abstract void Initialize(); /// /// Updates the position of the detector before each step. /// protected internal abstract void UpdateDetectorPosition(); } } ================================================ FILE: BEPUphysics/Vehicle/WheelSlidingFriction.cs ================================================ using System; using BEPUphysics.Constraints; using BEPUphysics.Entities; using BEPUutilities; using Microsoft.Xna.Framework; using BEPUphysics.Materials; namespace BEPUphysics.Vehicle { /// /// Attempts to resist sliding motion of a vehicle. /// public class WheelSlidingFriction : ISolverSettings { #region Static Stuff /// /// Default blender used by WheelSlidingFriction constraints. /// public static WheelFrictionBlender DefaultSlidingFrictionBlender; static WheelSlidingFriction() { DefaultSlidingFrictionBlender = BlendFriction; } /// /// Function which takes the friction values from a wheel and a supporting material and computes the blended friction. /// /// Friction coefficient associated with the wheel. /// Friction coefficient associated with the support material. /// True if the friction coefficients passed into the blender are kinetic coefficients, false otherwise. /// Wheel being blended. /// Blended friction coefficient. public static float BlendFriction(float wheelFriction, float materialFriction, bool usingKinematicFriction, Wheel wheel) { return wheelFriction * materialFriction; } #endregion internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; private float angularAX, angularAY, angularAZ; private float angularBX, angularBY, angularBZ; internal bool isActive = true; private float linearAX, linearAY, linearAZ; private float blendedCoefficient; private float kineticCoefficient; private WheelFrictionBlender frictionBlender = DefaultSlidingFrictionBlender; internal Vector3 slidingFrictionAxis; internal SolverSettings solverSettings = new SolverSettings(); private float staticCoefficient; private float staticFrictionVelocityThreshold = 5; private Wheel wheel; internal int numIterationsAtZeroImpulse; private Entity vehicleEntity, supportEntity; //Inverse effective mass matrix private float velocityToImpulse; /// /// Constructs a new sliding friction object for a wheel. /// /// Coefficient of dynamic sliding friction to be blended with the supporting entity's friction. /// Coefficient of static sliding friction to be blended with the supporting entity's friction. public WheelSlidingFriction(float dynamicCoefficient, float staticCoefficient) { KineticCoefficient = dynamicCoefficient; StaticCoefficient = staticCoefficient; } internal WheelSlidingFriction(Wheel wheel) { Wheel = wheel; } /// /// Gets the coefficient of sliding friction between the wheel and support. /// This coefficient is the blended result of the supporting entity's friction and the wheel's friction. /// public float BlendedCoefficient { get { return blendedCoefficient; } } /// /// Gets or sets the coefficient of dynamic horizontal sliding friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// public float KineticCoefficient { get { return kineticCoefficient; } set { kineticCoefficient = MathHelper.Max(value, 0); } } /// /// Gets or sets the function used to blend the supporting entity's friction and the wheel's friction. /// public WheelFrictionBlender FrictionBlender { get { return frictionBlender; } set { frictionBlender = value; } } /// /// Gets the axis along which sliding friction is applied. /// public Vector3 SlidingFrictionAxis { get { return slidingFrictionAxis; } } /// /// Gets or sets the coefficient of static horizontal sliding friction for this wheel. /// This coefficient and the supporting entity's coefficient of friction will be /// taken into account to determine the used coefficient at any given time. /// public float StaticCoefficient { get { return staticCoefficient; } set { staticCoefficient = MathHelper.Max(value, 0); } } /// /// Gets or sets the velocity under which the coefficient of static friction will be used instead of the dynamic one. /// public float StaticFrictionVelocityThreshold { get { return staticFrictionVelocityThreshold; } set { staticFrictionVelocityThreshold = Math.Abs(value); } } /// /// Gets the force /// public float TotalImpulse { get { return accumulatedImpulse; } } /// /// Gets the wheel that this sliding friction applies to. /// public Wheel Wheel { get { return wheel; } internal set { wheel = value; } } #region ISolverSettings Members /// /// Gets the solver settings used by this wheel constraint. /// public SolverSettings SolverSettings { get { return solverSettings; } } #endregion bool supportIsDynamic; /// /// Gets the relative velocity along the sliding direction at the wheel contact. /// public float RelativeVelocity { get { float velocity = vehicleEntity.linearVelocity.X * linearAX + vehicleEntity.linearVelocity.Y * linearAY + vehicleEntity.linearVelocity.Z * linearAZ + vehicleEntity.angularVelocity.X * angularAX + vehicleEntity.angularVelocity.Y * angularAY + vehicleEntity.angularVelocity.Z * angularAZ; if (supportEntity != null) velocity += -supportEntity.linearVelocity.X * linearAX - supportEntity.linearVelocity.Y * linearAY - supportEntity.linearVelocity.Z * linearAZ + supportEntity.angularVelocity.X * angularBX + supportEntity.angularVelocity.Y * angularBY + supportEntity.angularVelocity.Z * angularBZ; return velocity; } } internal float ApplyImpulse() { //Compute relative velocity and convert to an impulse float lambda = RelativeVelocity * velocityToImpulse; //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; float maxForce = -blendedCoefficient * wheel.suspension.accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maxForce, maxForce); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (vehicleEntity.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } return lambda; } internal void PreStep(float dt) { vehicleEntity = wheel.Vehicle.Body; supportEntity = wheel.SupportingEntity; supportIsDynamic = supportEntity != null && supportEntity.isDynamic; Vector3.Cross(ref wheel.worldForwardDirection, ref wheel.normal, out slidingFrictionAxis); float axisLength = slidingFrictionAxis.LengthSquared(); //Safety against bad cross product if (axisLength < Toolbox.BigEpsilon) { Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.UpVector, out slidingFrictionAxis); axisLength = slidingFrictionAxis.LengthSquared(); if (axisLength < Toolbox.BigEpsilon) { Vector3.Cross(ref wheel.worldForwardDirection, ref Toolbox.RightVector, out slidingFrictionAxis); } } slidingFrictionAxis.Normalize(); linearAX = slidingFrictionAxis.X; linearAY = slidingFrictionAxis.Y; linearAZ = slidingFrictionAxis.Z; //angular A = Ra x N angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY); angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ); angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y); angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z); angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (vehicleEntity.isDynamic) { tX = angularAX * vehicleEntity.inertiaTensorInverse.M11 + angularAY * vehicleEntity.inertiaTensorInverse.M21 + angularAZ * vehicleEntity.inertiaTensorInverse.M31; tY = angularAX * vehicleEntity.inertiaTensorInverse.M12 + angularAY * vehicleEntity.inertiaTensorInverse.M22 + angularAZ * vehicleEntity.inertiaTensorInverse.M32; tZ = angularAX * vehicleEntity.inertiaTensorInverse.M13 + angularAY * vehicleEntity.inertiaTensorInverse.M23 + angularAZ * vehicleEntity.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEntity.inverseMass; } else entryA = 0; if (supportIsDynamic) { tX = angularBX * supportEntity.inertiaTensorInverse.M11 + angularBY * supportEntity.inertiaTensorInverse.M21 + angularBZ * supportEntity.inertiaTensorInverse.M31; tY = angularBX * supportEntity.inertiaTensorInverse.M12 + angularBY * supportEntity.inertiaTensorInverse.M22 + angularBZ * supportEntity.inertiaTensorInverse.M32; tZ = angularBX * supportEntity.inertiaTensorInverse.M13 + angularBY * supportEntity.inertiaTensorInverse.M23 + angularBZ * supportEntity.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEntity.inverseMass; } else entryB = 0; velocityToImpulse = -1 / (entryA + entryB); //Softness? //Compute friction. //Which coefficient? Check velocity. if (Math.Abs(RelativeVelocity) < staticFrictionVelocityThreshold) blendedCoefficient = frictionBlender(staticCoefficient, wheel.supportMaterial.staticFriction, false, wheel); else blendedCoefficient = frictionBlender(kineticCoefficient, wheel.supportMaterial.kineticFriction, true, wheel); } internal void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (vehicleEntity.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUphysics/Vehicle/WheelSuspension.cs ================================================ using BEPUphysics.Constraints; using BEPUphysics.Entities; using Microsoft.Xna.Framework; using BEPUutilities; namespace BEPUphysics.Vehicle { /// /// Allows the connected wheel and vehicle to smoothly absorb bumps. /// public class WheelSuspension : ISpringSettings, ISolverSettings { private readonly SpringSettings springSettings = new SpringSettings(); internal float accumulatedImpulse; //float linearBX, linearBY, linearBZ; private float angularAX, angularAY, angularAZ; private float angularBX, angularBY, angularBZ; private float bias; internal bool isActive = true; private float linearAX, linearAY, linearAZ; private float allowedCompression = .01f; internal float currentLength; internal Vector3 localAttachmentPoint; internal Vector3 localDirection; private float maximumSpringCorrectionSpeed = float.MaxValue; private float maximumSpringForce = float.MaxValue; internal float restLength; internal SolverSettings solverSettings = new SolverSettings(); private Wheel wheel; internal Vector3 worldAttachmentPoint; internal Vector3 worldDirection; internal int numIterationsAtZeroImpulse; private Entity vehicleEntity, supportEntity; private float softness; //Inverse effective mass matrix private float velocityToImpulse; private bool supportIsDynamic; /// /// Constructs a new suspension for a wheel. /// /// Strength of the spring. Higher values resist compression more. /// Damping constant of the spring. Higher values remove more momentum. /// Direction of the suspension in the vehicle's local space. For a normal, straight down suspension, this would be (0, -1, 0). /// Length of the suspension when uncompressed. /// Place where the suspension hooks up to the body of the vehicle. public WheelSuspension(float stiffnessConstant, float dampingConstant, Vector3 localDirection, float restLength, Vector3 localAttachmentPoint) { SpringSettings.StiffnessConstant = stiffnessConstant; SpringSettings.DampingConstant = dampingConstant; LocalDirection = localDirection; RestLength = restLength; LocalAttachmentPoint = localAttachmentPoint; } internal WheelSuspension(Wheel wheel) { Wheel = wheel; } /// /// Gets or sets the allowed compression of the suspension before suspension forces take effect. /// Usually a very small number. Used to prevent 'jitter' where the wheel leaves the ground due to spring forces repeatedly. /// public float AllowedCompression { get { return allowedCompression; } set { allowedCompression = MathHelper.Max(0, value); } } /// /// Gets the the current length of the suspension. /// This will be less than the restLength if the suspension is compressed. /// public float CurrentLength { get { return currentLength; } } /// /// Gets or sets the attachment point of the suspension to the vehicle body in the body's local space. /// public Vector3 LocalAttachmentPoint { get { return localAttachmentPoint; } set { localAttachmentPoint = value; if (wheel != null && wheel.vehicle != null) { RigidTransform.Transform(ref localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.worldTransform, out worldAttachmentPoint); } else worldAttachmentPoint = localAttachmentPoint; } } /// /// Gets or sets the direction of the wheel suspension in the local space of the vehicle body. /// A normal, straight suspension would be (0,-1,0). /// public Vector3 LocalDirection { get { return localDirection; } set { localDirection = Vector3.Normalize(value); if (wheel != null && wheel.vehicle != null) Matrix3x3.Transform(ref localDirection, ref wheel.vehicle.Body.orientationMatrix, out worldDirection); else worldDirection = localDirection; } } /// /// Gets or sets the maximum speed at which the suspension will try to return the suspension to rest length. /// public float MaximumSpringCorrectionSpeed { get { return maximumSpringCorrectionSpeed; } set { maximumSpringCorrectionSpeed = MathHelper.Max(0, value); } } /// /// Gets or sets the maximum force that can be applied by this suspension. /// public float MaximumSpringForce { get { return maximumSpringForce; } set { maximumSpringForce = MathHelper.Max(0, value); } } /// /// Gets or sets the length of the uncompressed suspension. /// public float RestLength { get { return restLength; } set { restLength = value; } } /// /// Gets the force that the suspension is applying to support the vehicle. /// public float TotalImpulse { get { return -accumulatedImpulse; } } /// /// Gets the wheel that this suspension applies to. /// public Wheel Wheel { get { return wheel; } internal set { wheel = value; } } /// /// Gets or sets the attachment point of the suspension to the vehicle body in world space. /// public Vector3 WorldAttachmentPoint { get { return worldAttachmentPoint; } set { worldAttachmentPoint = value; if (wheel != null && wheel.vehicle != null) { RigidTransform.TransformByInverse(ref worldAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.worldTransform, out localAttachmentPoint); } else localAttachmentPoint = worldAttachmentPoint; } } /// /// Gets or sets the direction of the wheel suspension in the world space of the vehicle body. /// public Vector3 WorldDirection { get { return worldDirection; } set { worldDirection = Vector3.Normalize(value); if (wheel != null && wheel.vehicle != null) { Matrix3x3.TransformTranspose(ref worldDirection, ref wheel.Vehicle.Body.orientationMatrix, out localDirection); } else localDirection = worldDirection; } } #region ISolverSettings Members /// /// Gets the solver settings used by this wheel constraint. /// public SolverSettings SolverSettings { get { return solverSettings; } } #endregion #region ISpringSettings Members /// /// Gets the spring settings that define the behavior of the suspension. /// public SpringSettings SpringSettings { get { return springSettings; } } #endregion /// /// Gets the relative velocity along the support normal at the contact point. /// public float RelativeVelocity { get { float velocity = vehicleEntity.linearVelocity.X * linearAX + vehicleEntity.linearVelocity.Y * linearAY + vehicleEntity.linearVelocity.Z * linearAZ + vehicleEntity.angularVelocity.X * angularAX + vehicleEntity.angularVelocity.Y * angularAY + vehicleEntity.angularVelocity.Z * angularAZ; if (supportEntity != null) velocity += -supportEntity.linearVelocity.X * linearAX - supportEntity.linearVelocity.Y * linearAY - supportEntity.linearVelocity.Z * linearAZ + supportEntity.angularVelocity.X * angularBX + supportEntity.angularVelocity.Y * angularBY + supportEntity.angularVelocity.Z * angularBZ; return velocity; } } internal float ApplyImpulse() { //Compute relative velocity float lambda = (RelativeVelocity + bias //Add in position correction + softness * accumulatedImpulse) //Add in squishiness * velocityToImpulse; //convert to impulse //Clamp accumulated impulse float previousAccumulatedImpulse = accumulatedImpulse; accumulatedImpulse = MathHelper.Clamp(accumulatedImpulse + lambda, -maximumSpringForce, 0); lambda = accumulatedImpulse - previousAccumulatedImpulse; //Apply the impulse #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = lambda * linearAX; linear.Y = lambda * linearAY; linear.Z = lambda * linearAZ; if (vehicleEntity.isDynamic) { angular.X = lambda * angularAX; angular.Y = lambda * angularAY; angular.Z = lambda * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = lambda * angularBX; angular.Y = lambda * angularBY; angular.Z = lambda * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } return lambda; } internal void ComputeWorldSpaceData() { //Transform local space vectors to world space. RigidTransform.Transform(ref localAttachmentPoint, ref wheel.vehicle.Body.CollisionInformation.worldTransform, out worldAttachmentPoint); Matrix3x3.Transform(ref localDirection, ref wheel.vehicle.Body.orientationMatrix, out worldDirection); } internal void OnAdditionToVehicle() { //This looks weird, but it's just re-setting the world locations. //If the wheel doesn't belong to a vehicle (or this doesn't belong to a wheel) //then the world space location can't be set. LocalDirection = LocalDirection; LocalAttachmentPoint = LocalAttachmentPoint; } internal void PreStep(float dt) { vehicleEntity = wheel.vehicle.Body; supportEntity = wheel.supportingEntity; supportIsDynamic = supportEntity != null && supportEntity.isDynamic; //The next line is commented out because the world direction is computed by the wheelshape. Weird, but necessary. //Vector3.TransformNormal(ref myLocalDirection, ref parentA.myInternalOrientationMatrix, out myWorldDirection); //Set up the jacobians. linearAX = -wheel.normal.X; //myWorldDirection.X; linearAY = -wheel.normal.Y; //myWorldDirection.Y; linearAZ = -wheel.normal.Z; // myWorldDirection.Z; //linearBX = -linearAX; //linearBY = -linearAY; //linearBZ = -linearAZ; //angular A = Ra x N angularAX = (wheel.ra.Y * linearAZ) - (wheel.ra.Z * linearAY); angularAY = (wheel.ra.Z * linearAX) - (wheel.ra.X * linearAZ); angularAZ = (wheel.ra.X * linearAY) - (wheel.ra.Y * linearAX); //Angular B = N x Rb angularBX = (linearAY * wheel.rb.Z) - (linearAZ * wheel.rb.Y); angularBY = (linearAZ * wheel.rb.X) - (linearAX * wheel.rb.Z); angularBZ = (linearAX * wheel.rb.Y) - (linearAY * wheel.rb.X); //Compute inverse effective mass matrix float entryA, entryB; //these are the transformed coordinates float tX, tY, tZ; if (vehicleEntity.isDynamic) { tX = angularAX * vehicleEntity.inertiaTensorInverse.M11 + angularAY * vehicleEntity.inertiaTensorInverse.M21 + angularAZ * vehicleEntity.inertiaTensorInverse.M31; tY = angularAX * vehicleEntity.inertiaTensorInverse.M12 + angularAY * vehicleEntity.inertiaTensorInverse.M22 + angularAZ * vehicleEntity.inertiaTensorInverse.M32; tZ = angularAX * vehicleEntity.inertiaTensorInverse.M13 + angularAY * vehicleEntity.inertiaTensorInverse.M23 + angularAZ * vehicleEntity.inertiaTensorInverse.M33; entryA = tX * angularAX + tY * angularAY + tZ * angularAZ + vehicleEntity.inverseMass; } else entryA = 0; if (supportIsDynamic) { tX = angularBX * supportEntity.inertiaTensorInverse.M11 + angularBY * supportEntity.inertiaTensorInverse.M21 + angularBZ * supportEntity.inertiaTensorInverse.M31; tY = angularBX * supportEntity.inertiaTensorInverse.M12 + angularBY * supportEntity.inertiaTensorInverse.M22 + angularBZ * supportEntity.inertiaTensorInverse.M32; tZ = angularBX * supportEntity.inertiaTensorInverse.M13 + angularBY * supportEntity.inertiaTensorInverse.M23 + angularBZ * supportEntity.inertiaTensorInverse.M33; entryB = tX * angularBX + tY * angularBY + tZ * angularBZ + supportEntity.inverseMass; } else entryB = 0; //Convert spring constant and damping constant into ERP and CFM. float biasFactor; springSettings.ComputeErrorReductionAndSoftness(dt, out biasFactor, out softness); velocityToImpulse = -1 / (entryA + entryB + softness); //Correction velocity bias = MathHelper.Min(MathHelper.Max(0, (restLength - currentLength) - allowedCompression) * biasFactor, maximumSpringCorrectionSpeed); } internal void ExclusiveUpdate() { //Warm starting #if !WINDOWS Vector3 linear = new Vector3(); Vector3 angular = new Vector3(); #else Vector3 linear, angular; #endif linear.X = accumulatedImpulse * linearAX; linear.Y = accumulatedImpulse * linearAY; linear.Z = accumulatedImpulse * linearAZ; if (vehicleEntity.isDynamic) { angular.X = accumulatedImpulse * angularAX; angular.Y = accumulatedImpulse * angularAY; angular.Z = accumulatedImpulse * angularAZ; vehicleEntity.ApplyLinearImpulse(ref linear); vehicleEntity.ApplyAngularImpulse(ref angular); } if (supportIsDynamic) { linear.X = -linear.X; linear.Y = -linear.Y; linear.Z = -linear.Z; angular.X = accumulatedImpulse * angularBX; angular.Y = accumulatedImpulse * angularBY; angular.Z = accumulatedImpulse * angularBZ; supportEntity.ApplyLinearImpulse(ref linear); supportEntity.ApplyAngularImpulse(ref angular); } } } } ================================================ FILE: BEPUutilities/AffineTransform.cs ================================================  using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// A transformation composed of a linear transformation and a translation. /// public struct AffineTransform { /// /// Translation in the affine transform. /// public Vector3 Translation; /// /// Linear transform in the affine transform. /// public Matrix3x3 LinearTransform; /// /// Constructs a new affine transform. /// ///Translation to use in the transform. public AffineTransform(Vector3 translation) { LinearTransform = Matrix3x3.Identity; Translation = translation; } /// /// Constructs a new affine tranform. /// ///Orientation to use as the linear transform. ///Translation to use in the transform. public AffineTransform(Quaternion orientation, Vector3 translation) { Matrix3x3.CreateFromQuaternion(ref orientation, out LinearTransform); Translation = translation; } /// /// Constructs a new affine transform. /// ///Scaling to apply in the linear transform. ///Orientation to apply in the linear transform. ///Translation to apply. public AffineTransform(Vector3 scaling, Quaternion orientation, Vector3 translation) { //Create an SRT transform. Matrix3x3.CreateScale(ref scaling, out LinearTransform); Matrix3x3 rotation; Matrix3x3.CreateFromQuaternion(ref orientation, out rotation); Matrix3x3.Multiply(ref LinearTransform, ref rotation, out LinearTransform); Translation = translation; } /// /// Constructs a new affine transform. /// ///The linear transform component. ///Translation component of the transform. public AffineTransform(Matrix3x3 linearTransform, Vector3 translation) { LinearTransform = linearTransform; Translation = translation; } /// /// Gets or sets the 4x4 matrix representation of the affine transform. /// The linear transform is the upper left 3x3 part of the 4x4 matrix. /// The translation is included in the matrix's Translation property. /// public Matrix Matrix { get { Matrix toReturn; Matrix3x3.ToMatrix4X4(ref LinearTransform, out toReturn); toReturn.Translation = Translation; return toReturn; } set { Matrix3x3.CreateFromMatrix(ref value, out LinearTransform); Translation = value.Translation; } } /// /// Gets the identity affine transform. /// public static AffineTransform Identity { get { var t = new AffineTransform { LinearTransform = Matrix3x3.Identity, Translation = new Vector3() }; return t; } } /// /// Transforms a vector by an affine transform. /// ///Position to transform. ///Transform to apply. ///Transformed position. public static void Transform(ref Vector3 position, ref AffineTransform transform, out Vector3 transformed) { Matrix3x3.Transform(ref position, ref transform.LinearTransform, out transformed); Vector3.Add(ref transformed, ref transform.Translation, out transformed); } /// /// Transforms a vector by an affine transform's inverse. /// ///Position to transform. ///Transform to invert and apply. ///Transformed position. public static void TransformInverse(ref Vector3 position, ref AffineTransform transform, out Vector3 transformed) { Vector3.Subtract(ref position, ref transform.Translation, out transformed); Matrix3x3 inverse; Matrix3x3.Invert(ref transform.LinearTransform, out inverse); Matrix3x3.TransformTranspose(ref transformed, ref inverse, out transformed); } /// /// Inverts an affine transform. /// ///Transform to invert. /// Inverse of the transform. public static void Invert(ref AffineTransform transform, out AffineTransform inverse) { Matrix3x3.Invert(ref transform.LinearTransform, out inverse.LinearTransform); Matrix3x3.Transform(ref transform.Translation, ref inverse.LinearTransform, out inverse.Translation); Vector3.Negate(ref inverse.Translation, out inverse.Translation); } /// /// Multiplies a transform by another transform. /// /// First transform. /// Second transform. /// Combined transform. public static void Multiply(ref AffineTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3x3 linearTransform;//Have to use temporary variable just in case a or b reference is transform. Matrix3x3.Multiply(ref a.LinearTransform, ref b.LinearTransform, out linearTransform); Vector3 translation; Matrix3x3.Transform(ref a.Translation, ref b.LinearTransform, out translation); Vector3.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; } /// /// Multiplies a rigid transform by an affine transform. /// ///Rigid transform. ///Affine transform. ///Combined transform. public static void Multiply(ref RigidTransform a, ref AffineTransform b, out AffineTransform transform) { Matrix3x3 linearTransform;//Have to use temporary variable just in case b reference is transform. Matrix3x3.CreateFromQuaternion(ref a.Orientation, out linearTransform); Matrix3x3.Multiply(ref linearTransform, ref b.LinearTransform, out linearTransform); Vector3 translation; Matrix3x3.Transform(ref a.Position, ref b.LinearTransform, out translation); Vector3.Add(ref translation, ref b.Translation, out transform.Translation); transform.LinearTransform = linearTransform; } /// /// Transforms a vector using an affine transform. /// ///Position to transform. ///Transform to apply. ///Transformed position. public static Vector3 Transform(Vector3 position, AffineTransform affineTransform) { Vector3 toReturn; Transform(ref position, ref affineTransform, out toReturn); return toReturn; } /// /// Creates an affine transform from a rigid transform. /// /// Rigid transform to base the affine transform on. /// Affine transform created from the rigid transform. public static void CreateFromRigidTransform(ref RigidTransform rigid, out AffineTransform affine) { affine.Translation = rigid.Position; Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out affine.LinearTransform); } /// /// Creates an affine transform from a rigid transform. /// /// Rigid transform to base the affine transform on. /// Affine transform created from the rigid transform. public static AffineTransform CreateFromRigidTransform(RigidTransform rigid) { AffineTransform toReturn; toReturn.Translation = rigid.Position; Matrix3x3.CreateFromQuaternion(ref rigid.Orientation, out toReturn.LinearTransform); return toReturn; } } } ================================================ FILE: BEPUutilities/BEPUutilities.csproj ================================================  {E3AAEB61-D7DF-4E7E-A75B-B5282D2FF3F5} {6D335F3A-9D43-41b4-9D22-F6F17C4BE596};{FAE04EC0-301F-11D3-BF4B-00C04F79EFBC} Debug x86 Library Properties BEPUutilities BEPUutilities v4.0 Client v4.0 Windows Reach 0aedf39e-8385-4a55-9db6-916f6d31ad51 Library true full false bin\x86\Debug TRACE;DEBUG;WINDOWS;ALLOWUNSAFE prompt 4 true false x86 false true bin\x86\Debug\BEPUutilities.XML pdbonly true bin\x86\Release WINDOWS;ALLOWUNSAFE prompt 4 true false x86 true true bin\x86\Release\BEPUutilities.XML true strongNameKey.snk 4.0 4.0 ================================================ FILE: BEPUutilities/ConvexHullHelper.Pruning.cs ================================================ using System; using System.Collections.Generic; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUutilities { public static partial class ConvexHullHelper { /// /// Represents a cell in space which is already occupied by a point. Any other points which resolve to the same cell are considered redundant. /// public struct BlockedCell : IEquatable { public int X; public int Y; public int Z; public override int GetHashCode() { const long p1 = 961748927L; const long p2 = 961748941L; const long p3 = 982451653L; return (int)(X * p1 + Y * p2 + Z * p3); } public override bool Equals(object obj) { return this.Equals((BlockedCell)obj); } public bool Equals(BlockedCell other) { return other.X == X && other.Y == Y && other.Z == Z; } } /// /// Contains and manufactures cell sets used by the redundant point remover. To minimize memory usage, this can be cleared /// after using the RemoveRedundantPoints if it isn't going to be used again. /// public static LockingResourcePool> BlockedCellSets = new LockingResourcePool>(); /// /// Removes redundant points. Two points are redundant if they occupy the same hash grid cell of size 0.001. /// /// List of points to prune. public static void RemoveRedundantPoints(IList points) { RemoveRedundantPoints(points, .001); } /// /// Removes redundant points. Two points are redundant if they occupy the same hash grid cell. /// /// List of points to prune. /// Size of cells to determine redundancy. public static void RemoveRedundantPoints(IList points, double cellSize) { var rawPoints = CommonResources.GetVectorList(); rawPoints.AddRange(points); RemoveRedundantPoints(rawPoints, cellSize); points.Clear(); for (int i = 0; i < rawPoints.Count; ++i) { points.Add(rawPoints.Elements[i]); } CommonResources.GiveBack(rawPoints); } /// /// Removes redundant points. Two points are redundant if they occupy the same hash grid cell of size 0.001. /// /// List of points to prune. public static void RemoveRedundantPoints(RawList points) { RemoveRedundantPoints(points, .001); } /// /// Removes redundant points. Two points are redundant if they occupy the same hash grid cell. /// /// List of points to prune. /// Size of cells to determine redundancy. public static void RemoveRedundantPoints(RawList points, double cellSize) { var set = BlockedCellSets.Take(); for (int i = points.Count - 1; i >= 0; --i) { var element = points.Elements[i]; var cell = new BlockedCell { X = (int)Math.Floor(element.X / cellSize), Y = (int)Math.Floor(element.Y / cellSize), Z = (int)Math.Floor(element.Z / cellSize) }; if (set.Contains(cell)) { points.FastRemoveAt(i); } else { set.Add(cell); //TODO: Consider adding adjacent cells to guarantee that a point on the border between two cells will still detect the presence //of a point on the opposite side of that border. } } set.Clear(); BlockedCellSets.GiveBack(set); } } } ================================================ FILE: BEPUutilities/ConvexHullHelper.cs ================================================ using System; using System.Collections.Generic; using BEPUutilities; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// Processes vertex data into convex hulls. /// public static partial class ConvexHullHelper { /// /// Identifies the indices of points in a set which are on the outer convex hull of the set. /// /// List of points in the set. /// List of indices composing the triangulated surface of the convex hull. /// Each group of 3 indices represents a triangle on the surface of the hull. public static void GetConvexHull(IList points, IList indices) { var rawPoints = CommonResources.GetVectorList(); var rawIndices = CommonResources.GetIntList(); rawPoints.AddRange(points); GetConvexHull(rawPoints, rawIndices); CommonResources.GiveBack(rawPoints); for (int i = 0; i < rawIndices.Count; i++) { indices.Add(rawIndices[i]); } CommonResources.GiveBack(rawIndices); } /// /// Identifies the points on the surface of hull. /// /// List of points in the set. /// Unique points on the surface of the convex hull. public static void GetConvexHull(IList points, IList outputSurfacePoints) { var rawPoints = CommonResources.GetVectorList(); rawPoints.AddRange(points); GetConvexHull(rawPoints, outputSurfacePoints); CommonResources.GiveBack(rawPoints); } /// /// Identifies the points on the surface of hull. /// /// List of points in the set. /// Unique points on the surface of the convex hull. public static void GetConvexHull(RawList points, IList outputSurfacePoints) { var indices = CommonResources.GetIntList(); GetConvexHull(points, indices, outputSurfacePoints); CommonResources.GiveBack(indices); } /// /// Identifies the points on the surface of hull. /// /// List of points in the set. /// List of indices composing the triangulated surface of the convex hull. /// Each group of 3 indices represents a triangle on the surface of the hull. /// Unique points on the surface of the convex hull. public static void GetConvexHull(IList points, IList outputIndices, IList outputSurfacePoints) { var rawPoints = CommonResources.GetVectorList(); var rawIndices = CommonResources.GetIntList(); rawPoints.AddRange(points); GetConvexHull(rawPoints, rawIndices, outputSurfacePoints); CommonResources.GiveBack(rawPoints); for (int i = 0; i < rawIndices.Count; i++) { outputIndices.Add(rawIndices[i]); } CommonResources.GiveBack(rawIndices); } /// /// Identifies the points on the surface of hull. /// /// List of points in the set. /// List of indices composing the triangulated surface of the convex hull. /// Each group of 3 indices represents a triangle on the surface of the hull. /// Unique points on the surface of the convex hull. public static void GetConvexHull(RawList points, RawList outputTriangleIndices, IList outputSurfacePoints) { GetConvexHull(points, outputTriangleIndices); var alreadyContainedIndices = CommonResources.GetIntSet(); for (int i = outputTriangleIndices.Count - 1; i >= 0; i--) { int index = outputTriangleIndices[i]; if (!alreadyContainedIndices.Contains(index)) { outputSurfacePoints.Add(points[index]); alreadyContainedIndices.Add(index); } } CommonResources.GiveBack(alreadyContainedIndices); } /// /// Identifies the indices of points in a set which are on the outer convex hull of the set. /// /// List of points in the set. /// List of indices composing the triangulated surface of the convex hull. /// Each group of 3 indices represents a triangle on the surface of the hull. public static void GetConvexHull(RawList points, RawList outputTriangleIndices) { if (points.Count == 0) { throw new ArgumentException("Point set must have volume."); } RawList outsidePoints = CommonResources.GetIntList(); if (outsidePoints.Capacity < points.Count - 4) outsidePoints.Capacity = points.Count - 4; //Build the initial tetrahedron. //It will also give us the location of a point which is guaranteed to be within the //final convex hull. We can use this point to calibrate the winding of triangles. //A set of outside point candidates (all points other than those composing the tetrahedron) will be returned in the outsidePoints list. //That list will then be further pruned by the RemoveInsidePoints call. Vector3 insidePoint; ComputeInitialTetrahedron(points, outsidePoints, outputTriangleIndices, out insidePoint); //Compute outside points. RemoveInsidePoints(points, outputTriangleIndices, outsidePoints); var edges = CommonResources.GetIntList(); var toRemove = CommonResources.GetIntList(); var newTriangles = CommonResources.GetIntList(); //We're now ready to begin the main loop. while (outsidePoints.Count > 0) { //While the convex hull is incomplete... for (int k = 0; k < outputTriangleIndices.Count; k += 3) { //Find the normal of the triangle Vector3 normal; FindNormal(outputTriangleIndices, points, k, out normal); //Get the furthest point in the direction of the normal. int maxIndexInOutsideList = GetExtremePoint(ref normal, points, outsidePoints); int maxIndex = outsidePoints.Elements[maxIndexInOutsideList]; Vector3 maximum = points.Elements[maxIndex]; //If the point is beyond the current triangle, continue. Vector3 offset; Vector3.Subtract(ref maximum, ref points.Elements[outputTriangleIndices.Elements[k]], out offset); float dot; Vector3.Dot(ref normal, ref offset, out dot); if (dot > 0) { //It's been picked! Remove the maximum point from the outside. outsidePoints.FastRemoveAt(maxIndexInOutsideList); //Remove any triangles that can see the point, including itself! edges.Clear(); toRemove.Clear(); for (int n = outputTriangleIndices.Count - 3; n >= 0; n -= 3) { //Go through each triangle, if it can be seen, delete it and use maintainEdge on its edges. if (IsTriangleVisibleFromPoint(outputTriangleIndices, points, n, ref maximum)) { //This triangle can see it! //TODO: CONSIDER CONSISTENT WINDING HAPPYTIMES MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 1], edges); MaintainEdge(outputTriangleIndices[n], outputTriangleIndices[n + 2], edges); MaintainEdge(outputTriangleIndices[n + 1], outputTriangleIndices[n + 2], edges); //Because fast removals are being used, the order is very important. //It's pulling indices in from the end of the list in order, and also ensuring //that we never issue a removal order beyond the end of the list. outputTriangleIndices.FastRemoveAt(n + 2); outputTriangleIndices.FastRemoveAt(n + 1); outputTriangleIndices.FastRemoveAt(n); } } //Create new triangles. for (int n = 0; n < edges.Count; n += 2) { //For each edge, create a triangle with the extreme point. newTriangles.Add(edges[n]); newTriangles.Add(edges[n + 1]); newTriangles.Add(maxIndex); } //Only verify the windings of the new triangles. VerifyWindings(newTriangles, points, ref insidePoint); outputTriangleIndices.AddRange(newTriangles); newTriangles.Clear(); //Remove all points from the outsidePoints if they are inside the polyhedron RemoveInsidePoints(points, outputTriangleIndices, outsidePoints); //The list has been significantly messed with, so restart the loop. break; } } } CommonResources.GiveBack(outsidePoints); CommonResources.GiveBack(edges); CommonResources.GiveBack(toRemove); CommonResources.GiveBack(newTriangles); } private static void MaintainEdge(int a, int b, RawList edges) { bool contained = false; int index = 0; for (int k = 0; k < edges.Count; k += 2) { if ((edges[k] == a && edges[k + 1] == b) || (edges[k] == b && edges[k + 1] == a)) { contained = true; index = k; } } //If it isn't present, add it to the edge list. if (!contained) { edges.Add(a); edges.Add(b); } else { //If it is present, that means both edge-connected triangles were deleted now, so get rid of it. edges.FastRemoveAt(index + 1); edges.FastRemoveAt(index); } } private static int GetExtremePoint(ref Vector3 direction, RawList points, RawList outsidePoints) { float maximumDot = -float.MaxValue; int extremeIndex = 0; for (int i = 0; i < outsidePoints.Count; ++i) { float dot; Vector3.Dot(ref points.Elements[outsidePoints[i]], ref direction, out dot); if (dot > maximumDot) { maximumDot = dot; extremeIndex = i; } } return extremeIndex; } private static void GetExtremePoints(ref Vector3 direction, RawList points, out float maximumDot, out float minimumDot, out int maximumIndex, out int minimumIndex) { maximumIndex = 0; minimumIndex = 0; float dot; Vector3.Dot(ref points.Elements[0], ref direction, out dot); minimumDot = dot; maximumDot = dot; for (int i = 1; i < points.Count; ++i) { Vector3.Dot(ref points.Elements[i], ref direction, out dot); if (dot > maximumDot) { maximumDot = dot; maximumIndex = i; } else if (dot < minimumDot) { minimumDot = dot; minimumIndex = i; } } } private static void ComputeInitialTetrahedron(RawList points, RawList outsidePointCandidates, RawList triangleIndices, out Vector3 centroid) { //Find four points on the hull. //We'll start with using the x axis to identify two points on the hull. int a, b, c, d; Vector3 direction; //Find the extreme points along the x axis. float minimumX = float.MaxValue, maximumX = -float.MaxValue; int minimumXIndex = 0, maximumXIndex = 0; for (int i = 0; i < points.Count; ++i) { var v = points.Elements[i]; if (v.X > maximumX) { maximumX = v.X; maximumXIndex = i; } else if (v.X < minimumX) { minimumX = v.X; minimumXIndex = i; } } a = minimumXIndex; b = maximumXIndex; //Check for redundancies.. if (a == b) throw new ArgumentException("Point set is degenerate; convex hulls must have volume."); //Now, use a second axis perpendicular to the two points we found. Vector3 ab; Vector3.Subtract(ref points.Elements[b], ref points.Elements[a], out ab); Vector3.Cross(ref ab, ref Toolbox.UpVector, out direction); if (direction.LengthSquared() < Toolbox.Epsilon) Vector3.Cross(ref ab, ref Toolbox.RightVector, out direction); float minimumDot, maximumDot; int minimumIndex, maximumIndex; GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex); //Compare the location of the extreme points to the location of the axis. float dot; Vector3.Dot(ref direction, ref points.Elements[a], out dot); //Use the point further from the axis. if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot)) { //In this case, we should use the minimum index. c = minimumIndex; } else { //In this case, we should use the maximum index. c = maximumIndex; } //Check for redundancies.. if (a == c || b == c) throw new ArgumentException("Point set is degenerate; convex hulls must have volume."); //Use a third axis perpendicular to the plane defined by the three unique points a, b, and c. Vector3 ac; Vector3.Subtract(ref points.Elements[c], ref points.Elements[a], out ac); Vector3.Cross(ref ab, ref ac, out direction); GetExtremePoints(ref direction, points, out maximumDot, out minimumDot, out maximumIndex, out minimumIndex); //Compare the location of the extreme points to the location of the plane. Vector3.Dot(ref direction, ref points.Elements[a], out dot); //Use the point further from the plane. if (Math.Abs(dot - minimumDot) > Math.Abs(dot - maximumDot)) { //In this case, we should use the minimum index. d = minimumIndex; } else { //In this case, we should use the maximum index. d = maximumIndex; } //Check for redundancies.. if (a == d || b == d || c == d) throw new ArgumentException("Point set is degenerate; convex hulls must have volume."); //Add the triangles. triangleIndices.Add(a); triangleIndices.Add(b); triangleIndices.Add(c); triangleIndices.Add(a); triangleIndices.Add(b); triangleIndices.Add(d); triangleIndices.Add(a); triangleIndices.Add(c); triangleIndices.Add(d); triangleIndices.Add(b); triangleIndices.Add(c); triangleIndices.Add(d); //The centroid is guaranteed to be within the convex hull. It will be used to verify the windings of triangles throughout the hull process. Vector3.Add(ref points.Elements[a], ref points.Elements[b], out centroid); Vector3.Add(ref centroid, ref points.Elements[c], out centroid); Vector3.Add(ref centroid, ref points.Elements[d], out centroid); Vector3.Multiply(ref centroid, 0.25f, out centroid); for (int i = 0; i < triangleIndices.Count; i += 3) { var vA = points.Elements[triangleIndices.Elements[i]]; var vB = points.Elements[triangleIndices.Elements[i + 1]]; var vC = points.Elements[triangleIndices.Elements[i + 2]]; //Check the signed volume of a parallelepiped with the edges of this triangle and the centroid. Vector3 cross; Vector3.Subtract(ref vB, ref vA, out ab); Vector3.Subtract(ref vC, ref vA, out ac); Vector3.Cross(ref ac, ref ab, out cross); Vector3 offset; Vector3.Subtract(ref vA, ref centroid, out offset); float volume; Vector3.Dot(ref offset, ref cross, out volume); //This volume/cross product could also be used to check for degeneracy, but we already tested for that. if (Math.Abs(volume) < Toolbox.BigEpsilon) { throw new ArgumentException("Point set is degenerate; convex hulls must have volume."); } if (volume < 0) { //If the signed volume is negative, that means the triangle's winding is opposite of what we want. //Flip it around! var temp = triangleIndices.Elements[i]; triangleIndices.Elements[i] = triangleIndices.Elements[i + 1]; triangleIndices.Elements[i + 1] = temp; } } //Points which belong to the tetrahedra are guaranteed to be 'in' the convex hull. Do not allow them to be considered. var tetrahedronIndices = CommonResources.GetIntList(); tetrahedronIndices.Add(a); tetrahedronIndices.Add(b); tetrahedronIndices.Add(c); tetrahedronIndices.Add(d); //Sort the indices to allow a linear time loop. Array.Sort(tetrahedronIndices.Elements, 0, 4); int tetrahedronIndex = 0; for (int i = 0; i < points.Count; ++i) { if (tetrahedronIndex < 4 && i == tetrahedronIndices[tetrahedronIndex]) { //Don't add a tetrahedron index. Now that we've found this index, though, move on to the next one. ++tetrahedronIndex; } else { outsidePointCandidates.Add(i); } } CommonResources.GiveBack(tetrahedronIndices); } private static void RemoveInsidePoints(RawList points, RawList triangleIndices, RawList outsidePoints) { var insidePoints = CommonResources.GetIntList(); //We're going to remove points from this list as we go to prune it down to the truly inner points. insidePoints.AddRange(outsidePoints); outsidePoints.Clear(); for (int i = 0; i < triangleIndices.Count && insidePoints.Count > 0; i += 3) { //Compute the triangle's plane in point-normal representation to test other points against. Vector3 normal; FindNormal(triangleIndices, points, i, out normal); Vector3 p = points.Elements[triangleIndices.Elements[i]]; for (int j = insidePoints.Count - 1; j >= 0; --j) { //Offset from the triangle to the current point, tested against the normal, determines if the current point is visible //from the triangle face. Vector3 offset; Vector3.Subtract(ref points.Elements[insidePoints.Elements[j]], ref p, out offset); float dot; Vector3.Dot(ref offset, ref normal, out dot); //If it's visible, then it's outside! if (dot > 0) { //This point is known to be on the outside; put it on the outside! outsidePoints.Add(insidePoints.Elements[j]); insidePoints.FastRemoveAt(j); } } } CommonResources.GiveBack(insidePoints); } private static void FindNormal(RawList indices, RawList points, int triangleIndex, out Vector3 normal) { var a = points.Elements[indices.Elements[triangleIndex]]; Vector3 ab, ac; Vector3.Subtract(ref points.Elements[indices.Elements[triangleIndex + 1]], ref a, out ab); Vector3.Subtract(ref points.Elements[indices.Elements[triangleIndex + 2]], ref a, out ac); Vector3.Cross(ref ac, ref ab, out normal); } private static bool IsTriangleVisibleFromPoint(RawList indices, RawList points, int triangleIndex, ref Vector3 point) { //Compute the normal of the triangle. var a = points.Elements[indices.Elements[triangleIndex]]; Vector3 ab, ac; Vector3.Subtract(ref points.Elements[indices.Elements[triangleIndex + 1]], ref a, out ab); Vector3.Subtract(ref points.Elements[indices.Elements[triangleIndex + 2]], ref a, out ac); Vector3 normal; Vector3.Cross(ref ac, ref ab, out normal); //Assume a consistent winding. Check to see if the normal points at the point. Vector3 offset; Vector3.Subtract(ref point, ref a, out offset); float dot; Vector3.Dot(ref offset, ref normal, out dot); return dot >= 0; } private static void VerifyWindings(RawList newIndices, RawList points, ref Vector3 centroid) { //Go through every triangle. for (int k = 0; k < newIndices.Count; k += 3) { //Check if the triangle faces away or towards the centroid. if (IsTriangleVisibleFromPoint(newIndices, points, k, ref centroid)) { //If it's towards, flip the winding. int temp = newIndices[k + 1]; newIndices[k + 1] = newIndices[k + 2]; newIndices[k + 2] = temp; } } } } } ================================================ FILE: BEPUutilities/DataStructures/ConcurrentDeque.cs ================================================ using System; namespace BEPUutilities.DataStructures { /// /// Locked queue supporting dequeues from both ends. /// /// Type of contained elements. public class ConcurrentDeque { private readonly SpinLock locker = new SpinLock(); internal T[] array; private int count; internal int firstIndex; internal int lastIndex = -1; public ConcurrentDeque(int capacity) { array = new T[capacity]; } public ConcurrentDeque() : this(16) { } /// /// Number of elements in the deque. /// public int Count { get { return count; } } public override string ToString() { return "Count: " + count; } //TODO: SPEED UP THESE ENQUEUES! /// /// Enqueues an element to the tail of the queue with locking. /// /// Dequeued element, if any. /// True if an element could be dequeued, false otherwise. public void Enqueue(T item) { //bool taken = false; //locker.Enter(ref taken); locker.Enter(); try { //Enqueues go to the tail only; it's like a queue. //head ----> tail if (count == array.Length) { //Resize //TODO: Better shift-resize T[] oldArray = array; array = new T[Math.Max(4, oldArray.Length * 2)]; //Copy the old first-end to the first part of the new array. Array.Copy(oldArray, firstIndex, array, 0, oldArray.Length - firstIndex); //Copy the old begin-first to the second part of the new array. Array.Copy(oldArray, 0, array, oldArray.Length - firstIndex, firstIndex); firstIndex = 0; lastIndex = count - 1; } lastIndex++; if (lastIndex == array.Length) lastIndex = 0; array[lastIndex] = item; count++; } finally { locker.Exit(); //locker.Exit(); } } /// /// Tries to dequeue the first element of the queue with locking. /// /// Dequeued element, if any. /// True if an element could be dequeued, false otherwise. public bool TryDequeueFirst(out T item) { //bool taken = false; //locker.Enter(ref taken); locker.Enter(); try { if (count > 0) { item = array[firstIndex]; array[firstIndex] = default(T); firstIndex++; if (firstIndex == array.Length) firstIndex = 0; count--; return true; } item = default(T); return false; } finally { locker.Exit(); //locker.Exit(); } } /// /// Tries to dequeue the last element of the queue with locking. /// /// Dequeued element, if any. /// True if an element could be dequeued, false otherwise. public bool TryDequeueLast(out T item) { //bool taken = false; //locker.Enter(ref taken); locker.Enter(); try { if (count > 0) { item = array[lastIndex]; array[lastIndex] = default(T); lastIndex--; if (lastIndex < 0) lastIndex += array.Length; count--; return true; } item = default(T); return false; } finally { locker.Exit(); //locker.Exit(); } } /// /// Tries to dequeue the first element of the queue without locking. /// /// Dequeued element, if any. /// True if an element could be dequeued, false otherwise. public bool TryUnsafeDequeueFirst(out T item) { if (count > 0) { item = array[firstIndex]; array[firstIndex] = default(T); firstIndex++; if (firstIndex == array.Length) firstIndex = 0; count--; return true; } item = default(T); return false; } /// /// Tries to dequeue the last element of the queue without locking. /// /// Dequeued element, if any. /// True if an element could be dequeued, false otherwise. public bool TryUnsafeDequeueLast(out T item) { if (count > 0) { item = array[lastIndex]; array[lastIndex] = default(T); lastIndex--; if (lastIndex < 0) lastIndex += array.Length; count--; return true; } item = default(T); return false; } /// /// Enqueues an element onto the tail of the deque without locking. /// /// Element to enqueue. public void UnsafeEnqueue(T item) { //Enqueues go to the tail only; it's like a queue. //head ----> tail if (count == array.Length) { //TODO: if it's always powers of 2, then resizing is quicker. //Resize T[] oldArray = array; array = new T[oldArray.Length * 2]; //Copy the old first-end to the first part of the new array. Array.Copy(oldArray, firstIndex, array, 0, oldArray.Length - firstIndex); //Copy the old begin-first to the second part of the new array. Array.Copy(oldArray, 0, array, oldArray.Length - firstIndex, firstIndex); firstIndex = 0; lastIndex = count - 1; } lastIndex++; if (lastIndex == array.Length) lastIndex = 0; array[lastIndex] = item; count++; } } } ================================================ FILE: BEPUutilities/DataStructures/HashSet.cs ================================================ #if !WINDOWS using System.Collections; using System.Collections.Generic; namespace BEPUutilities.DataStructures { /// /// Provides basic .NET 3.5 HashSet functionality on non-Windows platforms. /// public class HashSet : IEnumerable { //TODO: A proper implementation of a HashSet would be nice. Dictionary fakeSet; /// /// Constructs a new HashSet. /// public HashSet() { fakeSet = new Dictionary(); } IEnumerator IEnumerable.GetEnumerator() { return fakeSet.Keys.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return fakeSet.Keys.GetEnumerator(); } Dictionary.KeyCollection.Enumerator GetEnumerator() { return fakeSet.Keys.GetEnumerator(); } /// /// Adds an element to the HashSet. /// ///Item to add. ///Whether or not the item could be added. public bool Add(T item) { if (fakeSet.ContainsKey(item)) return false; fakeSet.Add(item, false); return true; } /// /// Removes an element from the HashSet. /// ///Item to remove. ///Whether or not the item could be removed. public bool Remove(T item) { return fakeSet.Remove(item); } /// /// Clears the HashSet. /// public void Clear() { fakeSet.Clear(); } /// /// Determines if the set contains the item. /// /// Item to check for containment. /// Whether or not the item was contained in the set. public bool Contains(T item) { return fakeSet.ContainsKey(item); } } } #endif ================================================ FILE: BEPUutilities/DataStructures/ObservableDictionary.cs ================================================ using System; using System.Collections.Generic; namespace BEPUutilities.DataStructures { /// /// Dictionary that provides events when the inner dictionary is changed. /// ///Type of the keys in the dictionary. ///Type of the values in the dictionary. public class ObservableDictionary { /// /// Gets or sets the dictionary wrapped by the observable dictionary. /// While the inner dictionary can be changed, making modifications to it will /// not trigger any changed events. /// public Dictionary WrappedDictionary { get; private set; } /// /// Constructs a new observable dictionary. /// public ObservableDictionary() { WrappedDictionary = new Dictionary(); } /// /// Adds a pair to the dictionary. /// ///Key of the element. ///Value of the element. public void Add(TKey key, TValue value) { WrappedDictionary.Add(key, value); OnChanged(); } /// /// Removes a key and its associated value from the dictionary, if present. /// ///Key of the element to remove. ///Whether or not the object was found. public bool Remove(TKey key) { if (WrappedDictionary.Remove(key)) { OnChanged(); return true; } return false; } /// /// Clears the dictionary of all elements. /// public void Clear() { WrappedDictionary.Clear(); OnChanged(); } /// /// Fires when the dictionary's elements are changed using the wrapping functions. /// public event Action Changed; protected void OnChanged() { if (Changed != null) { Changed(); } } } } ================================================ FILE: BEPUutilities/DataStructures/ObservableList.cs ================================================ using System; using System.Collections.Generic; using System.Collections; namespace BEPUutilities.DataStructures { /// /// List of objects which fires events when it is changed. /// ///Type of elements in the list. public class ObservableList : IList { /// /// Gets the list wrapped by the observable list. Adds and removes made to this list directly will not trigger events. /// public RawList WrappedList { get; private set; } /// /// Fires when elements in the list are changed. /// public event Action> Changed; /// /// Constructs a new observable list. /// ///List to copy into the internal wrapped list. public ObservableList(IList list) { this.WrappedList = new RawList(list.Count); list.CopyTo(this.WrappedList.Elements, 0); } /// /// Constructs an empty observable list. /// public ObservableList() { WrappedList = new RawList(); } /// /// Constructs an empty observable list with a given capacity. /// ///Initial allocated storage in the list. public ObservableList(int initialCapacity) { WrappedList = new RawList(initialCapacity); } protected void OnChanged() { if (Changed != null) Changed(this); } /// /// Determines the index of a specific item in the . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the . public int IndexOf(T item) { return WrappedList.IndexOf(item); } /// /// Inserts an item to the at the specified index. /// /// The zero-based index at which should be inserted.The object to insert into the . is not a valid index in the .The is read-only. public void Insert(int index, T item) { WrappedList.Insert(index, item); } /// /// Removes the item at the specified index. /// /// The zero-based index of the item to remove. is not a valid index in the .The is read-only. public void RemoveAt(int index) { WrappedList.RemoveAt(index); } /// /// Gets or sets the element at the specified index. /// /// /// The element at the specified index. /// /// The zero-based index of the element to get or set. is not a valid index in the .The property is set and the is read-only. public T this[int index] { get { return WrappedList[index]; } set { WrappedList[index] = value; OnChanged(); } } /// /// Adds an item to the . /// /// The object to add to the .The is read-only. public void Add(T item) { WrappedList.Add(item); OnChanged(); } /// /// Removes all items from the . /// /// The is read-only. public void Clear() { WrappedList.Clear(); OnChanged(); } /// /// Determines whether the contains a specific value. /// /// /// true if is found in the ; otherwise, false. /// /// The object to locate in the . public bool Contains(T item) { return WrappedList.Contains(item); } /// /// Copies the elements of the to an , starting at a particular index. /// /// The one-dimensional that is the destination of the elements copied from . The must have zero-based indexing.The zero-based index in at which copying begins. is null. is less than 0. is multidimensional.-or-The number of elements in the source is greater than the available space from to the end of the destination .-or-Type cannot be cast automatically to the type of the destination . public void CopyTo(T[] array, int arrayIndex) { Array.Copy(WrappedList.Elements, 0, array, 0, WrappedList.Count); } /// /// Gets the number of elements contained in the . /// /// /// The number of elements contained in the . /// public int Count { get { return WrappedList.Count; } } bool ICollection.IsReadOnly { get { return (WrappedList as ICollection).IsReadOnly; } } /// /// Removes the first occurrence of a specific object from the . /// /// /// true if was successfully removed from the ; otherwise, false. This method also returns false if is not found in the original . /// /// The object to remove from the .The is read-only. public bool Remove(T item) { if (WrappedList.Remove(item)) { OnChanged(); return true; } return false; } /// /// Gets an enumerator for the list. /// ///Enumerator for the list. public RawList.Enumerator GetEnumerator() { return WrappedList.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return WrappedList.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return WrappedList.GetEnumerator(); } } } ================================================ FILE: BEPUutilities/DataStructures/RawList.cs ================================================ using System; using System.Collections.Generic; namespace BEPUutilities.DataStructures { /// /// No-frills list that wraps an accessible array. /// ///Type of elements contained by the list. public class RawList : IList { /// /// Direct access to the elements owned by the raw list. /// Be careful about the operations performed on this list; /// use the normal access methods if in doubt. /// public T[] Elements; /// /// Gets the number of elements contained in the . /// Can also be set; setting the count is a direct change to the count integer and does not change the state of the array. /// /// /// The number of elements contained in the . /// public int Count { get; set; } /// /// Constructs an empty list. /// public RawList() { Elements = new T[4]; } /// /// Constructs an empty list. /// ///Initial capacity to allocate for the list. ///Thrown when the initial capacity is zero or negative. public RawList(int initialCapacity) { if (initialCapacity <= 0) throw new ArgumentException("Initial capacity must be positive."); Elements = new T[initialCapacity]; } /// /// Constructs a raw list from another list. /// ///List to copy. public RawList(IList elements) : this(Math.Max(elements.Count, 4)) { elements.CopyTo(Elements, 0); Count = elements.Count; } /// /// Removes an element from the list. /// /// Index of the element to remove. public void RemoveAt(int index) { if (index >= Count) { throw new ArgumentOutOfRangeException("index"); } Count--; if (index < Count) Array.Copy(Elements, index + 1, Elements, index, Count - index); Elements[Count] = default(T); } /// /// Removes an element from the list without maintaining order. /// /// Index of the element to remove. public void FastRemoveAt(int index) { if (index >= Count) { throw new ArgumentOutOfRangeException("index"); } Count--; if (index < Count) { Elements[index] = Elements[Count]; } Elements[Count] = default(T); } /// /// Gets or sets the current size allocated for the list. /// public int Capacity { get { return Elements.Length; } set { T[] newArray = new T[value]; Array.Copy(Elements, newArray, Count); Elements = newArray; } } /// /// Adds an item to the . /// /// The object to add to the .The is read-only. public void Add(T item) { if (Count == Elements.Length) { Capacity = Elements.Length * 2; } Elements[Count++] = item; } /// /// Adds a range of elements to the list from another list. /// ///Elements to add. public void AddRange(RawList items) { int neededLength = Count + items.Count; if (neededLength > Elements.Length) { int newLength = Elements.Length * 2; if (newLength < neededLength) newLength = neededLength; Capacity = newLength; } Array.Copy(items.Elements, 0, Elements, Count, items.Count); Count = neededLength; } /// /// Adds a range of elements to the list from another list. /// ///Elements to add. public void AddRange(List items) { int neededLength = Count + items.Count; if (neededLength > Elements.Length) { int newLength = Elements.Length * 2; if (newLength < neededLength) newLength = neededLength; Capacity = newLength; } items.CopyTo(0, Elements, Count, items.Count); Count = neededLength; } /// /// Adds a range of elements to the list from another list. /// ///Elements to add. public void AddRange(IList items) { int neededLength = Count + items.Count; if (neededLength > Elements.Length) { int newLength = Elements.Length * 2; if (newLength < neededLength) newLength = neededLength; Capacity = newLength; } items.CopyTo(Elements, 0); Count = neededLength; } /// /// Removes all items from the . /// /// The is read-only. public void Clear() { Array.Clear(Elements, 0, Count); Count = 0; } /// /// Removes the first occurrence of a specific object from the . /// /// /// true if was successfully removed from the ; otherwise, false. This method also returns false if is not found in the original . /// /// The object to remove from the .The is read-only. public bool Remove(T item) { int index = IndexOf(item); if (index == -1) return false; RemoveAt(index); return true; } /// /// Removes the first occurrence of a specific object from the collection without maintaining element order. /// /// /// true if was successfully removed from the ; otherwise, false. This method also returns false if is not found in the original . /// /// The object to remove from the .The is read-only. public bool FastRemove(T item) { int index = IndexOf(item); if (index == -1) return false; FastRemoveAt(index); return true; } /// /// Determines the index of a specific item in the . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the . public int IndexOf(T item) { return Array.IndexOf(Elements, item, 0, Count); } /// /// Copies the elements from the list into an array. /// /// An array containing the elements in the list. public T[] ToArray() { var toReturn = new T[Count]; Array.Copy(Elements, toReturn, Count); return toReturn; } #region IList Members /// /// Inserts the element at the specified index. /// /// Index to insert the item. /// Element to insert. public void Insert(int index, T item) { if (index < Count) { if (Count == Elements.Length) { Capacity = Elements.Length * 2; } Array.Copy(Elements, index, Elements, index + 1, Count - index); Elements[index] = item; Count++; } else Add(item); } /// /// Inserts the element at the specified index without maintaining list order. /// /// Index to insert the item. /// Element to insert. public void FastInsert(int index, T item) { if (index < Count) { if (Count == Elements.Length) { Capacity = Elements.Length * 2; } Array.Copy(Elements, index, Elements, index + 1, Count - index); Elements[Count] = Elements[index]; Elements[index] = item; Count++; } else Add(item); } /// /// Gets or sets the element of the list at the given index. /// /// Index in the list. /// Element at the given index. public T this[int index] { get { if (index < Count && index >= 0) return Elements[index]; else throw new IndexOutOfRangeException("Index is outside of the list's bounds."); } set { if (index < Count && index >= 0) Elements[index] = value; else throw new IndexOutOfRangeException("Index is outside of the list's bounds."); } } #endregion #region ICollection Members /// /// Determines if an item is present in the list. /// /// Item to be tested. /// Whether or not the item was contained by the list. public bool Contains(T item) { return IndexOf(item) != -1; } /// /// Copies the list's contents to the array. /// /// Array to receive the list's contents. /// Index in the array to start the dump. public void CopyTo(T[] array, int arrayIndex) { Array.Copy(Elements, 0, array, arrayIndex, Count); } bool ICollection.IsReadOnly { get { return false; } } #endregion #region IEnumerable Members /// /// Gets an enumerator for the list. /// ///Enumerator for the list. public Enumerator GetEnumerator() { return new Enumerator(this); } IEnumerator IEnumerable.GetEnumerator() { return new Enumerator(this); } #endregion #region IEnumerable Members System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return new Enumerator(this); } #endregion /// /// Sorts the list. /// ///Comparer to use to sort the list. public void Sort(IComparer comparer) { Array.Sort(Elements, 0, Count, comparer); } /// /// Enumerator for the RawList. /// public struct Enumerator : IEnumerator { RawList list; int index; /// /// Constructs a new enumerator. /// /// public Enumerator(RawList list) { index = -1; this.list = list; } public T Current { get { return list.Elements[index]; } } public void Dispose() { } object System.Collections.IEnumerator.Current { get { return list.Elements[index]; } } public bool MoveNext() { return ++index < list.Count; } public void Reset() { index = -1; } } } } ================================================ FILE: BEPUutilities/DataStructures/RawValueList.cs ================================================ using System; namespace BEPUutilities.DataStructures { //THIS SHOULD ONLY BE USED ON VALUE TYPES WHICH HAVE NO REFERENCES TO HEAP OBJECTS. /// /// No-frills list used for value types that contain no reference types. /// ///Type of the elements in the list. public class RawValueList where T : struct { /// /// Directly accessible array of elements in the list. /// Be careful about which operations are applied to the array; /// if in doubt, use the regular access methods. /// public T[] Elements; /// /// Gets the number of elements contained in the . /// Can also be set; setting the count is a direct change to the count integer and does not change the state of the array. /// /// /// The number of elements contained in the . /// public int Count { get; set; } /// /// Constructs an empty list. /// public RawValueList() { Elements = new T[4]; } /// /// Constructs an empty list. /// ///Initial capacity of the list. ///Thrown when the initial capcity is less than or equal to zero. public RawValueList(int initialCapacity) { if (initialCapacity <= 0) throw new ArgumentException("Initial capacity must be positive."); Elements = new T[initialCapacity]; } /// /// Removes an element from the list. /// ///Index of the element to remove. ///Thrown when the index is not present in the list. public void RemoveAt(int index) { if (index >= Count) { throw new ArgumentOutOfRangeException("index"); } Count--; if (index < Count) { Elements[index] = Elements[Count]; } } /// /// Gets or sets the current size allocated for the list. /// public int Capacity { get { return Elements.Length; } set { T[] newArray = new T[value]; Array.Copy(Elements, newArray, Count); Elements = newArray; } } /// /// Adds an element to the list. /// ///Item to add. public void Add(ref T item) { if (Count == Elements.Length) { Capacity = Elements.Length * 2; } Elements[Count++] = item; } /// /// Clears the list of all elements. /// public void Clear() { Count = 0; } /// /// Removes an element from the list. /// ///Item to remove. ///Whether or not the item was present in the list. public bool Remove(ref T item) { int index = IndexOf(ref item); if (index == -1) return false; RemoveAt(index); return true; } /// /// Gets the index of an element in the list. /// ///Item to search for. ///Index of the searched element. public int IndexOf(ref T item) { return Array.IndexOf(Elements, item); } } } ================================================ FILE: BEPUutilities/DataStructures/ReadOnlyDictionary.cs ================================================ using System.Collections; using System.Collections.Generic; namespace BEPUutilities.DataStructures { //TODO: This could be handled better. /// /// Wraps a dictionary in a read only collection. /// ///Type of keys in the dictionary. ///Type of values in the dictionary. public struct ReadOnlyDictionary : IEnumerable> { private readonly IDictionary dictionary; /// /// Constructs a new read-only wrapper dictionary. /// /// Internal dictionary to use. public ReadOnlyDictionary(IDictionary dictionary) { this.dictionary = dictionary; } /// /// Gets the number of elements in the dictionary. /// public int Count { get { return dictionary.Count; } } /// /// Gets whether or not this dictionary is read-only. /// public bool IsReadOnly { get { return true; } } /// /// Gets the value associated with the key in the dictionary. /// /// Key to look for in the dictionary. /// Value associated with the key. public TValue this[TKey key] { get { return dictionary[key]; } } /// /// Gets an enumerable set of keys in the dictionary. /// public IEnumerable Keys { get { return new ReadOnlyEnumerable(dictionary.Keys); } } /// /// Gets an enumerable set of values in the dictionary. /// public IEnumerable Values { get { return new ReadOnlyEnumerable(dictionary.Values); } } #region IEnumerable> Members /// /// Gets an enumerator for key-value pairs in the dictionary. /// /// Enumerator for the dictionary. IEnumerator> IEnumerable>.GetEnumerator() { return dictionary.GetEnumerator(); } /// /// Gets an enumerator for key-value pairs in the dictionary. /// /// Enumerator for the dictionary. IEnumerator IEnumerable.GetEnumerator() { return dictionary.GetEnumerator(); } #endregion /// /// Determines if the dictionary contains a key-value pair. /// /// Key-value pair to look for. /// Whether or not the key-value pair is present. public bool Contains(KeyValuePair item) { return dictionary.Contains(item); } /// /// Determines if the dictionary contains a given key. /// /// Key to check for. /// Whether or not the key is contained. public bool ContainsKey(TKey key) { return dictionary.ContainsKey(key); } /// /// Copies the key-value pairs of the dictionary into an array. /// /// Target array. /// The zero-based index at which copying begins. public void CopyTo(KeyValuePair[] array, int arrayIndex) { dictionary.CopyTo(array, arrayIndex); } /// /// Tries to retrieve a value from the dictionary using a key. /// /// Key to look for. /// Value associated with the key. /// Whether or not the key exists. public bool TryGetValue(TKey key, out TValue value) { return dictionary.TryGetValue(key, out value); } } } ================================================ FILE: BEPUutilities/DataStructures/ReadOnlyEnumerable.cs ================================================ using System.Collections; using System.Collections.Generic; namespace BEPUutilities.DataStructures { /// /// WRaps an enumerable in a temporary enumeration struct. /// ///Type of the enumerable being iterated. public struct ReadOnlyEnumerable : IEnumerable { private readonly IEnumerable enumerable; /// /// Constructs a new read only enumerable. /// ///Enumerable to wrap. public ReadOnlyEnumerable(IEnumerable enumerable) { this.enumerable = enumerable; } #region IEnumerable Members /// /// Returns an enumerator that iterates through the collection. /// /// /// A that can be used to iterate through the collection. /// /// 1 public IEnumerator GetEnumerator() { return enumerable.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return enumerable.GetEnumerator(); } #endregion } } ================================================ FILE: BEPUutilities/DataStructures/ReadOnlyList.cs ================================================ using System; using System.Collections.Generic; namespace BEPUutilities.DataStructures { /// /// Wraps a list in an enumerable struct. /// /// Type contained by the list. public struct ReadOnlyList : IList { IList wrappedList; /// /// Constructs a new read-only list. /// /// List wrapped by the read-only list. public ReadOnlyList(IList wrappedList) { this.wrappedList = wrappedList; } /// /// Determines the index of a specific item in the . /// /// /// The index of if found in the list; otherwise, -1. /// /// The object to locate in the . public int IndexOf(T item) { return wrappedList.IndexOf(item); } void IList.Insert(int index, T item) { throw new NotSupportedException("The list is read-only."); } void IList.RemoveAt(int index) { throw new NotSupportedException("The list is read-only."); } /// /// Gets the element at the specified index. /// /// /// The element at the specified index. /// /// The zero-based index of the element to get. is not a valid index in the .The property is set and the is read-only. public T this[int index] { get { return wrappedList[index]; } set { throw new NotSupportedException("The list is read-only."); } } void ICollection.Add(T item) { throw new NotSupportedException("The list is read-only."); } void ICollection.Clear() { throw new NotSupportedException("The list is read-only."); } /// /// Determines whether the contains a specific value. /// /// /// true if is found in the ; otherwise, false. /// /// The object to locate in the . public bool Contains(T item) { return wrappedList.Contains(item); } /// /// Copies the elements of the to an , starting at a particular index. /// /// The one-dimensional that is the destination of the elements copied from . The must have zero-based indexing.The zero-based index in at which copying begins. is null. is less than 0. is multidimensional.-or-The number of elements in the source is greater than the available space from to the end of the destination .-or-Type cannot be cast automatically to the type of the destination . public void CopyTo(T[] array, int arrayIndex) { wrappedList.CopyTo(array, arrayIndex); } /// /// Gets the number of elements contained in the . /// /// /// The number of elements contained in the . /// public int Count { get { return wrappedList.Count; } } bool ICollection.IsReadOnly { get { return true; } } bool ICollection.Remove(T item) { throw new NotSupportedException("The list is read-only."); } public Enumerator GetEnumerator() { return new Enumerator(wrappedList); } IEnumerator IEnumerable.GetEnumerator() { return wrappedList.GetEnumerator(); } System.Collections.IEnumerator System.Collections.IEnumerable.GetEnumerator() { return wrappedList.GetEnumerator(); } /// /// Enumerates the read only list. /// public struct Enumerator : IEnumerator { private IList wrappedList; int index; /// /// Constructs an enumerator. /// ///Collection to which the enumerator belongs. public Enumerator(IList wrappedList) { this.wrappedList = wrappedList; index = -1; } /// /// Gets the element in the collection at the current position of the enumerator. /// /// /// The element in the collection at the current position of the enumerator. /// public T Current { get { return wrappedList[index]; } } /// /// Performs application-defined tasks associated with freeing, releasing, or resetting unmanaged resources. /// /// 2 public void Dispose() { } object System.Collections.IEnumerator.Current { get { return Current; } } /// /// Advances the enumerator to the next element of the collection. /// /// /// true if the enumerator was successfully advanced to the next element; false if the enumerator has passed the end of the collection. /// /// The collection was modified after the enumerator was created. 2 public bool MoveNext() { return ++index < wrappedList.Count; } /// /// Sets the enumerator to its initial position, which is before the first element in the collection. /// /// The collection was modified after the enumerator was created. 2 public void Reset() { index = -1; } } } } ================================================ FILE: BEPUutilities/DataStructures/TinyList.cs ================================================ using System; namespace BEPUutilities.DataStructures { /// /// Special datatype used for heapless lists without unsafe/stackalloc. /// Designed for object types or reference-sized structs (int, float...). /// Stores a maximum of 8 entries. /// /// Struct type to use. public struct TinyList where T : IEquatable { private T entry1; private T entry2; private T entry3; private T entry4; private T entry5; private T entry6; private T entry7; private T entry8; internal int count; /// /// Gets the current number of elements in the list. /// public int Count { get { return count; } } /// /// Gets the item at the specified index. /// /// Index to retrieve. /// Retrieved item. public T this[int index] { get { //Get if (index > count - 1 || index < 0) { return default(T); } switch (index) { case 0: return entry1; case 1: return entry2; case 2: return entry3; case 3: return entry4; case 4: return entry5; case 5: return entry6; case 6: return entry7; case 7: return entry8; default: //Curious! return default(T); } } set { //Replace if (index > count - 1 || index < 0) { return; } switch (index) { case 0: entry1 = value; break; case 1: entry2 = value; break; case 2: entry3 = value; break; case 3: entry4 = value; break; case 4: entry5 = value; break; case 5: entry6 = value; break; case 6: entry7 = value; break; case 7: entry8 = value; break; } } } /// /// Creates a string representation of the list. /// /// String representation of the list. public override string ToString() { return "TinyList<" + typeof (T) + ">, Count: " + count; } /// /// Tries to add an element to the list. /// /// Item to add. /// Whether or not the item could be added. /// Will return false when the list is full. public bool Add(T item) { switch (count) { case 0: entry1 = item; break; case 1: entry2 = item; break; case 2: entry3 = item; break; case 3: entry4 = item; break; case 4: entry5 = item; break; case 5: entry6 = item; break; case 6: entry7 = item; break; case 7: entry8 = item; break; default: return false; } count++; return true; } /// /// Clears the list. /// public void Clear() { //Things aren't guaranteed to be structs in this list. //It would be bad if we kept a reference around, hidden. count = 0; entry1 = default(T); entry2 = default(T); entry3 = default(T); entry4 = default(T); entry5 = default(T); entry6 = default(T); entry7 = default(T); entry8 = default(T); } /// /// Gets the index of the item in the list, if it is present. /// /// Item to look for. /// Index of the item, if present. -1 otherwise. public int IndexOf(T item) { //This isn't a super fast operation. if (entry1.Equals(item)) return 0; else if (entry2.Equals(item)) return 1; else if (entry3.Equals(item)) return 2; else if (entry4.Equals(item)) return 3; else if (entry5.Equals(item)) return 4; else if (entry6.Equals(item)) return 5; else if (entry7.Equals(item)) return 6; else if (entry8.Equals(item)) return 7; return -1; } /// /// Tries to remove an element from the list. /// /// Item to remove. /// Whether or not the item existed in the list. public bool Remove(T item) { //Identity-based removes aren't a super high priority feature, so can be a little slower. int index = IndexOf(item); if (index != -1) { RemoveAt(index); return true; } return false; } /// /// Removes the item at the specified index. /// /// Index of the element to remove. /// Whether or not the item could be removed. /// Returns false if the index is out of bounds. public bool RemoveAt(int index) { if (index > count - 1 || index < 0) return false; switch (index) { case 0: entry1 = entry2; entry2 = entry3; entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 1: entry2 = entry3; entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 2: entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 3: entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 4: entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 5: entry6 = entry7; entry7 = entry8; break; case 6: entry7 = entry8; break; } count--; return true; } } } ================================================ FILE: BEPUutilities/DataStructures/TinyStructList.cs ================================================ using System; namespace BEPUutilities.DataStructures { /// /// Special datatype used for heapless lists without unsafe/stackalloc. /// Since reference types would require heap-side allocation and /// do not match well with this structure's ref-parameter based access, /// only structs are allowed. /// Stores a maximum of 8 entries. /// /// Struct type to use. public struct TinyStructList where T : struct, IEquatable { private T entry1; private T entry2; private T entry3; private T entry4; private T entry5; private T entry6; private T entry7; private T entry8; internal int count; /// /// Gets the current number of elements in the list. /// public int Count { get { return count; } } /// /// Creates a string representation of the list. /// /// String representation of the list. public override string ToString() { return "TinyStructList<" + typeof (T) + ">, Count: " + count; } /// /// Tries to add an element to the list. /// /// Item to add. /// Whether or not the item could be added. /// Will return false when the list is full. public bool Add(ref T item) { switch (count) { case 0: entry1 = item; break; case 1: entry2 = item; break; case 2: entry3 = item; break; case 3: entry4 = item; break; case 4: entry5 = item; break; case 5: entry6 = item; break; case 6: entry7 = item; break; case 7: entry8 = item; break; default: return false; } count++; return true; } /// /// Clears the list. /// public void Clear() { //Everything is a struct in this kind of list, so not much work to do! count = 0; } /// /// Gets the item at the specified index. /// /// Index to retrieve. /// Retrieved item. /// Whether or not the index was valid. public bool Get(int index, out T item) { if (index > count - 1 || index < 0) { item = default(T); return false; } switch (index) { case 0: item = entry1; return true; case 1: item = entry2; return true; case 2: item = entry3; return true; case 3: item = entry4; return true; case 4: item = entry5; return true; case 5: item = entry6; return true; case 6: item = entry7; return true; case 7: item = entry8; return true; default: //Curious! item = default(T); return false; } } /// /// Gets the index of the item in the list, if it is present. /// /// Item to look for. /// Index of the item, if present. -1 otherwise. public int IndexOf(ref T item) { //This isn't a super fast operation. if (entry1.Equals(item)) return 0; if (entry2.Equals(item)) return 1; if (entry3.Equals(item)) return 2; if (entry4.Equals(item)) return 3; if (entry5.Equals(item)) return 4; if (entry6.Equals(item)) return 5; if (entry7.Equals(item)) return 6; if (entry8.Equals(item)) return 7; return -1; } /// /// Tries to remove an element from the list. /// /// Item to remove. /// Whether or not the item existed in the list. public bool Remove(ref T item) { //Identity-based removes aren't a super high priority feature, so can be a little slower. int index = IndexOf(ref item); if (index != -1) { RemoveAt(index); return true; } return false; } /// /// Removes the item at the specified index. /// /// Index of the element to remove. /// Whether or not the item could be removed. /// Returns false if the index is out of bounds. public bool RemoveAt(int index) { if (index > count - 1 || index < 0) return false; switch (index) { case 0: entry1 = entry2; entry2 = entry3; entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 1: entry2 = entry3; entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 2: entry3 = entry4; entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 3: entry4 = entry5; entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 4: entry5 = entry6; entry6 = entry7; entry7 = entry8; break; case 5: entry6 = entry7; entry7 = entry8; break; case 6: entry7 = entry8; break; } count--; return true; } /// /// Tries to add an element to the list. /// /// Index to replace. /// Item to add. /// Whether or not the item could be replaced. /// Returns false if the index is invalid. public bool Replace(int index, ref T item) { if (index > count - 1 || index < 0) { return false; } switch (index) { case 0: entry1 = item; break; case 1: entry2 = item; break; case 2: entry3 = item; break; case 3: entry4 = item; break; case 4: entry5 = item; break; case 5: entry6 = item; break; case 6: entry7 = item; break; case 7: entry8 = item; break; default: return false; } return true; } } } ================================================ FILE: BEPUutilities/MathChecker.cs ================================================ using System; using System.Diagnostics; using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// Contains conditional extensions to check for bad values in various structures. /// public static class MathChecker { /// /// Checks a single float for validity. Separate from the extension method to avoid throwing exceptions deep in a call tree. /// /// Value to validate. /// True if the value is invalid, false if it is valid. private static bool IsInvalid(float f) { return float.IsNaN(f) || float.IsInfinity(f); } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this float f) { if (IsInvalid(f)) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Vector3 v) { if (IsInvalid(v.LengthSquared())) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Matrix2x2 m) { if (IsInvalid(m.M11) || IsInvalid(m.M12) || IsInvalid(m.M21) || IsInvalid(m.M22)) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Matrix3x2 m) { if (IsInvalid(m.M11) || IsInvalid(m.M12) || IsInvalid(m.M21) || IsInvalid(m.M22) || IsInvalid(m.M31) || IsInvalid(m.M32)) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Matrix2x3 m) { if (IsInvalid(m.M11) || IsInvalid(m.M12) || IsInvalid(m.M13) || IsInvalid(m.M21) || IsInvalid(m.M22) || IsInvalid(m.M23)) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Matrix3x3 m) { m.Right.Validate(); m.Up.Validate(); m.Backward.Validate(); } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Matrix m) { m.Right.Validate(); m.Up.Validate(); m.Backward.Validate(); m.Translation.Validate(); if (IsInvalid(m.M14) || IsInvalid(m.M24) || IsInvalid(m.M34) || IsInvalid(m.M44)) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this Quaternion q) { if (IsInvalid(q.LengthSquared())) { throw new NotFiniteNumberException("Invalid value."); } } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this RigidTransform r) { r.Position.Validate(); r.Orientation.Validate(); } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this AffineTransform a) { a.LinearTransform.Validate(); a.Translation.Validate(); } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this BoundingBox b) { b.Min.Validate(); b.Max.Validate(); } /// /// Checks the value to see if it is a NaN or infinite. If it is, an exception is thrown. /// This is only run when the CHECKMATH symbol is defined. /// [Conditional("CHECKMATH")] public static void Validate(this BoundingSphere b) { b.Center.Validate(); if (IsInvalid(b.Radius)) throw new NotFiniteNumberException("Invalid value."); } } } ================================================ FILE: BEPUutilities/Matrix2x2.cs ================================================  using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// 2 row, 2 column matrix. /// public struct Matrix2x2 { /// /// Value at row 1, column 1 of the matrix. /// public float M11; /// /// Value at row 1, column 2 of the matrix. /// public float M12; /// /// Value at row 2, column 1 of the matrix. /// public float M21; /// /// Value at row 2, column 2 of the matrix. /// public float M22; /// /// Constructs a new 2 row, 2 column matrix. /// /// Value at row 1, column 1 of the matrix. /// Value at row 1, column 2 of the matrix. /// Value at row 2, column 1 of the matrix. /// Value at row 2, column 2 of the matrix. public Matrix2x2(float m11, float m12, float m21, float m22) { M11 = m11; M12 = m12; M21 = m21; M22 = m22; } /// /// Gets the 2x2 identity matrix. /// public static Matrix2x2 Identity { get { return new Matrix2x2(1, 0, 1, 0); } } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix2x2 a, ref Matrix2x2 b, out Matrix2x2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix a, ref Matrix2x2 b, out Matrix2x2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix2x2 a, ref Matrix b, out Matrix2x2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix a, ref Matrix b, out Matrix2x2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Constructs a uniform scaling matrix. /// /// Value to use in the diagonal. /// Scaling matrix. public static void CreateScale(float scale, out Matrix2x2 matrix) { matrix.M11 = scale; matrix.M22 = scale; matrix.M12 = 0; matrix.M21 = 0; } /// /// Inverts the given matix. /// /// Matrix to be inverted. /// Inverted matrix. public static void Invert(ref Matrix2x2 matrix, out Matrix2x2 result) { float determinantInverse = 1 / (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21); float m11 = matrix.M22 * determinantInverse; float m12 = -matrix.M12 * determinantInverse; float m21 = -matrix.M21 * determinantInverse; float m22 = matrix.M11 * determinantInverse; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix2x2 a, ref Matrix2x2 b, out Matrix2x2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix2x2 a, ref Matrix b, out Matrix2x2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix a, ref Matrix2x2 b, out Matrix2x2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix2x3 a, ref Matrix3x2 b, out Matrix2x2 result) { result.M11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; result.M12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; result.M21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; result.M22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; } /// /// Negates every element in the matrix. /// /// Matrix to negate. /// Negated matrix. public static void Negate(ref Matrix2x2 matrix, out Matrix2x2 result) { float m11 = -matrix.M11; float m12 = -matrix.M12; float m21 = -matrix.M21; float m22 = -matrix.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Subtracts the two matrices from each other on a per-element basis. /// /// First matrix to subtract. /// Second matrix to subtract. /// Difference of the two matrices. public static void Subtract(ref Matrix2x2 a, ref Matrix2x2 b, out Matrix2x2 result) { float m11 = a.M11 - b.M11; float m12 = a.M12 - b.M12; float m21 = a.M21 - b.M21; float m22 = a.M22 - b.M22; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; } /// /// Transforms the vector by the matrix. /// /// Vector2 to transform. /// Matrix to use as the transformation. /// Product of the transformation. public static void Transform(ref Vector2 v, ref Matrix2x2 matrix, out Vector2 result) { float vX = v.X; float vY = v.Y; #if !WINDOWS result = new Vector2(); #endif result.X = vX * matrix.M11 + vY * matrix.M21; result.Y = vX * matrix.M12 + vY * matrix.M22; } /// /// Computes the transposed matrix of a matrix. /// /// Matrix to transpose. /// Transposed matrix. public static void Transpose(ref Matrix2x2 matrix, out Matrix2x2 result) { float m21 = matrix.M12; result.M11 = matrix.M11; result.M12 = matrix.M21; result.M21 = m21; result.M22 = matrix.M22; } /// /// Creates a string representation of the matrix. /// /// A string representation of the matrix. public override string ToString() { return "{" + M11 + ", " + M12 + "} " + "{" + M21 + ", " + M22 + "}"; } /// /// Calculates the determinant of the matrix. /// /// The matrix's determinant. public float Determinant() { return M11 * M22 - M12 * M21; } } } ================================================ FILE: BEPUutilities/Matrix2x3.cs ================================================  using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// 2 row, 3 column matrix. /// public struct Matrix2x3 { /// /// Value at row 1, column 1 of the matrix. /// public float M11; /// /// Value at row 1, column 2 of the matrix. /// public float M12; /// /// Value at row 1, column 2 of the matrix. /// public float M13; /// /// Value at row 2, column 1 of the matrix. /// public float M21; /// /// Value at row 2, column 2 of the matrix. /// public float M22; /// /// Value at row 2, column 3 of the matrix. /// public float M23; /// /// Constructs a new 2 row, 2 column matrix. /// /// Value at row 1, column 1 of the matrix. /// Value at row 1, column 2 of the matrix. /// Value at row 1, column 3 of the matrix. /// Value at row 2, column 1 of the matrix. /// Value at row 2, column 2 of the matrix. /// Value at row 2, column 3 of the matrix. public Matrix2x3(float m11, float m12, float m13, float m21, float m22, float m23) { M11 = m11; M12 = m12; M13 = m13; M21 = m21; M22 = m22; M23 = m23; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix2x3 a, ref Matrix2x3 b, out Matrix2x3 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m13 = a.M13 + b.M13; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m23 = a.M23 + b.M23; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix2x3 a, ref Matrix3x3 b, out Matrix2x3 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix2x3 a, ref Matrix b, out Matrix2x3 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; } /// /// Negates every element in the matrix. /// /// Matrix to negate. /// Negated matrix. public static void Negate(ref Matrix2x3 matrix, out Matrix2x3 result) { float m11 = -matrix.M11; float m12 = -matrix.M12; float m13 = -matrix.M13; float m21 = -matrix.M21; float m22 = -matrix.M22; float m23 = -matrix.M23; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; } /// /// Subtracts the two matrices from each other on a per-element basis. /// /// First matrix to subtract. /// Second matrix to subtract. /// Difference of the two matrices. public static void Subtract(ref Matrix2x3 a, ref Matrix2x3 b, out Matrix2x3 result) { float m11 = a.M11 - b.M11; float m12 = a.M12 - b.M12; float m13 = a.M13 - b.M13; float m21 = a.M21 - b.M21; float m22 = a.M22 - b.M22; float m23 = a.M23 - b.M23; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; } /// /// Transforms the vector by the matrix. /// /// Vector2 to transform. Considered to be a row vector for purposes of multiplication. /// Matrix to use as the transformation. /// Row vector product of the transformation. public static void Transform(ref Vector2 v, ref Matrix2x3 matrix, out Vector3 result) { #if !WINDOWS result = new Vector3(); #endif result.X = v.X * matrix.M11 + v.Y * matrix.M21; result.Y = v.X * matrix.M12 + v.Y * matrix.M22; result.Z = v.X * matrix.M13 + v.Y * matrix.M23; } /// /// Transforms the vector by the matrix. /// /// Vector2 to transform. Considered to be a column vector for purposes of multiplication. /// Matrix to use as the transformation. /// Column vector product of the transformation. public static void Transform(ref Vector3 v, ref Matrix2x3 matrix, out Vector2 result) { #if !WINDOWS result = new Vector2(); #endif result.X = matrix.M11 * v.X + matrix.M12 * v.Y + matrix.M13 * v.Z; result.Y = matrix.M21 * v.X + matrix.M22 * v.Y + matrix.M23 * v.Z; } /// /// Computes the transposed matrix of a matrix. /// /// Matrix to transpose. /// Transposed matrix. public static void Transpose(ref Matrix2x3 matrix, out Matrix3x2 result) { result.M11 = matrix.M11; result.M12 = matrix.M21; result.M21 = matrix.M12; result.M22 = matrix.M22; result.M31 = matrix.M13; result.M32 = matrix.M23; } /// /// Creates a string representation of the matrix. /// /// A string representation of the matrix. public override string ToString() { return "{" + M11 + ", " + M12 + ", " + M13 + "} " + "{" + M21 + ", " + M22 + ", " + M23 + "}"; } } } ================================================ FILE: BEPUutilities/Matrix3x2.cs ================================================  using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// 3 row, 2 column matrix. /// public struct Matrix3x2 { /// /// Value at row 1, column 1 of the matrix. /// public float M11; /// /// Value at row 1, column 2 of the matrix. /// public float M12; /// /// Value at row 2, column 1 of the matrix. /// public float M21; /// /// Value at row 2, column 2 of the matrix. /// public float M22; /// /// Value at row 3, column 1 of the matrix. /// public float M31; /// /// Value at row 3, column 2 of the matrix. /// public float M32; /// /// Constructs a new 3 row, 2 column matrix. /// /// Value at row 1, column 1 of the matrix. /// Value at row 1, column 2 of the matrix. /// Value at row 2, column 1 of the matrix. /// Value at row 2, column 2 of the matrix. /// Value at row 2, column 1 of the matrix. /// Value at row 2, column 2 of the matrix. public Matrix3x2(float m11, float m12, float m21, float m22, float m31, float m32) { M11 = m11; M12 = m12; M21 = m21; M22 = m22; M31 = m31; M32 = m32; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix3x2 a, ref Matrix3x2 b, out Matrix3x2 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m31 = a.M31 + b.M31; float m32 = a.M32 + b.M32; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; result.M31 = m31; result.M32 = m32; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix3x3 a, ref Matrix3x2 b, out Matrix3x2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; result.M31 = resultM31; result.M32 = resultM32; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix a, ref Matrix3x2 b, out Matrix3x2 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; result.M11 = resultM11; result.M12 = resultM12; result.M21 = resultM21; result.M22 = resultM22; result.M31 = resultM31; result.M32 = resultM32; } /// /// Negates every element in the matrix. /// /// Matrix to negate. /// Negated matrix. public static void Negate(ref Matrix3x2 matrix, out Matrix3x2 result) { float m11 = -matrix.M11; float m12 = -matrix.M12; float m21 = -matrix.M21; float m22 = -matrix.M22; float m31 = -matrix.M31; float m32 = -matrix.M32; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; result.M31 = m31; result.M32 = m32; } /// /// Subtracts the two matrices from each other on a per-element basis. /// /// First matrix to subtract. /// Second matrix to subtract. /// Difference of the two matrices. public static void Subtract(ref Matrix3x2 a, ref Matrix3x2 b, out Matrix3x2 result) { float m11 = a.M11 - b.M11; float m12 = a.M12 - b.M12; float m21 = a.M21 - b.M21; float m22 = a.M22 - b.M22; float m31 = a.M31 - b.M31; float m32 = a.M32 - b.M32; result.M11 = m11; result.M12 = m12; result.M21 = m21; result.M22 = m22; result.M31 = m31; result.M32 = m32; } /// /// Transforms the vector by the matrix. /// /// Vector2 to transform. Considered to be a column vector for purposes of multiplication. /// Matrix to use as the transformation. /// Column vector product of the transformation. public static void Transform(ref Vector2 v, ref Matrix3x2 matrix, out Vector3 result) { #if !WINDOWS result = new Vector3(); #endif result.X = matrix.M11 * v.X + matrix.M12 * v.Y; result.Y = matrix.M21 * v.X + matrix.M22 * v.Y; result.Z = matrix.M31 * v.X + matrix.M32 * v.Y; } /// /// Transforms the vector by the matrix. /// /// Vector2 to transform. Considered to be a row vector for purposes of multiplication. /// Matrix to use as the transformation. /// Row vector product of the transformation. public static void Transform(ref Vector3 v, ref Matrix3x2 matrix, out Vector2 result) { #if !WINDOWS result = new Vector2(); #endif result.X = v.X * matrix.M11 + v.Y * matrix.M21 + v.Z * matrix.M31; result.Y = v.X * matrix.M12 + v.Y * matrix.M22 + v.Z * matrix.M32; } /// /// Computes the transposed matrix of a matrix. /// /// Matrix to transpose. /// Transposed matrix. public static void Transpose(ref Matrix3x2 matrix, out Matrix2x3 result) { result.M11 = matrix.M11; result.M12 = matrix.M21; result.M13 = matrix.M31; result.M21 = matrix.M12; result.M22 = matrix.M22; result.M23 = matrix.M32; } /// /// Creates a string representation of the matrix. /// /// A string representation of the matrix. public override string ToString() { return "{" + M11 + ", " + M12 + "} " + "{" + M21 + ", " + M22 + "} " + "{" + M31 + ", " + M32 + "}"; } } } ================================================ FILE: BEPUutilities/Matrix3x3.cs ================================================ using System; using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// 3 row, 3 column matrix. /// public struct Matrix3x3 { /// /// Value at row 1, column 1 of the matrix. /// public float M11; /// /// Value at row 1, column 2 of the matrix. /// public float M12; /// /// Value at row 1, column 3 of the matrix. /// public float M13; /// /// Value at row 2, column 1 of the matrix. /// public float M21; /// /// Value at row 2, column 2 of the matrix. /// public float M22; /// /// Value at row 2, column 3 of the matrix. /// public float M23; /// /// Value at row 3, column 1 of the matrix. /// public float M31; /// /// Value at row 3, column 2 of the matrix. /// public float M32; /// /// Value at row 3, column 3 of the matrix. /// public float M33; /// /// Constructs a new 3 row, 3 column matrix. /// /// Value at row 1, column 1 of the matrix. /// Value at row 1, column 2 of the matrix. /// Value at row 1, column 3 of the matrix. /// Value at row 2, column 1 of the matrix. /// Value at row 2, column 2 of the matrix. /// Value at row 2, column 3 of the matrix. /// Value at row 3, column 1 of the matrix. /// Value at row 3, column 2 of the matrix. /// Value at row 3, column 3 of the matrix. public Matrix3x3(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33) { M11 = m11; M12 = m12; M13 = m13; M21 = m21; M22 = m22; M23 = m23; M31 = m31; M32 = m32; M33 = m33; } /// /// Gets the 3x3 identity matrix. /// public static Matrix3x3 Identity { get { return new Matrix3x3(1, 0, 0, 0, 1, 0, 0, 0, 1); } } /// /// Gets or sets the backward vector of the matrix. /// public Vector3 Backward { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = M31; vector.Y = M32; vector.Z = M33; return vector; } set { M31 = value.X; M32 = value.Y; M33 = value.Z; } } /// /// Gets or sets the down vector of the matrix. /// public Vector3 Down { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = -M21; vector.Y = -M22; vector.Z = -M23; return vector; } set { M21 = -value.X; M22 = -value.Y; M23 = -value.Z; } } /// /// Gets or sets the forward vector of the matrix. /// public Vector3 Forward { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = -M31; vector.Y = -M32; vector.Z = -M33; return vector; } set { M31 = -value.X; M32 = -value.Y; M33 = -value.Z; } } /// /// Gets or sets the left vector of the matrix. /// public Vector3 Left { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = -M11; vector.Y = -M12; vector.Z = -M13; return vector; } set { M11 = -value.X; M12 = -value.Y; M13 = -value.Z; } } /// /// Gets or sets the right vector of the matrix. /// public Vector3 Right { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = M11; vector.Y = M12; vector.Z = M13; return vector; } set { M11 = value.X; M12 = value.Y; M13 = value.Z; } } /// /// Gets or sets the up vector of the matrix. /// public Vector3 Up { get { #if !WINDOWS Vector3 vector = new Vector3(); #else Vector3 vector; #endif vector.X = M21; vector.Y = M22; vector.Z = M23; return vector; } set { M21 = value.X; M22 = value.Y; M23 = value.Z; } } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m13 = a.M13 + b.M13; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m23 = a.M23 + b.M23; float m31 = a.M31 + b.M31; float m32 = a.M32 + b.M32; float m33 = a.M33 + b.M33; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix a, ref Matrix3x3 b, out Matrix3x3 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m13 = a.M13 + b.M13; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m23 = a.M23 + b.M23; float m31 = a.M31 + b.M31; float m32 = a.M32 + b.M32; float m33 = a.M33 + b.M33; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix3x3 a, ref Matrix b, out Matrix3x3 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m13 = a.M13 + b.M13; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m23 = a.M23 + b.M23; float m31 = a.M31 + b.M31; float m32 = a.M32 + b.M32; float m33 = a.M33 + b.M33; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Adds the two matrices together on a per-element basis. /// /// First matrix to add. /// Second matrix to add. /// Sum of the two matrices. public static void Add(ref Matrix a, ref Matrix b, out Matrix3x3 result) { float m11 = a.M11 + b.M11; float m12 = a.M12 + b.M12; float m13 = a.M13 + b.M13; float m21 = a.M21 + b.M21; float m22 = a.M22 + b.M22; float m23 = a.M23 + b.M23; float m31 = a.M31 + b.M31; float m32 = a.M32 + b.M32; float m33 = a.M33 + b.M33; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Creates a skew symmetric matrix M from vector A such that M * B for some other vector B is equivalent to the cross product of A and B. /// /// Vector to base the matrix on. /// Skew-symmetric matrix result. public static void CreateCrossProduct(ref Vector3 v, out Matrix3x3 result) { result.M11 = 0; result.M12 = -v.Z; result.M13 = v.Y; result.M21 = v.Z; result.M22 = 0; result.M23 = -v.X; result.M31 = -v.Y; result.M32 = v.X; result.M33 = 0; } /// /// Creates a 3x3 matrix from an XNA 4x4 matrix. /// /// Matrix to extract a 3x3 matrix from. /// Upper 3x3 matrix extracted from the XNA matrix. public static void CreateFromMatrix(ref Matrix matrix4X4, out Matrix3x3 matrix3X3) { matrix3X3.M11 = matrix4X4.M11; matrix3X3.M12 = matrix4X4.M12; matrix3X3.M13 = matrix4X4.M13; matrix3X3.M21 = matrix4X4.M21; matrix3X3.M22 = matrix4X4.M22; matrix3X3.M23 = matrix4X4.M23; matrix3X3.M31 = matrix4X4.M31; matrix3X3.M32 = matrix4X4.M32; matrix3X3.M33 = matrix4X4.M33; } /// /// Creates a 3x3 matrix from an XNA 4x4 matrix. /// /// Matrix to extract a 3x3 matrix from. /// Upper 3x3 matrix extracted from the XNA matrix. public static Matrix3x3 CreateFromMatrix(Matrix matrix4X4) { Matrix3x3 matrix3X3; matrix3X3.M11 = matrix4X4.M11; matrix3X3.M12 = matrix4X4.M12; matrix3X3.M13 = matrix4X4.M13; matrix3X3.M21 = matrix4X4.M21; matrix3X3.M22 = matrix4X4.M22; matrix3X3.M23 = matrix4X4.M23; matrix3X3.M31 = matrix4X4.M31; matrix3X3.M32 = matrix4X4.M32; matrix3X3.M33 = matrix4X4.M33; return matrix3X3; } /// /// Constructs a uniform scaling matrix. /// /// Value to use in the diagonal. /// Scaling matrix. public static void CreateScale(float scale, out Matrix3x3 matrix) { matrix = new Matrix3x3 {M11 = scale, M22 = scale, M33 = scale}; } /// /// Constructs a uniform scaling matrix. /// /// Value to use in the diagonal. /// Scaling matrix. public static Matrix3x3 CreateScale(float scale) { var matrix = new Matrix3x3 {M11 = scale, M22 = scale, M33 = scale}; return matrix; } /// /// Constructs a non-uniform scaling matrix. /// /// Values defining the axis scales. /// Scaling matrix. public static void CreateScale(ref Vector3 scale, out Matrix3x3 matrix) { matrix = new Matrix3x3 {M11 = scale.X, M22 = scale.Y, M33 = scale.Z}; } /// /// Constructs a non-uniform scaling matrix. /// /// Values defining the axis scales. /// Scaling matrix. public static Matrix3x3 CreateScale(ref Vector3 scale) { var matrix = new Matrix3x3 {M11 = scale.X, M22 = scale.Y, M33 = scale.Z}; return matrix; } /// /// Constructs a non-uniform scaling matrix. /// /// Scaling along the x axis. /// Scaling along the y axis. /// Scaling along the z axis. /// Scaling matrix. public static void CreateScale(float x, float y, float z, out Matrix3x3 matrix) { matrix = new Matrix3x3 {M11 = x, M22 = y, M33 = z}; } /// /// Constructs a non-uniform scaling matrix. /// /// Scaling along the x axis. /// Scaling along the y axis. /// Scaling along the z axis. /// Scaling matrix. public static Matrix3x3 CreateScale(float x, float y, float z) { var matrix = new Matrix3x3 {M11 = x, M22 = y, M33 = z}; return matrix; } /// /// Inverts the given matix. /// /// Matrix to be inverted. /// Inverted matrix. public static void Invert(ref Matrix3x3 matrix, out Matrix3x3 result) { float determinantInverse = 1 / matrix.Determinant(); float m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse; float m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse; float m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse; float m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse; float m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse; float m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse; float m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse; float m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse; float m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Inverts the largest nonsingular submatrix in the matrix, excluding 2x2's that involve M13 or M31, and excluding 1x1's that include nondiagonal elements. /// /// Matrix to be inverted. /// Inverted matrix. public static void AdaptiveInvert(ref Matrix3x3 matrix, out Matrix3x3 result) { int submatrix; float determinantInverse = 1 / matrix.AdaptiveDeterminant(out submatrix); float m11, m12, m13, m21, m22, m23, m31, m32, m33; switch (submatrix) { case 0: //Full matrix. m11 = (matrix.M22 * matrix.M33 - matrix.M23 * matrix.M32) * determinantInverse; m12 = (matrix.M13 * matrix.M32 - matrix.M33 * matrix.M12) * determinantInverse; m13 = (matrix.M12 * matrix.M23 - matrix.M22 * matrix.M13) * determinantInverse; m21 = (matrix.M23 * matrix.M31 - matrix.M21 * matrix.M33) * determinantInverse; m22 = (matrix.M11 * matrix.M33 - matrix.M13 * matrix.M31) * determinantInverse; m23 = (matrix.M13 * matrix.M21 - matrix.M11 * matrix.M23) * determinantInverse; m31 = (matrix.M21 * matrix.M32 - matrix.M22 * matrix.M31) * determinantInverse; m32 = (matrix.M12 * matrix.M31 - matrix.M11 * matrix.M32) * determinantInverse; m33 = (matrix.M11 * matrix.M22 - matrix.M12 * matrix.M21) * determinantInverse; break; case 1: //Upper left matrix, m11, m12, m21, m22. m11 = matrix.M22 * determinantInverse; m12 = -matrix.M12 * determinantInverse; m13 = 0; m21 = -matrix.M21 * determinantInverse; m22 = matrix.M11 * determinantInverse; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 2: //Lower right matrix, m22, m23, m32, m33. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = matrix.M33 * determinantInverse; m23 = -matrix.M23 * determinantInverse; m31 = 0; m32 = -matrix.M32 * determinantInverse; m33 = matrix.M22 * determinantInverse; break; case 3: //Corners, m11, m31, m13, m33. m11 = matrix.M33 * determinantInverse; m12 = 0; m13 = -matrix.M13 * determinantInverse; m21 = 0; m22 = 0; m23 = 0; m31 = -matrix.M31 * determinantInverse; m32 = 0; m33 = matrix.M11 * determinantInverse; break; case 4: //M11 m11 = 1 / matrix.M11; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 5: //M22 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 1 / matrix.M22; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; case 6: //M33 m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 1 / matrix.M33; break; default: //Completely singular. m11 = 0; m12 = 0; m13 = 0; m21 = 0; m22 = 0; m23 = 0; m31 = 0; m32 = 0; m33 = 0; break; } result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static Matrix3x3 operator *(Matrix3x3 a, Matrix3x3 b) { Matrix3x3 result; float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; float resultM33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; return result; } /// /// Scales all components of the matrix by the given value. /// /// First matrix to multiply. /// Scaling value to apply to all components of the matrix. /// Product of the multiplication. public static Matrix3x3 operator *(Matrix3x3 m, float f) { Matrix3x3 result; Multiply(ref m, f, out result); return result; } /// /// Scales all components of the matrix by the given value. /// /// First matrix to multiply. /// Scaling value to apply to all components of the matrix. /// Product of the multiplication. public static Matrix3x3 operator *(float f, Matrix3x3 m) { Matrix3x3 result; Multiply(ref m, f, out result); return result; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; float resultM33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix3x3 a, ref Matrix b, out Matrix3x3 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; float resultM33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; } /// /// Multiplies the two matrices. /// /// First matrix to multiply. /// Second matrix to multiply. /// Product of the multiplication. public static void Multiply(ref Matrix a, ref Matrix3x3 b, out Matrix3x3 result) { float resultM11 = a.M11 * b.M11 + a.M12 * b.M21 + a.M13 * b.M31; float resultM12 = a.M11 * b.M12 + a.M12 * b.M22 + a.M13 * b.M32; float resultM13 = a.M11 * b.M13 + a.M12 * b.M23 + a.M13 * b.M33; float resultM21 = a.M21 * b.M11 + a.M22 * b.M21 + a.M23 * b.M31; float resultM22 = a.M21 * b.M12 + a.M22 * b.M22 + a.M23 * b.M32; float resultM23 = a.M21 * b.M13 + a.M22 * b.M23 + a.M23 * b.M33; float resultM31 = a.M31 * b.M11 + a.M32 * b.M21 + a.M33 * b.M31; float resultM32 = a.M31 * b.M12 + a.M32 * b.M22 + a.M33 * b.M32; float resultM33 = a.M31 * b.M13 + a.M32 * b.M23 + a.M33 * b.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; } /// /// Multiplies a transposed matrix with another matrix. /// /// Matrix to be multiplied. /// Matrix to be transposed and multiplied. /// Product of the multiplication. public static void MultiplyTransposed(ref Matrix3x3 transpose, ref Matrix3x3 matrix, out Matrix3x3 result) { float resultM11 = transpose.M11 * matrix.M11 + transpose.M21 * matrix.M21 + transpose.M31 * matrix.M31; float resultM12 = transpose.M11 * matrix.M12 + transpose.M21 * matrix.M22 + transpose.M31 * matrix.M32; float resultM13 = transpose.M11 * matrix.M13 + transpose.M21 * matrix.M23 + transpose.M31 * matrix.M33; float resultM21 = transpose.M12 * matrix.M11 + transpose.M22 * matrix.M21 + transpose.M32 * matrix.M31; float resultM22 = transpose.M12 * matrix.M12 + transpose.M22 * matrix.M22 + transpose.M32 * matrix.M32; float resultM23 = transpose.M12 * matrix.M13 + transpose.M22 * matrix.M23 + transpose.M32 * matrix.M33; float resultM31 = transpose.M13 * matrix.M11 + transpose.M23 * matrix.M21 + transpose.M33 * matrix.M31; float resultM32 = transpose.M13 * matrix.M12 + transpose.M23 * matrix.M22 + transpose.M33 * matrix.M32; float resultM33 = transpose.M13 * matrix.M13 + transpose.M23 * matrix.M23 + transpose.M33 * matrix.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; } /// /// Multiplies a matrix with a transposed matrix. /// /// Matrix to be multiplied. /// Matrix to be transposed and multiplied. /// Product of the multiplication. public static void MultiplyByTransposed(ref Matrix3x3 matrix, ref Matrix3x3 transpose, out Matrix3x3 result) { float resultM11 = matrix.M11 * transpose.M11 + matrix.M12 * transpose.M12 + matrix.M13 * transpose.M13; float resultM12 = matrix.M11 * transpose.M21 + matrix.M12 * transpose.M22 + matrix.M13 * transpose.M23; float resultM13 = matrix.M11 * transpose.M31 + matrix.M12 * transpose.M32 + matrix.M13 * transpose.M33; float resultM21 = matrix.M21 * transpose.M11 + matrix.M22 * transpose.M12 + matrix.M23 * transpose.M13; float resultM22 = matrix.M21 * transpose.M21 + matrix.M22 * transpose.M22 + matrix.M23 * transpose.M23; float resultM23 = matrix.M21 * transpose.M31 + matrix.M22 * transpose.M32 + matrix.M23 * transpose.M33; float resultM31 = matrix.M31 * transpose.M11 + matrix.M32 * transpose.M12 + matrix.M33 * transpose.M13; float resultM32 = matrix.M31 * transpose.M21 + matrix.M32 * transpose.M22 + matrix.M33 * transpose.M23; float resultM33 = matrix.M31 * transpose.M31 + matrix.M32 * transpose.M32 + matrix.M33 * transpose.M33; result.M11 = resultM11; result.M12 = resultM12; result.M13 = resultM13; result.M21 = resultM21; result.M22 = resultM22; result.M23 = resultM23; result.M31 = resultM31; result.M32 = resultM32; result.M33 = resultM33; } /// /// Scales all components of the matrix. /// /// Matrix to scale. /// Amount to scale. /// Scaled matrix. public static void Multiply(ref Matrix3x3 matrix, float scale, out Matrix3x3 result) { result.M11 = matrix.M11 * scale; result.M12 = matrix.M12 * scale; result.M13 = matrix.M13 * scale; result.M21 = matrix.M21 * scale; result.M22 = matrix.M22 * scale; result.M23 = matrix.M23 * scale; result.M31 = matrix.M31 * scale; result.M32 = matrix.M32 * scale; result.M33 = matrix.M33 * scale; } /// /// Negates every element in the matrix. /// /// Matrix to negate. /// Negated matrix. public static void Negate(ref Matrix3x3 matrix, out Matrix3x3 result) { result.M11 = -matrix.M11; result.M12 = -matrix.M12; result.M13 = -matrix.M13; result.M21 = -matrix.M21; result.M22 = -matrix.M22; result.M23 = -matrix.M23; result.M31 = -matrix.M31; result.M32 = -matrix.M32; result.M33 = -matrix.M33; } /// /// Subtracts the two matrices from each other on a per-element basis. /// /// First matrix to subtract. /// Second matrix to subtract. /// Difference of the two matrices. public static void Subtract(ref Matrix3x3 a, ref Matrix3x3 b, out Matrix3x3 result) { float m11 = a.M11 - b.M11; float m12 = a.M12 - b.M12; float m13 = a.M13 - b.M13; float m21 = a.M21 - b.M21; float m22 = a.M22 - b.M22; float m23 = a.M23 - b.M23; float m31 = a.M31 - b.M31; float m32 = a.M32 - b.M32; float m33 = a.M33 - b.M33; result.M11 = m11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = m22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = m33; } /// /// Creates a 4x4 matrix from a 3x3 matrix. /// /// 3x3 matrix. /// Created 4x4 matrix. public static void ToMatrix4X4(ref Matrix3x3 a, out Matrix b) { #if !WINDOWS b = new Matrix(); #endif b.M11 = a.M11; b.M12 = a.M12; b.M13 = a.M13; b.M21 = a.M21; b.M22 = a.M22; b.M23 = a.M23; b.M31 = a.M31; b.M32 = a.M32; b.M33 = a.M33; b.M44 = 1; b.M14 = 0; b.M24 = 0; b.M34 = 0; b.M41 = 0; b.M42 = 0; b.M43 = 0; } /// /// Creates a 4x4 matrix from a 3x3 matrix. /// /// 3x3 matrix. /// Created 4x4 matrix. public static Matrix ToMatrix4X4(Matrix3x3 a) { #if !WINDOWS Matrix b = new Matrix(); #else Matrix b; #endif b.M11 = a.M11; b.M12 = a.M12; b.M13 = a.M13; b.M21 = a.M21; b.M22 = a.M22; b.M23 = a.M23; b.M31 = a.M31; b.M32 = a.M32; b.M33 = a.M33; b.M44 = 1; b.M14 = 0; b.M24 = 0; b.M34 = 0; b.M41 = 0; b.M42 = 0; b.M43 = 0; return b; } /// /// Transforms the vector by the matrix. /// /// Vector3 to transform. /// Matrix to use as the transformation. /// Product of the transformation. public static Vector3 Transform(Vector3 v, Matrix3x3 matrix) { Vector3 result; #if !WINDOWS result = new Vector3(); #endif float vX = v.X; float vY = v.Y; float vZ = v.Z; result.X = vX * matrix.M11 + vY * matrix.M21 + vZ * matrix.M31; result.Y = vX * matrix.M12 + vY * matrix.M22 + vZ * matrix.M32; result.Z = vX * matrix.M13 + vY * matrix.M23 + vZ * matrix.M33; return result; } /// /// Transforms the vector by the matrix. /// /// Vector3 to transform. /// Matrix to use as the transformation. /// Product of the transformation. public static void Transform(ref Vector3 v, ref Matrix3x3 matrix, out Vector3 result) { float vX = v.X; float vY = v.Y; float vZ = v.Z; #if !WINDOWS result = new Vector3(); #endif result.X = vX * matrix.M11 + vY * matrix.M21 + vZ * matrix.M31; result.Y = vX * matrix.M12 + vY * matrix.M22 + vZ * matrix.M32; result.Z = vX * matrix.M13 + vY * matrix.M23 + vZ * matrix.M33; } /// /// Transforms the vector by the matrix. /// /// Vector3 to transform. /// Matrix to use as the transformation. /// Product of the transformation. public static void Transform(ref Vector3 v, ref Matrix matrix, out Vector3 result) { float vX = v.X; float vY = v.Y; float vZ = v.Z; #if !WINDOWS result = new Vector3(); #endif result.X = vX * matrix.M11 + vY * matrix.M21 + vZ * matrix.M31; result.Y = vX * matrix.M12 + vY * matrix.M22 + vZ * matrix.M32; result.Z = vX * matrix.M13 + vY * matrix.M23 + vZ * matrix.M33; } /// /// Transforms the vector by the matrix's transpose. /// /// Vector3 to transform. /// Matrix to use as the transformation transpose. /// Product of the transformation. public static Vector3 TransformTranspose(Vector3 v, Matrix3x3 matrix) { float vX = v.X; float vY = v.Y; float vZ = v.Z; Vector3 result; #if !WINDOWS result = new Vector3(); #endif result.X = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13; result.Y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23; result.Z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33; return result; } /// /// Transforms the vector by the matrix's transpose. /// /// Vector3 to transform. /// Matrix to use as the transformation transpose. /// Product of the transformation. public static void TransformTranspose(ref Vector3 v, ref Matrix3x3 matrix, out Vector3 result) { float vX = v.X; float vY = v.Y; float vZ = v.Z; #if !WINDOWS result = new Vector3(); #endif result.X = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13; result.Y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23; result.Z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33; } /// /// Transforms the vector by the matrix's transpose. /// /// Vector3 to transform. /// Matrix to use as the transformation transpose. /// Product of the transformation. public static void TransformTranspose(ref Vector3 v, ref Matrix matrix, out Vector3 result) { float vX = v.X; float vY = v.Y; float vZ = v.Z; #if !WINDOWS result = new Vector3(); #endif result.X = vX * matrix.M11 + vY * matrix.M12 + vZ * matrix.M13; result.Y = vX * matrix.M21 + vY * matrix.M22 + vZ * matrix.M23; result.Z = vX * matrix.M31 + vY * matrix.M32 + vZ * matrix.M33; } /// /// Computes the transposed matrix of a matrix. /// /// Matrix to transpose. /// Transposed matrix. public static void Transpose(ref Matrix3x3 matrix, out Matrix3x3 result) { float m21 = matrix.M12; float m31 = matrix.M13; float m12 = matrix.M21; float m32 = matrix.M23; float m13 = matrix.M31; float m23 = matrix.M32; result.M11 = matrix.M11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = matrix.M22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = matrix.M33; } /// /// Computes the transposed matrix of a matrix. /// /// Matrix to transpose. /// Transposed matrix. public static void Transpose(ref Matrix matrix, out Matrix3x3 result) { float m21 = matrix.M12; float m31 = matrix.M13; float m12 = matrix.M21; float m32 = matrix.M23; float m13 = matrix.M31; float m23 = matrix.M32; result.M11 = matrix.M11; result.M12 = m12; result.M13 = m13; result.M21 = m21; result.M22 = matrix.M22; result.M23 = m23; result.M31 = m31; result.M32 = m32; result.M33 = matrix.M33; } /// /// Creates a string representation of the matrix. /// /// A string representation of the matrix. public override string ToString() { return "{" + M11 + ", " + M12 + ", " + M13 + "} " + "{" + M21 + ", " + M22 + ", " + M23 + "} " + "{" + M31 + ", " + M32 + ", " + M33 + "}"; } /// /// Calculates the determinant of the matrix. /// /// The matrix's determinant. public float Determinant() { return M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 - M31 * M22 * M13 - M32 * M23 * M11 - M33 * M21 * M12; } /// /// Calculates the determinant of largest nonsingular submatrix, excluding 2x2's that involve M13 or M31, and excluding all 1x1's that involve nondiagonal elements. /// /// Represents the submatrix that was used to compute the determinant. /// 0 is the full 3x3. 1 is the upper left 2x2. 2 is the lower right 2x2. 3 is the four corners. /// 4 is M11. 5 is M22. 6 is M33. /// The matrix's determinant. internal float AdaptiveDeterminant(out int subMatrixCode) { //Try the full matrix first. float determinant = M11 * M22 * M33 + M12 * M23 * M31 + M13 * M21 * M32 - M31 * M22 * M13 - M32 * M23 * M11 - M33 * M21 * M12; if (determinant != 0) //This could be a little numerically flimsy. Fortunately, the way this method is used, that doesn't matter! { subMatrixCode = 0; return determinant; } //Try m11, m12, m21, m22. determinant = M11 * M22 - M12 * M21; if (determinant != 0) { subMatrixCode = 1; return determinant; } //Try m22, m23, m32, m33. determinant = M22 * M33 - M23 * M32; if (determinant != 0) { subMatrixCode = 2; return determinant; } //Try m11, m13, m31, m33. determinant = M11 * M33 - M13 * M12; if (determinant != 0) { subMatrixCode = 3; return determinant; } //Try m11. if (M11 != 0) { subMatrixCode = 4; return M11; } //Try m22. if (M22 != 0) { subMatrixCode = 5; return M22; } //Try m33. if (M33 != 0) { subMatrixCode = 6; return M33; } //It's completely singular! subMatrixCode = -1; return 0; } /// /// Constructs a quaternion from a 3x3 rotation matrix. /// /// Rotation matrix to create the quaternion from. /// Quaternion based on the rotation matrix. public static void CreateQuaternion(ref Matrix3x3 r, out Quaternion q) { float trace = r.M11 + r.M22 + r.M33; #if !WINDOWS q = new Quaternion(); #endif if (trace >= 0) { var S = (float)System.Math.Sqrt(trace + 1.0) * 2; // S=4*qw var inverseS = 1 / S; q.W = 0.25f * S; q.X = (r.M23 - r.M32) * inverseS; q.Y = (r.M31 - r.M13) * inverseS; q.Z = (r.M12 - r.M21) * inverseS; } else if ((r.M11 > r.M22) & (r.M11 > r.M33)) { var S = (float)System.Math.Sqrt(1.0 + r.M11 - r.M22 - r.M33) * 2; // S=4*qx var inverseS = 1 / S; q.W = (r.M23 - r.M32) * inverseS; q.X = 0.25f * S; q.Y = (r.M21 + r.M12) * inverseS; q.Z = (r.M31 + r.M13) * inverseS; } else if (r.M22 > r.M33) { var S = (float)System.Math.Sqrt(1.0 + r.M22 - r.M11 - r.M33) * 2; // S=4*qy var inverseS = 1 / S; q.W = (r.M31 - r.M13) * inverseS; q.X = (r.M21 + r.M12) * inverseS; q.Y = 0.25f * S; q.Z = (r.M32 + r.M23) * inverseS; } else { var S = (float)System.Math.Sqrt(1.0 + r.M33 - r.M11 - r.M22) * 2; // S=4*qz var inverseS = 1 / S; q.W = (r.M12 - r.M21) * inverseS; q.X = (r.M31 + r.M13) * inverseS; q.Y = (r.M32 + r.M23) * inverseS; q.Z = 0.25f * S; } } /// /// Constructs a quaternion from a 3x3 rotation matrix. /// /// Rotation matrix to create the quaternion from. /// Quaternion based on the rotation matrix. public static Quaternion CreateQuaternion(Matrix3x3 r) { Quaternion result; CreateQuaternion(ref r, out result); return result; } /// /// Creates a 3x3 matrix representing the orientation stored in the quaternion. /// /// Quaternion to use to create a matrix. /// Matrix representing the quaternion's orientation. public static void CreateFromQuaternion(ref Quaternion quaternion, out Matrix3x3 result) { float XX = 2 * quaternion.X * quaternion.X; float YY = 2 * quaternion.Y * quaternion.Y; float ZZ = 2 * quaternion.Z * quaternion.Z; float XY = 2 * quaternion.X * quaternion.Y; float XZ = 2 * quaternion.X * quaternion.Z; float XW = 2 * quaternion.X * quaternion.W; float YZ = 2 * quaternion.Y * quaternion.Z; float YW = 2 * quaternion.Y * quaternion.W; float ZW = 2 * quaternion.Z * quaternion.W; result.M11 = 1 - YY - ZZ; result.M21 = XY - ZW; result.M31 = XZ + YW; result.M12 = XY + ZW; result.M22 = 1 - XX - ZZ; result.M32 = YZ - XW; result.M13 = XZ - YW; result.M23 = YZ + XW; result.M33 = 1 - XX - YY; } /// /// Creates a 3x3 matrix representing the orientation stored in the quaternion. /// /// Quaternion to use to create a matrix. /// Matrix representing the quaternion's orientation. public static Matrix3x3 CreateFromQuaternion(Quaternion quaternion) { Matrix3x3 result; CreateFromQuaternion(ref quaternion, out result); return result; } /// /// Computes the outer product of the given vectors. /// /// First vector. /// Second vector. /// Outer product result. public static void CreateOuterProduct(ref Vector3 a, ref Vector3 b, out Matrix3x3 result) { result.M11 = a.X * b.X; result.M12 = a.X * b.Y; result.M13 = a.X * b.Z; result.M21 = a.Y * b.X; result.M22 = a.Y * b.Y; result.M23 = a.Y * b.Z; result.M31 = a.Z * b.X; result.M32 = a.Z * b.Y; result.M33 = a.Z * b.Z; } /// /// Creates a matrix representing a rotation of a given angle around a given axis. /// /// Axis around which to rotate. /// Amount to rotate. /// Matrix representing the rotation. public static Matrix3x3 CreateFromAxisAngle(Vector3 axis, float angle) { Matrix3x3 toReturn; CreateFromAxisAngle(ref axis, angle, out toReturn); return toReturn; } /// /// Creates a matrix representing a rotation of a given angle around a given axis. /// /// Axis around which to rotate. /// Amount to rotate. /// Matrix representing the rotation. public static void CreateFromAxisAngle(ref Vector3 axis, float angle, out Matrix3x3 result) { float xx = axis.X * axis.X; float yy = axis.Y * axis.Y; float zz = axis.Z * axis.Z; float xy = axis.X * axis.Y; float xz = axis.X * axis.Z; float yz = axis.Y * axis.Z; float sinAngle = (float)System.Math.Sin(angle); float oneMinusCosAngle = 1 - (float)System.Math.Cos(angle); result.M11 = 1 + oneMinusCosAngle * (xx - 1); result.M21 = -axis.Z * sinAngle + oneMinusCosAngle * xy; result.M31 = axis.Y * sinAngle + oneMinusCosAngle * xz; result.M12 = axis.Z * sinAngle + oneMinusCosAngle * xy; result.M22 = 1 + oneMinusCosAngle * (yy - 1); result.M32 = -axis.X * sinAngle + oneMinusCosAngle * yz; result.M13 = -axis.Y * sinAngle + oneMinusCosAngle * xz; result.M23 = axis.X * sinAngle + oneMinusCosAngle * yz; result.M33 = 1 + oneMinusCosAngle * (zz - 1); } } } ================================================ FILE: BEPUutilities/PermutationMapper.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace BEPUutilities { /// /// Maps indices to permuted versions of the indices. /// public class PermutationMapper { /// /// Constructs a new permutation mapper. /// public PermutationMapper() { PermutationIndex = 0; } /// /// Gets or sets the permutation index used by the solver. If the simulation is restarting from a given frame, /// setting this index to be consistent is required for deterministic results. /// public int PermutationIndex { get { return primeIndex; } set { primeIndex = value % primes.Length; currentPrime = primes[primeIndex = (primeIndex + 1) % primes.Length]; } } long currentPrime; int primeIndex; static long[] primes = { 472882049, 492876847, 492876863, 512927357, 512927377, 533000389, 533000401, 553105243, 553105253, 573259391, 573259433, 593441843, 593441861, 613651349, 613651369, 633910099, 633910111, 654188383, 654188429, 674506081, 674506111, 694847533, 694847539, 715225739, 715225741, 735632791, 735632797, 756065159, 756065179, 776531401, 776531419, 797003413, 797003437, 817504243, 817504253, 838041641, 838041647, 858599503, 858599509, 879190747, 879190841, 899809343, 899809363, 920419813, 920419823, 941083981, 941083987, 961748927, 961748941, 982451653 }; /// /// Gets a remapped index. /// /// Original index of an element in the set to be redirected to a shuffled position. /// Size of the set being permuted. Must be smaller than 472882049. /// The remapped index. public long GetMappedIndex(long index, int setSize) { return (index * currentPrime) % setSize; } /// /// Gets a remapped index. /// /// Original index of an element in the set to be redirected to a shuffled position. /// Size of the set being permuted. Must be smaller than 472882049. /// The remapped index. public int GetMappedIndex(int index, int setSize) { return (int)((index * currentPrime) % setSize); } } } ================================================ FILE: BEPUutilities/Properties/AssemblyInfo.cs ================================================ using System.Reflection; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; // General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. [assembly: AssemblyTitle("BEPUutilities")] [assembly: AssemblyProduct("BEPUutilities")] [assembly: AssemblyDescription("Extensions and helpers used by various bepu things.")] [assembly: AssemblyCompany("Bepu Entertainment LLC.")] [assembly: AssemblyCopyright("Copyright © 2012")] [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] // Setting ComVisible to false makes the types in this assembly not visible // to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. Only Windows // assemblies support COM. [assembly: ComVisible(false)] // On Windows, the following GUID is for the ID of the typelib if this // project is exposed to COM. On other platforms, it unique identifies the // title storage container when deploying this assembly to the device. [assembly: Guid("a068ca77-0b61-484b-80dd-0dac40a2043c")] // Version information for an assembly consists of the following four values: // // Major Version // Minor Version // Build Number // Revision // [assembly: AssemblyVersion("1.3.0.0")] [assembly: AssemblyFileVersionAttribute("1.3.0.0")] ================================================ FILE: BEPUutilities/RayHit.cs ================================================ using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// Contains ray hit data. /// public struct RayHit { /// /// Location of the ray hit. /// public Vector3 Location; /// /// Normal of the ray hit. /// public Vector3 Normal; /// /// T parameter of the ray hit. /// The ray hit location is equal to the ray origin added to the ray direction multiplied by T. /// public float T; } } ================================================ FILE: BEPUutilities/ResourceManagement/CommonResources.cs ================================================ using System.Collections.Generic; using BEPUutilities.DataStructures; using Microsoft.Xna.Framework; namespace BEPUutilities.ResourceManagement { /// /// Handles allocation and management of commonly used resources. /// public static class CommonResources { static CommonResources() { ResetPools(); } public static void ResetPools() { SubPoolIntList = new LockingResourcePool>(); SubPoolIntSet = new LockingResourcePool>(); SubPoolFloatList = new LockingResourcePool>(); SubPoolVectorList = new LockingResourcePool>(); SubPoolRayHitList = new LockingResourcePool>(); } static ResourcePool> SubPoolRayHitList; static ResourcePool> SubPoolIntList; static ResourcePool> SubPoolIntSet; static ResourcePool> SubPoolFloatList; static ResourcePool> SubPoolVectorList; /// /// Retrieves a ray hit list from the resource pool. /// /// Empty ray hit list. public static RawList GetRayHitList() { return SubPoolRayHitList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolRayHitList.GiveBack(list); } /// /// Retrieves a int list from the resource pool. /// /// Empty int list. public static RawList GetIntList() { return SubPoolIntList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolIntList.GiveBack(list); } /// /// Retrieves a int hash set from the resource pool. /// /// Empty int set. public static HashSet GetIntSet() { return SubPoolIntSet.Take(); } /// /// Returns a resource to the pool. /// /// Set to return. public static void GiveBack(HashSet set) { set.Clear(); SubPoolIntSet.GiveBack(set); } /// /// Retrieves a float list from the resource pool. /// /// Empty float list. public static RawList GetFloatList() { return SubPoolFloatList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolFloatList.GiveBack(list); } /// /// Retrieves a Vector3 list from the resource pool. /// /// Empty Vector3 list. public static RawList GetVectorList() { return SubPoolVectorList.Take(); } /// /// Returns a resource to the pool. /// /// List to return. public static void GiveBack(RawList list) { list.Clear(); SubPoolVectorList.GiveBack(list); } } } ================================================ FILE: BEPUutilities/ResourceManagement/LockingResourcePool.cs ================================================ using System; using BEPUutilities.DataStructures; namespace BEPUutilities.ResourceManagement { /// /// Uses a spinlock to safely access resources. /// /// Type of object to store in the pool. public class LockingResourcePool : ResourcePool where T : class, new() { private readonly ConcurrentDeque stack; /// /// Constructs a new thread-unsafe resource pool. /// /// Number of resources to include in the pool by default. /// Function to initialize new instances in the resource pool with. public LockingResourcePool(int initialResourceCount, Action initializer) { InstanceInitializer = initializer; stack = new ConcurrentDeque(initialResourceCount); Initialize(initialResourceCount); } /// /// Constructs a new thread-unsafe resource pool. /// /// Number of resources to include in the pool by default. public LockingResourcePool(int initialResourceCount) : this(initialResourceCount, null) { } /// /// Constructs a new thread-unsafe resource pool. /// public LockingResourcePool() : this(10) { } /// /// Gets the number of resources in the pool. /// Even if the resource count hits 0, resources /// can still be requested; they will be allocated /// dynamically. /// public override int Count { get { return stack.Count; } } /// /// Gives an item back to the resource pool. /// /// Item to return. public override void GiveBack(T item) { stack.Enqueue(item); } /// /// Initializes the pool with some resources. /// Throws away excess resources. /// /// Number of resources to include. public override void Initialize(int initialResourceCount) { while (stack.Count > initialResourceCount) { T toRemove; stack.TryUnsafeDequeueFirst(out toRemove); } int length = stack.lastIndex - stack.firstIndex + 1; //lastIndex is inclusive, so add 1. if (InstanceInitializer != null) for (int i = 0; i < length; i++) { InstanceInitializer(stack.array[(stack.firstIndex + i) % stack.array.Length]); } while (stack.Count < initialResourceCount) { stack.UnsafeEnqueue(CreateNewResource()); } } /// /// Takes an item from the resource pool. /// /// Item to take. public override T Take() { T toTake; if (stack.TryDequeueFirst(out toTake)) { return toTake; } return CreateNewResource(); } /// /// Clears out the resource pool. /// public override void Clear() { while (stack.Count > 0) { T item; stack.TryDequeueFirst(out item); } } } } ================================================ FILE: BEPUutilities/ResourceManagement/ResourcePool.cs ================================================ using System; namespace BEPUutilities.ResourceManagement { /// /// Manages a cache of a type of resource. /// /// Type of object to pool. public abstract class ResourcePool where T : class, new() { /// /// Gets the number of resources in the pool. /// Even if the resource count hits 0, resources /// can still be requested; they will be allocated /// dynamically. /// public abstract int Count { get; } /// /// Gets or sets the function that configures new instances. /// This is only called once per object created for the resource pool. /// public Action InstanceInitializer { get; set; } /// /// Gives an item back to the resource pool. /// /// Item to return. public abstract void GiveBack(T item); /// /// Initializes the pool with some resources. /// Throws away excess resources. /// /// Number of resources to include. public abstract void Initialize(int initialResourceCount); /// /// Takes an item from the resource pool. /// /// Item to take. public abstract T Take(); /// /// Creates and returns a new resource. /// /// New resource. protected T CreateNewResource() { var toReturn = new T(); if (InstanceInitializer != null) InstanceInitializer(toReturn); return toReturn; } /// /// Removes all elements from the pool. /// public abstract void Clear(); } } ================================================ FILE: BEPUutilities/ResourceManagement/UnsafeResourcePool.cs ================================================ using System; using System.Collections.Generic; namespace BEPUutilities.ResourceManagement { /// /// Manages a resource type, but performs no locking to handle asynchronous access. /// /// Type of object to store in the pool. public class UnsafeResourcePool : ResourcePool where T : class, new() { private readonly Stack stack; /// /// Constructs a new locking resource pool. /// /// Number of resources to include in the pool by default. /// Function to initialize new instances in the resource pool with. public UnsafeResourcePool(int initialResourceCount, Action initializer) { InstanceInitializer = initializer; stack = new Stack(initialResourceCount); Initialize(initialResourceCount); } /// /// Constructs a new locking resource pool. /// /// Number of resources to include in the pool by default. public UnsafeResourcePool(int initialResourceCount) : this(initialResourceCount, null) { } /// /// Constructs a new locking resource pool. /// public UnsafeResourcePool() : this(10) { } /// /// Gets the number of resources in the pool. /// Even if the resource count hits 0, resources /// can still be requested; they will be allocated /// dynamically. /// public override int Count { get { return stack.Count; } } /// /// Gives an item back to the resource pool. /// /// Item to return. public override void GiveBack(T item) { stack.Push(item); } /// /// Initializes the pool with some resources. /// Throws away excess resources. /// /// Number of resources to include. public override void Initialize(int initialResourceCount) { while (stack.Count > initialResourceCount) { stack.Pop(); } if (InstanceInitializer != null) foreach (T t in stack) { InstanceInitializer(t); } while (stack.Count < initialResourceCount) { stack.Push(CreateNewResource()); } } /// /// Takes an item from the resource pool. /// /// Item to take. public override T Take() { if (stack.Count > 0) { return stack.Pop(); } else { return CreateNewResource(); } } /// /// Clears out the resource pool. /// public override void Clear() { stack.Clear(); } } } ================================================ FILE: BEPUutilities/RigidTransform.cs ================================================  using Microsoft.Xna.Framework; namespace BEPUutilities { /// /// Transform composed of a rotation and translation. /// public struct RigidTransform { /// /// Translation component of the transform. /// public Vector3 Position; /// /// Rotation component of the transform. /// public Quaternion Orientation; /// /// Constructs a new rigid transform. /// ///Translation component of the transform. ///Rotation component of the transform. public RigidTransform(Vector3 position, Quaternion orienation) { Position = position; Orientation = orienation; } /// /// Constructs a new rigid transform. /// ///Translation component of the transform. public RigidTransform(Vector3 position) { Position = position; Orientation = Quaternion.Identity; } /// /// Constructs a new rigid transform. /// ///Rotation component of the transform. public RigidTransform(Quaternion orienation) { Position = new Vector3(); Orientation = orienation; } /// /// Gets the orientation matrix created from the orientation of the rigid transform. /// public Matrix OrientationMatrix { get { Matrix toReturn; Matrix.CreateFromQuaternion(ref Orientation, out toReturn); return toReturn; } } /// /// Gets the 4x4 matrix created from the rigid transform. /// public Matrix Matrix { get { Matrix toReturn; Matrix.CreateFromQuaternion(ref Orientation, out toReturn); toReturn.Translation = Position; return toReturn; } } /// /// Gets the identity rigid transform. /// public static RigidTransform Identity { get { var t = new RigidTransform {Orientation = Quaternion.Identity, Position = new Vector3()}; return t; } } /// /// Inverts a rigid transform. /// /// Transform to invert. /// Inverse of the transform. public static void Invert(ref RigidTransform transform, out RigidTransform inverse) { Quaternion.Conjugate(ref transform.Orientation, out inverse.Orientation); Vector3.Transform(ref transform.Position, ref inverse.Orientation, out inverse.Position); Vector3.Negate(ref inverse.Position, out inverse.Position); } /// /// Transforms a rigid transform by another rigid transform. /// ///The first, "local" rigid transform. ///The second, "world" rigid transform. ///Combined rigid transform. public static void Transform(ref RigidTransform a, ref RigidTransform b, out RigidTransform combined) { Vector3 intermediate; Vector3.Transform(ref a.Position, ref b.Orientation, out intermediate); Vector3.Add(ref intermediate, ref b.Position, out combined.Position); Quaternion.Concatenate(ref a.Orientation, ref b.Orientation, out combined.Orientation); } /// /// Transforms a rigid transform by another rigid transform's inverse. /// ///The first rigid transform. ///The second rigid transform, to be inverted. ///Combined rigid transform. public static void TransformByInverse(ref RigidTransform a, ref RigidTransform b, out RigidTransform combinedTransform) { Invert(ref b, out combinedTransform); Transform(ref a, ref combinedTransform, out combinedTransform); } /// /// Transforms a position by a rigid transform. /// ///Position to transform. ///Transform to apply. ///Transformed position. public static void Transform(ref Vector3 position, ref RigidTransform transform, out Vector3 result) { Vector3 intermediate; Vector3.Transform(ref position, ref transform.Orientation, out intermediate); Vector3.Add(ref intermediate, ref transform.Position, out result); } /// /// Transforms a position by a rigid transform's inverse. /// ///Position to transform. ///Transform to invert and apply. ///Transformed position. public static void TransformByInverse(ref Vector3 position, ref RigidTransform transform, out Vector3 result) { Quaternion orientation; Vector3 intermediate; Vector3.Subtract(ref position, ref transform.Position, out intermediate); Quaternion.Conjugate(ref transform.Orientation, out orientation); Vector3.Transform(ref intermediate, ref orientation, out result); } } } ================================================ FILE: BEPUutilities/SpinLock.cs ================================================ using System; using System.Threading; namespace BEPUutilities { /// /// Synchronizes using a busy wait. Take care when using this; if the critical section is long or there's any doubt about the use of a busy wait, consider using Monitor locks or other approaches instead. /// Replaces the .NET SpinLock on PC and provides its functionality on the Xbox360. /// public class SpinLock { private const int MaximumSpinWait = 15; private const int SleepInterval = 10; private int owner = -1; /// /// Enters the critical section. A thread cannot attempt to enter the spinlock if it already owns the spinlock. /// public void Enter() { int count = 0; while (Interlocked.CompareExchange(ref owner, 0, -1) != -1) { //Lock is owned by someone else. count++; WaitBriefly(ref count); } //It's my lock now! } /// /// Attempts to enters the critical section. A thread cannot attempt to enter the spinlock if it already owns the spinlock. /// public bool TryEnter() { return Interlocked.CompareExchange(ref owner, 0, -1) == -1; } /// /// Exits the critical section. This can only be safely called from the same /// thread of execution after a corresponding Enter. /// public void Exit() { //To be safe, technically should check the identity of the exiter. //But since this is a very low-level, restricted access class, //assume that enter has to succeed before exit is tried. owner = -1; } internal void WaitBriefly(ref int attempt) { if (attempt == SleepInterval) { #if WINDOWS Thread.Yield(); #else Thread.Sleep(0); #endif //TODO: Thread.Yield on windows? //Check multithreaded bookmarks performance conscious //and .netspinlock attempt -= SleepInterval; } else { Thread.SpinWait(Math.Min(3 << attempt, MaximumSpinWait)); } } } } ================================================ FILE: BEPUutilities/Toolbox.cs ================================================ using System; using System.Collections.Generic; using BEPUphysics.CollisionTests; using BEPUutilities.DataStructures; using BEPUutilities.ResourceManagement; using Microsoft.Xna.Framework; namespace BEPUutilities { //TODO: It would be nice to split and improve this monolith into individually superior, organized components. /// /// Helper class with many algorithms for intersection testing and 3D math. /// public static class Toolbox { /// /// Large tolerance value. Defaults to 1e-5f. /// public static float BigEpsilon = 1E-5f; /// /// Tolerance value. Defaults to 1e-7f. /// public static float Epsilon = 1E-7f; /// /// Represents an invalid Vector3. /// public static readonly Vector3 NoVector = new Vector3(-float.MaxValue, -float.MaxValue, -float.MaxValue); /// /// Reference for a vector with dimensions (0,0,1). /// public static Vector3 BackVector = Vector3.Backward; /// /// Reference for a vector with dimensions (0,-1,0). /// public static Vector3 DownVector = Vector3.Down; /// /// Reference for a vector with dimensions (0,0,-1). /// public static Vector3 ForwardVector = Vector3.Forward; /// /// Refers to the identity quaternion. /// public static Quaternion IdentityOrientation = Quaternion.Identity; /// /// Reference for a vector with dimensions (-1,0,0). /// public static Vector3 LeftVector = Vector3.Left; /// /// Reference for a vector with dimensions (1,0,0). /// public static Vector3 RightVector = Vector3.Right; /// /// Reference for a vector with dimensions (0,1,0). /// public static Vector3 UpVector = Vector3.Up; /// /// Matrix containing zeroes for every element. /// public static Matrix ZeroMatrix = new Matrix(0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0); /// /// Reference for a vector with dimensions (0,0,0). /// public static Vector3 ZeroVector = Vector3.Zero; /// /// Refers to the rigid identity transformation. /// public static RigidTransform RigidIdentity = RigidTransform.Identity; #region Segment/Ray-Triangle Tests /// /// Determines the intersection between a ray and a triangle. /// /// Ray to test. /// Maximum length to travel in units of the direction's length. /// First vertex of the triangle. /// Second vertex of the triangle. /// Third vertex of the triangle. /// True if the the triangle was hit on the clockwise face, false otherwise. /// Hit data of the ray, if any /// Whether or not the ray and triangle intersect. public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, ref Vector3 a, ref Vector3 b, ref Vector3 c, out bool hitClockwise, out RayHit hit) { hitClockwise = false; hit = new RayHit(); Vector3 ab, ac; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref c, ref a, out ac); Vector3.Cross(ref ab, ref ac, out hit.Normal); if (hit.Normal.LengthSquared() < Epsilon) return false; //Degenerate triangle! float d; Vector3.Dot(ref ray.Direction, ref hit.Normal, out d); d = -d; hitClockwise = d >= 0; Vector3 ap; Vector3.Subtract(ref ray.Position, ref a, out ap); Vector3.Dot(ref ap, ref hit.Normal, out hit.T); hit.T /= d; if (hit.T < 0 || hit.T > maximumLength) return false;//Hit is behind origin, or too far away. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location); // Compute barycentric coordinates Vector3.Subtract(ref hit.Location, ref a, out ap); float ABdotAB, ABdotAC, ABdotAP; float ACdotAC, ACdotAP; Vector3.Dot(ref ab, ref ab, out ABdotAB); Vector3.Dot(ref ab, ref ac, out ABdotAC); Vector3.Dot(ref ab, ref ap, out ABdotAP); Vector3.Dot(ref ac, ref ac, out ACdotAC); Vector3.Dot(ref ac, ref ap, out ACdotAP); float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC); float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom; float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom; return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon); } /// /// Determines the intersection between a ray and a triangle. /// /// Ray to test. /// Maximum length to travel in units of the direction's length. /// Sidedness of the triangle to test. /// First vertex of the triangle. /// Second vertex of the triangle. /// Third vertex of the triangle. /// Hit data of the ray, if any /// Whether or not the ray and triangle intersect. public static bool FindRayTriangleIntersection(ref Ray ray, float maximumLength, TriangleSidedness sidedness, ref Vector3 a, ref Vector3 b, ref Vector3 c, out RayHit hit) { hit = new RayHit(); Vector3 ab, ac; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref c, ref a, out ac); Vector3.Cross(ref ab, ref ac, out hit.Normal); if (hit.Normal.LengthSquared() < Epsilon) return false; //Degenerate triangle! float d; Vector3.Dot(ref ray.Direction, ref hit.Normal, out d); d = -d; switch (sidedness) { case TriangleSidedness.DoubleSided: if (d <= 0) //Pointing the wrong way. Flip the normal. { Vector3.Negate(ref hit.Normal, out hit.Normal); d = -d; } break; case TriangleSidedness.Clockwise: if (d <= 0) //Pointing the wrong way. Can't hit. return false; break; case TriangleSidedness.Counterclockwise: if (d >= 0) //Pointing the wrong way. Can't hit. return false; Vector3.Negate(ref hit.Normal, out hit.Normal); d = -d; break; } Vector3 ap; Vector3.Subtract(ref ray.Position, ref a, out ap); Vector3.Dot(ref ap, ref hit.Normal, out hit.T); hit.T /= d; if (hit.T < 0 || hit.T > maximumLength) return false;//Hit is behind origin, or too far away. Vector3.Multiply(ref ray.Direction, hit.T, out hit.Location); Vector3.Add(ref ray.Position, ref hit.Location, out hit.Location); // Compute barycentric coordinates Vector3.Subtract(ref hit.Location, ref a, out ap); float ABdotAB, ABdotAC, ABdotAP; float ACdotAC, ACdotAP; Vector3.Dot(ref ab, ref ab, out ABdotAB); Vector3.Dot(ref ab, ref ac, out ABdotAC); Vector3.Dot(ref ab, ref ap, out ABdotAP); Vector3.Dot(ref ac, ref ac, out ACdotAC); Vector3.Dot(ref ac, ref ap, out ACdotAP); float denom = 1 / (ABdotAB * ACdotAC - ABdotAC * ABdotAC); float u = (ACdotAC * ABdotAP - ABdotAC * ACdotAP) * denom; float v = (ABdotAB * ACdotAP - ABdotAC * ABdotAP) * denom; return (u >= -Toolbox.BigEpsilon) && (v >= -Toolbox.BigEpsilon) && (u + v <= 1 + Toolbox.BigEpsilon); } /// /// Finds the intersection between the given segment and the given plane defined by three points. /// /// First endpoint of segment. /// Second endpoint of segment. /// First vertex of a triangle which lies on the plane. /// Second vertex of a triangle which lies on the plane. /// Third vertex of a triangle which lies on the plane. /// Intersection point. /// Whether or not the segment intersects the plane. public static bool GetSegmentPlaneIntersection(Vector3 a, Vector3 b, Vector3 d, Vector3 e, Vector3 f, out Vector3 q) { Plane p; p.Normal = Vector3.Cross(e - d, f - d); p.D = Vector3.Dot(p.Normal, d); float t; return GetSegmentPlaneIntersection(a, b, p, out t, out q); } /// /// Finds the intersection between the given segment and the given plane. /// /// First endpoint of segment. /// Second enpoint of segment. /// Plane for comparison. /// Intersection point. /// Whether or not the segment intersects the plane. public static bool GetSegmentPlaneIntersection(Vector3 a, Vector3 b, Plane p, out Vector3 q) { float t; return GetLinePlaneIntersection(ref a, ref b, ref p, out t, out q) && t >= 0 && t <= 1; } /// /// Finds the intersection between the given segment and the given plane. /// /// First endpoint of segment. /// Second endpoint of segment. /// Plane for comparison. /// Interval along segment to intersection. /// Intersection point. /// Whether or not the segment intersects the plane. public static bool GetSegmentPlaneIntersection(Vector3 a, Vector3 b, Plane p, out float t, out Vector3 q) { return GetLinePlaneIntersection(ref a, ref b, ref p, out t, out q) && t >= 0 && t <= 1; } /// /// Finds the intersection between the given line and the given plane. /// /// First endpoint of segment defining the line. /// Second endpoint of segment defining the line. /// Plane for comparison. /// Interval along line to intersection (A + t * AB). /// Intersection point. /// Whether or not the line intersects the plane. If false, the line is parallel to the plane's surface. public static bool GetLinePlaneIntersection(ref Vector3 a, ref Vector3 b, ref Plane p, out float t, out Vector3 q) { Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); float denominator; Vector3.Dot(ref p.Normal, ref ab, out denominator); if (denominator < Epsilon && denominator > -Epsilon) { //Surface of plane and line are parallel (or very close to it). q = new Vector3(); t = float.MaxValue; return false; } float numerator; Vector3.Dot(ref p.Normal, ref a, out numerator); t = (p.D - numerator) / denominator; //Compute the intersection position. Vector3.Multiply(ref ab, t, out q); Vector3.Add(ref a, ref q, out q); return true; } /// /// Finds the intersection between the given ray and the given plane. /// /// Ray to test against the plane. /// Plane for comparison. /// Interval along line to intersection (A + t * AB). /// Intersection point. /// Whether or not the line intersects the plane. If false, the line is parallel to the plane's surface. public static bool GetRayPlaneIntersection(ref Ray ray, ref Plane p, out float t, out Vector3 q) { float denominator; Vector3.Dot(ref p.Normal, ref ray.Direction, out denominator); if (denominator < Epsilon && denominator > -Epsilon) { //Surface of plane and line are parallel (or very close to it). q = new Vector3(); t = float.MaxValue; return false; } float numerator; Vector3.Dot(ref p.Normal, ref ray.Position, out numerator); t = (p.D - numerator) / denominator; //Compute the intersection position. Vector3.Multiply(ref ray.Direction, t, out q); Vector3.Add(ref ray.Position, ref q, out q); return t >= 0; } #endregion #region Point-Triangle Tests /// /// Determines the closest point on a triangle given by points a, b, and c to point p. /// /// First vertex of triangle. /// Second vertex of triangle. /// Third vertex of triangle. /// Point for comparison. /// Closest point on tetrahedron to point. /// Voronoi region containing the closest point. public static VoronoiRegion GetClosestPointOnTriangleToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 p, out Vector3 closestPoint) { float v, w; Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ac; Vector3.Subtract(ref c, ref a, out ac); //Vertex region A? Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float d1; Vector3.Dot(ref ab, ref ap, out d1); float d2; Vector3.Dot(ref ac, ref ap, out d2); if (d1 <= 0 && d2 < 0) { closestPoint = a; return VoronoiRegion.A; } //Vertex region B? Vector3 bp; Vector3.Subtract(ref p, ref b, out bp); float d3; Vector3.Dot(ref ab, ref bp, out d3); float d4; Vector3.Dot(ref ac, ref bp, out d4); if (d3 >= 0 && d4 <= d3) { closestPoint = b; return VoronoiRegion.B; } //Edge region AB? float vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { v = d1 / (d1 - d3); Vector3.Multiply(ref ab, v, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return VoronoiRegion.AB; } //Vertex region C? Vector3 cp; Vector3.Subtract(ref p, ref c, out cp); float d5; Vector3.Dot(ref ab, ref cp, out d5); float d6; Vector3.Dot(ref ac, ref cp, out d6); if (d6 >= 0 && d5 <= d6) { closestPoint = c; return VoronoiRegion.C; } //Edge region AC? float vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { w = d2 / (d2 - d6); Vector3.Multiply(ref ac, w, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return VoronoiRegion.AC; } //Edge region BC? float va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); Vector3.Subtract(ref c, ref b, out closestPoint); Vector3.Multiply(ref closestPoint, w, out closestPoint); Vector3.Add(ref closestPoint, ref b, out closestPoint); return VoronoiRegion.BC; } //Inside triangle? float denom = 1 / (va + vb + vc); v = vb * denom; w = vc * denom; Vector3 abv; Vector3.Multiply(ref ab, v, out abv); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref a, ref abv, out closestPoint); Vector3.Add(ref closestPoint, ref acw, out closestPoint); return VoronoiRegion.ABC; } /// /// Determines the closest point on a triangle given by points a, b, and c to point p and provides the subsimplex whose voronoi region contains the point. /// /// First vertex of triangle. /// Second vertex of triangle. /// Third vertex of triangle. /// Point for comparison. /// The source of the voronoi region which contains the point. /// Closest point on tetrahedron to point. [Obsolete("Used for simplex tests; consider using the PairSimplex and its variants instead for simplex-related testing.")] public static void GetClosestPointOnTriangleToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 p, RawList subsimplex, out Vector3 closestPoint) { subsimplex.Clear(); float v, w; Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ac; Vector3.Subtract(ref c, ref a, out ac); //Vertex region A? Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float d1; Vector3.Dot(ref ab, ref ap, out d1); float d2; Vector3.Dot(ref ac, ref ap, out d2); if (d1 <= 0 && d2 < 0) { subsimplex.Add(a); closestPoint = a; return; } //Vertex region B? Vector3 bp; Vector3.Subtract(ref p, ref b, out bp); float d3; Vector3.Dot(ref ab, ref bp, out d3); float d4; Vector3.Dot(ref ac, ref bp, out d4); if (d3 >= 0 && d4 <= d3) { subsimplex.Add(b); closestPoint = b; return; } //Edge region AB? float vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { subsimplex.Add(a); subsimplex.Add(b); v = d1 / (d1 - d3); Vector3.Multiply(ref ab, v, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return; } //Vertex region C? Vector3 cp; Vector3.Subtract(ref p, ref c, out cp); float d5; Vector3.Dot(ref ab, ref cp, out d5); float d6; Vector3.Dot(ref ac, ref cp, out d6); if (d6 >= 0 && d5 <= d6) { subsimplex.Add(c); closestPoint = c; return; } //Edge region AC? float vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { subsimplex.Add(a); subsimplex.Add(c); w = d2 / (d2 - d6); Vector3.Multiply(ref ac, w, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return; } //Edge region BC? float va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { subsimplex.Add(b); subsimplex.Add(c); w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); Vector3.Subtract(ref c, ref b, out closestPoint); Vector3.Multiply(ref closestPoint, w, out closestPoint); Vector3.Add(ref closestPoint, ref b, out closestPoint); return; } //Inside triangle? subsimplex.Add(a); subsimplex.Add(b); subsimplex.Add(c); float denom = 1 / (va + vb + vc); v = vb * denom; w = vc * denom; Vector3 abv; Vector3.Multiply(ref ab, v, out abv); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref a, ref abv, out closestPoint); Vector3.Add(ref closestPoint, ref acw, out closestPoint); } /// /// Determines the closest point on a triangle given by points a, b, and c to point p and provides the subsimplex whose voronoi region contains the point. /// /// Simplex containing triangle for testing. /// Index of first vertex of triangle. /// Index of second vertex of triangle. /// Index of third vertex of triangle. /// Point for comparison. /// The source of the voronoi region which contains the point, enumerated as a = 0, b = 1, c = 2. /// Barycentric coordinates of the point on the triangle. /// Closest point on tetrahedron to point. [Obsolete("Used for simplex tests; consider using the PairSimplex and its variants instead for simplex-related testing.")] public static void GetClosestPointOnTriangleToPoint(RawList q, int i, int j, int k, ref Vector3 p, RawList subsimplex, RawList baryCoords, out Vector3 closestPoint) { subsimplex.Clear(); baryCoords.Clear(); float v, w; Vector3 a = q[i]; Vector3 b = q[j]; Vector3 c = q[k]; Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ac; Vector3.Subtract(ref c, ref a, out ac); //Vertex region A? Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float d1; Vector3.Dot(ref ab, ref ap, out d1); float d2; Vector3.Dot(ref ac, ref ap, out d2); if (d1 <= 0 && d2 < 0) { subsimplex.Add(i); baryCoords.Add(1); closestPoint = a; return; //barycentric coordinates (1,0,0) } //Vertex region B? Vector3 bp; Vector3.Subtract(ref p, ref b, out bp); float d3; Vector3.Dot(ref ab, ref bp, out d3); float d4; Vector3.Dot(ref ac, ref bp, out d4); if (d3 >= 0 && d4 <= d3) { subsimplex.Add(j); baryCoords.Add(1); closestPoint = b; return; //barycentric coordinates (0,1,0) } //Edge region AB? float vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { subsimplex.Add(i); subsimplex.Add(j); v = d1 / (d1 - d3); baryCoords.Add(1 - v); baryCoords.Add(v); Vector3.Multiply(ref ab, v, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return; //barycentric coordinates (1-v, v, 0) } //Vertex region C? Vector3 cp; Vector3.Subtract(ref p, ref c, out cp); float d5; Vector3.Dot(ref ab, ref cp, out d5); float d6; Vector3.Dot(ref ac, ref cp, out d6); if (d6 >= 0 && d5 <= d6) { subsimplex.Add(k); baryCoords.Add(1); closestPoint = c; return; //barycentric coordinates (0,0,1) } //Edge region AC? float vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { subsimplex.Add(i); subsimplex.Add(k); w = d2 / (d2 - d6); baryCoords.Add(1 - w); baryCoords.Add(w); Vector3.Multiply(ref ac, w, out closestPoint); Vector3.Add(ref closestPoint, ref a, out closestPoint); return; //barycentric coordinates (1-w, 0, w) } //Edge region BC? float va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { subsimplex.Add(j); subsimplex.Add(k); w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); baryCoords.Add(1 - w); baryCoords.Add(w); Vector3.Subtract(ref c, ref b, out closestPoint); Vector3.Multiply(ref closestPoint, w, out closestPoint); Vector3.Add(ref closestPoint, ref b, out closestPoint); return; //barycentric coordinates (0, 1 - w ,w) } //Inside triangle? subsimplex.Add(i); subsimplex.Add(j); subsimplex.Add(k); float denom = 1 / (va + vb + vc); v = vb * denom; w = vc * denom; baryCoords.Add(1 - v - w); baryCoords.Add(v); baryCoords.Add(w); Vector3 abv; Vector3.Multiply(ref ab, v, out abv); Vector3 acw; Vector3.Multiply(ref ac, w, out acw); Vector3.Add(ref a, ref abv, out closestPoint); Vector3.Add(ref closestPoint, ref acw, out closestPoint); //return a + ab * v + ac * w; //barycentric coordinates (1 - v - w, v, w) } /// /// Determines if supplied point is within the triangle as defined by the provided vertices. /// /// A vertex of the triangle. /// A vertex of the triangle. /// A vertex of the triangle. /// The point for comparison against the triangle. /// Whether or not the point is within the triangle. public static bool IsPointInsideTriangle(ref Vector3 vA, ref Vector3 vB, ref Vector3 vC, ref Vector3 p) { float u, v, w; GetBarycentricCoordinates(ref p, ref vA, ref vB, ref vC, out u, out v, out w); //Are the barycoords valid? return (u > -Epsilon) && (v > -Epsilon) && (w > -Epsilon); } /// /// Determines if supplied point is within the triangle as defined by the provided vertices. /// /// A vertex of the triangle. /// A vertex of the triangle. /// A vertex of the triangle. /// The point for comparison against the triangle. /// Extra area on the edges of the triangle to include. Can be negative. /// Whether or not the point is within the triangle. public static bool IsPointInsideTriangle(ref Vector3 vA, ref Vector3 vB, ref Vector3 vC, ref Vector3 p, float margin) { float u, v, w; GetBarycentricCoordinates(ref p, ref vA, ref vB, ref vC, out u, out v, out w); //Are the barycoords valid? return (u > -margin) && (v > -margin) && (w > -margin); } #endregion #region Point-Line Tests /// /// Determines the closest point on the provided segment ab to point p. /// /// First endpoint of segment. /// Second endpoint of segment. /// Point for comparison. /// Closest point on the edge to p. public static void GetClosestPointOnSegmentToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 p, out Vector3 closestPoint) { Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float t; Vector3.Dot(ref ap, ref ab, out t); if (t <= 0) { closestPoint = a; } else { float denom = ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z; if (t >= denom) { closestPoint = b; } else { t = t / denom; Vector3 tab; Vector3.Multiply(ref ab, t, out tab); Vector3.Add(ref a, ref tab, out closestPoint); } } } /// /// Determines the closest point on the provided segment ab to point p. /// /// First endpoint of segment. /// Second endpoint of segment. /// Point for comparison. /// The source of the voronoi region which contains the point. /// Closest point on the edge to p. [Obsolete("Used for simplex tests; consider using the PairSimplex and its variants instead for simplex-related testing.")] public static void GetClosestPointOnSegmentToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 p, List subsimplex, out Vector3 closestPoint) { subsimplex.Clear(); Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float t; Vector3.Dot(ref ap, ref ab, out t); if (t <= 0) { //t = 0;//Don't need this for returning purposes. subsimplex.Add(a); closestPoint = a; } else { float denom = ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z; if (t >= denom) { //t = 1;//Don't need this for returning purposes. subsimplex.Add(b); closestPoint = b; } else { t = t / denom; subsimplex.Add(a); subsimplex.Add(b); Vector3 tab; Vector3.Multiply(ref ab, t, out tab); Vector3.Add(ref a, ref tab, out closestPoint); } } } /// /// Determines the closest point on the provided segment ab to point p. /// /// List of points in the containing simplex. /// Index of first endpoint of segment. /// Index of second endpoint of segment. /// Point for comparison. /// The source of the voronoi region which contains the point, enumerated as a = 0, b = 1. /// Barycentric coordinates of the point. /// Closest point on the edge to p. [Obsolete("Used for simplex tests; consider using the PairSimplex and its variants instead for simplex-related testing.")] public static void GetClosestPointOnSegmentToPoint(List q, int i, int j, ref Vector3 p, List subsimplex, List baryCoords, out Vector3 closestPoint) { Vector3 a = q[i]; Vector3 b = q[j]; subsimplex.Clear(); baryCoords.Clear(); Vector3 ab; Vector3.Subtract(ref b, ref a, out ab); Vector3 ap; Vector3.Subtract(ref p, ref a, out ap); float t; Vector3.Dot(ref ap, ref ab, out t); if (t <= 0) { subsimplex.Add(i); baryCoords.Add(1); closestPoint = a; } else { float denom = ab.X * ab.X + ab.Y * ab.Y + ab.Z * ab.Z; if (t >= denom) { subsimplex.Add(j); baryCoords.Add(1); closestPoint = b; } else { t = t / denom; subsimplex.Add(i); subsimplex.Add(j); baryCoords.Add(1 - t); baryCoords.Add(t); Vector3 tab; Vector3.Multiply(ref ab, t, out tab); Vector3.Add(ref a, ref tab, out closestPoint); } } } /// /// Determines the shortest squared distance from the point to the line. /// /// Point to check against the line. /// First point on the line. /// Second point on the line. /// Shortest squared distance from the point to the line. public static float GetSquaredDistanceFromPointToLine(ref Vector3 p, ref Vector3 a, ref Vector3 b) { Vector3 ap, ab; Vector3.Subtract(ref p, ref a, out ap); Vector3.Subtract(ref b, ref a, out ab); float e; Vector3.Dot(ref ap, ref ab, out e); return ap.LengthSquared() - e * e / ab.LengthSquared(); } #endregion #region Line-Line Tests /// /// Computes closest points c1 and c2 betwen segments p1q1 and p2q2. /// /// First point of first segment. /// Second point of first segment. /// First point of second segment. /// Second point of second segment. /// Closest point on first segment. /// Closest point on second segment. public static void GetClosestPointsBetweenSegments(Vector3 p1, Vector3 q1, Vector3 p2, Vector3 q2, out Vector3 c1, out Vector3 c2) { float s, t; GetClosestPointsBetweenSegments(ref p1, ref q1, ref p2, ref q2, out s, out t, out c1, out c2); } /// /// Computes closest points c1 and c2 betwen segments p1q1 and p2q2. /// /// First point of first segment. /// Second point of first segment. /// First point of second segment. /// Second point of second segment. /// Distance along the line to the point for first segment. /// Distance along the line to the point for second segment. /// Closest point on first segment. /// Closest point on second segment. public static void GetClosestPointsBetweenSegments(ref Vector3 p1, ref Vector3 q1, ref Vector3 p2, ref Vector3 q2, out float s, out float t, out Vector3 c1, out Vector3 c2) { //Segment direction vectors Vector3 d1; Vector3.Subtract(ref q1, ref p1, out d1); Vector3 d2; Vector3.Subtract(ref q2, ref p2, out d2); Vector3 r; Vector3.Subtract(ref p1, ref p2, out r); //distance float a = d1.LengthSquared(); float e = d2.LengthSquared(); float f; Vector3.Dot(ref d2, ref r, out f); if (a <= Epsilon && e <= Epsilon) { //These segments are more like points. s = t = 0.0f; c1 = p1; c2 = p2; return; } if (a <= Epsilon) { // First segment is basically a point. s = 0.0f; t = MathHelper.Clamp(f / e, 0.0f, 1.0f); } else { float c = Vector3.Dot(d1, r); if (e <= Epsilon) { // Second segment is basically a point. t = 0.0f; s = MathHelper.Clamp(-c / a, 0.0f, 1.0f); } else { float b = Vector3.Dot(d1, d2); float denom = a * e - b * b; // If segments not parallel, compute closest point on L1 to L2, and // clamp to segment S1. Else pick some s (here .5f) if (denom != 0.0f) s = MathHelper.Clamp((b * f - c * e) / denom, 0.0f, 1.0f); else //Parallel, just use .5f s = .5f; t = (b * s + f) / e; if (t < 0) { //Closest point is before the segment. t = 0; s = MathHelper.Clamp(-c / a, 0, 1); } else if (t > 1) { //Closest point is after the segment. t = 1; s = MathHelper.Clamp((b - c) / a, 0, 1); } } } Vector3.Multiply(ref d1, s, out c1); Vector3.Add(ref c1, ref p1, out c1); Vector3.Multiply(ref d2, t, out c2); Vector3.Add(ref c2, ref p2, out c2); } /// /// Computes closest points c1 and c2 betwen lines p1q1 and p2q2. /// /// First point of first segment. /// Second point of first segment. /// First point of second segment. /// Second point of second segment. /// Distance along the line to the point for first segment. /// Distance along the line to the point for second segment. /// Closest point on first segment. /// Closest point on second segment. public static void GetClosestPointsBetweenLines(ref Vector3 p1, ref Vector3 q1, ref Vector3 p2, ref Vector3 q2, out float s, out float t, out Vector3 c1, out Vector3 c2) { //Segment direction vectors Vector3 d1; Vector3.Subtract(ref q1, ref p1, out d1); Vector3 d2; Vector3.Subtract(ref q2, ref p2, out d2); Vector3 r; Vector3.Subtract(ref p1, ref p2, out r); //distance float a = d1.LengthSquared(); float e = d2.LengthSquared(); float f; Vector3.Dot(ref d2, ref r, out f); if (a <= Epsilon && e <= Epsilon) { //These segments are more like points. s = t = 0.0f; c1 = p1; c2 = p2; return; } if (a <= Epsilon) { // First segment is basically a point. s = 0.0f; t = MathHelper.Clamp(f / e, 0.0f, 1.0f); } else { float c = Vector3.Dot(d1, r); if (e <= Epsilon) { // Second segment is basically a point. t = 0.0f; s = MathHelper.Clamp(-c / a, 0.0f, 1.0f); } else { float b = Vector3.Dot(d1, d2); float denom = a * e - b * b; // If segments not parallel, compute closest point on L1 to L2, and // clamp to segment S1. Else pick some s (here .5f) if (denom != 0f) s = (b * f - c * e) / denom; else //Parallel, just use .5f s = .5f; t = (b * s + f) / e; } } Vector3.Multiply(ref d1, s, out c1); Vector3.Add(ref c1, ref p1, out c1); Vector3.Multiply(ref d2, t, out c2); Vector3.Add(ref c2, ref p2, out c2); } #endregion #region Point-Plane Tests /// /// Determines if vectors o and p are on opposite sides of the plane defined by a, b, and c. /// /// First point for comparison. /// Second point for comparison. /// First vertex of the plane. /// Second vertex of plane. /// Third vertex of plane. /// Whether or not vectors o and p reside on opposite sides of the plane. public static bool ArePointsOnOppositeSidesOfPlane(ref Vector3 o, ref Vector3 p, ref Vector3 a, ref Vector3 b, ref Vector3 c) { Vector3 ab, ac, ap, ao; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref c, ref a, out ac); Vector3.Subtract(ref p, ref a, out ap); Vector3.Subtract(ref o, ref a, out ao); Vector3 q; Vector3.Cross(ref ab, ref ac, out q); float signp; Vector3.Dot(ref ap, ref q, out signp); float signo; Vector3.Dot(ref ao, ref q, out signo); if (signp * signo <= 0) return true; return false; } /// /// Determines the distance between a point and a plane.. /// /// Point to project onto plane. /// Normal of the plane. /// Point located on the plane. /// Distance from the point to the plane. public static float GetDistancePointToPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane) { Vector3 offset; Vector3.Subtract(ref point, ref pointOnPlane, out offset); float dot; Vector3.Dot(ref normal, ref offset, out dot); return dot / normal.LengthSquared(); } /// /// Determines the location of the point when projected onto the plane defined by the normal and a point on the plane. /// /// Point to project onto plane. /// Normal of the plane. /// Point located on the plane. /// Projected location of point onto plane. public static void GetPointProjectedOnPlane(ref Vector3 point, ref Vector3 normal, ref Vector3 pointOnPlane, out Vector3 projectedPoint) { float dot; Vector3.Dot(ref normal, ref point, out dot); float dot2; Vector3.Dot(ref pointOnPlane, ref normal, out dot2); float t = (dot - dot2) / normal.LengthSquared(); Vector3 multiply; Vector3.Multiply(ref normal, t, out multiply); Vector3.Subtract(ref point, ref multiply, out projectedPoint); } /// /// Determines if a point is within a set of planes defined by the edges of a triangle. /// /// Point for comparison. /// Edge planes. /// A point known to be inside of the planes. /// Whether or not the point is within the edge planes. public static bool IsPointWithinFaceExtrusion(Vector3 point, List planes, Vector3 centroid) { foreach (Plane plane in planes) { float centroidPlaneDot; plane.DotCoordinate(ref centroid, out centroidPlaneDot); float pointPlaneDot; plane.DotCoordinate(ref point, out pointPlaneDot); if (!((centroidPlaneDot <= Epsilon && pointPlaneDot <= Epsilon) || (centroidPlaneDot >= -Epsilon && pointPlaneDot >= -Epsilon))) { //Point's NOT the same side of the centroid, so it's 'outside.' return false; } } return true; } #endregion #region Tetrahedron Tests //Note: These methods are unused in modern systems, but are kept around for verification. /// /// Determines the closest point on a tetrahedron to a provided point p. /// /// First vertex of the tetrahedron. /// Second vertex of the tetrahedron. /// Third vertex of the tetrahedron. /// Fourth vertex of the tetrahedron. /// Point for comparison. /// Closest point on the tetrahedron to the point. public static void GetClosestPointOnTetrahedronToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, ref Vector3 p, out Vector3 closestPoint) { // Start out assuming point inside all halfspaces, so closest to itself closestPoint = p; Vector3 pq; Vector3 q; float bestSqDist = float.MaxValue; // If point outside face abc then compute closest point on abc if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c)) { GetClosestPointOnTriangleToPoint(ref a, ref b, ref c, ref p, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face acd if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d)) { GetClosestPointOnTriangleToPoint(ref a, ref c, ref d, ref p, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face adb if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b)) { GetClosestPointOnTriangleToPoint(ref a, ref d, ref b, ref p, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face bdc if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c)) { GetClosestPointOnTriangleToPoint(ref b, ref d, ref c, ref p, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { closestPoint = q; } } } /// /// Determines the closest point on a tetrahedron to a provided point p. /// /// First vertex of the tetrahedron. /// Second vertex of the tetrahedron. /// Third vertex of the tetrahedron. /// Fourth vertex of the tetrahedron. /// Point for comparison. /// The source of the voronoi region which contains the point. /// Closest point on the tetrahedron to the point. [Obsolete("This method was used for older GJK simplex tests. If you need simplex tests, consider the PairSimplex class and its variants.")] public static void GetClosestPointOnTetrahedronToPoint(ref Vector3 a, ref Vector3 b, ref Vector3 c, ref Vector3 d, ref Vector3 p, RawList subsimplex, out Vector3 closestPoint) { // Start out assuming point inside all halfspaces, so closest to itself subsimplex.Clear(); subsimplex.Add(a); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself. subsimplex.Add(b); subsimplex.Add(c); subsimplex.Add(d); closestPoint = p; Vector3 pq; Vector3 q; float bestSqDist = float.MaxValue; // If point outside face abc then compute closest point on abc if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c)) { GetClosestPointOnTriangleToPoint(ref a, ref b, ref c, ref p, subsimplex, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face acd if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d)) { GetClosestPointOnTriangleToPoint(ref a, ref c, ref d, ref p, subsimplex, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face adb if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b)) { GetClosestPointOnTriangleToPoint(ref a, ref d, ref b, ref p, subsimplex, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; } } // Repeat test for face bdc if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c)) { GetClosestPointOnTriangleToPoint(ref b, ref d, ref c, ref p, subsimplex, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.X * pq.X + pq.Y * pq.Y + pq.Z * pq.Z; if (sqDist < bestSqDist) { closestPoint = q; } } } /// /// Determines the closest point on a tetrahedron to a provided point p. /// /// List of 4 points composing the tetrahedron. /// Point for comparison. /// The source of the voronoi region which contains the point, enumerated as a = 0, b = 1, c = 2, d = 3. /// Barycentric coordinates of p on the tetrahedron. /// Closest point on the tetrahedron to the point. [Obsolete("This method was used for older GJK simplex tests. If you need simplex tests, consider the PairSimplex class and its variants.")] public static void GetClosestPointOnTetrahedronToPoint(RawList tetrahedron, ref Vector3 p, RawList subsimplex, RawList baryCoords, out Vector3 closestPoint) { var subsimplexCandidate = CommonResources.GetIntList(); var baryCoordsCandidate = CommonResources.GetFloatList(); Vector3 a = tetrahedron[0]; Vector3 b = tetrahedron[1]; Vector3 c = tetrahedron[2]; Vector3 d = tetrahedron[3]; closestPoint = p; Vector3 pq; float bestSqDist = float.MaxValue; subsimplex.Clear(); subsimplex.Add(0); //Provides a baseline; if the object is not outside of any planes, then it's inside and the subsimplex is the tetrahedron itself. subsimplex.Add(1); subsimplex.Add(2); subsimplex.Add(3); baryCoords.Clear(); Vector3 q; bool baryCoordsFound = false; // If point outside face abc then compute closest point on abc if (ArePointsOnOppositeSidesOfPlane(ref p, ref d, ref a, ref b, ref c)) { GetClosestPointOnTriangleToPoint(tetrahedron, 0, 1, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.LengthSquared(); // Update best closest point if (squared) distance is less than current best if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; subsimplex.Clear(); baryCoords.Clear(); for (int k = 0; k < subsimplexCandidate.Count; k++) { subsimplex.Add(subsimplexCandidate[k]); baryCoords.Add(baryCoordsCandidate[k]); } //subsimplex.AddRange(subsimplexCandidate); //baryCoords.AddRange(baryCoordsCandidate); baryCoordsFound = true; } } // Repeat test for face acd if (ArePointsOnOppositeSidesOfPlane(ref p, ref b, ref a, ref c, ref d)) { GetClosestPointOnTriangleToPoint(tetrahedron, 0, 2, 3, ref p, subsimplexCandidate, baryCoordsCandidate, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.LengthSquared(); if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; subsimplex.Clear(); baryCoords.Clear(); for (int k = 0; k < subsimplexCandidate.Count; k++) { subsimplex.Add(subsimplexCandidate[k]); baryCoords.Add(baryCoordsCandidate[k]); } //subsimplex.AddRange(subsimplexCandidate); //baryCoords.AddRange(baryCoordsCandidate); baryCoordsFound = true; } } // Repeat test for face adb if (ArePointsOnOppositeSidesOfPlane(ref p, ref c, ref a, ref d, ref b)) { GetClosestPointOnTriangleToPoint(tetrahedron, 0, 3, 1, ref p, subsimplexCandidate, baryCoordsCandidate, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.LengthSquared(); if (sqDist < bestSqDist) { bestSqDist = sqDist; closestPoint = q; subsimplex.Clear(); baryCoords.Clear(); for (int k = 0; k < subsimplexCandidate.Count; k++) { subsimplex.Add(subsimplexCandidate[k]); baryCoords.Add(baryCoordsCandidate[k]); } //subsimplex.AddRange(subsimplexCandidate); //baryCoords.AddRange(baryCoordsCandidate); baryCoordsFound = true; } } // Repeat test for face bdc if (ArePointsOnOppositeSidesOfPlane(ref p, ref a, ref b, ref d, ref c)) { GetClosestPointOnTriangleToPoint(tetrahedron, 1, 3, 2, ref p, subsimplexCandidate, baryCoordsCandidate, out q); Vector3.Subtract(ref q, ref p, out pq); float sqDist = pq.LengthSquared(); if (sqDist < bestSqDist) { closestPoint = q; subsimplex.Clear(); baryCoords.Clear(); for (int k = 0; k < subsimplexCandidate.Count; k++) { subsimplex.Add(subsimplexCandidate[k]); baryCoords.Add(baryCoordsCandidate[k]); } //subsimplex.AddRange(subsimplexCandidate); //baryCoords.AddRange(baryCoordsCandidate); baryCoordsFound = true; } } if (!baryCoordsFound) { //subsimplex is the entire tetrahedron, can only occur when objects intersect! Determinants of each of the tetrahedrons based on triangles composing the sides and the point itself. //This is basically computing the volume of parallelepipeds (triple scalar product). //Could be quicker just to do it directly. float abcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1, tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1, tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1, tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant(); float pbcd = (new Matrix(p.X, p.Y, p.Z, 1, tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1, tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1, tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant(); float apcd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1, p.X, p.Y, p.Z, 1, tetrahedron[2].X, tetrahedron[2].Y, tetrahedron[2].Z, 1, tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant(); float abpd = (new Matrix(tetrahedron[0].X, tetrahedron[0].Y, tetrahedron[0].Z, 1, tetrahedron[1].X, tetrahedron[1].Y, tetrahedron[1].Z, 1, p.X, p.Y, p.Z, 1, tetrahedron[3].X, tetrahedron[3].Y, tetrahedron[3].Z, 1)).Determinant(); abcd = 1 / abcd; baryCoords.Add(pbcd * abcd); //u baryCoords.Add(apcd * abcd); //v baryCoords.Add(abpd * abcd); //w baryCoords.Add(1 - baryCoords[0] - baryCoords[1] - baryCoords[2]); //x = 1-u-v-w } CommonResources.GiveBack(subsimplexCandidate); CommonResources.GiveBack(baryCoordsCandidate); } #endregion #region Miscellaneous /// /// Tests a ray against a sphere. /// ///Ray to test. ///Position of the sphere. ///Radius of the sphere. ///Maximum length of the ray in units of the ray direction's length. ///Hit data of the ray, if any. ///Whether or not the ray hits the sphere. public static bool RayCastSphere(ref Ray ray, ref Vector3 spherePosition, float radius, float maximumLength, out RayHit hit) { Vector3 normalizedDirection; float length = ray.Direction.Length(); Vector3.Divide(ref ray.Direction, length, out normalizedDirection); maximumLength *= length; hit = new RayHit(); Vector3 m; Vector3.Subtract(ref ray.Position, ref spherePosition, out m); float b = Vector3.Dot(m, normalizedDirection); float c = m.LengthSquared() - radius * radius; if (c > 0 && b > 0) return false; float discriminant = b * b - c; if (discriminant < 0) return false; hit.T = -b - (float)Math.Sqrt(discriminant); if (hit.T < 0) hit.T = 0; if (hit.T > maximumLength) return false; hit.T /= length; Vector3.Multiply(ref normalizedDirection, hit.T, out hit.Location); Vector3.Add(ref hit.Location, ref ray.Position, out hit.Location); Vector3.Subtract(ref hit.Location, ref spherePosition, out hit.Normal); hit.Normal.Normalize(); return true; } /// /// Computes the velocity of a point as if it were attached to an object with the given center and velocity. /// /// Point to compute the velocity of. /// Center of the object to which the point is attached. /// Linear velocity of the object. /// Angular velocity of the object. /// Velocity of the point. public static void GetVelocityOfPoint(ref Vector3 point, ref Vector3 center, ref Vector3 linearVelocity, ref Vector3 angularVelocity, out Vector3 velocity) { Vector3 offset; Vector3.Subtract(ref point, ref center, out offset); Vector3.Cross(ref angularVelocity, ref offset, out velocity); Vector3.Add(ref velocity, ref linearVelocity, out velocity); } /// /// Computes the velocity of a point as if it were attached to an object with the given center and velocity. /// /// Point to compute the velocity of. /// Center of the object to which the point is attached. /// Linear velocity of the object. /// Angular velocity of the object. /// Velocity of the point. public static Vector3 GetVelocityOfPoint(Vector3 point, Vector3 center, Vector3 linearVelocity, Vector3 angularVelocity) { Vector3 toReturn; GetVelocityOfPoint(ref point, ref center, ref linearVelocity, ref angularVelocity, out toReturn); return toReturn; } /// /// Expands a bounding box by the given sweep. /// /// Bounding box to expand. /// Sweep to expand the bounding box with. public static void ExpandBoundingBox(ref BoundingBox boundingBox, ref Vector3 sweep) { if (sweep.X > 0) boundingBox.Max.X += sweep.X; else boundingBox.Min.X += sweep.X; if (sweep.Y > 0) boundingBox.Max.Y += sweep.Y; else boundingBox.Min.Y += sweep.Y; if (sweep.Z > 0) boundingBox.Max.Z += sweep.Z; else boundingBox.Min.Z += sweep.Z; } /// /// Computes the bounding box of three points. /// /// First vertex of the triangle. /// Second vertex of the triangle. /// Third vertex of the triangle. /// Bounding box of the triangle. public static void GetTriangleBoundingBox(ref Vector3 a, ref Vector3 b, ref Vector3 c, out BoundingBox aabb) { #if !WINDOWS aabb = new BoundingBox(); #endif //X axis if (a.X > b.X && a.X > c.X) { //A is max aabb.Max.X = a.X; aabb.Min.X = b.X > c.X ? c.X : b.X; } else if (b.X > c.X) { //B is max aabb.Max.X = b.X; aabb.Min.X = a.X > c.X ? c.X : a.X; } else { //C is max aabb.Max.X = c.X; aabb.Min.X = a.X > b.X ? b.X : a.X; } //Y axis if (a.Y > b.Y && a.Y > c.Y) { //A is max aabb.Max.Y = a.Y; aabb.Min.Y = b.Y > c.Y ? c.Y : b.Y; } else if (b.Y > c.Y) { //B is max aabb.Max.Y = b.Y; aabb.Min.Y = a.Y > c.Y ? c.Y : a.Y; } else { //C is max aabb.Max.Y = c.Y; aabb.Min.Y = a.Y > b.Y ? b.Y : a.Y; } //Z axis if (a.Z > b.Z && a.Z > c.Z) { //A is max aabb.Max.Z = a.Z; aabb.Min.Z = b.Z > c.Z ? c.Z : b.Z; } else if (b.Z > c.Z) { //B is max aabb.Max.Z = b.Z; aabb.Min.Z = a.Z > c.Z ? c.Z : a.Z; } else { //C is max aabb.Max.Z = c.Z; aabb.Min.Z = a.Z > b.Z ? b.Z : a.Z; } } /// /// Computes the angle change represented by a normalized quaternion. /// /// Quaternion to be converted. /// Angle around the axis represented by the quaternion. public static float GetAngleFromQuaternion(ref Quaternion q) { float qw = Math.Abs(q.W); if (qw > 1) return 0; return 2 * (float)Math.Acos(qw); } /// /// Computes the axis angle representation of a normalized quaternion. /// /// Quaternion to be converted. /// Axis represented by the quaternion. /// Angle around the axis represented by the quaternion. public static void GetAxisAngleFromQuaternion(ref Quaternion q, out Vector3 axis, out float angle) { #if !WINDOWS axis = new Vector3(); #endif float qx = q.X; float qy = q.Y; float qz = q.Z; float qw = q.W; if (qw < 0) { qx = -qx; qy = -qy; qz = -qz; qw = -qw; } if (qw > 1 - 1e-12) { axis = UpVector; angle = 0; } else { angle = 2 * (float)Math.Acos(qw); float denominator = 1 / (float)Math.Sqrt(1 - qw * qw); axis.X = qx * denominator; axis.Y = qy * denominator; axis.Z = qz * denominator; } } /// /// Computes the quaternion rotation between two normalized vectors. /// /// First unit-length vector. /// Second unit-length vector. /// Quaternion representing the rotation from v1 to v2. public static void GetQuaternionBetweenNormalizedVectors(ref Vector3 v1, ref Vector3 v2, out Quaternion q) { float dot; Vector3.Dot(ref v1, ref v2, out dot); //For non-normal vectors, the multiplying the axes length squared would be necessary: //float w = dot + (float)Math.Sqrt(v1.LengthSquared() * v2.LengthSquared()); if (dot < -0.9999f) //parallel, opposing direction { //Project onto the plane which has the lowest component magnitude. if (v1.X < v1.Y && v1.X < v1.Z) q = new Quaternion(0, -v1.Z, v1.Y, 0); else if (v1.Y < v1.Z) q = new Quaternion(-v1.Z, 0, v1.X, 0); else q = new Quaternion(-v1.Y, -v1.X, 0, 0); } else { Vector3 axis; Vector3.Cross(ref v1, ref v2, out axis); q = new Quaternion(axis.X, axis.Y, axis.Z, dot + 1); } q.Normalize(); } /// /// Updates the quaternion using RK4 integration. /// /// Quaternion to update. /// Local-space inertia tensor of the object being updated. /// Angular momentum of the object. /// Time since last frame, in seconds. /// New orientation quaternion. public static void UpdateOrientationRK4(ref Quaternion q, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, float dt, out Quaternion newOrientation) { //TODO: This is a little goofy //Quaternion diff = differentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum); Quaternion d1; DifferentiateQuaternion(ref q, ref localInertiaTensorInverse, ref angularMomentum, out d1); Quaternion s2; Quaternion.Multiply(ref d1, dt * .5f, out s2); Quaternion.Add(ref q, ref s2, out s2); Quaternion d2; DifferentiateQuaternion(ref s2, ref localInertiaTensorInverse, ref angularMomentum, out d2); Quaternion s3; Quaternion.Multiply(ref d2, dt * .5f, out s3); Quaternion.Add(ref q, ref s3, out s3); Quaternion d3; DifferentiateQuaternion(ref s3, ref localInertiaTensorInverse, ref angularMomentum, out d3); Quaternion s4; Quaternion.Multiply(ref d3, dt, out s4); Quaternion.Add(ref q, ref s4, out s4); Quaternion d4; DifferentiateQuaternion(ref s4, ref localInertiaTensorInverse, ref angularMomentum, out d4); Quaternion.Multiply(ref d1, dt / 6, out d1); Quaternion.Multiply(ref d2, dt / 3, out d2); Quaternion.Multiply(ref d3, dt / 3, out d3); Quaternion.Multiply(ref d4, dt / 6, out d4); Quaternion added; Quaternion.Add(ref q, ref d1, out added); Quaternion.Add(ref added, ref d2, out added); Quaternion.Add(ref added, ref d3, out added); Quaternion.Add(ref added, ref d4, out added); Quaternion.Normalize(ref added, out newOrientation); } /// /// Finds the change in the rotation state quaternion provided the local inertia tensor and angular velocity. /// /// Orienatation of the object. /// Local-space inertia tensor of the object being updated. /// Angular momentum of the object. /// Change in quaternion. public static void DifferentiateQuaternion(ref Quaternion orientation, ref Matrix3x3 localInertiaTensorInverse, ref Vector3 angularMomentum, out Quaternion orientationChange) { Quaternion normalizedOrientation; Quaternion.Normalize(ref orientation, out normalizedOrientation); Matrix3x3 tempRotMat; Matrix3x3.CreateFromQuaternion(ref normalizedOrientation, out tempRotMat); Matrix3x3 tempInertiaTensorInverse; Matrix3x3.MultiplyTransposed(ref tempRotMat, ref localInertiaTensorInverse, out tempInertiaTensorInverse); Matrix3x3.Multiply(ref tempInertiaTensorInverse, ref tempRotMat, out tempInertiaTensorInverse); Vector3 halfspin; Matrix3x3.Transform(ref angularMomentum, ref tempInertiaTensorInverse, out halfspin); Vector3.Multiply(ref halfspin, .5f, out halfspin); var halfspinQuaternion = new Quaternion(halfspin.X, halfspin.Y, halfspin.Z, 0); Quaternion.Multiply(ref halfspinQuaternion, ref normalizedOrientation, out orientationChange); } /// /// Gets the barycentric coordinates of the point with respect to a triangle's vertices. /// /// Point to compute the barycentric coordinates of. /// First vertex in the triangle. /// Second vertex in the triangle. /// Third vertex in the triangle. /// Weight of the first vertex. /// Weight of the second vertex. /// Weight of the third vertex. public static void GetBarycentricCoordinates(ref Vector3 p, ref Vector3 a, ref Vector3 b, ref Vector3 c, out float aWeight, out float bWeight, out float cWeight) { Vector3 ab, ac; Vector3.Subtract(ref b, ref a, out ab); Vector3.Subtract(ref c, ref a, out ac); Vector3 triangleNormal; Vector3.Cross(ref ab, ref ac, out triangleNormal); float x = triangleNormal.X < 0 ? -triangleNormal.X : triangleNormal.X; float y = triangleNormal.Y < 0 ? -triangleNormal.Y : triangleNormal.Y; float z = triangleNormal.Z < 0 ? -triangleNormal.Z : triangleNormal.Z; float numeratorU, numeratorV, denominator; if (x >= y && x >= z) { //The projection of the triangle on the YZ plane is the largest. numeratorU = (p.Y - b.Y) * (b.Z - c.Z) - (b.Y - c.Y) * (p.Z - b.Z); //PBC numeratorV = (p.Y - c.Y) * (c.Z - a.Z) - (c.Y - a.Y) * (p.Z - c.Z); //PCA denominator = triangleNormal.X; } else if (y >= z) { //The projection of the triangle on the XZ plane is the largest. numeratorU = (p.X - b.X) * (b.Z - c.Z) - (b.X - c.X) * (p.Z - b.Z); //PBC numeratorV = (p.X - c.X) * (c.Z - a.Z) - (c.X - a.X) * (p.Z - c.Z); //PCA denominator = -triangleNormal.Y; } else { //The projection of the triangle on the XY plane is the largest. numeratorU = (p.X - b.X) * (b.Y - c.Y) - (b.X - c.X) * (p.Y - b.Y); //PBC numeratorV = (p.X - c.X) * (c.Y - a.Y) - (c.X - a.X) * (p.Y - c.Y); //PCA denominator = triangleNormal.Z; } if (denominator < -1e-9 || denominator > 1e-9) { denominator = 1 / denominator; aWeight = numeratorU * denominator; bWeight = numeratorV * denominator; cWeight = 1 - aWeight - bWeight; } else { //It seems to be a degenerate triangle. //In that case, pick one of the closest vertices. //MOST of the time, this will happen when the vertices //are all very close together (all three points form a single point). //Sometimes, though, it could be that it's more of a line. //If it's a little inefficient, don't worry- this is a corner case anyway. float distance1, distance2, distance3; Vector3.DistanceSquared(ref p, ref a, out distance1); Vector3.DistanceSquared(ref p, ref b, out distance2); Vector3.DistanceSquared(ref p, ref c, out distance3); if (distance1 < distance2 && distance1 < distance3) { aWeight = 1; bWeight = 0; cWeight = 0; } else if (distance2 < distance3) { aWeight = 0; bWeight = 1; cWeight = 0; } else { aWeight = 0; bWeight = 0; cWeight = 1; } } } #endregion } } ================================================ FILE: BEPUutilities/TriangleSidedness.cs ================================================ namespace BEPUutilities { /// /// Sidedness of a triangle or mesh. /// A triangle can be double sided, or allow one of its sides to let interacting objects through. /// public enum TriangleSidedness { /// /// The triangle will interact with objects coming from both directions. /// DoubleSided, /// /// The triangle will interact with objects from which the winding of the triangle appears to be clockwise. /// Clockwise, /// /// The triangle will interact with objects from which the winding of the triangle appears to be counterclockwise.. /// Counterclockwise } } ================================================ FILE: BEPUutilities/VoronoiRegion.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace BEPUphysics.CollisionTests { public enum VoronoiRegion { A, B, C, AB, AC, BC, ABC } } ================================================ FILE: ComponentBind/BaseMain.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using Microsoft.Xna.Framework; namespace ComponentBind { public class BaseMain : Microsoft.Xna.Framework.Game { public Command EntityAdded = new Command(); public Command EntityRemoved = new Command(); public Property Paused = new Property(); public Property ElapsedTime = new Property(); public Property TotalTime = new Property(); public Property EditorEnabled = new Property { Value = false }; public GameTime GameTime; public ListProperty Entities; protected List componentsToRemove = new List(); protected List componentsToAdd = new List(); public void Add(Entity entity) { if (entity.Active && !entity.Added) { entity.Added = true; this.Entities.Add(entity); this.EntityAdded.Execute(entity); } } public void AddComponent(IComponent component) { if (this.EditorEnabled || component.Entity == null || component.Entity.CannotSuspend) component.Suspended.Value = false; component.SetMain(this); if (typeof(IGraphicsComponent).IsAssignableFrom(component.GetType())) ((IGraphicsComponent)component).LoadContent(false); component.Awake(); this.componentsToAdd.Add(component); } public void RemoveComponent(IComponent component) { this.componentsToRemove.Add(component); } public void Remove(Entity entity) { if (entity.Active) { // Trigger all delete commands // then call this Remove function again, but this time the entity will be inactive. entity.Delete.Execute(); } else if (entity.Added) { entity.Added = false; this.Entities.Remove(entity); this.EntityRemoved.Execute(entity); } } public IEnumerable Get(string type) { return this.Entities.Where(x => x.Type == type && x.Active); } public Entity GetByID(string id) { return Entity.GetByID(id); } public Entity GetByGUID(ulong id) { return Entity.GetByGUID(id); } } } ================================================ FILE: ComponentBind/Binding.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace ComponentBind { public interface IBinding { void Delete(); } public interface IPropertyBinding : IBinding { void OnChanged(IProperty changed); } public class Binding : IPropertyBinding { protected Property destination; protected Func get; protected IProperty[] sources; protected Binding() { } public Binding(Property _destination, Func transform, Property _source) { this.destination = _destination; _source.AddBinding(this); this.get = () => transform(_source.Value); this.sources = new IProperty[] { _source }; this.OnChanged(_source); } public virtual void OnChanged(IProperty changed) { this.destination.InternalSet(this.get(), this); } public virtual void Delete() { for (int i = 0; i < this.sources.Length; i++) this.sources[i].RemoveBinding(this); this.sources = null; this.get = null; this.destination = null; } } public class SetBinding : IPropertyBinding { protected Action notify; protected Property source; protected SetBinding() { } public SetBinding(Property source, Action _notify) { this.notify = _notify; this.source = source; source.AddBinding(this); Type v = this.source.Value; if (v != null) this.notify(v); } public void OnChanged(IProperty changed) { this.notify(this.source.Value); } public void Delete() { this.source.RemoveBinding(this); this.notify = null; this.source = null; } } public class ChangeBinding : IPropertyBinding { protected Action notify; protected Property source; protected Type value; protected ChangeBinding() { } public ChangeBinding(Property source, Action _notify) { this.notify = _notify; this.source = source; this.value = source.Value; source.AddBinding(this); if (this.value != null && !this.value.Equals(default(Type))) this.notify(default(Type), this.value); } public void OnChanged(IProperty changed) { Type old = this.value; this.value = this.source.Value; this.notify(old, this.value); } public void Delete() { this.source.RemoveBinding(this); this.notify = null; this.source = null; } } public class NotifyBinding : IPropertyBinding { protected Action notify; protected IProperty[] sources; protected NotifyBinding() { } public NotifyBinding(Action _notify, params IProperty[] _sources) { this.notify = _notify; this.sources = _sources; for (int i = 0; i < this.sources.Length; i++) this.sources[i].AddBinding(this); } public void OnChanged(IProperty changed) { this.notify(); } public void Delete() { for (int i = 0; i < this.sources.Length; i++) this.sources[i].RemoveBinding(this); this.notify = null; this.sources = null; } } public class Binding : Binding { protected Binding() { } public Binding(Property _destination, Property _source) : base(_destination, x => x, _source) { } public Binding(Property _destination, Func transform, Property _source) : base(_destination, transform, _source) { } public Binding(Property _destination, Func _get, params IProperty[] _sources) { this.destination = _destination; this.get = _get; this.sources = _sources; for (int i = 0; i < this.sources.Length; i++) this.sources[i].AddBinding(this); this.OnChanged(_sources.FirstOrDefault()); } } /// /// Important: When initializing, the first given property takes precedence. /// /// /// public class TwoWayBinding : Binding { protected Property property1; protected IProperty[] property1Sources; protected Property property2; protected IProperty[] property2Sources; protected Func transform1; protected Func transform2; protected bool reevaluating = false; protected TwoWayBinding() { } public TwoWayBinding( Property _property1, Func _transform1, Property _property2, Func _transform2) : this(_property1, _transform1, new IProperty[] { }, _property2, _transform2, new IProperty[] { }) { } public TwoWayBinding( Property _property1, Func _transform1, IEnumerable _property1Sources, Property _property2, Func _transform2, IEnumerable _property2Sources) { this.property1 = _property1; this.property2 = _property2; this.property1Sources = _property1Sources.Union(new IProperty[] { this.property2 }).ToArray(); this.property2Sources = _property2Sources.Union(new IProperty[] { this.property1 }).ToArray(); this.transform1 = _transform1; this.transform2 = _transform2; for (int i = 0; i < this.property1Sources.Length; i++) this.property1Sources[i].AddBinding(this); for (int i = 0; i < this.property2Sources.Length; i++) this.property2Sources[i].AddBinding(this); this.OnChanged(this.property1); } public void Reevaluate(IProperty destination) { if (!this.reevaluating) { this.reevaluating = true; if (destination == this.property1) this.property1.InternalSet(this.transform1(this.property2.Value), this); else if (destination == this.property2) this.property2.InternalSet(this.transform2(this.property1.Value), this); else throw new ArgumentException("Binding received improper property change notification."); this.reevaluating = false; } } public override void OnChanged(IProperty changed) { if (this.property2Sources.Contains(changed)) this.property2.InternalSet(this.transform2(this.property1.Value), this); else if (this.property1Sources.Contains(changed)) this.property1.InternalSet(this.transform1(this.property2.Value), this); else throw new ArgumentException("Binding received improper property change notification."); } public override void Delete() { for (int i = 0; i < this.property1Sources.Length; i++) this.property1Sources[i].RemoveBinding(this); for (int i = 0; i < this.property2Sources.Length; i++) this.property2Sources[i].RemoveBinding(this); this.property1 = null; this.property1Sources = null; this.property2 = null; this.property2Sources = null; this.transform1 = null; this.transform2 = null; } } /// /// Important: When initializing, the first given property takes precedence. /// /// public class TwoWayBinding : TwoWayBinding { protected TwoWayBinding() { } public TwoWayBinding(Property _property1, Property _property2) : base(_property1, x => x, _property2, x => x) { } } public interface IListBinding : IPropertyBinding { void Add(Type x, IProperty property); void Remove(Type x, IProperty property); void OnChanged(Type x, Type y, IProperty property); void Clear(IProperty property); } public class ListBinding : IListBinding { protected class Entry { public int Index; } protected ListProperty destination; protected Func transform; protected Func filter; protected Dictionary mapping = new Dictionary(); protected IProperty[] sources; public bool Enabled { get; set; } protected ListBinding() { this.Enabled = true; } public ListBinding(ListProperty _destination, ListProperty _source, Func _transform, Func _filter) { this.Enabled = true; this.destination = _destination; this.transform = _transform; this.filter = _filter; _source.AddBinding(this); this.sources = new IProperty[] { _source }; this.OnChanged(_source); } public ListBinding(ListProperty _destination, ListProperty _source, Func _transform) : this(_destination, _source, _transform, (x) => true) { } public void OnChanged(IProperty property) { if (this.Enabled) { this.Clear(property); ListProperty source = (ListProperty)this.sources[0]; for (int i = 0; i < source.Count; i++) this.Add(source[i], property); } } public void Add(Type2 x, IProperty property) { if (this.Enabled && this.filter((Type2)x)) { Type y = this.transform(x); this.destination.Add(y); this.mapping.Add((Type2)x, new Entry { Index = this.destination.Length - 1 }); } } public void Remove(Type2 x, IProperty property) { if (this.Enabled && this.filter((Type2)x)) { int e = this.mapping[(Type2)x].Index; this.mapping.Remove((Type2)x); this.destination.RemoveAt(e); this.recalculate(e + 1, e); } } private void recalculate(int oldIndex, int newIndex) { if (newIndex != oldIndex) { int diff = newIndex - oldIndex; foreach (Entry entry in this.mapping.Values) { if (entry.Index >= oldIndex) entry.Index += diff; } } } public void OnChanged(Type2 from, Type2 to, IProperty property) { if (this.Enabled) { bool originallyIncluded = this.filter((Type2)from), nowIncluded = this.filter((Type2)to); if (originallyIncluded && nowIncluded) { Entry oldEntry = this.mapping[(Type2)from]; Type newValue = this.transform((Type2)to); Entry newEntry = new Entry { Index = oldEntry.Index }; this.destination.Changed(oldEntry.Index, newValue); this.mapping[(Type2)from] = newEntry; } else if (!originallyIncluded && nowIncluded) { Type newValue = this.transform((Type2)to); this.destination.Add(newValue); this.mapping.Add((Type2)to, new Entry { Index = this.destination.Length - 1 }); } else if (originallyIncluded && !nowIncluded) { Entry entry = this.mapping[(Type2)from]; this.destination.RemoveAt(entry.Index); this.mapping.Remove((Type2)from); this.recalculate(entry.Index + 1, entry.Index); } } } public void Clear(IProperty property) { this.mapping.Clear(); if (this.Enabled) this.destination.Clear(); } public void Delete() { for (int i = 0; i < this.sources.Length; i++) this.sources[i].RemoveBinding(this); this.sources = null; this.destination = null; this.transform = null; this.filter = null; this.mapping.Clear(); } } public class ListNotifyBinding : IListBinding { protected IProperty[] sources; private Action notify; public bool Enabled { get; set; } protected ListNotifyBinding() { this.Enabled = true; } public ListNotifyBinding(Action notify, ListProperty _source) { this.Enabled = true; this.notify = notify; _source.AddBinding(this); this.sources = new IProperty[] { _source }; } public void OnChanged(IProperty property) { if (this.Enabled) this.notify(); } public void Add(Type x, IProperty property) { if (this.Enabled) this.notify(); } public void Remove(Type x, IProperty property) { if (this.Enabled) this.notify(); } public void OnChanged(Type from, Type to, IProperty property) { if (this.Enabled) this.notify(); } public void Clear(IProperty property) { if (this.Enabled) this.notify(); } public void Delete() { for (int i = 0; i < this.sources.Length; i++) this.sources[i].RemoveBinding(this); this.sources = null; } } public class ListBinding : ListBinding { public ListBinding(ListProperty _destination, ListProperty _source) : base(_destination, _source, x => x, x => true) { } } } ================================================ FILE: ComponentBind/Command.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace ComponentBind { public abstract class BaseCommand { public List Bindings = new List(); public void AddBinding(ICommandBinding binding) { this.Bindings.Add(binding); } public void RemoveBinding(ICommandBinding binding) { this.Bindings.Remove(binding); } } public class Command : BaseCommand { public enum Perms { Linkable, Executable, LinkableAndExecutable } public class Entry { public Command Command; public Perms Permissions; public string Description; public string Key; } public Action Action; public void Execute() { for (int j = this.Bindings.Count - 1; j >= 0; j = Math.Min(this.Bindings.Count - 1, j - 1)) ((CommandBinding)this.Bindings[j]).Execute(); if (this.Action != null) this.Action(); } } public class Command : BaseCommand { public Action Action; public void Execute(Type parameter) { for (int j = this.Bindings.Count - 1; j >= 0; j = Math.Min(this.Bindings.Count - 1, j - 1)) ((CommandBinding)this.Bindings[j]).Execute(parameter); if (this.Action != null) this.Action(parameter); } } public class Command : BaseCommand { public Action Action; public void Execute(Type parameter1, Type2 parameter2) { for (int j = this.Bindings.Count - 1; j >= 0; j = Math.Min(this.Bindings.Count - 1, j - 1)) ((CommandBinding)this.Bindings[j]).Execute(parameter1, parameter2); if (this.Action != null) this.Action(parameter1, parameter2); } } public class Command : BaseCommand { public Action Action; public void Execute(Type parameter1, Type2 parameter2, Type3 parameter3) { for (int j = this.Bindings.Count - 1; j >= 0; j = Math.Min(this.Bindings.Count - 1, j - 1)) ((CommandBinding)this.Bindings[j]).Execute(parameter1, parameter2, parameter3); if (this.Action != null) this.Action(parameter1, parameter2, parameter3); } } public class Command : BaseCommand { public Action Action; public void Execute(Type parameter1, Type2 parameter2, Type3 parameter3, Type4 parameter4) { for (int j = this.Bindings.Count - 1; j >= 0; j = Math.Min(this.Bindings.Count - 1, j - 1)) ((CommandBinding)this.Bindings[j]).Execute(parameter1, parameter2, parameter3, parameter4); if (this.Action != null) this.Action(parameter1, parameter2, parameter3, parameter4); } } } ================================================ FILE: ComponentBind/CommandBinding.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; namespace ComponentBind { public interface ICommandBinding : IBinding { } public abstract class BaseCommandBinding : ICommandBinding where CommandType : BaseCommand { protected CommandType source; protected CommandType[] destinations; protected Func enabled; public bool Enabled { get; set; } protected BaseCommandBinding() { this.Enabled = true; } public void Delete() { this.Enabled = false; this.source.RemoveBinding(this); this.source = null; this.destinations = null; this.enabled = null; } } public class CommandBinding : BaseCommandBinding { public CommandBinding(Command _source, params Command[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Command[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations; this.enabled = enabled; } public CommandBinding(Command _source, params Action[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Action[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations.Select(x => new Command { Action = x }).ToArray(); this.enabled = enabled; } public void Execute() { if (this.Enabled && this.enabled()) { for (int i = 0; i < this.destinations.Length; i++) { this.destinations[i].Execute(); if (this.destinations == null) break; } } } } public class CommandBinding : BaseCommandBinding> { public CommandBinding(Command _source, params Command[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, params Action[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Action[] _destinations) : this(_source, enabled, _destinations.Select(x => new Command { Action = x }).ToArray()) { } public CommandBinding(Command _source, Func enabled, params Command[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations; this.enabled = enabled; } public void Execute(Type parameter1) { if (this.Enabled && this.enabled()) { for (int i = 0; i < this.destinations.Length; i++) { this.destinations[i].Execute(parameter1); if (this.destinations == null) break; } } } } public class CommandBinding : BaseCommandBinding> { public CommandBinding(Command _source, params Command[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, params Action[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Action[] _destinations) : this(_source, enabled, _destinations.Select(x => new Command { Action = x }).ToArray()) { } public CommandBinding(Command _source, Func enabled, params Command[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations; this.enabled = enabled; } public void Execute(Type parameter1, Type2 parameter2) { if (this.Enabled && this.enabled()) { for (int i = 0; i < this.destinations.Length; i++) { this.destinations[i].Execute(parameter1, parameter2); if (this.destinations == null) break; } } } } public class CommandBinding : BaseCommandBinding> { public CommandBinding(Command _source, params Command[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, params Action[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Action[] _destinations) : this(_source, enabled, _destinations.Select(x => new Command { Action = x }).ToArray()) { } public CommandBinding(Command _source, Func enabled, params Command[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations; this.enabled = enabled; } public void Execute(Type parameter1, Type2 parameter2, Type3 parameter3) { if (this.Enabled && this.enabled()) { for (int i = 0; i < this.destinations.Length; i++) { this.destinations[i].Execute(parameter1, parameter2, parameter3); if (this.destinations == null) break; } } } } public class CommandBinding : BaseCommandBinding> { public CommandBinding(Command _source, params Command[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, params Action[] _destinations) : this(_source, () => true, _destinations) { } public CommandBinding(Command _source, Func enabled, params Action[] _destinations) : this(_source, enabled, _destinations.Select(x => new Command { Action = x }).ToArray()) { } public CommandBinding(Command _source, Func enabled, params Command[] _destinations) { this.source = _source; this.source.AddBinding(this); this.destinations = _destinations; this.enabled = enabled; } public void Execute(Type parameter1, Type2 parameter2, Type3 parameter3, Type4 parameter4) { if (this.Enabled && this.enabled()) { for (int i = 0; i < this.destinations.Length; i++) { this.destinations[i].Execute(parameter1, parameter2, parameter3, parameter4); if (this.destinations == null) break; } } } } } ================================================ FILE: ComponentBind/Component.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using Microsoft.Xna.Framework; using System.Xml.Serialization; using System.ComponentModel; using Newtonsoft.Json; namespace ComponentBind { public interface IBindable { void delete(); } public interface IComponent : IBindable { Entity Entity { get; set; } bool Active { get; } bool Serialize { get; set; } void SetMain(BaseMain main); Property Enabled { get; } bool EnabledInEditMode { get; } bool EnabledWhenPaused { get; } Property Suspended { get; } Command Delete { get; } void OnSave(); void Start(); void Awake(); } public interface IGraphicsComponent : IComponent { void LoadContent(bool reload); } public interface IUpdateableComponent : IComponent { void Update(float dt); } public interface IEarlyUpdateableComponent : IComponent { void Update(float dt); Property UpdateOrder { get; } } public class Bindable : IBindable { private List bindings = new List(); public void Add(IBinding binding) { this.bindings.Add(binding); } public void Remove(IBinding binding) { binding.Delete(); this.bindings.Remove(binding); } public void RemoveAllBindings() { for (int i = 0; i < this.bindings.Count; i++) this.bindings[i].Delete(); this.bindings.Clear(); } public virtual void delete() { for (int i = 0; i < this.bindings.Count; i++) this.bindings[i].Delete(); this.bindings.Clear(); } } public class Component : Bindable, IComponent where MainClass : BaseMain { [XmlIgnore] [JsonIgnore] public bool Serialize { get; set; } public Property Enabled { get; set; } [XmlIgnore] [JsonIgnore] public Property Suspended { get; set; } [XmlIgnore] [JsonIgnore] public Command Enable = new Command(); [XmlIgnore] [JsonIgnore] public Command Disable = new Command(); [XmlIgnore] [JsonIgnore] public Command OnSuspended = new Command(); [XmlIgnore] [JsonIgnore] public Command OnResumed = new Command(); private Command del = new Command(); [XmlIgnore] [JsonIgnore] public Command Delete { get { return this.del; } } [XmlIgnore] [JsonIgnore] public bool EnabledInEditMode { get; set; } [XmlIgnore] [JsonIgnore] public bool EnabledWhenPaused { get; set; } protected MainClass main; [XmlIgnore] [JsonIgnore] public virtual Entity Entity { get; set; } [XmlIgnore] [JsonIgnore] public bool Active { get; private set; } public Component() { this.Serialize = true; this.Enabled = new Property { Value = true }; this.Suspended = new Property { Value = false }; this.EnabledInEditMode = true; this.EnabledWhenPaused = true; this.Delete.Action = delegate() { if (this.Active) { this.Active = false; this.main.RemoveComponent(this); } }; this.Add(new CommandBinding(Enable, () => !Enabled, delegate() { Enabled.Value = true; })); this.Add(new CommandBinding(Disable, () => Enabled, delegate() { Enabled.Value = false; })); } public virtual void OnSave() { } public virtual void Start() { } public virtual void Awake() { this.Add(new ChangeBinding(this.Enabled, delegate(bool old, bool value) { if (!old && value) this.Enable.Execute(); else if (old && !value) this.Disable.Execute(); })); this.Add(new ChangeBinding(this.Suspended, delegate(bool old, bool value) { if (!old && value) this.OnSuspended.Execute(); else if (old && !value) this.OnResumed.Execute(); })); } public void SetMain(BaseMain _main) { this.Active = true; this.main = (MainClass)_main; } public override void delete() { if (this.Entity != null) this.Entity.Remove(this); base.delete(); } } } ================================================ FILE: ComponentBind/ComponentBind.csproj ================================================  Debug AnyCPU 8.0.30703 2.0 {2D0637AB-A380-4C97-A329-BF7C40C99495} Library Properties ComponentBind ComponentBind v4.0 512 true full false bin\Debug\ DEBUG;TRACE prompt 4 pdbonly true bin\Release\ TRACE prompt 4 {a9ae40ff-1a21-414a-9fe7-3be13644cc6d} Newtonsoft.Json.Net40 ================================================ FILE: ComponentBind/Entity.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Xml.Serialization; using System.Collections; using System.ComponentModel; using System.Reflection; using Newtonsoft.Json; namespace ComponentBind { [XmlInclude(typeof(Entity.Handle))] [XmlInclude(typeof(Property))] [XmlInclude(typeof(ListProperty))] [XmlInclude(typeof(ListProperty))] [XmlInclude(typeof(Transform))] public class Entity { public class CommandLink { public Handle TargetEntity; [XmlAttribute] [DefaultValue("")] public string TargetCommand; [XmlAttribute] [DefaultValue("")] public string SourceCommand; [XmlIgnore] [JsonIgnore] public Command LinkedTargetCmd; [XmlIgnore] [JsonIgnore] public Command LinkedSourceCmd; } public struct Handle { private ulong guid; [XmlAttribute] [DefaultValue(0)] public ulong GUID; private Entity target; [XmlIgnore] [JsonIgnore] public Entity Target { get { if (this.target == null || this.target.GUID != this.GUID) Entity.guidTable.TryGetValue(this.GUID, out this.target); return this.target; } set { this.target = value; this.GUID = this.target == null ? 0 : this.target.GUID; } } public static implicit operator Entity(Handle obj) { return obj.Target; } public static implicit operator Handle(Entity obj) { return new Handle { Target = obj, GUID = obj == null ? 0 : obj.GUID }; } public override bool Equals(object obj) { if (obj is Handle) return ((Handle)obj).GUID == this.GUID; else if (obj is Entity) return ((Entity)obj).GUID == this.GUID; else return false; } public override int GetHashCode() { return (int)(this.GUID & 0xffffffff); } } private static Dictionary guidTable = new Dictionary(); private static Dictionary idTable = new Dictionary(); private static List componentCache = new List(); public static Entity GetByID(string id) { Entity result; Entity.idTable.TryGetValue(id, out result); return result; } public static Entity GetByGUID(ulong id) { Entity result; Entity.guidTable.TryGetValue(id, out result); return result; } [XmlIgnore] [JsonIgnore] public bool Active = true; [XmlAttribute] public string Type; [XmlIgnore] [JsonIgnore] public bool EditorCanDelete = true; public Property ID = new Property(); public override string ToString() { return string.Format("{0} [{1}]", string.IsNullOrEmpty(this.ID.Value) ? this.GUID.ToString() : this.ID.Value, this.Type); } [XmlIgnore] [JsonIgnore] public bool Serialize = true; [XmlIgnore] [JsonIgnore] public bool CannotSuspend; [XmlIgnore] [JsonIgnore] public bool CannotSuspendByDistance; [XmlIgnore] [JsonIgnore] public bool Added; private BaseMain main; private Dictionary components = new Dictionary(); private Dictionary componentsByType = new Dictionary(); private List bindings = new List(); public static ulong CurrentGUID = 1; [XmlAttribute] public ulong GUID; private Dictionary commands = new Dictionary(); private readonly Dictionary properties = new Dictionary(); [XmlIgnore] [JsonIgnore] public Command Delete = new Command(); [XmlArray("LinkedCommands")] [XmlArrayItem("CommandLink", typeof(CommandLink))] [JsonProperty] public ListProperty LinkedCommands = new ListProperty(); [XmlArray("Components")] [XmlArrayItem("Component", Type = typeof(DictionaryEntry))] [JsonProperty] public DictionaryEntry[] Components { get { // Make an array of DictionaryEntries to return IEnumerable> serializableComponents = this.components.Where(x => x.Value.Serialize); DictionaryEntry[] ret = new DictionaryEntry[serializableComponents.Count()]; int i = 0; DictionaryEntry de; // Iterate through properties to load items into the array. foreach (KeyValuePair component in serializableComponents) { de = new DictionaryEntry(); de.Key = component.Key; de.Value = component.Value; component.Value.OnSave(); ret[i] = de; i++; } if (this.OnSave != null) this.OnSave.Execute(); return ret; } set { this.components.Clear(); for (int i = 0; i < value.Length; i++) { IComponent c = value[i].Value as IComponent; if (c != null) { this.components.Add((string)value[i].Key, c); Type t = c.GetType(); do { this.componentsByType[t] = c; t = t.BaseType; } while (t.Assembly != Entity.componentBindAssembly); } } } } [XmlIgnore] [JsonIgnore] public Dictionary ComponentDictionary { get { return this.components; } } [XmlIgnore] [JsonIgnore] public IEnumerable> Commands { get { return this.commands; } } [XmlIgnore] [JsonIgnore] public IEnumerable> Properties { get { return this.properties; } } public Entity() { // Called by XmlSerializer this.Delete.Action = (Action)this.delete; } [XmlIgnore] [JsonIgnore] public Command OnSave; [XmlIgnore] [JsonIgnore] public Property EditorSelected; private static Assembly componentBindAssembly; static Entity() { Entity.componentBindAssembly = Assembly.GetExecutingAssembly(); } public Entity(BaseMain _main, string _type) : this() { // Called by a Factory this.Type = _type; } public void ClearGUID() { if (this.GUID != 0) Entity.guidTable.Remove(this.GUID); } public void NewGUID() { this.ClearGUID(); this.GUID = Entity.CurrentGUID; Entity.CurrentGUID++; Entity.guidTable.Add(this.GUID, this); } public void SetMain(BaseMain _main) { if (this.GUID == 0) this.GUID = Entity.CurrentGUID; Entity.CurrentGUID = Math.Max(Entity.CurrentGUID, this.GUID + 1); Entity.guidTable.Add(this.GUID, this); this.main = _main; if (!string.IsNullOrEmpty(this.ID)) Entity.idTable.Add(this.ID, this); if (_main.EditorEnabled) { this.OnSave = new Command(); this.EditorSelected = new Property(); string oldId = this.ID; this.Add(new NotifyBinding(delegate() { if (!string.IsNullOrEmpty(oldId)) Entity.idTable.Remove(oldId); if (!string.IsNullOrEmpty(this.ID)) Entity.idTable.Add(this.ID, this); oldId = this.ID; }, this.ID)); } Entity.componentCache.AddRange(this.components.Values); for (int i = 0; i < Entity.componentCache.Count; i++) { IComponent c = Entity.componentCache[i]; c.Entity = this; this.main.AddComponent(c); } Entity.componentCache.Clear(); } public void SetSuspended(bool suspended) { Entity.componentCache.AddRange(this.components.Values); for (int i = 0; i < Entity.componentCache.Count; i++) { IComponent c = Entity.componentCache[i]; if (c.Suspended.Value != suspended) c.Suspended.Value = suspended; } Entity.componentCache.Clear(); } public void LinkedCommandCall(CommandLink link) { if (link.LinkedTargetCmd != null) link.LinkedTargetCmd.Execute(); else if (link.TargetEntity.Target != null) { Command destCommand = link.TargetEntity.Target.getCommand(link.TargetCommand); if (destCommand != null) { link.LinkedTargetCmd = destCommand; destCommand.Execute(); } } } public void Add(string name, Command cmd, Command.Perms perms = Command.Perms.Linkable, string description = null) { Command.Entry entry = new Command.Entry { Command = cmd, Permissions = perms, Key = name }; if (this.main.EditorEnabled) entry.Description = description; this.commands.Add(name, entry); for (int i = 0; i < this.LinkedCommands.Count; i++) { CommandLink link = this.LinkedCommands[i]; if (link.LinkedSourceCmd == null && name == link.SourceCommand) { link.LinkedSourceCmd = cmd; this.Add(new CommandBinding(link.LinkedSourceCmd, () => LinkedCommandCall(link))); } } } public void Add(string name, IComponent component) { this.components.Add(name, component); Type t = component.GetType(); do { this.componentsByType[t] = component; t = t.BaseType; } while (t.Assembly != Entity.componentBindAssembly); if (this.main != null) { component.Entity = this; this.main.AddComponent(component); } } public void AddWithoutOverwriting(string name, IComponent component) { this.components.Add(name, component); Type t = component.GetType(); do { if (!this.componentsByType.ContainsKey(t)) this.componentsByType[t] = component; t = t.BaseType; } while (t.Assembly != Entity.componentBindAssembly); if (this.main != null) { component.Entity = this; this.main.AddComponent(component); } } public void Add(IComponent component) { component.Serialize = false; this.Add(Guid.NewGuid().ToString(), component); } public void Add(string name, IProperty prop, PropertyEntry.EditorData editorData) { this.properties.Add(name, new PropertyEntry(prop, this.main.EditorEnabled ? editorData : null)); } public void Add(string name, IProperty prop, string description = null, bool readOnly = false) { PropertyEntry.EditorData data = null; if (this.main.EditorEnabled) { data = new PropertyEntry.EditorData(); data.Readonly = readOnly; data.Description = description; } this.properties.Add(name, new PropertyEntry(prop, data)); } public void RemoveProperty(string name) { try { this.properties.Remove(name); } catch (KeyNotFoundException) { } } public void RemoveCommand(string name) { try { this.commands.Remove(name); } catch (KeyNotFoundException) { } for (int i = this.LinkedCommands.Count - 1; i >= 0; i--) { CommandLink link = this.LinkedCommands[i]; if (link.SourceCommand == name) this.LinkedCommands.RemoveAt(i); } } public Property GetProperty(string name) { if (name == null) return null; PropertyEntry result; this.properties.TryGetValue(name, out result); if (result == null) return null; else return (Property)result.Property; } public IProperty GetProperty(string name) { if (name == null) return null; PropertyEntry result; this.properties.TryGetValue(name, out result); if (result == null) return null; else return result.Property; } public void AddWithoutOverwriting(IComponent component) { this.AddWithoutOverwriting(Guid.NewGuid().ToString(), component); } public void Add(IBinding binding) { this.bindings.Add(binding); } public void RemoveComponent(string name) { IComponent c; this.components.TryGetValue(name, out c); if (c != null) { this.components.Remove(name); this.removeComponentTypeMapping(c); } } public void Remove(IBinding b) { b.Delete(); this.bindings.Remove(b); } public void Remove(IComponent c) { foreach (KeyValuePair pair in this.components) { if (pair.Value == c) { this.components.Remove(pair.Key); break; } } this.removeComponentTypeMapping(c); } private void removeComponentTypeMapping(IComponent c) { Type type = c.GetType(); do { IComponent typeComponent = null; this.componentsByType.TryGetValue(type, out typeComponent); if (typeComponent == c) { bool foundReplacement = false; foreach (IComponent c2 in this.components.Values) { if (c2.GetType().Equals(type)) { this.componentsByType[type] = c2; foundReplacement = true; break; } } if (!foundReplacement) this.componentsByType.Remove(type); } type = type.BaseType; } while (type.Assembly != Entity.componentBindAssembly); } public T Get() where T : IComponent { IComponent result = null; this.componentsByType.TryGetValue(typeof(T), out result); return (T)result; } public T GetOrCreate() where T : IComponent, new() { IComponent result = null; this.componentsByType.TryGetValue(typeof(T), out result); if (result == null) { result = new T(); this.Add(result); } return (T)result; } public IEnumerable GetAll() where T : IComponent { return this.components.Values.OfType(); } public T Get(string name) where T : IComponent { IComponent result = null; this.components.TryGetValue(name, out result); return (T)result; } public T GetOrCreate(string name) where T : IComponent, new() { IComponent result = null; this.components.TryGetValue(name, out result); if (result == null) { result = new T(); this.Add(name, result); } return (T)result; } public T GetOrCreateWithoutOverwriting(string name) where T : IComponent, new() { IComponent result = null; this.components.TryGetValue(name, out result); if (result == null) { result = new T(); this.AddWithoutOverwriting(name, result); } return (T)result; } public T Create(string name = null) where T : IComponent, new() { T result = new T(); if (name == null) this.Add(result); else this.Add(name, result); return (T)result; } public T GetOrCreate(string name, out bool created) where T : IComponent, new() { IComponent result = null; this.components.TryGetValue(name, out result); created = false; if (result == null) { created = true; result = new T(); this.Add(name, result); } return (T)result; } private Command getCommand(string name) { Command.Entry result; if (this.commands.TryGetValue(name, out result)) return result.Command; else return null; } protected void delete() { if (this.Active) { this.Active = false; Entity.componentCache.AddRange(this.components.Values); this.components.Clear(); this.componentsByType.Clear(); for (int i = 0; i < Entity.componentCache.Count; i++) Entity.componentCache[i].Delete.Execute(); Entity.componentCache.Clear(); for (int i = 0; i < this.bindings.Count; i++) this.bindings[i].Delete(); this.bindings.Clear(); this.commands.Clear(); this.main.Remove(this); this.ClearGUID(); if (!string.IsNullOrEmpty(this.ID)) Entity.idTable.Remove(this.ID); } } } } ================================================ FILE: ComponentBind/Factory.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using Microsoft.Xna.Framework; using System.Collections; using System.Reflection; using System.Xml.Serialization; namespace ComponentBind { public class Factory { protected static Dictionary factories = new Dictionary(); protected static Dictionary factoriesByType = new Dictionary(); public static T Get() where T : Factory { Factory result = null; Factory.factoriesByType.TryGetValue(typeof(T), out result); return (T)result; } public static Factory Get(string type) { Factory result = null; Factory.factories.TryGetValue(type, out result); return result; } public Vector3 Color = Vector3.One; public bool EditorCanSpawn = true; public bool AvailableInRelease = true; } public class Factory : Factory where MainClass : BaseMain { static Factory() { Factory.Initialize(); } public static void Initialize() { Factory.factories.Clear(); Factory.factoriesByType.Clear(); Type baseType = typeof(Factory); foreach (Type type in Assembly.GetEntryAssembly().GetTypes().Where(t => t != baseType && baseType.IsAssignableFrom(t))) { if (!type.ContainsGenericParameters) { Factory factory = (Factory)type.GetConstructor(new Type[] { }).Invoke(new object[] { }); Factory.factories.Add(type.Name.Substring(0, type.Name.Length - "Factory".Length), factory); Factory.factoriesByType.Add(type, factory); } } } public static new Factory Get(string type) { Factory result = null; Factory.factories.TryGetValue(type, out result); return (Factory)result; } public virtual Entity Create(MainClass main) { return null; } public Entity CreateAndBind(MainClass main) { Entity entity = this.Create(main); this.Bind(entity, main, true); return entity; } public void SetMain(Entity entity, MainClass main) { entity.SetMain(main); if (main.EditorEnabled) { Factory.GlobalEditorComponents(entity, main); this.AttachEditorComponents(entity, main); } } public virtual void Bind(Entity entity, MainClass main, bool creating = false) { this.SetMain(entity, main); } public static Action, Entity, MainClass> DefaultEditorComponents; public static Action GlobalEditorComponents; public virtual void AttachEditorComponents(Entity entity, MainClass main) { Factory.DefaultEditorComponents(this, entity, main); } } } ================================================ FILE: ComponentBind/Log.cs ================================================ using System; using ComponentBind; using System.Collections.Generic; using System.Linq; using System.Text; using System.Reflection; using System.Diagnostics; namespace ComponentBind { public class Log { public static Action Handler = (Action)Console.WriteLine; public static void d(string log) { StackTrace trace = new StackTrace(); MethodBase method = trace.GetFrame(1).GetMethod(); Log.Handler(string.Format("{0}.{1}: {2}", method.ReflectedType.Name, method.Name, log)); } } } ================================================ FILE: ComponentBind/Properties/AssemblyInfo.cs ================================================ using System.Reflection; using System.Runtime.CompilerServices; using System.Runtime.InteropServices; // General Information about an assembly is controlled through the following // set of attributes. Change these attribute values to modify the information // associated with an assembly. [assembly: AssemblyTitle("ComponentBind")] [assembly: AssemblyDescription("")] [assembly: AssemblyConfiguration("")] [assembly: AssemblyCompany("")] [assembly: AssemblyProduct("ComponentBind")] [assembly: AssemblyCopyright("Copyright © 2014")] [assembly: AssemblyTrademark("")] [assembly: AssemblyCulture("")] // Setting ComVisible to false makes the types in this assembly not visible // to COM components. If you need to access a type in this assembly from // COM, set the ComVisible attribute to true on that type. [assembly: ComVisible(false)] // The following GUID is for the ID of the typelib if this project is exposed to COM [assembly: Guid("406447d3-9e9c-4c83-a9de-4c362711c098")] // Version information for an assembly consists of the following four values: // // Major Version // Minor Version // Build Number // Revision // // You can specify all the values or you can default the Build and Revision Numbers // by using the '*' as shown below: // [assembly: AssemblyVersion("1.0.*")] [assembly: AssemblyVersion("1.0.0.0")] [assembly: AssemblyFileVersion("1.0.0.0")] ================================================ FILE: ComponentBind/Property.cs ================================================ using System; using System.Collections.Generic; using System.Linq; using System.Text; using System.Xml.Serialization; using System.ComponentModel; using System.Collections; using System.Diagnostics; using Newtonsoft.Json; namespace ComponentBind { public interface IProperty { void AddBinding(IPropertyBinding binding); void RemoveBinding(IPropertyBinding binding); void Reset(); Type PropertyType { get; } } public class PropertyEntry { public IProperty Property; public EditorData Data; public PropertyEntry(IProperty property, string description) { this.Property = property; this.Data = new EditorData(); this.Data.Description = description; } public PropertyEntry(IProperty property, EditorData data) { this.Property = property; this.Data = data; } public class EditorData { public Property Visible; public string Description; public IListProperty Options; public bool Readonly; public bool RefreshOnChange; public int IChangeBy = 1; public float FChangeBy = 1f; public byte BChangeBy = 1; } } [DebuggerDisplay("Property {Value}")] public class Property : IProperty { [XmlIgnore] [JsonIgnore] protected Type _value; protected List bindings = new List(); public void AddBinding(IPropertyBinding binding) { this.bindings.Add(binding); } public void RemoveBinding(IPropertyBinding binding) { this.bindings.Remove(binding); } public void Changed() { for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].OnChanged(this); } public Type Value { get { return this._value; } set { this.InternalSet(value, null); } } public void Reset() { this.InternalSet(this._value, null); } [XmlIgnore] [JsonIgnore] public System.Type PropertyType { get { return typeof(Type); } } public void SetStealthy(Type t) { this._value = t; } public void InternalSet(Type obj, IPropertyBinding binding) { this._value = obj; for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) { IPropertyBinding b = this.bindings[j]; if (b != binding) b.OnChanged(this); } } public static implicit operator Type(Property obj) { return obj._value; } public override string ToString() { return this._value.ToString(); } } public interface IListProperty : IProperty { void CopyTo(IListProperty dest); } public class ListProperty : ICollection, IListProperty { public delegate void ItemAddedEventHandler(int index, Type t); public delegate void ItemRemovedEventHandler(int index, Type t); public delegate void ItemChangedEventHandler(int index, Type old, Type newValue); public delegate void ClearEventHandler(); public int Count { get { return this.list.Count; } } public Property Length = new Property(); public event ItemAddedEventHandler ItemAdded; public event ItemRemovedEventHandler ItemRemoved; public event ItemChangedEventHandler ItemChanged; public event ClearEventHandler Cleared; public event ClearEventHandler Clearing; public void Reset() { } public System.Type PropertyType { get { return typeof (Type); } } private List list = new List(); protected List> bindings = new List>(); public void AddBinding(IPropertyBinding binding) { if (!this.bindings.Contains(binding)) this.bindings.Add((IListBinding)binding); } public void RemoveBinding(IPropertyBinding binding) { this.bindings.Remove((IListBinding)binding); } public bool IsReadOnly { get { return false; } } public bool Contains(Type t) { return this.list.Contains(t); } public void CopyTo(Type[] array, int arrayIndex) { this.list.CopyTo(array, arrayIndex); } IEnumerator IEnumerable.GetEnumerator() { return this.list.GetEnumerator(); } IEnumerator IEnumerable.GetEnumerator() { return this.list.GetEnumerator(); } public Type this[int i] { get { return this.list[i]; } set { this.Changed(i, value); } } public void Changed() { for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].OnChanged(this); } public void CopyTo(IListProperty dest) { ListProperty list = (ListProperty)dest; list.Clear(); for (int i = 0; i < this.list.Count; i++) list.Add(this.list[i]); } public void Add(Type t) { this.list.Add(t); this.Length.Value = this.list.Count; for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].Add(t, this); if (this.ItemAdded != null) this.ItemAdded(this.list.Count - 1, t); } public int IndexOf(Type t) { return this.list.IndexOf(t); } public void AddAll(IEnumerable items) { foreach (Type t in items) this.Add(t); } public void Insert(int index, Type t) { this.list.Insert(index, t); this.Length.Value = this.list.Count; for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].Add(t, this); if (this.ItemAdded != null) this.ItemAdded(index, t); } public void RemoveAt(int index) { Type t = this.list[index]; this.list.RemoveAt(index); this.Length.Value = this.list.Count; if (this.ItemRemoved != null) this.ItemRemoved(index, t); for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].Remove(t, this); } public bool Remove(Type t) { int index = this.list.IndexOf(t); this.list.RemoveAt(index); this.Length.Value = this.list.Count; if (this.ItemRemoved != null) this.ItemRemoved(index, t); for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].Remove(t, this); return true; } public void RemoveWithoutNotifying(Type t) { int index = this.list.IndexOf(t); this.list.RemoveAt(index); this.Length.Value = this.list.Count; for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].Remove(t, this); } public void RemoveAll(IEnumerable items) { foreach (Type t in items) this.Remove(t); } public void Changed(Type from, Type to) { int i = this.list.IndexOf(from); this.list[i] = to; if (this.ItemChanged != null) this.ItemChanged(i, from, to); for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].OnChanged(from, to, this); } public void Changed(int i, Type to) { Type from = this.list[i]; this.list[i] = to; if (this.ItemChanged != null) this.ItemChanged(i, from, to); for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].OnChanged(from, to, this); } public void Clear() { bool notify = this.list.Count > 0; if (notify && this.Clearing != null) this.Clearing(); this.list.Clear(); this.Length.Value = 0; if (notify) { if (this.Cleared != null) this.Cleared(); for (int i = this.bindings.Count - 1; i >= 0; i = Math.Min(this.bindings.Count - 1, i - 1)) this.bindings[i].Clear(this); } } public void Changed(Type t) { if (this.ItemChanged != null) { int i = this.list.IndexOf(t); this.ItemChanged(i, t, t); } for (int j = this.bindings.Count - 1; j >= 0; j = Math.Min(this.bindings.Count - 1, j - 1)) this.bindings[j].OnChanged(t, t, this); } } } ================================================ FILE: ComponentBind/Transform.cs ================================================ using System; using ComponentBind; using System.Collections.Generic; using System.Linq; using System.Text; using Microsoft.Xna.Framework; using System.Xml.Serialization; namespace ComponentBind { public class Transform : Component { public Property Position = new Property(); public Property Quaternion = new Property(); [XmlIgnore] public Property Matrix = new Property { Value = Microsoft.Xna.Framework.Matrix.Identity }; [XmlIgnore] public Property Selectable = new Property { Value = true }; public void EditorProperties() { this.Entity.Add("Position", this.Position); this.Entity.Add("Quaternion", this.Quaternion); } public override void Awake() { base.Awake(); this.Add(new TwoWayBinding( this.Position, delegate(Matrix value) { return value.Translation; }, this.Matrix, delegate(Vector3 value) { Matrix matrix = this.Matrix.Value; matrix.Translation = value; return matrix; })); this.Add(new TwoWayBinding( this.Quaternion, delegate(Matrix value) { Vector3 scale, translation; Quaternion rotation; value.Decompose(out scale, out rotation, out translation); return rotation; }, this.Matrix, delegate(Quaternion value) { Matrix original = this.Matrix; Matrix result = Microsoft.Xna.Framework.Matrix.CreateFromQuaternion(value); result.Translation = original.Translation; return result; })); } } } ================================================ FILE: Dialogger/app.js ================================================ var app = module.exports = require('appjs'); app.serveFilesFrom(__dirname + '/content'); var window = app.createWindow( { width : 1280, height : 720, icons : __dirname + '/content/icons' }); window.on('create', function() { window.frame.show(); window.frame.center(); }); window.on('ready', function() { window.require = require; window.process = process; window.module = module; function F12(e) { return e.keyIdentifier === 'F12' } function Command_Option_J(e) { return e.keyCode === 74 && e.metaKey && e.altKey } window.addEventListener('keydown', function(e) { if (F12(e) || Command_Option_J(e)) window.frame.openDevTools(); }); window.dispatchEvent(new window.Event('app-ready')); }); ================================================ FILE: Dialogger/dialogger.js ================================================ var fs = null; addEventListener('app-ready', function(e) { // We're running inside app.js fs = require('fs'); $('#import').hide(); $('#export').hide(); $('#export-game').hide(); }); var graph = new joint.dia.Graph(); var defaultLink = new joint.dia.Link( { attrs: { '.marker-target': { d: 'M 10 0 L 0 5 L 10 10 z', }, '.link-tools .tool-remove circle, .marker-vertex': { r: 8 }, }, }); defaultLink.set('smooth', true); var allowableConnections = [ ['dialogue.Text', 'dialogue.Text'], ['dialogue.Text', 'dialogue.Node'], ['dialogue.Text', 'dialogue.Choice'], ['dialogue.Text', 'dialogue.Set'], ['dialogue.Text', 'dialogue.Branch'], ['dialogue.Node', 'dialogue.Text'], ['dialogue.Node', 'dialogue.Node'], ['dialogue.Node', 'dialogue.Choice'], ['dialogue.Node', 'dialogue.Set'], ['dialogue.Node', 'dialogue.Branch'], ['dialogue.Choice', 'dialogue.Text'], ['dialogue.Choice', 'dialogue.Node'], ['dialogue.Choice', 'dialogue.Set'], ['dialogue.Choice', 'dialogue.Branch'], ['dialogue.Set', 'dialogue.Text'], ['dialogue.Set', 'dialogue.Node'], ['dialogue.Set', 'dialogue.Set'], ['dialogue.Set', 'dialogue.Branch'], ['dialogue.Branch', 'dialogue.Text'], ['dialogue.Branch', 'dialogue.Node'], ['dialogue.Branch', 'dialogue.Set'], ['dialogue.Branch', 'dialogue.Branch'], ]; function validateConnection(cellViewS, magnetS, cellViewT, magnetT, end, linkView) { // Prevent loop linking if (magnetS == magnetT) return false; if (cellViewS == cellViewT) return false; if (magnetT.attributes.magnet.nodeValue !== 'passive') // Can't connect to an output port return false; var sourceType = cellViewS.model.attributes.type; var targetType = cellViewT.model.attributes.type; var valid = false; for (var i = 0; i < allowableConnections.length; i++) { var rule = allowableConnections[i]; if (sourceType == rule[0] && targetType == rule[1]) { valid = true; break; } } if (!valid) return false; var links = graph.getConnectedLinks(cellViewS.model); for (var i = 0; i < links.length; i++) { var link = links[i]; if (link.attributes.source.id === cellViewS.model.id && link.attributes.source.port === magnetS.attributes.port.nodeValue && link.attributes.target.id) { var targetCell = graph.getCell(link.attributes.target.id); if (targetCell.attributes.type !== targetType) return false; // We can only connect to multiple targets of the same type if (targetCell == cellViewT.model) return false; // Already connected } } return true; } function validateMagnet(cellView, magnet) { if (magnet.getAttribute('magnet') === 'passive') return false; // If unlimited connections attribute is null, we can only ever connect to one object // If it is not null, it is an array of type strings which are allowed to have unlimited connections var unlimitedConnections = magnet.getAttribute('unlimitedConnections'); var links = graph.getConnectedLinks(cellView.model); for (var i = 0; i < links.length; i++) { var link = links[i]; if (link.attributes.source.id === cellView.model.id && link.attributes.source.port === magnet.attributes.port.nodeValue) { // This port already has a connection if (unlimitedConnections && link.attributes.target.id) { var targetCell = graph.getCell(link.attributes.target.id); if (unlimitedConnections.indexOf(targetCell.attributes.type) !== -1) return true; // It's okay because this target type has unlimited connections } return false; } } return true; } joint.shapes.dialogue = {}; joint.shapes.dialogue.Base = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Base', size: { width: 200, height: 64 }, name: '', attrs: { rect: { stroke: 'none', 'fill-opacity': 0 }, text: { display: 'none' }, '.inPorts circle': { magnet: 'passive' }, '.outPorts circle': { magnet: true, }, }, }, joint.shapes.devs.Model.prototype.defaults ), }); joint.shapes.dialogue.BaseView = joint.shapes.devs.ModelView.extend( { template: [ '
', '', '', '', '
', ].join(''), initialize: function() { _.bindAll(this, 'updateBox'); joint.shapes.devs.ModelView.prototype.initialize.apply(this, arguments); this.$box = $(_.template(this.template)()); // Prevent paper from handling pointerdown. this.$box.find('input').on('mousedown click', function(evt) { evt.stopPropagation(); }); // This is an example of reacting on the input change and storing the input data in the cell model. this.$box.find('input.name').on('change', _.bind(function(evt) { this.model.set('name', $(evt.target).val()); }, this)); this.$box.find('.delete').on('click', _.bind(this.model.remove, this.model)); // Update the box position whenever the underlying model changes. this.model.on('change', this.updateBox, this); // Remove the box when the model gets removed from the graph. this.model.on('remove', this.removeBox, this); this.updateBox(); }, render: function() { joint.shapes.devs.ModelView.prototype.render.apply(this, arguments); this.paper.$el.prepend(this.$box); this.updateBox(); return this; }, updateBox: function() { // Set the position and dimension of the box so that it covers the JointJS element. var bbox = this.model.getBBox(); // Example of updating the HTML with a data stored in the cell model. var nameField = this.$box.find('input.name'); if (!nameField.is(':focus')) nameField.val(this.model.get('name')); var label = this.$box.find('.label'); var type = this.model.get('type').slice('dialogue.'.length); label.text(type); label.attr('class', 'label ' + type); this.$box.css({ width: bbox.width, height: bbox.height, left: bbox.x, top: bbox.y, transform: 'rotate(' + (this.model.get('angle') || 0) + 'deg)' }); }, removeBox: function(evt) { this.$box.remove(); }, }); joint.shapes.dialogue.Node = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Node', inPorts: ['input'], outPorts: ['output'], attrs: { '.outPorts circle': { unlimitedConnections: ['dialogue.Choice'], } }, }, joint.shapes.dialogue.Base.prototype.defaults ), }); joint.shapes.dialogue.NodeView = joint.shapes.dialogue.BaseView; joint.shapes.dialogue.Text = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Text', inPorts: ['input'], outPorts: ['output'], attrs: { '.outPorts circle': { unlimitedConnections: ['dialogue.Choice'], } }, }, joint.shapes.dialogue.Base.prototype.defaults ), }); joint.shapes.dialogue.TextView = joint.shapes.dialogue.BaseView; joint.shapes.dialogue.Choice = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Choice', inPorts: ['input'], outPorts: ['output'], }, joint.shapes.dialogue.Base.prototype.defaults ), }); joint.shapes.dialogue.ChoiceView = joint.shapes.dialogue.BaseView; joint.shapes.dialogue.Branch = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Branch', size: { width: 200, height: 100, }, inPorts: ['input'], outPorts: ['output0'], values: [], }, joint.shapes.dialogue.Base.prototype.defaults ), }); joint.shapes.dialogue.BranchView = joint.shapes.dialogue.BaseView.extend( { template: [ '
', '', '', '', '', '', '', '
', ].join(''), initialize: function() { joint.shapes.dialogue.BaseView.prototype.initialize.apply(this, arguments); this.$box.find('.add').on('click', _.bind(this.addPort, this)); this.$box.find('.remove').on('click', _.bind(this.removePort, this)); }, removePort: function() { if (this.model.get('outPorts').length > 1) { var outPorts = this.model.get('outPorts').slice(0); outPorts.pop(); this.model.set('outPorts', outPorts); var values = this.model.get('values').slice(0); values.pop(); this.model.set('values', values); this.updateSize(); } }, addPort: function() { var outPorts = this.model.get('outPorts').slice(0); outPorts.push('output' + outPorts.length.toString()); this.model.set('outPorts', outPorts); var values = this.model.get('values').slice(0); values.push(null); this.model.set('values', values); this.updateSize(); }, updateBox: function() { joint.shapes.dialogue.BaseView.prototype.updateBox.apply(this, arguments); var values = this.model.get('values'); var valueFields = this.$box.find('input.value'); // Add value fields if necessary for (var i = valueFields.length; i < values.length; i++) { // Prevent paper from handling pointerdown. var field = $(''); field.attr('placeholder', 'Value ' + (i + 1).toString()); field.attr('index', i); this.$box.append(field); field.on('mousedown click', function(evt) { evt.stopPropagation(); }); // This is an example of reacting on the input change and storing the input data in the cell model. field.on('change', _.bind(function(evt) { var values = this.model.get('values').slice(0); values[$(evt.target).attr('index')] = $(evt.target).val(); this.model.set('values', values); }, this)); } // Remove value fields if necessary for (var i = values.length; i < valueFields.length; i++) $(valueFields[i]).remove(); // Update value fields valueFields = this.$box.find('input.value'); for (var i = 0; i < valueFields.length; i++) { var field = $(valueFields[i]); if (!field.is(':focus')) field.val(values[i]); } }, updateSize: function() { var textField = this.$box.find('input.name'); var height = textField.outerHeight(true); this.model.set('size', { width: 200, height: 100 + Math.max(0, (this.model.get('outPorts').length - 1) * height) }); }, }); joint.shapes.dialogue.Set = joint.shapes.devs.Model.extend( { defaults: joint.util.deepSupplement ( { type: 'dialogue.Set', inPorts: ['input'], outPorts: ['output'], size: { width: 200, height: 100, }, value: '', }, joint.shapes.dialogue.Base.prototype.defaults ), }); joint.shapes.dialogue.SetView = joint.shapes.dialogue.BaseView.extend( { template: [ '
', '', '', '', '', '
', ].join(''), initialize: function() { joint.shapes.dialogue.BaseView.prototype.initialize.apply(this, arguments); this.$box.find('input.value').on('change', _.bind(function(evt) { this.model.set('value', $(evt.target).val()); }, this)); }, updateBox: function() { joint.shapes.dialogue.BaseView.prototype.updateBox.apply(this, arguments); var field = this.$box.find('input.value'); if (!field.is(':focus')) field.val(this.model.get('value')); }, }); function gameData() { var cells = graph.toJSON().cells; var nodesByID = {}; var cellsByID = {}; var nodes = []; for (var i = 0; i < cells.length; i++) { var cell = cells[i]; if (cell.type != 'link') { var node = { type: cell.type.slice('dialogue.'.length), id: cell.id, }; if (node.type == 'Branch') { node.variable = cell.name; node.branches = {}; for (var j = 0; j < cell.values.length; j++) { var branch = cell.values[j]; node.branches[branch] = null; } } else if (node.type == 'Set') { node.variable = cell.name; node.value = cell.value; node.next = null; } else { node.name = cell.name; node.next = null; } nodes.push(node); nodesByID[cell.id] = node; cellsByID[cell.id] = cell; } } for (var i = 0; i < cells.length; i++) { var cell = cells[i]; if (cell.type == 'link') { var source = nodesByID[cell.source.id]; var target = cell.target ? nodesByID[cell.target.id] : null; if (source) { if (source.type == 'Branch') { var portNumber = parseInt(cell.source.port.slice('output'.length)); var value; if (portNumber == 0) value = '_default'; else { var sourceCell = cellsByID[source.id]; value = sourceCell.values[portNumber - 1]; } source.branches[value] = target ? target.id : null; } else if ((source.type == 'Text' || source.type == 'Node') && target && target.type == 'Choice') { if (!source.choices) { source.choices = []; delete source.next; } source.choices.push(target.id); } else source.next = target ? target.id : null; } } } return nodes; } // Menu actions var filename = null; var defaultFilename = 'dialogue.dl'; function flash(text) { var $flash = $('#flash'); $flash.text(text); $flash.stop(true, true); $flash.show(); $flash.css('opacity', 1.0); $flash.fadeOut({ duration: 1500 }); } function offerDownload(name, data) { var a = $(''); a.attr('download', name); a.attr('href', 'data:application/json,' + encodeURIComponent(JSON.stringify(data))); a.attr('target', '_blank'); a.hide(); $('body').append(a); a[0].click(); a.remove(); } function promptFilename(callback) { if (fs) { filename = null; window.frame.openDialog( { type: 'save', }, function(err, files) { if (!err && files.length == 1) { filename = files[0]; callback(filename); } }); } else { filename = prompt('Filename', defaultFilename); callback(filename); } } function applyTextFields() { $('input[type=text]').blur(); } function save() { applyTextFields(); if (!filename) promptFilename(doSave); else doSave(); } function doSave() { if (filename) { if (fs) { fs.writeFileSync(filename, JSON.stringify(graph), 'utf8'); fs.writeFileSync(gameFilenameFromNormalFilename(filename), JSON.stringify(gameData()), 'utf8'); } else { if (!localStorage[filename]) addFileEntry(filename); localStorage[filename] = JSON.stringify(graph); } flash('Saved ' + filename); } } function load() { if (fs) { window.frame.openDialog( { type: 'open', multiSelect: false, }, function(err, files) { if (!err && files.length == 1) { graph.clear(); filename = files[0]; graph.fromJSON(JSON.parse(fs.readFileSync(filename, 'utf8'))); } }); } else $('#menu').show(); } function exportFile() { if (!fs) { applyTextFields(); offerDownload(filename ? filename : defaultFilename, graph); } } function gameFilenameFromNormalFilename(f) { return f.substring(0, f.length - 2) + 'dlz'; } function exportGameFile() { if (!fs) { applyTextFields(); offerDownload(gameFilenameFromNormalFilename(filename ? filename : defaultFilename), gameData()); } } function importFile() { if (!fs) $('#file').click(); } function add(constructor) { return function() { var position = $('#cmroot').position(); var container = $('#container')[0]; var element = new constructor( { position: { x: position.left + container.scrollLeft, y: position.top + container.scrollTop }, }); graph.addCells([element]); }; } function clear() { graph.clear(); filename = null; } // Browser stuff var paper = new joint.dia.Paper( { el: $('#paper'), width: 16000, height: 8000, model: graph, gridSize: 1, defaultLink: defaultLink, validateConnection: validateConnection, validateMagnet: validateMagnet, // Enable link snapping within 75px lookup radius snapLinks: { radius: 75 } }); var panning = false; var mousePosition = { x: 0, y: 0 }; paper.on('blank:pointerdown', function(e, x, y) { panning = true; mousePosition.x = e.pageX; mousePosition.y = e.pageY; $('body').css('cursor', 'move'); applyTextFields(); }); paper.on('cell:pointerdown', function(e, x, y) { applyTextFields(); }); $('#container').mousemove(function(e) { if (panning) { var $this = $(this); $this.scrollLeft($this.scrollLeft() + mousePosition.x - e.pageX); $this.scrollTop($this.scrollTop() + mousePosition.y - e.pageY); mousePosition.x = e.pageX; mousePosition.y = e.pageY; } }); $('#container').mouseup(function (e) { panning = false; $('body').css('cursor', 'default'); }); function handleFiles(files) { filename = files[0].name; var fileReader = new FileReader(); fileReader.onload = function(e) { graph.clear(); graph.fromJSON(JSON.parse(e.target.result)); }; fileReader.readAsText(files[0]); } $('#file').on('change', function() { handleFiles(this.files); }); $('body').on('dragenter', function(e) { e.stopPropagation(); e.preventDefault(); }); $('body').on('dragexit', function(e) { e.stopPropagation(); e.preventDefault(); }); $('body').on('dragover', function(e) { e.stopPropagation(); e.preventDefault(); }); $('body').on('drop', function(e) { e.stopPropagation(); e.preventDefault(); handleFiles(e.originalEvent.dataTransfer.files); }); $(window).on('keydown', function(event) { // Catch Ctrl-S or key code 19 on Mac (Cmd-S) if (((event.ctrlKey || event.metaKey) && String.fromCharCode(event.which).toLowerCase() == 's') || event.which == 19) { event.stopPropagation(); event.preventDefault(); save(); return false; } else if ((event.ctrlKey || event.metaKey) && String.fromCharCode(event.which).toLowerCase() == 'o') { event.stopPropagation(); event.preventDefault(); load(); return false; } else if ((event.ctrlKey || event.metaKey) && String.fromCharCode(event.which).toLowerCase() == 'e') { event.stopPropagation(); event.preventDefault(); exportFile(); return false; } return true; }); $(window).resize(function() { applyTextFields(); var $window = $(window); var $container = $('#container'); $container.height($window.innerHeight()); $container.width($window.innerWidth()); var $menu = $('#menu'); $menu.css('top', Math.max(0, (($window.height() - $menu.outerHeight()) / 2)) + 'px'); $menu.css('left', Math.max(0, (($window.width() - $menu.outerWidth()) / 2)) + 'px'); return this; }); function addFileEntry(name) { var entry = $('
'); entry.text(name); var deleteButton = $(''); entry.append(deleteButton); $('#menu').append(entry); deleteButton.on('click', function(event) { localStorage.removeItem(name); entry.remove(); event.stopPropagation(); }); entry.on('click', function(event) { graph.clear(); graph.fromJSON(JSON.parse(localStorage[name])); filename = name; $('#menu').hide(); }); } (function() { for (var i = 0; i < localStorage.length; i++) addFileEntry(localStorage.key(i)); })(); $('#menu button.close').click(function() { $('#menu').hide(); }); $(window).trigger('resize'); $('#paper').contextmenu( { width: 150, items: [ { text: 'Text', alias: '1-1', action: add(joint.shapes.dialogue.Text) }, { text: 'Choice', alias: '1-2', action: add(joint.shapes.dialogue.Choice) }, { text: 'Branch', alias: '1-3', action: add(joint.shapes.dialogue.Branch) }, { text: 'Set', alias: '1-4', action: add(joint.shapes.dialogue.Set) }, { text: 'Node', alias: '1-5', action: add(joint.shapes.dialogue.Node) }, { type: 'splitLine' }, { text: 'Save', alias: '2-1', action: save }, { text: 'Load', alias: '2-2', action: load }, { text: 'Import', id: 'import', alias: '2-3', action: importFile }, { text: 'New', alias: '2-4', action: clear }, { text: 'Export', id: 'export', alias: '2-5', action: exportFile }, { text: 'Export game file', id: 'export-game', alias: '2-6', action: exportGameFile }, ] }); ================================================ FILE: Dialogger/index.css ================================================ html, body { padding: 0px; color: #fff; margin: 0px; font-family: sans-serif; font-size: small; background: #7d8ea2; /* Old browsers */ background: -moz-linear-gradient(top, #7d8ea2 0%, #121413 100%); /* FF3.6+ */ background: -webkit-gradient(linear, left top, left bottom, color-stop(0%,#7d8ea2), color-stop(100%,#121413)); /* Chrome,Safari4+ */ background: -webkit-linear-gradient(top, #7d8ea2 0%,#121413 100%); /* Chrome10+,Safari5.1+ */ background: -o-linear-gradient(top, #7d8ea2 0%,#121413 100%); /* Opera 11.10+ */ background: -ms-linear-gradient(top, #7d8ea2 0%,#121413 100%); /* IE10+ */ background: linear-gradient(to bottom, #7d8ea2 0%,#121413 100%); /* W3C */ filter: progid:DXImageTransform.Microsoft.gradient( startColorstr='#7d8ea2', endColorstr='#121413',GradientType=0 ); /* IE6-9 */ overflow: hidden; } #container { overflow: scroll; } #paper { position: relative; display: inline-block; background: transparent; background-image: url(g/grid.png); } #paper svg { background: transparent; } #paper svg .link { z-index: 2; } #menu { position: absolute; display: none; width: 40%; z-index: 10000; padding: 8px; cursor: pointer; } #menu div { clear: right; padding: 4px; margin-left: 20px; margin-right: 20px; border-bottom: 1px solid #ccc; } #menu div:hover { background-color: black; } #flash { position: absolute; top: 10px; left: 10px; padding: 8px; font-weight: bold; display: none; } /* Editor tools */ .marker-target { fill: #fff; stroke-width: 0; } .marker-vertex { fill: #fff; } .marker-vertex:hover { fill: #000; stroke: none; } .marker-arrowhead { fill: #fff; stroke: none; } .marker-arrowhead:hover { fill: #000; } .link-tools .tool-remove circle { fill: #c0392b; } .connection { stroke: #fff; } .inPorts circle { fill: #c0392b; stroke-width: 0; } .outPorts circle { fill: #8c6; stroke-width: 0; } /* Node styles */ .node { position: absolute; background: #666; /* Make sure events are propagated to the JointJS element so, e.g. dragging works.*/ pointer-events: none; -webkit-user-select: none; padding: 8px; box-sizing: border-box; z-index: 2; } .node input, button { /* Enable interacting with inputs only. */ pointer-events: auto; border: none; box-sizing: border-box; background-color: #456; color: #fff; } .node .label { color: #ddd; } .node .label.Text { color: #8c6; } .node .label.Set { color: #acf; } .node .label.Choice { color: #dc5; } .node .label.Branch { color: #ea6; } button { float: right; border: none; border-radius: 8px; font-weight: bold; width: 16px; height: 16px; line-height: 0px; text-align: middle; padding: 0; margin: 0; cursor: pointer; } button.delete:hover { background-color: #c0392b; } .node button.add:hover { background-color: #8c6; } .node button.remove:hover { background-color: #dc5; } .node input { width: 100%; padding: 4px; margin-top: 8px; } .node { color: #fff; } /* Context menu */ .b-m-mpanel, #menu, #flash { background-color: #444; position: absolute; z-index: 99997; -moz-box-shadow: 0px 0px 16px 0px rgba(0, 0, 0, 0.5); -webkit-box-shadow: 0px 0px 16px 0px rgba(0, 0, 0, 0.5); box-shadow: 0px 0px 16px 0px rgba(0, 0, 0, 0.5); } .b-m-split { font-size: 0px; margin: 2px; border-bottom: 1px solid #777; } .b-m-item, .b-m-ifocus { padding: 8px; line-height: 100%; } span { -moz-user-select: none; -webkit-user-select: none; -ms-user-select: none; user-select: none; cursor: default; } .b-m-ibody { overflow: hidden; text-overflow: ellipsis; } .b-m-ifocus { background-color: #000; } ================================================ FILE: Dialogger/index.html ================================================ Dialogger
================================================ FILE: Dialogger/lib/joint.css ================================================ /*! JointJS v0.8.1 - JavaScript diagramming library 2014-02-24 This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/. */ /* This Source Code Form is subject to the terms of the Mozilla Public * License, v. 2.0. If a copy of the MPL was not distributed with this * file, You can obtain one at http://mozilla.org/MPL/2.0/. */ /* A complete list of SVG properties that can be set through CSS is here: http://www.w3.org/TR/SVG/styling.html Important note: Presentation attributes have a lower precedence over CSS style rules. */ /* .viewport is a node wrapping all diagram elements in the paper */ .viewport { -webkit-user-select: none; -moz-user-select: none; user-select: none; } /* .magnet is an element that can be either source or a target of a link */ /* .magnet { fill: black; fill-opacity: 0; stroke: black; stroke-width: 15; stroke-opacity: 0; pointer-events: visibleStroke; cursor: crosshair; vector-effect: non-scaling-stroke; } .magnet:hover { stroke-opacity: .5; } */ [magnet=true]:not(.element) { cursor: crosshair; } [magnet=true]:not(.element):hover { opacity: .7; } /* Elements have CSS classes named by their types. E.g. type: basic.Rect has a CSS class "element basic Rect". This makes it possible to easilly style elements in CSS and have generic CSS rules applying to the whole group of elements. Each plugin can provide its own stylesheet. */ .element { /* Give the user a hint that he can drag&drop the element. */ cursor: move; } .element * { /* The default behavior when scaling an element is not to scale the stroke in order to prevent the ugly effect of stroke with different proportions. */ vector-effect: non-scaling-stroke; -moz-user-select: none; user-drag: none; } /* connection-wrap is a element of the joint.dia.Link that follows the .connection of that link. In other words, the `d` attribute of the .connection-wrap contains the same data as the `d` attribute of the .connection . The advantage of using .connection-wrap is to be able to catch pointer events in the neighborhood of the .connection . This is especially handy if the .connection is very thin. */ .connection-wrap { fill: none; stroke: black; stroke-width: 15; stroke-linecap: round; stroke-linejoin: round; opacity: 0; cursor: move; } .connection-wrap:hover { opacity: .4; stroke-opacity: .4; } .connection { /* stroke: black; */ /* stroke width cannot be overriden by attribute? */ /* stroke-width: 1; */ fill: none; stroke-linejoin: round; } .marker-source, .marker-target { /* Cannot be in CSS otherwise it could not be overruled by attributes. fill: black; stroke: black; */ /* This makes the arrowheads point to the border of objects even though the transform: scale() is applied on them. */ vector-effect: non-scaling-stroke; } /* Vertex markers are `` elements that appear at connection vertex positions. */ /* element wrapping .marker-vertex-group. */ .marker-vertices { opacity: 0; cursor: move; } .marker-arrowheads { opacity: 0; cursor: move; cursor: -webkit-grab; cursor: -moz-grab; /* display: none; */ /* setting `display: none` on .marker-arrowheads effectivelly switches of links reconnecting */ } .link-tools { opacity: 0; cursor: pointer; } .link-tools .tool-options { display: none; /* by default, we don't display link options tool */ } .link-tools .tool-remove circle { fill: red; } .link-tools .tool-remove path { fill: white; } .link:hover .marker-vertices, .link:hover .marker-arrowheads, .link:hover .link-tools { opacity: 1; } /* element inside .marker-vertex-group element */ .marker-vertex { fill: #1ABC9C; } .marker-vertex:hover { fill: #34495E; stroke: none; } .marker-arrowhead { fill: #1ABC9C; } .marker-arrowhead:hover { fill: #F39C12; stroke: none; } /* element used to remove a vertex */ .marker-vertex-remove { cursor: pointer; opacity: .1; fill: white; } .marker-vertex-group:hover .marker-vertex-remove { opacity: 1; } .marker-vertex-remove-area { opacity: .1; cursor: pointer; } .marker-vertex-group:hover .marker-vertex-remove-area { opacity: 1; } /* /* Cell highlighting - e.g a cell underneath the dragged link get highlighted. See joint.dia.cell.js highlight(); */ /* For some reason, CSS `outline` property does not work on `` elements. */ text.highlighted { fill: #FF0000; } .highlighted { outline: 2px solid #FF0000; /* `outline` doesn't work in Firefox, Opera and IE9+ correctly. */ opacity: 0.7 \9; /* It targets only IE9. */ } /* use '@-moz-document url-prefix()' to target all versions if Firefox and nothing else. See `https://developer.mozilla.org/en-US/docs/Web/CSS/@document`. */ @-moz-document url-prefix() { .highlighted { opacity: 0.7; } /* only for FF */ } /* `-o-prefocus` is a pseudo-class that allows styles to be targeted for Opera only. See `http://www.opera.com/docs/specs/presto2.12/css/o-vendor/`. */ doesnotexist:-o-prefocus, .highlighted { opacity: 0.7; } /* Example of custom changes (in pure CSS only!): Do not show marker vertices at all: .marker-vertices { display: none; } Do not allow adding new vertices: .connection-wrap { pointer-events: none; } */ /* foreignObject in joint.shapes.basic.TextBlock */ .TextBlock .fobj body { background-color: transparent; margin: 0px; } .TextBlock .fobj div { text-align: center; vertical-align: middle; display: table-cell; padding: 0px 5px 0px 5px; } ================================================ FILE: Dialogger/lib/joint.js ================================================ /*! JointJS v0.8.1 - JavaScript diagramming library 2014-02-24 This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/. */ /*! * jQuery JavaScript Library v2.0.3 * http://jquery.com/ * * Includes Sizzle.js * http://sizzlejs.com/ * * Copyright 2005, 2013 jQuery Foundation, Inc. and other contributors * Released under the MIT license * http://jquery.org/license * * Date: 2013-07-03T13:30Z */ (function( window, undefined ) { // Can't do this because several apps including ASP.NET trace // the stack via arguments.caller.callee and Firefox dies if // you try to trace through "use strict" call chains. (#13335) // Support: Firefox 18+ //"use strict"; var // A central reference to the root jQuery(document) rootjQuery, // The deferred used on DOM ready readyList, // Support: IE9 // For `typeof xmlNode.method` instead of `xmlNode.method !== undefined` core_strundefined = typeof undefined, // Use the correct document accordingly with window argument (sandbox) location = window.location, document = window.document, docElem = document.documentElement, // Map over jQuery in case of overwrite _jQuery = window.jQuery, // Map over the $ in case of overwrite _$ = window.$, // [[Class]] -> type pairs class2type = {}, // List of deleted data cache ids, so we can reuse them core_deletedIds = [], core_version = "2.0.3", // Save a reference to some core methods core_concat = core_deletedIds.concat, core_push = core_deletedIds.push, core_slice = core_deletedIds.slice, core_indexOf = core_deletedIds.indexOf, core_toString = class2type.toString, core_hasOwn = class2type.hasOwnProperty, core_trim = core_version.trim, // Define a local copy of jQuery jQuery = function( selector, context ) { // The jQuery object is actually just the init constructor 'enhanced' return new jQuery.fn.init( selector, context, rootjQuery ); }, // Used for matching numbers core_pnum = /[+-]?(?:\d*\.|)\d+(?:[eE][+-]?\d+|)/.source, // Used for splitting on whitespace core_rnotwhite = /\S+/g, // A simple way to check for HTML strings // Prioritize #id over to avoid XSS via location.hash (#9521) // Strict HTML recognition (#11290: must start with <) rquickExpr = /^(?:\s*(<[\w\W]+>)[^>]*|#([\w-]*))$/, // Match a standalone tag rsingleTag = /^<(\w+)\s*\/?>(?:<\/\1>|)$/, // Matches dashed string for camelizing rmsPrefix = /^-ms-/, rdashAlpha = /-([\da-z])/gi, // Used by jQuery.camelCase as callback to replace() fcamelCase = function( all, letter ) { return letter.toUpperCase(); }, // The ready event handler and self cleanup method completed = function() { document.removeEventListener( "DOMContentLoaded", completed, false ); window.removeEventListener( "load", completed, false ); jQuery.ready(); }; jQuery.fn = jQuery.prototype = { // The current version of jQuery being used jquery: core_version, constructor: jQuery, init: function( selector, context, rootjQuery ) { var match, elem; // HANDLE: $(""), $(null), $(undefined), $(false) if ( !selector ) { return this; } // Handle HTML strings if ( typeof selector === "string" ) { if ( selector.charAt(0) === "<" && selector.charAt( selector.length - 1 ) === ">" && selector.length >= 3 ) { // Assume that strings that start and end with <> are HTML and skip the regex check match = [ null, selector, null ]; } else { match = rquickExpr.exec( selector ); } // Match html or make sure no context is specified for #id if ( match && (match[1] || !context) ) { // HANDLE: $(html) -> $(array) if ( match[1] ) { context = context instanceof jQuery ? context[0] : context; // scripts is true for back-compat jQuery.merge( this, jQuery.parseHTML( match[1], context && context.nodeType ? context.ownerDocument || context : document, true ) ); // HANDLE: $(html, props) if ( rsingleTag.test( match[1] ) && jQuery.isPlainObject( context ) ) { for ( match in context ) { // Properties of context are called as methods if possible if ( jQuery.isFunction( this[ match ] ) ) { this[ match ]( context[ match ] ); // ...and otherwise set as attributes } else { this.attr( match, context[ match ] ); } } } return this; // HANDLE: $(#id) } else { elem = document.getElementById( match[2] ); // Check parentNode to catch when Blackberry 4.6 returns // nodes that are no longer in the document #6963 if ( elem && elem.parentNode ) { // Inject the element directly into the jQuery object this.length = 1; this[0] = elem; } this.context = document; this.selector = selector; return this; } // HANDLE: $(expr, $(...)) } else if ( !context || context.jquery ) { return ( context || rootjQuery ).find( selector ); // HANDLE: $(expr, context) // (which is just equivalent to: $(context).find(expr) } else { return this.constructor( context ).find( selector ); } // HANDLE: $(DOMElement) } else if ( selector.nodeType ) { this.context = this[0] = selector; this.length = 1; return this; // HANDLE: $(function) // Shortcut for document ready } else if ( jQuery.isFunction( selector ) ) { return rootjQuery.ready( selector ); } if ( selector.selector !== undefined ) { this.selector = selector.selector; this.context = selector.context; } return jQuery.makeArray( selector, this ); }, // Start with an empty selector selector: "", // The default length of a jQuery object is 0 length: 0, toArray: function() { return core_slice.call( this ); }, // Get the Nth element in the matched element set OR // Get the whole matched element set as a clean array get: function( num ) { return num == null ? // Return a 'clean' array this.toArray() : // Return just the object ( num < 0 ? this[ this.length + num ] : this[ num ] ); }, // Take an array of elements and push it onto the stack // (returning the new matched element set) pushStack: function( elems ) { // Build a new jQuery matched element set var ret = jQuery.merge( this.constructor(), elems ); // Add the old object onto the stack (as a reference) ret.prevObject = this; ret.context = this.context; // Return the newly-formed element set return ret; }, // Execute a callback for every element in the matched set. // (You can seed the arguments with an array of args, but this is // only used internally.) each: function( callback, args ) { return jQuery.each( this, callback, args ); }, ready: function( fn ) { // Add the callback jQuery.ready.promise().done( fn ); return this; }, slice: function() { return this.pushStack( core_slice.apply( this, arguments ) ); }, first: function() { return this.eq( 0 ); }, last: function() { return this.eq( -1 ); }, eq: function( i ) { var len = this.length, j = +i + ( i < 0 ? len : 0 ); return this.pushStack( j >= 0 && j < len ? [ this[j] ] : [] ); }, map: function( callback ) { return this.pushStack( jQuery.map(this, function( elem, i ) { return callback.call( elem, i, elem ); })); }, end: function() { return this.prevObject || this.constructor(null); }, // For internal use only. // Behaves like an Array's method, not like a jQuery method. push: core_push, sort: [].sort, splice: [].splice }; // Give the init function the jQuery prototype for later instantiation jQuery.fn.init.prototype = jQuery.fn; jQuery.extend = jQuery.fn.extend = function() { var options, name, src, copy, copyIsArray, clone, target = arguments[0] || {}, i = 1, length = arguments.length, deep = false; // Handle a deep copy situation if ( typeof target === "boolean" ) { deep = target; target = arguments[1] || {}; // skip the boolean and the target i = 2; } // Handle case when target is a string or something (possible in deep copy) if ( typeof target !== "object" && !jQuery.isFunction(target) ) { target = {}; } // extend jQuery itself if only one argument is passed if ( length === i ) { target = this; --i; } for ( ; i < length; i++ ) { // Only deal with non-null/undefined values if ( (options = arguments[ i ]) != null ) { // Extend the base object for ( name in options ) { src = target[ name ]; copy = options[ name ]; // Prevent never-ending loop if ( target === copy ) { continue; } // Recurse if we're merging plain objects or arrays if ( deep && copy && ( jQuery.isPlainObject(copy) || (copyIsArray = jQuery.isArray(copy)) ) ) { if ( copyIsArray ) { copyIsArray = false; clone = src && jQuery.isArray(src) ? src : []; } else { clone = src && jQuery.isPlainObject(src) ? src : {}; } // Never move original objects, clone them target[ name ] = jQuery.extend( deep, clone, copy ); // Don't bring in undefined values } else if ( copy !== undefined ) { target[ name ] = copy; } } } } // Return the modified object return target; }; jQuery.extend({ // Unique for each copy of jQuery on the page expando: "jQuery" + ( core_version + Math.random() ).replace( /\D/g, "" ), noConflict: function( deep ) { if ( window.$ === jQuery ) { window.$ = _$; } if ( deep && window.jQuery === jQuery ) { window.jQuery = _jQuery; } return jQuery; }, // Is the DOM ready to be used? Set to true once it occurs. isReady: false, // A counter to track how many items to wait for before // the ready event fires. See #6781 readyWait: 1, // Hold (or release) the ready event holdReady: function( hold ) { if ( hold ) { jQuery.readyWait++; } else { jQuery.ready( true ); } }, // Handle when the DOM is ready ready: function( wait ) { // Abort if there are pending holds or we're already ready if ( wait === true ? --jQuery.readyWait : jQuery.isReady ) { return; } // Remember that the DOM is ready jQuery.isReady = true; // If a normal DOM Ready event fired, decrement, and wait if need be if ( wait !== true && --jQuery.readyWait > 0 ) { return; } // If there are functions bound, to execute readyList.resolveWith( document, [ jQuery ] ); // Trigger any bound ready events if ( jQuery.fn.trigger ) { jQuery( document ).trigger("ready").off("ready"); } }, // See test/unit/core.js for details concerning isFunction. // Since version 1.3, DOM methods and functions like alert // aren't supported. They return false on IE (#2968). isFunction: function( obj ) { return jQuery.type(obj) === "function"; }, isArray: Array.isArray, isWindow: function( obj ) { return obj != null && obj === obj.window; }, isNumeric: function( obj ) { return !isNaN( parseFloat(obj) ) && isFinite( obj ); }, type: function( obj ) { if ( obj == null ) { return String( obj ); } // Support: Safari <= 5.1 (functionish RegExp) return typeof obj === "object" || typeof obj === "function" ? class2type[ core_toString.call(obj) ] || "object" : typeof obj; }, isPlainObject: function( obj ) { // Not plain objects: // - Any object or value whose internal [[Class]] property is not "[object Object]" // - DOM nodes // - window if ( jQuery.type( obj ) !== "object" || obj.nodeType || jQuery.isWindow( obj ) ) { return false; } // Support: Firefox <20 // The try/catch suppresses exceptions thrown when attempting to access // the "constructor" property of certain host objects, ie. |window.location| // https://bugzilla.mozilla.org/show_bug.cgi?id=814622 try { if ( obj.constructor && !core_hasOwn.call( obj.constructor.prototype, "isPrototypeOf" ) ) { return false; } } catch ( e ) { return false; } // If the function hasn't returned already, we're confident that // |obj| is a plain object, created by {} or constructed with new Object return true; }, isEmptyObject: function( obj ) { var name; for ( name in obj ) { return false; } return true; }, error: function( msg ) { throw new Error( msg ); }, // data: string of html // context (optional): If specified, the fragment will be created in this context, defaults to document // keepScripts (optional): If true, will include scripts passed in the html string parseHTML: function( data, context, keepScripts ) { if ( !data || typeof data !== "string" ) { return null; } if ( typeof context === "boolean" ) { keepScripts = context; context = false; } context = context || document; var parsed = rsingleTag.exec( data ), scripts = !keepScripts && []; // Single tag if ( parsed ) { return [ context.createElement( parsed[1] ) ]; } parsed = jQuery.buildFragment( [ data ], context, scripts ); if ( scripts ) { jQuery( scripts ).remove(); } return jQuery.merge( [], parsed.childNodes ); }, parseJSON: JSON.parse, // Cross-browser xml parsing parseXML: function( data ) { var xml, tmp; if ( !data || typeof data !== "string" ) { return null; } // Support: IE9 try { tmp = new DOMParser(); xml = tmp.parseFromString( data , "text/xml" ); } catch ( e ) { xml = undefined; } if ( !xml || xml.getElementsByTagName( "parsererror" ).length ) { jQuery.error( "Invalid XML: " + data ); } return xml; }, noop: function() {}, // Evaluates a script in a global context globalEval: function( code ) { var script, indirect = eval; code = jQuery.trim( code ); if ( code ) { // If the code includes a valid, prologue position // strict mode pragma, execute code by injecting a // script tag into the document. if ( code.indexOf("use strict") === 1 ) { script = document.createElement("script"); script.text = code; document.head.appendChild( script ).parentNode.removeChild( script ); } else { // Otherwise, avoid the DOM node creation, insertion // and removal by using an indirect global eval indirect( code ); } } }, // Convert dashed to camelCase; used by the css and data modules // Microsoft forgot to hump their vendor prefix (#9572) camelCase: function( string ) { return string.replace( rmsPrefix, "ms-" ).replace( rdashAlpha, fcamelCase ); }, nodeName: function( elem, name ) { return elem.nodeName && elem.nodeName.toLowerCase() === name.toLowerCase(); }, // args is for internal usage only each: function( obj, callback, args ) { var value, i = 0, length = obj.length, isArray = isArraylike( obj ); if ( args ) { if ( isArray ) { for ( ; i < length; i++ ) { value = callback.apply( obj[ i ], args ); if ( value === false ) { break; } } } else { for ( i in obj ) { value = callback.apply( obj[ i ], args ); if ( value === false ) { break; } } } // A special, fast, case for the most common use of each } else { if ( isArray ) { for ( ; i < length; i++ ) { value = callback.call( obj[ i ], i, obj[ i ] ); if ( value === false ) { break; } } } else { for ( i in obj ) { value = callback.call( obj[ i ], i, obj[ i ] ); if ( value === false ) { break; } } } } return obj; }, trim: function( text ) { return text == null ? "" : core_trim.call( text ); }, // results is for internal usage only makeArray: function( arr, results ) { var ret = results || []; if ( arr != null ) { if ( isArraylike( Object(arr) ) ) { jQuery.merge( ret, typeof arr === "string" ? [ arr ] : arr ); } else { core_push.call( ret, arr ); } } return ret; }, inArray: function( elem, arr, i ) { return arr == null ? -1 : core_indexOf.call( arr, elem, i ); }, merge: function( first, second ) { var l = second.length, i = first.length, j = 0; if ( typeof l === "number" ) { for ( ; j < l; j++ ) { first[ i++ ] = second[ j ]; } } else { while ( second[j] !== undefined ) { first[ i++ ] = second[ j++ ]; } } first.length = i; return first; }, grep: function( elems, callback, inv ) { var retVal, ret = [], i = 0, length = elems.length; inv = !!inv; // Go through the array, only saving the items // that pass the validator function for ( ; i < length; i++ ) { retVal = !!callback( elems[ i ], i ); if ( inv !== retVal ) { ret.push( elems[ i ] ); } } return ret; }, // arg is for internal usage only map: function( elems, callback, arg ) { var value, i = 0, length = elems.length, isArray = isArraylike( elems ), ret = []; // Go through the array, translating each of the items to their if ( isArray ) { for ( ; i < length; i++ ) { value = callback( elems[ i ], i, arg ); if ( value != null ) { ret[ ret.length ] = value; } } // Go through every key on the object, } else { for ( i in elems ) { value = callback( elems[ i ], i, arg ); if ( value != null ) { ret[ ret.length ] = value; } } } // Flatten any nested arrays return core_concat.apply( [], ret ); }, // A global GUID counter for objects guid: 1, // Bind a function to a context, optionally partially applying any // arguments. proxy: function( fn, context ) { var tmp, args, proxy; if ( typeof context === "string" ) { tmp = fn[ context ]; context = fn; fn = tmp; } // Quick check to determine if target is callable, in the spec // this throws a TypeError, but we will just return undefined. if ( !jQuery.isFunction( fn ) ) { return undefined; } // Simulated bind args = core_slice.call( arguments, 2 ); proxy = function() { return fn.apply( context || this, args.concat( core_slice.call( arguments ) ) ); }; // Set the guid of unique handler to the same of original handler, so it can be removed proxy.guid = fn.guid = fn.guid || jQuery.guid++; return proxy; }, // Multifunctional method to get and set values of a collection // The value/s can optionally be executed if it's a function access: function( elems, fn, key, value, chainable, emptyGet, raw ) { var i = 0, length = elems.length, bulk = key == null; // Sets many values if ( jQuery.type( key ) === "object" ) { chainable = true; for ( i in key ) { jQuery.access( elems, fn, i, key[i], true, emptyGet, raw ); } // Sets one value } else if ( value !== undefined ) { chainable = true; if ( !jQuery.isFunction( value ) ) { raw = true; } if ( bulk ) { // Bulk operations run against the entire set if ( raw ) { fn.call( elems, value ); fn = null; // ...except when executing function values } else { bulk = fn; fn = function( elem, key, value ) { return bulk.call( jQuery( elem ), value ); }; } } if ( fn ) { for ( ; i < length; i++ ) { fn( elems[i], key, raw ? value : value.call( elems[i], i, fn( elems[i], key ) ) ); } } } return chainable ? elems : // Gets bulk ? fn.call( elems ) : length ? fn( elems[0], key ) : emptyGet; }, now: Date.now, // A method for quickly swapping in/out CSS properties to get correct calculations. // Note: this method belongs to the css module but it's needed here for the support module. // If support gets modularized, this method should be moved back to the css module. swap: function( elem, options, callback, args ) { var ret, name, old = {}; // Remember the old values, and insert the new ones for ( name in options ) { old[ name ] = elem.style[ name ]; elem.style[ name ] = options[ name ]; } ret = callback.apply( elem, args || [] ); // Revert the old values for ( name in options ) { elem.style[ name ] = old[ name ]; } return ret; } }); jQuery.ready.promise = function( obj ) { if ( !readyList ) { readyList = jQuery.Deferred(); // Catch cases where $(document).ready() is called after the browser event has already occurred. // we once tried to use readyState "interactive" here, but it caused issues like the one // discovered by ChrisS here: http://bugs.jquery.com/ticket/12282#comment:15 if ( document.readyState === "complete" ) { // Handle it asynchronously to allow scripts the opportunity to delay ready setTimeout( jQuery.ready ); } else { // Use the handy event callback document.addEventListener( "DOMContentLoaded", completed, false ); // A fallback to window.onload, that will always work window.addEventListener( "load", completed, false ); } } return readyList.promise( obj ); }; // Populate the class2type map jQuery.each("Boolean Number String Function Array Date RegExp Object Error".split(" "), function(i, name) { class2type[ "[object " + name + "]" ] = name.toLowerCase(); }); function isArraylike( obj ) { var length = obj.length, type = jQuery.type( obj ); if ( jQuery.isWindow( obj ) ) { return false; } if ( obj.nodeType === 1 && length ) { return true; } return type === "array" || type !== "function" && ( length === 0 || typeof length === "number" && length > 0 && ( length - 1 ) in obj ); } // All jQuery objects should point back to these rootjQuery = jQuery(document); /*! * Sizzle CSS Selector Engine v1.9.4-pre * http://sizzlejs.com/ * * Copyright 2013 jQuery Foundation, Inc. and other contributors * Released under the MIT license * http://jquery.org/license * * Date: 2013-06-03 */ (function( window, undefined ) { var i, support, cachedruns, Expr, getText, isXML, compile, outermostContext, sortInput, // Local document vars setDocument, document, docElem, documentIsHTML, rbuggyQSA, rbuggyMatches, matches, contains, // Instance-specific data expando = "sizzle" + -(new Date()), preferredDoc = window.document, dirruns = 0, done = 0, classCache = createCache(), tokenCache = createCache(), compilerCache = createCache(), hasDuplicate = false, sortOrder = function( a, b ) { if ( a === b ) { hasDuplicate = true; return 0; } return 0; }, // General-purpose constants strundefined = typeof undefined, MAX_NEGATIVE = 1 << 31, // Instance methods hasOwn = ({}).hasOwnProperty, arr = [], pop = arr.pop, push_native = arr.push, push = arr.push, slice = arr.slice, // Use a stripped-down indexOf if we can't use a native one indexOf = arr.indexOf || function( elem ) { var i = 0, len = this.length; for ( ; i < len; i++ ) { if ( this[i] === elem ) { return i; } } return -1; }, booleans = "checked|selected|async|autofocus|autoplay|controls|defer|disabled|hidden|ismap|loop|multiple|open|readonly|required|scoped", // Regular expressions // Whitespace characters http://www.w3.org/TR/css3-selectors/#whitespace whitespace = "[\\x20\\t\\r\\n\\f]", // http://www.w3.org/TR/css3-syntax/#characters characterEncoding = "(?:\\\\.|[\\w-]|[^\\x00-\\xa0])+", // Loosely modeled on CSS identifier characters // An unquoted value should be a CSS identifier http://www.w3.org/TR/css3-selectors/#attribute-selectors // Proper syntax: http://www.w3.org/TR/CSS21/syndata.html#value-def-identifier identifier = characterEncoding.replace( "w", "w#" ), // Acceptable operators http://www.w3.org/TR/selectors/#attribute-selectors attributes = "\\[" + whitespace + "*(" + characterEncoding + ")" + whitespace + "*(?:([*^$|!~]?=)" + whitespace + "*(?:(['\"])((?:\\\\.|[^\\\\])*?)\\3|(" + identifier + ")|)|)" + whitespace + "*\\]", // Prefer arguments quoted, // then not containing pseudos/brackets, // then attribute selectors/non-parenthetical expressions, // then anything else // These preferences are here to reduce the number of selectors // needing tokenize in the PSEUDO preFilter pseudos = ":(" + characterEncoding + ")(?:\\(((['\"])((?:\\\\.|[^\\\\])*?)\\3|((?:\\\\.|[^\\\\()[\\]]|" + attributes.replace( 3, 8 ) + ")*)|.*)\\)|)", // Leading and non-escaped trailing whitespace, capturing some non-whitespace characters preceding the latter rtrim = new RegExp( "^" + whitespace + "+|((?:^|[^\\\\])(?:\\\\.)*)" + whitespace + "+$", "g" ), rcomma = new RegExp( "^" + whitespace + "*," + whitespace + "*" ), rcombinators = new RegExp( "^" + whitespace + "*([>+~]|" + whitespace + ")" + whitespace + "*" ), rsibling = new RegExp( whitespace + "*[+~]" ), rattributeQuotes = new RegExp( "=" + whitespace + "*([^\\]'\"]*)" + whitespace + "*\\]", "g" ), rpseudo = new RegExp( pseudos ), ridentifier = new RegExp( "^" + identifier + "$" ), matchExpr = { "ID": new RegExp( "^#(" + characterEncoding + ")" ), "CLASS": new RegExp( "^\\.(" + characterEncoding + ")" ), "TAG": new RegExp( "^(" + characterEncoding.replace( "w", "w*" ) + ")" ), "ATTR": new RegExp( "^" + attributes ), "PSEUDO": new RegExp( "^" + pseudos ), "CHILD": new RegExp( "^:(only|first|last|nth|nth-last)-(child|of-type)(?:\\(" + whitespace + "*(even|odd|(([+-]|)(\\d*)n|)" + whitespace + "*(?:([+-]|)" + whitespace + "*(\\d+)|))" + whitespace + "*\\)|)", "i" ), "bool": new RegExp( "^(?:" + booleans + ")$", "i" ), // For use in libraries implementing .is() // We use this for POS matching in `select` "needsContext": new RegExp( "^" + whitespace + "*[>+~]|:(even|odd|eq|gt|lt|nth|first|last)(?:\\(" + whitespace + "*((?:-\\d)?\\d*)" + whitespace + "*\\)|)(?=[^-]|$)", "i" ) }, rnative = /^[^{]+\{\s*\[native \w/, // Easily-parseable/retrievable ID or TAG or CLASS selectors rquickExpr = /^(?:#([\w-]+)|(\w+)|\.([\w-]+))$/, rinputs = /^(?:input|select|textarea|button)$/i, rheader = /^h\d$/i, rescape = /'|\\/g, // CSS escapes http://www.w3.org/TR/CSS21/syndata.html#escaped-characters runescape = new RegExp( "\\\\([\\da-f]{1,6}" + whitespace + "?|(" + whitespace + ")|.)", "ig" ), funescape = function( _, escaped, escapedWhitespace ) { var high = "0x" + escaped - 0x10000; // NaN means non-codepoint // Support: Firefox // Workaround erroneous numeric interpretation of +"0x" return high !== high || escapedWhitespace ? escaped : // BMP codepoint high < 0 ? String.fromCharCode( high + 0x10000 ) : // Supplemental Plane codepoint (surrogate pair) String.fromCharCode( high >> 10 | 0xD800, high & 0x3FF | 0xDC00 ); }; // Optimize for push.apply( _, NodeList ) try { push.apply( (arr = slice.call( preferredDoc.childNodes )), preferredDoc.childNodes ); // Support: Android<4.0 // Detect silently failing push.apply arr[ preferredDoc.childNodes.length ].nodeType; } catch ( e ) { push = { apply: arr.length ? // Leverage slice if possible function( target, els ) { push_native.apply( target, slice.call(els) ); } : // Support: IE<9 // Otherwise append directly function( target, els ) { var j = target.length, i = 0; // Can't trust NodeList.length while ( (target[j++] = els[i++]) ) {} target.length = j - 1; } }; } function Sizzle( selector, context, results, seed ) { var match, elem, m, nodeType, // QSA vars i, groups, old, nid, newContext, newSelector; if ( ( context ? context.ownerDocument || context : preferredDoc ) !== document ) { setDocument( context ); } context = context || document; results = results || []; if ( !selector || typeof selector !== "string" ) { return results; } if ( (nodeType = context.nodeType) !== 1 && nodeType !== 9 ) { return []; } if ( documentIsHTML && !seed ) { // Shortcuts if ( (match = rquickExpr.exec( selector )) ) { // Speed-up: Sizzle("#ID") if ( (m = match[1]) ) { if ( nodeType === 9 ) { elem = context.getElementById( m ); // Check parentNode to catch when Blackberry 4.6 returns // nodes that are no longer in the document #6963 if ( elem && elem.parentNode ) { // Handle the case where IE, Opera, and Webkit return items // by name instead of ID if ( elem.id === m ) { results.push( elem ); return results; } } else { return results; } } else { // Context is not a document if ( context.ownerDocument && (elem = context.ownerDocument.getElementById( m )) && contains( context, elem ) && elem.id === m ) { results.push( elem ); return results; } } // Speed-up: Sizzle("TAG") } else if ( match[2] ) { push.apply( results, context.getElementsByTagName( selector ) ); return results; // Speed-up: Sizzle(".CLASS") } else if ( (m = match[3]) && support.getElementsByClassName && context.getElementsByClassName ) { push.apply( results, context.getElementsByClassName( m ) ); return results; } } // QSA path if ( support.qsa && (!rbuggyQSA || !rbuggyQSA.test( selector )) ) { nid = old = expando; newContext = context; newSelector = nodeType === 9 && selector; // qSA works strangely on Element-rooted queries // We can work around this by specifying an extra ID on the root // and working up from there (Thanks to Andrew Dupont for the technique) // IE 8 doesn't work on object elements if ( nodeType === 1 && context.nodeName.toLowerCase() !== "object" ) { groups = tokenize( selector ); if ( (old = context.getAttribute("id")) ) { nid = old.replace( rescape, "\\$&" ); } else { context.setAttribute( "id", nid ); } nid = "[id='" + nid + "'] "; i = groups.length; while ( i-- ) { groups[i] = nid + toSelector( groups[i] ); } newContext = rsibling.test( selector ) && context.parentNode || context; newSelector = groups.join(","); } if ( newSelector ) { try { push.apply( results, newContext.querySelectorAll( newSelector ) ); return results; } catch(qsaError) { } finally { if ( !old ) { context.removeAttribute("id"); } } } } } // All others return select( selector.replace( rtrim, "$1" ), context, results, seed ); } /** * Create key-value caches of limited size * @returns {Function(string, Object)} Returns the Object data after storing it on itself with * property name the (space-suffixed) string and (if the cache is larger than Expr.cacheLength) * deleting the oldest entry */ function createCache() { var keys = []; function cache( key, value ) { // Use (key + " ") to avoid collision with native prototype properties (see Issue #157) if ( keys.push( key += " " ) > Expr.cacheLength ) { // Only keep the most recent entries delete cache[ keys.shift() ]; } return (cache[ key ] = value); } return cache; } /** * Mark a function for special use by Sizzle * @param {Function} fn The function to mark */ function markFunction( fn ) { fn[ expando ] = true; return fn; } /** * Support testing using an element * @param {Function} fn Passed the created div and expects a boolean result */ function assert( fn ) { var div = document.createElement("div"); try { return !!fn( div ); } catch (e) { return false; } finally { // Remove from its parent by default if ( div.parentNode ) { div.parentNode.removeChild( div ); } // release memory in IE div = null; } } /** * Adds the same handler for all of the specified attrs * @param {String} attrs Pipe-separated list of attributes * @param {Function} handler The method that will be applied */ function addHandle( attrs, handler ) { var arr = attrs.split("|"), i = attrs.length; while ( i-- ) { Expr.attrHandle[ arr[i] ] = handler; } } /** * Checks document order of two siblings * @param {Element} a * @param {Element} b * @returns {Number} Returns less than 0 if a precedes b, greater than 0 if a follows b */ function siblingCheck( a, b ) { var cur = b && a, diff = cur && a.nodeType === 1 && b.nodeType === 1 && ( ~b.sourceIndex || MAX_NEGATIVE ) - ( ~a.sourceIndex || MAX_NEGATIVE ); // Use IE sourceIndex if available on both nodes if ( diff ) { return diff; } // Check if b follows a if ( cur ) { while ( (cur = cur.nextSibling) ) { if ( cur === b ) { return -1; } } } return a ? 1 : -1; } /** * Returns a function to use in pseudos for input types * @param {String} type */ function createInputPseudo( type ) { return function( elem ) { var name = elem.nodeName.toLowerCase(); return name === "input" && elem.type === type; }; } /** * Returns a function to use in pseudos for buttons * @param {String} type */ function createButtonPseudo( type ) { return function( elem ) { var name = elem.nodeName.toLowerCase(); return (name === "input" || name === "button") && elem.type === type; }; } /** * Returns a function to use in pseudos for positionals * @param {Function} fn */ function createPositionalPseudo( fn ) { return markFunction(function( argument ) { argument = +argument; return markFunction(function( seed, matches ) { var j, matchIndexes = fn( [], seed.length, argument ), i = matchIndexes.length; // Match elements found at the specified indexes while ( i-- ) { if ( seed[ (j = matchIndexes[i]) ] ) { seed[j] = !(matches[j] = seed[j]); } } }); }); } /** * Detect xml * @param {Element|Object} elem An element or a document */ isXML = Sizzle.isXML = function( elem ) { // documentElement is verified for cases where it doesn't yet exist // (such as loading iframes in IE - #4833) var documentElement = elem && (elem.ownerDocument || elem).documentElement; return documentElement ? documentElement.nodeName !== "HTML" : false; }; // Expose support vars for convenience support = Sizzle.support = {}; /** * Sets document-related variables once based on the current document * @param {Element|Object} [doc] An element or document object to use to set the document * @returns {Object} Returns the current document */ setDocument = Sizzle.setDocument = function( node ) { var doc = node ? node.ownerDocument || node : preferredDoc, parent = doc.defaultView; // If no document and documentElement is available, return if ( doc === document || doc.nodeType !== 9 || !doc.documentElement ) { return document; } // Set our document document = doc; docElem = doc.documentElement; // Support tests documentIsHTML = !isXML( doc ); // Support: IE>8 // If iframe document is assigned to "document" variable and if iframe has been reloaded, // IE will throw "permission denied" error when accessing "document" variable, see jQuery #13936 // IE6-8 do not support the defaultView property so parent will be undefined if ( parent && parent.attachEvent && parent !== parent.top ) { parent.attachEvent( "onbeforeunload", function() { setDocument(); }); } /* Attributes ---------------------------------------------------------------------- */ // Support: IE<8 // Verify that getAttribute really returns attributes and not properties (excepting IE8 booleans) support.attributes = assert(function( div ) { div.className = "i"; return !div.getAttribute("className"); }); /* getElement(s)By* ---------------------------------------------------------------------- */ // Check if getElementsByTagName("*") returns only elements support.getElementsByTagName = assert(function( div ) { div.appendChild( doc.createComment("") ); return !div.getElementsByTagName("*").length; }); // Check if getElementsByClassName can be trusted support.getElementsByClassName = assert(function( div ) { div.innerHTML = "
"; // Support: Safari<4 // Catch class over-caching div.firstChild.className = "i"; // Support: Opera<10 // Catch gEBCN failure to find non-leading classes return div.getElementsByClassName("i").length === 2; }); // Support: IE<10 // Check if getElementById returns elements by name // The broken getElementById methods don't pick up programatically-set names, // so use a roundabout getElementsByName test support.getById = assert(function( div ) { docElem.appendChild( div ).id = expando; return !doc.getElementsByName || !doc.getElementsByName( expando ).length; }); // ID find and filter if ( support.getById ) { Expr.find["ID"] = function( id, context ) { if ( typeof context.getElementById !== strundefined && documentIsHTML ) { var m = context.getElementById( id ); // Check parentNode to catch when Blackberry 4.6 returns // nodes that are no longer in the document #6963 return m && m.parentNode ? [m] : []; } }; Expr.filter["ID"] = function( id ) { var attrId = id.replace( runescape, funescape ); return function( elem ) { return elem.getAttribute("id") === attrId; }; }; } else { // Support: IE6/7 // getElementById is not reliable as a find shortcut delete Expr.find["ID"]; Expr.filter["ID"] = function( id ) { var attrId = id.replace( runescape, funescape ); return function( elem ) { var node = typeof elem.getAttributeNode !== strundefined && elem.getAttributeNode("id"); return node && node.value === attrId; }; }; } // Tag Expr.find["TAG"] = support.getElementsByTagName ? function( tag, context ) { if ( typeof context.getElementsByTagName !== strundefined ) { return context.getElementsByTagName( tag ); } } : function( tag, context ) { var elem, tmp = [], i = 0, results = context.getElementsByTagName( tag ); // Filter out possible comments if ( tag === "*" ) { while ( (elem = results[i++]) ) { if ( elem.nodeType === 1 ) { tmp.push( elem ); } } return tmp; } return results; }; // Class Expr.find["CLASS"] = support.getElementsByClassName && function( className, context ) { if ( typeof context.getElementsByClassName !== strundefined && documentIsHTML ) { return context.getElementsByClassName( className ); } }; /* QSA/matchesSelector ---------------------------------------------------------------------- */ // QSA and matchesSelector support // matchesSelector(:active) reports false when true (IE9/Opera 11.5) rbuggyMatches = []; // qSa(:focus) reports false when true (Chrome 21) // We allow this because of a bug in IE8/9 that throws an error // whenever `document.activeElement` is accessed on an iframe // So, we allow :focus to pass through QSA all the time to avoid the IE error // See http://bugs.jquery.com/ticket/13378 rbuggyQSA = []; if ( (support.qsa = rnative.test( doc.querySelectorAll )) ) { // Build QSA regex // Regex strategy adopted from Diego Perini assert(function( div ) { // Select is set to empty string on purpose // This is to test IE's treatment of not explicitly // setting a boolean content attribute, // since its presence should be enough // http://bugs.jquery.com/ticket/12359 div.innerHTML = ""; // Support: IE8 // Boolean attributes and "value" are not treated correctly if ( !div.querySelectorAll("[selected]").length ) { rbuggyQSA.push( "\\[" + whitespace + "*(?:value|" + booleans + ")" ); } // Webkit/Opera - :checked should return selected option elements // http://www.w3.org/TR/2011/REC-css3-selectors-20110929/#checked // IE8 throws error here and will not see later tests if ( !div.querySelectorAll(":checked").length ) { rbuggyQSA.push(":checked"); } }); assert(function( div ) { // Support: Opera 10-12/IE8 // ^= $= *= and empty values // Should not select anything // Support: Windows 8 Native Apps // The type attribute is restricted during .innerHTML assignment var input = doc.createElement("input"); input.setAttribute( "type", "hidden" ); div.appendChild( input ).setAttribute( "t", "" ); if ( div.querySelectorAll("[t^='']").length ) { rbuggyQSA.push( "[*^$]=" + whitespace + "*(?:''|\"\")" ); } // FF 3.5 - :enabled/:disabled and hidden elements (hidden elements are still enabled) // IE8 throws error here and will not see later tests if ( !div.querySelectorAll(":enabled").length ) { rbuggyQSA.push( ":enabled", ":disabled" ); } // Opera 10-11 does not throw on post-comma invalid pseudos div.querySelectorAll("*,:x"); rbuggyQSA.push(",.*:"); }); } if ( (support.matchesSelector = rnative.test( (matches = docElem.webkitMatchesSelector || docElem.mozMatchesSelector || docElem.oMatchesSelector || docElem.msMatchesSelector) )) ) { assert(function( div ) { // Check to see if it's possible to do matchesSelector // on a disconnected node (IE 9) support.disconnectedMatch = matches.call( div, "div" ); // This should fail with an exception // Gecko does not error, returns false instead matches.call( div, "[s!='']:x" ); rbuggyMatches.push( "!=", pseudos ); }); } rbuggyQSA = rbuggyQSA.length && new RegExp( rbuggyQSA.join("|") ); rbuggyMatches = rbuggyMatches.length && new RegExp( rbuggyMatches.join("|") ); /* Contains ---------------------------------------------------------------------- */ // Element contains another // Purposefully does not implement inclusive descendent // As in, an element does not contain itself contains = rnative.test( docElem.contains ) || docElem.compareDocumentPosition ? function( a, b ) { var adown = a.nodeType === 9 ? a.documentElement : a, bup = b && b.parentNode; return a === bup || !!( bup && bup.nodeType === 1 && ( adown.contains ? adown.contains( bup ) : a.compareDocumentPosition && a.compareDocumentPosition( bup ) & 16 )); } : function( a, b ) { if ( b ) { while ( (b = b.parentNode) ) { if ( b === a ) { return true; } } } return false; }; /* Sorting ---------------------------------------------------------------------- */ // Document order sorting sortOrder = docElem.compareDocumentPosition ? function( a, b ) { // Flag for duplicate removal if ( a === b ) { hasDuplicate = true; return 0; } var compare = b.compareDocumentPosition && a.compareDocumentPosition && a.compareDocumentPosition( b ); if ( compare ) { // Disconnected nodes if ( compare & 1 || (!support.sortDetached && b.compareDocumentPosition( a ) === compare) ) { // Choose the first element that is related to our preferred document if ( a === doc || contains(preferredDoc, a) ) { return -1; } if ( b === doc || contains(preferredDoc, b) ) { return 1; } // Maintain original order return sortInput ? ( indexOf.call( sortInput, a ) - indexOf.call( sortInput, b ) ) : 0; } return compare & 4 ? -1 : 1; } // Not directly comparable, sort on existence of method return a.compareDocumentPosition ? -1 : 1; } : function( a, b ) { var cur, i = 0, aup = a.parentNode, bup = b.parentNode, ap = [ a ], bp = [ b ]; // Exit early if the nodes are identical if ( a === b ) { hasDuplicate = true; return 0; // Parentless nodes are either documents or disconnected } else if ( !aup || !bup ) { return a === doc ? -1 : b === doc ? 1 : aup ? -1 : bup ? 1 : sortInput ? ( indexOf.call( sortInput, a ) - indexOf.call( sortInput, b ) ) : 0; // If the nodes are siblings, we can do a quick check } else if ( aup === bup ) { return siblingCheck( a, b ); } // Otherwise we need full lists of their ancestors for comparison cur = a; while ( (cur = cur.parentNode) ) { ap.unshift( cur ); } cur = b; while ( (cur = cur.parentNode) ) { bp.unshift( cur ); } // Walk down the tree looking for a discrepancy while ( ap[i] === bp[i] ) { i++; } return i ? // Do a sibling check if the nodes have a common ancestor siblingCheck( ap[i], bp[i] ) : // Otherwise nodes in our document sort first ap[i] === preferredDoc ? -1 : bp[i] === preferredDoc ? 1 : 0; }; return doc; }; Sizzle.matches = function( expr, elements ) { return Sizzle( expr, null, null, elements ); }; Sizzle.matchesSelector = function( elem, expr ) { // Set document vars if needed if ( ( elem.ownerDocument || elem ) !== document ) { setDocument( elem ); } // Make sure that attribute selectors are quoted expr = expr.replace( rattributeQuotes, "='$1']" ); if ( support.matchesSelector && documentIsHTML && ( !rbuggyMatches || !rbuggyMatches.test( expr ) ) && ( !rbuggyQSA || !rbuggyQSA.test( expr ) ) ) { try { var ret = matches.call( elem, expr ); // IE 9's matchesSelector returns false on disconnected nodes if ( ret || support.disconnectedMatch || // As well, disconnected nodes are said to be in a document // fragment in IE 9 elem.document && elem.document.nodeType !== 11 ) { return ret; } } catch(e) {} } return Sizzle( expr, document, null, [elem] ).length > 0; }; Sizzle.contains = function( context, elem ) { // Set document vars if needed if ( ( context.ownerDocument || context ) !== document ) { setDocument( context ); } return contains( context, elem ); }; Sizzle.attr = function( elem, name ) { // Set document vars if needed if ( ( elem.ownerDocument || elem ) !== document ) { setDocument( elem ); } var fn = Expr.attrHandle[ name.toLowerCase() ], // Don't get fooled by Object.prototype properties (jQuery #13807) val = fn && hasOwn.call( Expr.attrHandle, name.toLowerCase() ) ? fn( elem, name, !documentIsHTML ) : undefined; return val === undefined ? support.attributes || !documentIsHTML ? elem.getAttribute( name ) : (val = elem.getAttributeNode(name)) && val.specified ? val.value : null : val; }; Sizzle.error = function( msg ) { throw new Error( "Syntax error, unrecognized expression: " + msg ); }; /** * Document sorting and removing duplicates * @param {ArrayLike} results */ Sizzle.uniqueSort = function( results ) { var elem, duplicates = [], j = 0, i = 0; // Unless we *know* we can detect duplicates, assume their presence hasDuplicate = !support.detectDuplicates; sortInput = !support.sortStable && results.slice( 0 ); results.sort( sortOrder ); if ( hasDuplicate ) { while ( (elem = results[i++]) ) { if ( elem === results[ i ] ) { j = duplicates.push( i ); } } while ( j-- ) { results.splice( duplicates[ j ], 1 ); } } return results; }; /** * Utility function for retrieving the text value of an array of DOM nodes * @param {Array|Element} elem */ getText = Sizzle.getText = function( elem ) { var node, ret = "", i = 0, nodeType = elem.nodeType; if ( !nodeType ) { // If no nodeType, this is expected to be an array for ( ; (node = elem[i]); i++ ) { // Do not traverse comment nodes ret += getText( node ); } } else if ( nodeType === 1 || nodeType === 9 || nodeType === 11 ) { // Use textContent for elements // innerText usage removed for consistency of new lines (see #11153) if ( typeof elem.textContent === "string" ) { return elem.textContent; } else { // Traverse its children for ( elem = elem.firstChild; elem; elem = elem.nextSibling ) { ret += getText( elem ); } } } else if ( nodeType === 3 || nodeType === 4 ) { return elem.nodeValue; } // Do not include comment or processing instruction nodes return ret; }; Expr = Sizzle.selectors = { // Can be adjusted by the user cacheLength: 50, createPseudo: markFunction, match: matchExpr, attrHandle: {}, find: {}, relative: { ">": { dir: "parentNode", first: true }, " ": { dir: "parentNode" }, "+": { dir: "previousSibling", first: true }, "~": { dir: "previousSibling" } }, preFilter: { "ATTR": function( match ) { match[1] = match[1].replace( runescape, funescape ); // Move the given value to match[3] whether quoted or unquoted match[3] = ( match[4] || match[5] || "" ).replace( runescape, funescape ); if ( match[2] === "~=" ) { match[3] = " " + match[3] + " "; } return match.slice( 0, 4 ); }, "CHILD": function( match ) { /* matches from matchExpr["CHILD"] 1 type (only|nth|...) 2 what (child|of-type) 3 argument (even|odd|\d*|\d*n([+-]\d+)?|...) 4 xn-component of xn+y argument ([+-]?\d*n|) 5 sign of xn-component 6 x of xn-component 7 sign of y-component 8 y of y-component */ match[1] = match[1].toLowerCase(); if ( match[1].slice( 0, 3 ) === "nth" ) { // nth-* requires argument if ( !match[3] ) { Sizzle.error( match[0] ); } // numeric x and y parameters for Expr.filter.CHILD // remember that false/true cast respectively to 0/1 match[4] = +( match[4] ? match[5] + (match[6] || 1) : 2 * ( match[3] === "even" || match[3] === "odd" ) ); match[5] = +( ( match[7] + match[8] ) || match[3] === "odd" ); // other types prohibit arguments } else if ( match[3] ) { Sizzle.error( match[0] ); } return match; }, "PSEUDO": function( match ) { var excess, unquoted = !match[5] && match[2]; if ( matchExpr["CHILD"].test( match[0] ) ) { return null; } // Accept quoted arguments as-is if ( match[3] && match[4] !== undefined ) { match[2] = match[4]; // Strip excess characters from unquoted arguments } else if ( unquoted && rpseudo.test( unquoted ) && // Get excess from tokenize (recursively) (excess = tokenize( unquoted, true )) && // advance to the next closing parenthesis (excess = unquoted.indexOf( ")", unquoted.length - excess ) - unquoted.length) ) { // excess is a negative index match[0] = match[0].slice( 0, excess ); match[2] = unquoted.slice( 0, excess ); } // Return only captures needed by the pseudo filter method (type and argument) return match.slice( 0, 3 ); } }, filter: { "TAG": function( nodeNameSelector ) { var nodeName = nodeNameSelector.replace( runescape, funescape ).toLowerCase(); return nodeNameSelector === "*" ? function() { return true; } : function( elem ) { return elem.nodeName && elem.nodeName.toLowerCase() === nodeName; }; }, "CLASS": function( className ) { var pattern = classCache[ className + " " ]; return pattern || (pattern = new RegExp( "(^|" + whitespace + ")" + className + "(" + whitespace + "|$)" )) && classCache( className, function( elem ) { return pattern.test( typeof elem.className === "string" && elem.className || typeof elem.getAttribute !== strundefined && elem.getAttribute("class") || "" ); }); }, "ATTR": function( name, operator, check ) { return function( elem ) { var result = Sizzle.attr( elem, name ); if ( result == null ) { return operator === "!="; } if ( !operator ) { return true; } result += ""; return operator === "=" ? result === check : operator === "!=" ? result !== check : operator === "^=" ? check && result.indexOf( check ) === 0 : operator === "*=" ? check && result.indexOf( check ) > -1 : operator === "$=" ? check && result.slice( -check.length ) === check : operator === "~=" ? ( " " + result + " " ).indexOf( check ) > -1 : operator === "|=" ? result === check || result.slice( 0, check.length + 1 ) === check + "-" : false; }; }, "CHILD": function( type, what, argument, first, last ) { var simple = type.slice( 0, 3 ) !== "nth", forward = type.slice( -4 ) !== "last", ofType = what === "of-type"; return first === 1 && last === 0 ? // Shortcut for :nth-*(n) function( elem ) { return !!elem.parentNode; } : function( elem, context, xml ) { var cache, outerCache, node, diff, nodeIndex, start, dir = simple !== forward ? "nextSibling" : "previousSibling", parent = elem.parentNode, name = ofType && elem.nodeName.toLowerCase(), useCache = !xml && !ofType; if ( parent ) { // :(first|last|only)-(child|of-type) if ( simple ) { while ( dir ) { node = elem; while ( (node = node[ dir ]) ) { if ( ofType ? node.nodeName.toLowerCase() === name : node.nodeType === 1 ) { return false; } } // Reverse direction for :only-* (if we haven't yet done so) start = dir = type === "only" && !start && "nextSibling"; } return true; } start = [ forward ? parent.firstChild : parent.lastChild ]; // non-xml :nth-child(...) stores cache data on `parent` if ( forward && useCache ) { // Seek `elem` from a previously-cached index outerCache = parent[ expando ] || (parent[ expando ] = {}); cache = outerCache[ type ] || []; nodeIndex = cache[0] === dirruns && cache[1]; diff = cache[0] === dirruns && cache[2]; node = nodeIndex && parent.childNodes[ nodeIndex ]; while ( (node = ++nodeIndex && node && node[ dir ] || // Fallback to seeking `elem` from the start (diff = nodeIndex = 0) || start.pop()) ) { // When found, cache indexes on `parent` and break if ( node.nodeType === 1 && ++diff && node === elem ) { outerCache[ type ] = [ dirruns, nodeIndex, diff ]; break; } } // Use previously-cached element index if available } else if ( useCache && (cache = (elem[ expando ] || (elem[ expando ] = {}))[ type ]) && cache[0] === dirruns ) { diff = cache[1]; // xml :nth-child(...) or :nth-last-child(...) or :nth(-last)?-of-type(...) } else { // Use the same loop as above to seek `elem` from the start while ( (node = ++nodeIndex && node && node[ dir ] || (diff = nodeIndex = 0) || start.pop()) ) { if ( ( ofType ? node.nodeName.toLowerCase() === name : node.nodeType === 1 ) && ++diff ) { // Cache the index of each encountered element if ( useCache ) { (node[ expando ] || (node[ expando ] = {}))[ type ] = [ dirruns, diff ]; } if ( node === elem ) { break; } } } } // Incorporate the offset, then check against cycle size diff -= last; return diff === first || ( diff % first === 0 && diff / first >= 0 ); } }; }, "PSEUDO": function( pseudo, argument ) { // pseudo-class names are case-insensitive // http://www.w3.org/TR/selectors/#pseudo-classes // Prioritize by case sensitivity in case custom pseudos are added with uppercase letters // Remember that setFilters inherits from pseudos var args, fn = Expr.pseudos[ pseudo ] || Expr.setFilters[ pseudo.toLowerCase() ] || Sizzle.error( "unsupported pseudo: " + pseudo ); // The user may use createPseudo to indicate that // arguments are needed to create the filter function // just as Sizzle does if ( fn[ expando ] ) { return fn( argument ); } // But maintain support for old signatures if ( fn.length > 1 ) { args = [ pseudo, pseudo, "", argument ]; return Expr.setFilters.hasOwnProperty( pseudo.toLowerCase() ) ? markFunction(function( seed, matches ) { var idx, matched = fn( seed, argument ), i = matched.length; while ( i-- ) { idx = indexOf.call( seed, matched[i] ); seed[ idx ] = !( matches[ idx ] = matched[i] ); } }) : function( elem ) { return fn( elem, 0, args ); }; } return fn; } }, pseudos: { // Potentially complex pseudos "not": markFunction(function( selector ) { // Trim the selector passed to compile // to avoid treating leading and trailing // spaces as combinators var input = [], results = [], matcher = compile( selector.replace( rtrim, "$1" ) ); return matcher[ expando ] ? markFunction(function( seed, matches, context, xml ) { var elem, unmatched = matcher( seed, null, xml, [] ), i = seed.length; // Match elements unmatched by `matcher` while ( i-- ) { if ( (elem = unmatched[i]) ) { seed[i] = !(matches[i] = elem); } } }) : function( elem, context, xml ) { input[0] = elem; matcher( input, null, xml, results ); return !results.pop(); }; }), "has": markFunction(function( selector ) { return function( elem ) { return Sizzle( selector, elem ).length > 0; }; }), "contains": markFunction(function( text ) { return function( elem ) { return ( elem.textContent || elem.innerText || getText( elem ) ).indexOf( text ) > -1; }; }), // "Whether an element is represented by a :lang() selector // is based solely on the element's language value // being equal to the identifier C, // or beginning with the identifier C immediately followed by "-". // The matching of C against the element's language value is performed case-insensitively. // The identifier C does not have to be a valid language name." // http://www.w3.org/TR/selectors/#lang-pseudo "lang": markFunction( function( lang ) { // lang value must be a valid identifier if ( !ridentifier.test(lang || "") ) { Sizzle.error( "unsupported lang: " + lang ); } lang = lang.replace( runescape, funescape ).toLowerCase(); return function( elem ) { var elemLang; do { if ( (elemLang = documentIsHTML ? elem.lang : elem.getAttribute("xml:lang") || elem.getAttribute("lang")) ) { elemLang = elemLang.toLowerCase(); return elemLang === lang || elemLang.indexOf( lang + "-" ) === 0; } } while ( (elem = elem.parentNode) && elem.nodeType === 1 ); return false; }; }), // Miscellaneous "target": function( elem ) { var hash = window.location && window.location.hash; return hash && hash.slice( 1 ) === elem.id; }, "root": function( elem ) { return elem === docElem; }, "focus": function( elem ) { return elem === document.activeElement && (!document.hasFocus || document.hasFocus()) && !!(elem.type || elem.href || ~elem.tabIndex); }, // Boolean properties "enabled": function( elem ) { return elem.disabled === false; }, "disabled": function( elem ) { return elem.disabled === true; }, "checked": function( elem ) { // In CSS3, :checked should return both checked and selected elements // http://www.w3.org/TR/2011/REC-css3-selectors-20110929/#checked var nodeName = elem.nodeName.toLowerCase(); return (nodeName === "input" && !!elem.checked) || (nodeName === "option" && !!elem.selected); }, "selected": function( elem ) { // Accessing this property makes selected-by-default // options in Safari work properly if ( elem.parentNode ) { elem.parentNode.selectedIndex; } return elem.selected === true; }, // Contents "empty": function( elem ) { // http://www.w3.org/TR/selectors/#empty-pseudo // :empty is only affected by element nodes and content nodes(including text(3), cdata(4)), // not comment, processing instructions, or others // Thanks to Diego Perini for the nodeName shortcut // Greater than "@" means alpha characters (specifically not starting with "#" or "?") for ( elem = elem.firstChild; elem; elem = elem.nextSibling ) { if ( elem.nodeName > "@" || elem.nodeType === 3 || elem.nodeType === 4 ) { return false; } } return true; }, "parent": function( elem ) { return !Expr.pseudos["empty"]( elem ); }, // Element/input types "header": function( elem ) { return rheader.test( elem.nodeName ); }, "input": function( elem ) { return rinputs.test( elem.nodeName ); }, "button": function( elem ) { var name = elem.nodeName.toLowerCase(); return name === "input" && elem.type === "button" || name === "button"; }, "text": function( elem ) { var attr; // IE6 and 7 will map elem.type to 'text' for new HTML5 types (search, etc) // use getAttribute instead to test this case return elem.nodeName.toLowerCase() === "input" && elem.type === "text" && ( (attr = elem.getAttribute("type")) == null || attr.toLowerCase() === elem.type ); }, // Position-in-collection "first": createPositionalPseudo(function() { return [ 0 ]; }), "last": createPositionalPseudo(function( matchIndexes, length ) { return [ length - 1 ]; }), "eq": createPositionalPseudo(function( matchIndexes, length, argument ) { return [ argument < 0 ? argument + length : argument ]; }), "even": createPositionalPseudo(function( matchIndexes, length ) { var i = 0; for ( ; i < length; i += 2 ) { matchIndexes.push( i ); } return matchIndexes; }), "odd": createPositionalPseudo(function( matchIndexes, length ) { var i = 1; for ( ; i < length; i += 2 ) { matchIndexes.push( i ); } return matchIndexes; }), "lt": createPositionalPseudo(function( matchIndexes, length, argument ) { var i = argument < 0 ? argument + length : argument; for ( ; --i >= 0; ) { matchIndexes.push( i ); } return matchIndexes; }), "gt": createPositionalPseudo(function( matchIndexes, length, argument ) { var i = argument < 0 ? argument + length : argument; for ( ; ++i < length; ) { matchIndexes.push( i ); } return matchIndexes; }) } }; Expr.pseudos["nth"] = Expr.pseudos["eq"]; // Add button/input type pseudos for ( i in { radio: true, checkbox: true, file: true, password: true, image: true } ) { Expr.pseudos[ i ] = createInputPseudo( i ); } for ( i in { submit: true, reset: true } ) { Expr.pseudos[ i ] = createButtonPseudo( i ); } // Easy API for creating new setFilters function setFilters() {} setFilters.prototype = Expr.filters = Expr.pseudos; Expr.setFilters = new setFilters(); function tokenize( selector, parseOnly ) { var matched, match, tokens, type, soFar, groups, preFilters, cached = tokenCache[ selector + " " ]; if ( cached ) { return parseOnly ? 0 : cached.slice( 0 ); } soFar = selector; groups = []; preFilters = Expr.preFilter; while ( soFar ) { // Comma and first run if ( !matched || (match = rcomma.exec( soFar )) ) { if ( match ) { // Don't consume trailing commas as valid soFar = soFar.slice( match[0].length ) || soFar; } groups.push( tokens = [] ); } matched = false; // Combinators if ( (match = rcombinators.exec( soFar )) ) { matched = match.shift(); tokens.push({ value: matched, // Cast descendant combinators to space type: match[0].replace( rtrim, " " ) }); soFar = soFar.slice( matched.length ); } // Filters for ( type in Expr.filter ) { if ( (match = matchExpr[ type ].exec( soFar )) && (!preFilters[ type ] || (match = preFilters[ type ]( match ))) ) { matched = match.shift(); tokens.push({ value: matched, type: type, matches: match }); soFar = soFar.slice( matched.length ); } } if ( !matched ) { break; } } // Return the length of the invalid excess // if we're just parsing // Otherwise, throw an error or return tokens return parseOnly ? soFar.length : soFar ? Sizzle.error( selector ) : // Cache the tokens tokenCache( selector, groups ).slice( 0 ); } function toSelector( tokens ) { var i = 0, len = tokens.length, selector = ""; for ( ; i < len; i++ ) { selector += tokens[i].value; } return selector; } function addCombinator( matcher, combinator, base ) { var dir = combinator.dir, checkNonElements = base && dir === "parentNode", doneName = done++; return combinator.first ? // Check against closest ancestor/preceding element function( elem, context, xml ) { while ( (elem = elem[ dir ]) ) { if ( elem.nodeType === 1 || checkNonElements ) { return matcher( elem, context, xml ); } } } : // Check against all ancestor/preceding elements function( elem, context, xml ) { var data, cache, outerCache, dirkey = dirruns + " " + doneName; // We can't set arbitrary data on XML nodes, so they don't benefit from dir caching if ( xml ) { while ( (elem = elem[ dir ]) ) { if ( elem.nodeType === 1 || checkNonElements ) { if ( matcher( elem, context, xml ) ) { return true; } } } } else { while ( (elem = elem[ dir ]) ) { if ( elem.nodeType === 1 || checkNonElements ) { outerCache = elem[ expando ] || (elem[ expando ] = {}); if ( (cache = outerCache[ dir ]) && cache[0] === dirkey ) { if ( (data = cache[1]) === true || data === cachedruns ) { return data === true; } } else { cache = outerCache[ dir ] = [ dirkey ]; cache[1] = matcher( elem, context, xml ) || cachedruns; if ( cache[1] === true ) { return true; } } } } } }; } function elementMatcher( matchers ) { return matchers.length > 1 ? function( elem, context, xml ) { var i = matchers.length; while ( i-- ) { if ( !matchers[i]( elem, context, xml ) ) { return false; } } return true; } : matchers[0]; } function condense( unmatched, map, filter, context, xml ) { var elem, newUnmatched = [], i = 0, len = unmatched.length, mapped = map != null; for ( ; i < len; i++ ) { if ( (elem = unmatched[i]) ) { if ( !filter || filter( elem, context, xml ) ) { newUnmatched.push( elem ); if ( mapped ) { map.push( i ); } } } } return newUnmatched; } function setMatcher( preFilter, selector, matcher, postFilter, postFinder, postSelector ) { if ( postFilter && !postFilter[ expando ] ) { postFilter = setMatcher( postFilter ); } if ( postFinder && !postFinder[ expando ] ) { postFinder = setMatcher( postFinder, postSelector ); } return markFunction(function( seed, results, context, xml ) { var temp, i, elem, preMap = [], postMap = [], preexisting = results.length, // Get initial elements from seed or context elems = seed || multipleContexts( selector || "*", context.nodeType ? [ context ] : context, [] ), // Prefilter to get matcher input, preserving a map for seed-results synchronization matcherIn = preFilter && ( seed || !selector ) ? condense( elems, preMap, preFilter, context, xml ) : elems, matcherOut = matcher ? // If we have a postFinder, or filtered seed, or non-seed postFilter or preexisting results, postFinder || ( seed ? preFilter : preexisting || postFilter ) ? // ...intermediate processing is necessary [] : // ...otherwise use results directly results : matcherIn; // Find primary matches if ( matcher ) { matcher( matcherIn, matcherOut, context, xml ); } // Apply postFilter if ( postFilter ) { temp = condense( matcherOut, postMap ); postFilter( temp, [], context, xml ); // Un-match failing elements by moving them back to matcherIn i = temp.length; while ( i-- ) { if ( (elem = temp[i]) ) { matcherOut[ postMap[i] ] = !(matcherIn[ postMap[i] ] = elem); } } } if ( seed ) { if ( postFinder || preFilter ) { if ( postFinder ) { // Get the final matcherOut by condensing this intermediate into postFinder contexts temp = []; i = matcherOut.length; while ( i-- ) { if ( (elem = matcherOut[i]) ) { // Restore matcherIn since elem is not yet a final match temp.push( (matcherIn[i] = elem) ); } } postFinder( null, (matcherOut = []), temp, xml ); } // Move matched elements from seed to results to keep them synchronized i = matcherOut.length; while ( i-- ) { if ( (elem = matcherOut[i]) && (temp = postFinder ? indexOf.call( seed, elem ) : preMap[i]) > -1 ) { seed[temp] = !(results[temp] = elem); } } } // Add elements to results, through postFinder if defined } else { matcherOut = condense( matcherOut === results ? matcherOut.splice( preexisting, matcherOut.length ) : matcherOut ); if ( postFinder ) { postFinder( null, results, matcherOut, xml ); } else { push.apply( results, matcherOut ); } } }); } function matcherFromTokens( tokens ) { var checkContext, matcher, j, len = tokens.length, leadingRelative = Expr.relative[ tokens[0].type ], implicitRelative = leadingRelative || Expr.relative[" "], i = leadingRelative ? 1 : 0, // The foundational matcher ensures that elements are reachable from top-level context(s) matchContext = addCombinator( function( elem ) { return elem === checkContext; }, implicitRelative, true ), matchAnyContext = addCombinator( function( elem ) { return indexOf.call( checkContext, elem ) > -1; }, implicitRelative, true ), matchers = [ function( elem, context, xml ) { return ( !leadingRelative && ( xml || context !== outermostContext ) ) || ( (checkContext = context).nodeType ? matchContext( elem, context, xml ) : matchAnyContext( elem, context, xml ) ); } ]; for ( ; i < len; i++ ) { if ( (matcher = Expr.relative[ tokens[i].type ]) ) { matchers = [ addCombinator(elementMatcher( matchers ), matcher) ]; } else { matcher = Expr.filter[ tokens[i].type ].apply( null, tokens[i].matches ); // Return special upon seeing a positional matcher if ( matcher[ expando ] ) { // Find the next relative operator (if any) for proper handling j = ++i; for ( ; j < len; j++ ) { if ( Expr.relative[ tokens[j].type ] ) { break; } } return setMatcher( i > 1 && elementMatcher( matchers ), i > 1 && toSelector( // If the preceding token was a descendant combinator, insert an implicit any-element `*` tokens.slice( 0, i - 1 ).concat({ value: tokens[ i - 2 ].type === " " ? "*" : "" }) ).replace( rtrim, "$1" ), matcher, i < j && matcherFromTokens( tokens.slice( i, j ) ), j < len && matcherFromTokens( (tokens = tokens.slice( j )) ), j < len && toSelector( tokens ) ); } matchers.push( matcher ); } } return elementMatcher( matchers ); } function matcherFromGroupMatchers( elementMatchers, setMatchers ) { // A counter to specify which element is currently being matched var matcherCachedRuns = 0, bySet = setMatchers.length > 0, byElement = elementMatchers.length > 0, superMatcher = function( seed, context, xml, results, expandContext ) { var elem, j, matcher, setMatched = [], matchedCount = 0, i = "0", unmatched = seed && [], outermost = expandContext != null, contextBackup = outermostContext, // We must always have either seed elements or context elems = seed || byElement && Expr.find["TAG"]( "*", expandContext && context.parentNode || context ), // Use integer dirruns iff this is the outermost matcher dirrunsUnique = (dirruns += contextBackup == null ? 1 : Math.random() || 0.1); if ( outermost ) { outermostContext = context !== document && context; cachedruns = matcherCachedRuns; } // Add elements passing elementMatchers directly to results // Keep `i` a string if there are no elements so `matchedCount` will be "00" below for ( ; (elem = elems[i]) != null; i++ ) { if ( byElement && elem ) { j = 0; while ( (matcher = elementMatchers[j++]) ) { if ( matcher( elem, context, xml ) ) { results.push( elem ); break; } } if ( outermost ) { dirruns = dirrunsUnique; cachedruns = ++matcherCachedRuns; } } // Track unmatched elements for set filters if ( bySet ) { // They will have gone through all possible matchers if ( (elem = !matcher && elem) ) { matchedCount--; } // Lengthen the array for every element, matched or not if ( seed ) { unmatched.push( elem ); } } } // Apply set filters to unmatched elements matchedCount += i; if ( bySet && i !== matchedCount ) { j = 0; while ( (matcher = setMatchers[j++]) ) { matcher( unmatched, setMatched, context, xml ); } if ( seed ) { // Reintegrate element matches to eliminate the need for sorting if ( matchedCount > 0 ) { while ( i-- ) { if ( !(unmatched[i] || setMatched[i]) ) { setMatched[i] = pop.call( results ); } } } // Discard index placeholder values to get only actual matches setMatched = condense( setMatched ); } // Add matches to results push.apply( results, setMatched ); // Seedless set matches succeeding multiple successful matchers stipulate sorting if ( outermost && !seed && setMatched.length > 0 && ( matchedCount + setMatchers.length ) > 1 ) { Sizzle.uniqueSort( results ); } } // Override manipulation of globals by nested matchers if ( outermost ) { dirruns = dirrunsUnique; outermostContext = contextBackup; } return unmatched; }; return bySet ? markFunction( superMatcher ) : superMatcher; } compile = Sizzle.compile = function( selector, group /* Internal Use Only */ ) { var i, setMatchers = [], elementMatchers = [], cached = compilerCache[ selector + " " ]; if ( !cached ) { // Generate a function of recursive functions that can be used to check each element if ( !group ) { group = tokenize( selector ); } i = group.length; while ( i-- ) { cached = matcherFromTokens( group[i] ); if ( cached[ expando ] ) { setMatchers.push( cached ); } else { elementMatchers.push( cached ); } } // Cache the compiled function cached = compilerCache( selector, matcherFromGroupMatchers( elementMatchers, setMatchers ) ); } return cached; }; function multipleContexts( selector, contexts, results ) { var i = 0, len = contexts.length; for ( ; i < len; i++ ) { Sizzle( selector, contexts[i], results ); } return results; } function select( selector, context, results, seed ) { var i, tokens, token, type, find, match = tokenize( selector ); if ( !seed ) { // Try to minimize operations if there is only one group if ( match.length === 1 ) { // Take a shortcut and set the context if the root selector is an ID tokens = match[0] = match[0].slice( 0 ); if ( tokens.length > 2 && (token = tokens[0]).type === "ID" && support.getById && context.nodeType === 9 && documentIsHTML && Expr.relative[ tokens[1].type ] ) { context = ( Expr.find["ID"]( token.matches[0].replace(runescape, funescape), context ) || [] )[0]; if ( !context ) { return results; } selector = selector.slice( tokens.shift().value.length ); } // Fetch a seed set for right-to-left matching i = matchExpr["needsContext"].test( selector ) ? 0 : tokens.length; while ( i-- ) { token = tokens[i]; // Abort if we hit a combinator if ( Expr.relative[ (type = token.type) ] ) { break; } if ( (find = Expr.find[ type ]) ) { // Search, expanding context for leading sibling combinators if ( (seed = find( token.matches[0].replace( runescape, funescape ), rsibling.test( tokens[0].type ) && context.parentNode || context )) ) { // If seed is empty or no tokens remain, we can return early tokens.splice( i, 1 ); selector = seed.length && toSelector( tokens ); if ( !selector ) { push.apply( results, seed ); return results; } break; } } } } } // Compile and execute a filtering function // Provide `match` to avoid retokenization if we modified the selector above compile( selector, match )( seed, context, !documentIsHTML, results, rsibling.test( selector ) ); return results; } // One-time assignments // Sort stability support.sortStable = expando.split("").sort( sortOrder ).join("") === expando; // Support: Chrome<14 // Always assume duplicates if they aren't passed to the comparison function support.detectDuplicates = hasDuplicate; // Initialize against the default document setDocument(); // Support: Webkit<537.32 - Safari 6.0.3/Chrome 25 (fixed in Chrome 27) // Detached nodes confoundingly follow *each other* support.sortDetached = assert(function( div1 ) { // Should return 1, but returns 4 (following) return div1.compareDocumentPosition( document.createElement("div") ) & 1; }); // Support: IE<8 // Prevent attribute/property "interpolation" // http://msdn.microsoft.com/en-us/library/ms536429%28VS.85%29.aspx if ( !assert(function( div ) { div.innerHTML = ""; return div.firstChild.getAttribute("href") === "#" ; }) ) { addHandle( "type|href|height|width", function( elem, name, isXML ) { if ( !isXML ) { return elem.getAttribute( name, name.toLowerCase() === "type" ? 1 : 2 ); } }); } // Support: IE<9 // Use defaultValue in place of getAttribute("value") if ( !support.attributes || !assert(function( div ) { div.innerHTML = ""; div.firstChild.setAttribute( "value", "" ); return div.firstChild.getAttribute( "value" ) === ""; }) ) { addHandle( "value", function( elem, name, isXML ) { if ( !isXML && elem.nodeName.toLowerCase() === "input" ) { return elem.defaultValue; } }); } // Support: IE<9 // Use getAttributeNode to fetch booleans when getAttribute lies if ( !assert(function( div ) { return div.getAttribute("disabled") == null; }) ) { addHandle( booleans, function( elem, name, isXML ) { var val; if ( !isXML ) { return (val = elem.getAttributeNode( name )) && val.specified ? val.value : elem[ name ] === true ? name.toLowerCase() : null; } }); } jQuery.find = Sizzle; jQuery.expr = Sizzle.selectors; jQuery.expr[":"] = jQuery.expr.pseudos; jQuery.unique = Sizzle.uniqueSort; jQuery.text = Sizzle.getText; jQuery.isXMLDoc = Sizzle.isXML; jQuery.contains = Sizzle.contains; })( window ); // String to Object options format cache var optionsCache = {}; // Convert String-formatted options into Object-formatted ones and store in cache function createOptions( options ) { var object = optionsCache[ options ] = {}; jQuery.each( options.match( core_rnotwhite ) || [], function( _, flag ) { object[ flag ] = true; }); return object; } /* * Create a callback list using the following parameters: * * options: an optional list of space-separated options that will change how * the callback list behaves or a more traditional option object * * By default a callback list will act like an event callback list and can be * "fired" multiple times. * * Possible options: * * once: will ensure the callback list can only be fired once (like a Deferred) * * memory: will keep track of previous values and will call any callback added * after the list has been fired right away with the latest "memorized" * values (like a Deferred) * * unique: will ensure a callback can only be added once (no duplicate in the list) * * stopOnFalse: interrupt callings when a callback returns false * */ jQuery.Callbacks = function( options ) { // Convert options from String-formatted to Object-formatted if needed // (we check in cache first) options = typeof options === "string" ? ( optionsCache[ options ] || createOptions( options ) ) : jQuery.extend( {}, options ); var // Last fire value (for non-forgettable lists) memory, // Flag to know if list was already fired fired, // Flag to know if list is currently firing firing, // First callback to fire (used internally by add and fireWith) firingStart, // End of the loop when firing firingLength, // Index of currently firing callback (modified by remove if needed) firingIndex, // Actual callback list list = [], // Stack of fire calls for repeatable lists stack = !options.once && [], // Fire callbacks fire = function( data ) { memory = options.memory && data; fired = true; firingIndex = firingStart || 0; firingStart = 0; firingLength = list.length; firing = true; for ( ; list && firingIndex < firingLength; firingIndex++ ) { if ( list[ firingIndex ].apply( data[ 0 ], data[ 1 ] ) === false && options.stopOnFalse ) { memory = false; // To prevent further calls using add break; } } firing = false; if ( list ) { if ( stack ) { if ( stack.length ) { fire( stack.shift() ); } } else if ( memory ) { list = []; } else { self.disable(); } } }, // Actual Callbacks object self = { // Add a callback or a collection of callbacks to the list add: function() { if ( list ) { // First, we save the current length var start = list.length; (function add( args ) { jQuery.each( args, function( _, arg ) { var type = jQuery.type( arg ); if ( type === "function" ) { if ( !options.unique || !self.has( arg ) ) { list.push( arg ); } } else if ( arg && arg.length && type !== "string" ) { // Inspect recursively add( arg ); } }); })( arguments ); // Do we need to add the callbacks to the // current firing batch? if ( firing ) { firingLength = list.length; // With memory, if we're not firing then // we should call right away } else if ( memory ) { firingStart = start; fire( memory ); } } return this; }, // Remove a callback from the list remove: function() { if ( list ) { jQuery.each( arguments, function( _, arg ) { var index; while( ( index = jQuery.inArray( arg, list, index ) ) > -1 ) { list.splice( index, 1 ); // Handle firing indexes if ( firing ) { if ( index <= firingLength ) { firingLength--; } if ( index <= firingIndex ) { firingIndex--; } } } }); } return this; }, // Check if a given callback is in the list. // If no argument is given, return whether or not list has callbacks attached. has: function( fn ) { return fn ? jQuery.inArray( fn, list ) > -1 : !!( list && list.length ); }, // Remove all callbacks from the list empty: function() { list = []; firingLength = 0; return this; }, // Have the list do nothing anymore disable: function() { list = stack = memory = undefined; return this; }, // Is it disabled? disabled: function() { return !list; }, // Lock the list in its current state lock: function() { stack = undefined; if ( !memory ) { self.disable(); } return this; }, // Is it locked? locked: function() { return !stack; }, // Call all callbacks with the given context and arguments fireWith: function( context, args ) { if ( list && ( !fired || stack ) ) { args = args || []; args = [ context, args.slice ? args.slice() : args ]; if ( firing ) { stack.push( args ); } else { fire( args ); } } return this; }, // Call all the callbacks with the given arguments fire: function() { self.fireWith( this, arguments ); return this; }, // To know if the callbacks have already been called at least once fired: function() { return !!fired; } }; return self; }; jQuery.extend({ Deferred: function( func ) { var tuples = [ // action, add listener, listener list, final state [ "resolve", "done", jQuery.Callbacks("once memory"), "resolved" ], [ "reject", "fail", jQuery.Callbacks("once memory"), "rejected" ], [ "notify", "progress", jQuery.Callbacks("memory") ] ], state = "pending", promise = { state: function() { return state; }, always: function() { deferred.done( arguments ).fail( arguments ); return this; }, then: function( /* fnDone, fnFail, fnProgress */ ) { var fns = arguments; return jQuery.Deferred(function( newDefer ) { jQuery.each( tuples, function( i, tuple ) { var action = tuple[ 0 ], fn = jQuery.isFunction( fns[ i ] ) && fns[ i ]; // deferred[ done | fail | progress ] for forwarding actions to newDefer deferred[ tuple[1] ](function() { var returned = fn && fn.apply( this, arguments ); if ( returned && jQuery.isFunction( returned.promise ) ) { returned.promise() .done( newDefer.resolve ) .fail( newDefer.reject ) .progress( newDefer.notify ); } else { newDefer[ action + "With" ]( this === promise ? newDefer.promise() : this, fn ? [ returned ] : arguments ); } }); }); fns = null; }).promise(); }, // Get a promise for this deferred // If obj is provided, the promise aspect is added to the object promise: function( obj ) { return obj != null ? jQuery.extend( obj, promise ) : promise; } }, deferred = {}; // Keep pipe for back-compat promise.pipe = promise.then; // Add list-specific methods jQuery.each( tuples, function( i, tuple ) { var list = tuple[ 2 ], stateString = tuple[ 3 ]; // promise[ done | fail | progress ] = list.add promise[ tuple[1] ] = list.add; // Handle state if ( stateString ) { list.add(function() { // state = [ resolved | rejected ] state = stateString; // [ reject_list | resolve_list ].disable; progress_list.lock }, tuples[ i ^ 1 ][ 2 ].disable, tuples[ 2 ][ 2 ].lock ); } // deferred[ resolve | reject | notify ] deferred[ tuple[0] ] = function() { deferred[ tuple[0] + "With" ]( this === deferred ? promise : this, arguments ); return this; }; deferred[ tuple[0] + "With" ] = list.fireWith; }); // Make the deferred a promise promise.promise( deferred ); // Call given func if any if ( func ) { func.call( deferred, deferred ); } // All done! return deferred; }, // Deferred helper when: function( subordinate /* , ..., subordinateN */ ) { var i = 0, resolveValues = core_slice.call( arguments ), length = resolveValues.length, // the count of uncompleted subordinates remaining = length !== 1 || ( subordinate && jQuery.isFunction( subordinate.promise ) ) ? length : 0, // the master Deferred. If resolveValues consist of only a single Deferred, just use that. deferred = remaining === 1 ? subordinate : jQuery.Deferred(), // Update function for both resolve and progress values updateFunc = function( i, contexts, values ) { return function( value ) { contexts[ i ] = this; values[ i ] = arguments.length > 1 ? core_slice.call( arguments ) : value; if( values === progressValues ) { deferred.notifyWith( contexts, values ); } else if ( !( --remaining ) ) { deferred.resolveWith( contexts, values ); } }; }, progressValues, progressContexts, resolveContexts; // add listeners to Deferred subordinates; treat others as resolved if ( length > 1 ) { progressValues = new Array( length ); progressContexts = new Array( length ); resolveContexts = new Array( length ); for ( ; i < length; i++ ) { if ( resolveValues[ i ] && jQuery.isFunction( resolveValues[ i ].promise ) ) { resolveValues[ i ].promise() .done( updateFunc( i, resolveContexts, resolveValues ) ) .fail( deferred.reject ) .progress( updateFunc( i, progressContexts, progressValues ) ); } else { --remaining; } } } // if we're not waiting on anything, resolve the master if ( !remaining ) { deferred.resolveWith( resolveContexts, resolveValues ); } return deferred.promise(); } }); jQuery.support = (function( support ) { var input = document.createElement("input"), fragment = document.createDocumentFragment(), div = document.createElement("div"), select = document.createElement("select"), opt = select.appendChild( document.createElement("option") ); // Finish early in limited environments if ( !input.type ) { return support; } input.type = "checkbox"; // Support: Safari 5.1, iOS 5.1, Android 4.x, Android 2.3 // Check the default checkbox/radio value ("" on old WebKit; "on" elsewhere) support.checkOn = input.value !== ""; // Must access the parent to make an option select properly // Support: IE9, IE10 support.optSelected = opt.selected; // Will be defined later support.reliableMarginRight = true; support.boxSizingReliable = true; support.pixelPosition = false; // Make sure checked status is properly cloned // Support: IE9, IE10 input.checked = true; support.noCloneChecked = input.cloneNode( true ).checked; // Make sure that the options inside disabled selects aren't marked as disabled // (WebKit marks them as disabled) select.disabled = true; support.optDisabled = !opt.disabled; // Check if an input maintains its value after becoming a radio // Support: IE9, IE10 input = document.createElement("input"); input.value = "t"; input.type = "radio"; support.radioValue = input.value === "t"; // #11217 - WebKit loses check when the name is after the checked attribute input.setAttribute( "checked", "t" ); input.setAttribute( "name", "t" ); fragment.appendChild( input ); // Support: Safari 5.1, Android 4.x, Android 2.3 // old WebKit doesn't clone checked state correctly in fragments support.checkClone = fragment.cloneNode( true ).cloneNode( true ).lastChild.checked; // Support: Firefox, Chrome, Safari // Beware of CSP restrictions (https://developer.mozilla.org/en/Security/CSP) support.focusinBubbles = "onfocusin" in window; div.style.backgroundClip = "content-box"; div.cloneNode( true ).style.backgroundClip = ""; support.clearCloneStyle = div.style.backgroundClip === "content-box"; // Run tests that need a body at doc ready jQuery(function() { var container, marginDiv, // Support: Firefox, Android 2.3 (Prefixed box-sizing versions). divReset = "padding:0;margin:0;border:0;display:block;-webkit-box-sizing:content-box;-moz-box-sizing:content-box;box-sizing:content-box", body = document.getElementsByTagName("body")[ 0 ]; if ( !body ) { // Return for frameset docs that don't have a body return; } container = document.createElement("div"); container.style.cssText = "border:0;width:0;height:0;position:absolute;top:0;left:-9999px;margin-top:1px"; // Check box-sizing and margin behavior. body.appendChild( container ).appendChild( div ); div.innerHTML = ""; // Support: Firefox, Android 2.3 (Prefixed box-sizing versions). div.style.cssText = "-webkit-box-sizing:border-box;-moz-box-sizing:border-box;box-sizing:border-box;padding:1px;border:1px;display:block;width:4px;margin-top:1%;position:absolute;top:1%"; // Workaround failing boxSizing test due to offsetWidth returning wrong value // with some non-1 values of body zoom, ticket #13543 jQuery.swap( body, body.style.zoom != null ? { zoom: 1 } : {}, function() { support.boxSizing = div.offsetWidth === 4; }); // Use window.getComputedStyle because jsdom on node.js will break without it. if ( window.getComputedStyle ) { support.pixelPosition = ( window.getComputedStyle( div, null ) || {} ).top !== "1%"; support.boxSizingReliable = ( window.getComputedStyle( div, null ) || { width: "4px" } ).width === "4px"; // Support: Android 2.3 // Check if div with explicit width and no margin-right incorrectly // gets computed margin-right based on width of container. (#3333) // WebKit Bug 13343 - getComputedStyle returns wrong value for margin-right marginDiv = div.appendChild( document.createElement("div") ); marginDiv.style.cssText = div.style.cssText = divReset; marginDiv.style.marginRight = marginDiv.style.width = "0"; div.style.width = "1px"; support.reliableMarginRight = !parseFloat( ( window.getComputedStyle( marginDiv, null ) || {} ).marginRight ); } body.removeChild( container ); }); return support; })( {} ); /* Implementation Summary 1. Enforce API surface and semantic compatibility with 1.9.x branch 2. Improve the module's maintainability by reducing the storage paths to a single mechanism. 3. Use the same single mechanism to support "private" and "user" data. 4. _Never_ expose "private" data to user code (TODO: Drop _data, _removeData) 5. Avoid exposing implementation details on user objects (eg. expando properties) 6. Provide a clear path for implementation upgrade to WeakMap in 2014 */ var data_user, data_priv, rbrace = /(?:\{[\s\S]*\}|\[[\s\S]*\])$/, rmultiDash = /([A-Z])/g; function Data() { // Support: Android < 4, // Old WebKit does not have Object.preventExtensions/freeze method, // return new empty object instead with no [[set]] accessor Object.defineProperty( this.cache = {}, 0, { get: function() { return {}; } }); this.expando = jQuery.expando + Math.random(); } Data.uid = 1; Data.accepts = function( owner ) { // Accepts only: // - Node // - Node.ELEMENT_NODE // - Node.DOCUMENT_NODE // - Object // - Any return owner.nodeType ? owner.nodeType === 1 || owner.nodeType === 9 : true; }; Data.prototype = { key: function( owner ) { // We can accept data for non-element nodes in modern browsers, // but we should not, see #8335. // Always return the key for a frozen object. if ( !Data.accepts( owner ) ) { return 0; } var descriptor = {}, // Check if the owner object already has a cache key unlock = owner[ this.expando ]; // If not, create one if ( !unlock ) { unlock = Data.uid++; // Secure it in a non-enumerable, non-writable property try { descriptor[ this.expando ] = { value: unlock }; Object.defineProperties( owner, descriptor ); // Support: Android < 4 // Fallback to a less secure definition } catch ( e ) { descriptor[ this.expando ] = unlock; jQuery.extend( owner, descriptor ); } } // Ensure the cache object if ( !this.cache[ unlock ] ) { this.cache[ unlock ] = {}; } return unlock; }, set: function( owner, data, value ) { var prop, // There may be an unlock assigned to this node, // if there is no entry for this "owner", create one inline // and set the unlock as though an owner entry had always existed unlock = this.key( owner ), cache = this.cache[ unlock ]; // Handle: [ owner, key, value ] args if ( typeof data === "string" ) { cache[ data ] = value; // Handle: [ owner, { properties } ] args } else { // Fresh assignments by object are shallow copied if ( jQuery.isEmptyObject( cache ) ) { jQuery.extend( this.cache[ unlock ], data ); // Otherwise, copy the properties one-by-one to the cache object } else { for ( prop in data ) { cache[ prop ] = data[ prop ]; } } } return cache; }, get: function( owner, key ) { // Either a valid cache is found, or will be created. // New caches will be created and the unlock returned, // allowing direct access to the newly created // empty data object. A valid owner object must be provided. var cache = this.cache[ this.key( owner ) ]; return key === undefined ? cache : cache[ key ]; }, access: function( owner, key, value ) { var stored; // In cases where either: // // 1. No key was specified // 2. A string key was specified, but no value provided // // Take the "read" path and allow the get method to determine // which value to return, respectively either: // // 1. The entire cache object // 2. The data stored at the key // if ( key === undefined || ((key && typeof key === "string") && value === undefined) ) { stored = this.get( owner, key ); return stored !== undefined ? stored : this.get( owner, jQuery.camelCase(key) ); } // [*]When the key is not a string, or both a key and value // are specified, set or extend (existing objects) with either: // // 1. An object of properties // 2. A key and value // this.set( owner, key, value ); // Since the "set" path can have two possible entry points // return the expected data based on which path was taken[*] return value !== undefined ? value : key; }, remove: function( owner, key ) { var i, name, camel, unlock = this.key( owner ), cache = this.cache[ unlock ]; if ( key === undefined ) { this.cache[ unlock ] = {}; } else { // Support array or space separated string of keys if ( jQuery.isArray( key ) ) { // If "name" is an array of keys... // When data is initially created, via ("key", "val") signature, // keys will be converted to camelCase. // Since there is no way to tell _how_ a key was added, remove // both plain key and camelCase key. #12786 // This will only penalize the array argument path. name = key.concat( key.map( jQuery.camelCase ) ); } else { camel = jQuery.camelCase( key ); // Try the string as a key before any manipulation if ( key in cache ) { name = [ key, camel ]; } else { // If a key with the spaces exists, use it. // Otherwise, create an array by matching non-whitespace name = camel; name = name in cache ? [ name ] : ( name.match( core_rnotwhite ) || [] ); } } i = name.length; while ( i-- ) { delete cache[ name[ i ] ]; } } }, hasData: function( owner ) { return !jQuery.isEmptyObject( this.cache[ owner[ this.expando ] ] || {} ); }, discard: function( owner ) { if ( owner[ this.expando ] ) { delete this.cache[ owner[ this.expando ] ]; } } }; // These may be used throughout the jQuery core codebase data_user = new Data(); data_priv = new Data(); jQuery.extend({ acceptData: Data.accepts, hasData: function( elem ) { return data_user.hasData( elem ) || data_priv.hasData( elem ); }, data: function( elem, name, data ) { return data_user.access( elem, name, data ); }, removeData: function( elem, name ) { data_user.remove( elem, name ); }, // TODO: Now that all calls to _data and _removeData have been replaced // with direct calls to data_priv methods, these can be deprecated. _data: function( elem, name, data ) { return data_priv.access( elem, name, data ); }, _removeData: function( elem, name ) { data_priv.remove( elem, name ); } }); jQuery.fn.extend({ data: function( key, value ) { var attrs, name, elem = this[ 0 ], i = 0, data = null; // Gets all values if ( key === undefined ) { if ( this.length ) { data = data_user.get( elem ); if ( elem.nodeType === 1 && !data_priv.get( elem, "hasDataAttrs" ) ) { attrs = elem.attributes; for ( ; i < attrs.length; i++ ) { name = attrs[ i ].name; if ( name.indexOf( "data-" ) === 0 ) { name = jQuery.camelCase( name.slice(5) ); dataAttr( elem, name, data[ name ] ); } } data_priv.set( elem, "hasDataAttrs", true ); } } return data; } // Sets multiple values if ( typeof key === "object" ) { return this.each(function() { data_user.set( this, key ); }); } return jQuery.access( this, function( value ) { var data, camelKey = jQuery.camelCase( key ); // The calling jQuery object (element matches) is not empty // (and therefore has an element appears at this[ 0 ]) and the // `value` parameter was not undefined. An empty jQuery object // will result in `undefined` for elem = this[ 0 ] which will // throw an exception if an attempt to read a data cache is made. if ( elem && value === undefined ) { // Attempt to get data from the cache // with the key as-is data = data_user.get( elem, key ); if ( data !== undefined ) { return data; } // Attempt to get data from the cache // with the key camelized data = data_user.get( elem, camelKey ); if ( data !== undefined ) { return data; } // Attempt to "discover" the data in // HTML5 custom data-* attrs data = dataAttr( elem, camelKey, undefined ); if ( data !== undefined ) { return data; } // We tried really hard, but the data doesn't exist. return; } // Set the data... this.each(function() { // First, attempt to store a copy or reference of any // data that might've been store with a camelCased key. var data = data_user.get( this, camelKey ); // For HTML5 data-* attribute interop, we have to // store property names with dashes in a camelCase form. // This might not apply to all properties...* data_user.set( this, camelKey, value ); // *... In the case of properties that might _actually_ // have dashes, we need to also store a copy of that // unchanged property. if ( key.indexOf("-") !== -1 && data !== undefined ) { data_user.set( this, key, value ); } }); }, null, value, arguments.length > 1, null, true ); }, removeData: function( key ) { return this.each(function() { data_user.remove( this, key ); }); } }); function dataAttr( elem, key, data ) { var name; // If nothing was found internally, try to fetch any // data from the HTML5 data-* attribute if ( data === undefined && elem.nodeType === 1 ) { name = "data-" + key.replace( rmultiDash, "-$1" ).toLowerCase(); data = elem.getAttribute( name ); if ( typeof data === "string" ) { try { data = data === "true" ? true : data === "false" ? false : data === "null" ? null : // Only convert to a number if it doesn't change the string +data + "" === data ? +data : rbrace.test( data ) ? JSON.parse( data ) : data; } catch( e ) {} // Make sure we set the data so it isn't changed later data_user.set( elem, key, data ); } else { data = undefined; } } return data; } jQuery.extend({ queue: function( elem, type, data ) { var queue; if ( elem ) { type = ( type || "fx" ) + "queue"; queue = data_priv.get( elem, type ); // Speed up dequeue by getting out quickly if this is just a lookup if ( data ) { if ( !queue || jQuery.isArray( data ) ) { queue = data_priv.access( elem, type, jQuery.makeArray(data) ); } else { queue.push( data ); } } return queue || []; } }, dequeue: function( elem, type ) { type = type || "fx"; var queue = jQuery.queue( elem, type ), startLength = queue.length, fn = queue.shift(), hooks = jQuery._queueHooks( elem, type ), next = function() { jQuery.dequeue( elem, type ); }; // If the fx queue is dequeued, always remove the progress sentinel if ( fn === "inprogress" ) { fn = queue.shift(); startLength--; } if ( fn ) { // Add a progress sentinel to prevent the fx queue from being // automatically dequeued if ( type === "fx" ) { queue.unshift( "inprogress" ); } // clear up the last queue stop function delete hooks.stop; fn.call( elem, next, hooks ); } if ( !startLength && hooks ) { hooks.empty.fire(); } }, // not intended for public consumption - generates a queueHooks object, or returns the current one _queueHooks: function( elem, type ) { var key = type + "queueHooks"; return data_priv.get( elem, key ) || data_priv.access( elem, key, { empty: jQuery.Callbacks("once memory").add(function() { data_priv.remove( elem, [ type + "queue", key ] ); }) }); } }); jQuery.fn.extend({ queue: function( type, data ) { var setter = 2; if ( typeof type !== "string" ) { data = type; type = "fx"; setter--; } if ( arguments.length < setter ) { return jQuery.queue( this[0], type ); } return data === undefined ? this : this.each(function() { var queue = jQuery.queue( this, type, data ); // ensure a hooks for this queue jQuery._queueHooks( this, type ); if ( type === "fx" && queue[0] !== "inprogress" ) { jQuery.dequeue( this, type ); } }); }, dequeue: function( type ) { return this.each(function() { jQuery.dequeue( this, type ); }); }, // Based off of the plugin by Clint Helfers, with permission. // http://blindsignals.com/index.php/2009/07/jquery-delay/ delay: function( time, type ) { time = jQuery.fx ? jQuery.fx.speeds[ time ] || time : time; type = type || "fx"; return this.queue( type, function( next, hooks ) { var timeout = setTimeout( next, time ); hooks.stop = function() { clearTimeout( timeout ); }; }); }, clearQueue: function( type ) { return this.queue( type || "fx", [] ); }, // Get a promise resolved when queues of a certain type // are emptied (fx is the type by default) promise: function( type, obj ) { var tmp, count = 1, defer = jQuery.Deferred(), elements = this, i = this.length, resolve = function() { if ( !( --count ) ) { defer.resolveWith( elements, [ elements ] ); } }; if ( typeof type !== "string" ) { obj = type; type = undefined; } type = type || "fx"; while( i-- ) { tmp = data_priv.get( elements[ i ], type + "queueHooks" ); if ( tmp && tmp.empty ) { count++; tmp.empty.add( resolve ); } } resolve(); return defer.promise( obj ); } }); var nodeHook, boolHook, rclass = /[\t\r\n\f]/g, rreturn = /\r/g, rfocusable = /^(?:input|select|textarea|button)$/i; jQuery.fn.extend({ attr: function( name, value ) { return jQuery.access( this, jQuery.attr, name, value, arguments.length > 1 ); }, removeAttr: function( name ) { return this.each(function() { jQuery.removeAttr( this, name ); }); }, prop: function( name, value ) { return jQuery.access( this, jQuery.prop, name, value, arguments.length > 1 ); }, removeProp: function( name ) { return this.each(function() { delete this[ jQuery.propFix[ name ] || name ]; }); }, addClass: function( value ) { var classes, elem, cur, clazz, j, i = 0, len = this.length, proceed = typeof value === "string" && value; if ( jQuery.isFunction( value ) ) { return this.each(function( j ) { jQuery( this ).addClass( value.call( this, j, this.className ) ); }); } if ( proceed ) { // The disjunction here is for better compressibility (see removeClass) classes = ( value || "" ).match( core_rnotwhite ) || []; for ( ; i < len; i++ ) { elem = this[ i ]; cur = elem.nodeType === 1 && ( elem.className ? ( " " + elem.className + " " ).replace( rclass, " " ) : " " ); if ( cur ) { j = 0; while ( (clazz = classes[j++]) ) { if ( cur.indexOf( " " + clazz + " " ) < 0 ) { cur += clazz + " "; } } elem.className = jQuery.trim( cur ); } } } return this; }, removeClass: function( value ) { var classes, elem, cur, clazz, j, i = 0, len = this.length, proceed = arguments.length === 0 || typeof value === "string" && value; if ( jQuery.isFunction( value ) ) { return this.each(function( j ) { jQuery( this ).removeClass( value.call( this, j, this.className ) ); }); } if ( proceed ) { classes = ( value || "" ).match( core_rnotwhite ) || []; for ( ; i < len; i++ ) { elem = this[ i ]; // This expression is here for better compressibility (see addClass) cur = elem.nodeType === 1 && ( elem.className ? ( " " + elem.className + " " ).replace( rclass, " " ) : "" ); if ( cur ) { j = 0; while ( (clazz = classes[j++]) ) { // Remove *all* instances while ( cur.indexOf( " " + clazz + " " ) >= 0 ) { cur = cur.replace( " " + clazz + " ", " " ); } } elem.className = value ? jQuery.trim( cur ) : ""; } } } return this; }, toggleClass: function( value, stateVal ) { var type = typeof value; if ( typeof stateVal === "boolean" && type === "string" ) { return stateVal ? this.addClass( value ) : this.removeClass( value ); } if ( jQuery.isFunction( value ) ) { return this.each(function( i ) { jQuery( this ).toggleClass( value.call(this, i, this.className, stateVal), stateVal ); }); } return this.each(function() { if ( type === "string" ) { // toggle individual class names var className, i = 0, self = jQuery( this ), classNames = value.match( core_rnotwhite ) || []; while ( (className = classNames[ i++ ]) ) { // check each className given, space separated list if ( self.hasClass( className ) ) { self.removeClass( className ); } else { self.addClass( className ); } } // Toggle whole class name } else if ( type === core_strundefined || type === "boolean" ) { if ( this.className ) { // store className if set data_priv.set( this, "__className__", this.className ); } // If the element has a class name or if we're passed "false", // then remove the whole classname (if there was one, the above saved it). // Otherwise bring back whatever was previously saved (if anything), // falling back to the empty string if nothing was stored. this.className = this.className || value === false ? "" : data_priv.get( this, "__className__" ) || ""; } }); }, hasClass: function( selector ) { var className = " " + selector + " ", i = 0, l = this.length; for ( ; i < l; i++ ) { if ( this[i].nodeType === 1 && (" " + this[i].className + " ").replace(rclass, " ").indexOf( className ) >= 0 ) { return true; } } return false; }, val: function( value ) { var hooks, ret, isFunction, elem = this[0]; if ( !arguments.length ) { if ( elem ) { hooks = jQuery.valHooks[ elem.type ] || jQuery.valHooks[ elem.nodeName.toLowerCase() ]; if ( hooks && "get" in hooks && (ret = hooks.get( elem, "value" )) !== undefined ) { return ret; } ret = elem.value; return typeof ret === "string" ? // handle most common string cases ret.replace(rreturn, "") : // handle cases where value is null/undef or number ret == null ? "" : ret; } return; } isFunction = jQuery.isFunction( value ); return this.each(function( i ) { var val; if ( this.nodeType !== 1 ) { return; } if ( isFunction ) { val = value.call( this, i, jQuery( this ).val() ); } else { val = value; } // Treat null/undefined as ""; convert numbers to string if ( val == null ) { val = ""; } else if ( typeof val === "number" ) { val += ""; } else if ( jQuery.isArray( val ) ) { val = jQuery.map(val, function ( value ) { return value == null ? "" : value + ""; }); } hooks = jQuery.valHooks[ this.type ] || jQuery.valHooks[ this.nodeName.toLowerCase() ]; // If set returns undefined, fall back to normal setting if ( !hooks || !("set" in hooks) || hooks.set( this, val, "value" ) === undefined ) { this.value = val; } }); } }); jQuery.extend({ valHooks: { option: { get: function( elem ) { // attributes.value is undefined in Blackberry 4.7 but // uses .value. See #6932 var val = elem.attributes.value; return !val || val.specified ? elem.value : elem.text; } }, select: { get: function( elem ) { var value, option, options = elem.options, index = elem.selectedIndex, one = elem.type === "select-one" || index < 0, values = one ? null : [], max = one ? index + 1 : options.length, i = index < 0 ? max : one ? index : 0; // Loop through all the selected options for ( ; i < max; i++ ) { option = options[ i ]; // IE6-9 doesn't update selected after form reset (#2551) if ( ( option.selected || i === index ) && // Don't return options that are disabled or in a disabled optgroup ( jQuery.support.optDisabled ? !option.disabled : option.getAttribute("disabled") === null ) && ( !option.parentNode.disabled || !jQuery.nodeName( option.parentNode, "optgroup" ) ) ) { // Get the specific value for the option value = jQuery( option ).val(); // We don't need an array for one selects if ( one ) { return value; } // Multi-Selects return an array values.push( value ); } } return values; }, set: function( elem, value ) { var optionSet, option, options = elem.options, values = jQuery.makeArray( value ), i = options.length; while ( i-- ) { option = options[ i ]; if ( (option.selected = jQuery.inArray( jQuery(option).val(), values ) >= 0) ) { optionSet = true; } } // force browsers to behave consistently when non-matching value is set if ( !optionSet ) { elem.selectedIndex = -1; } return values; } } }, attr: function( elem, name, value ) { var hooks, ret, nType = elem.nodeType; // don't get/set attributes on text, comment and attribute nodes if ( !elem || nType === 3 || nType === 8 || nType === 2 ) { return; } // Fallback to prop when attributes are not supported if ( typeof elem.getAttribute === core_strundefined ) { return jQuery.prop( elem, name, value ); } // All attributes are lowercase // Grab necessary hook if one is defined if ( nType !== 1 || !jQuery.isXMLDoc( elem ) ) { name = name.toLowerCase(); hooks = jQuery.attrHooks[ name ] || ( jQuery.expr.match.bool.test( name ) ? boolHook : nodeHook ); } if ( value !== undefined ) { if ( value === null ) { jQuery.removeAttr( elem, name ); } else if ( hooks && "set" in hooks && (ret = hooks.set( elem, value, name )) !== undefined ) { return ret; } else { elem.setAttribute( name, value + "" ); return value; } } else if ( hooks && "get" in hooks && (ret = hooks.get( elem, name )) !== null ) { return ret; } else { ret = jQuery.find.attr( elem, name ); // Non-existent attributes return null, we normalize to undefined return ret == null ? undefined : ret; } }, removeAttr: function( elem, value ) { var name, propName, i = 0, attrNames = value && value.match( core_rnotwhite ); if ( attrNames && elem.nodeType === 1 ) { while ( (name = attrNames[i++]) ) { propName = jQuery.propFix[ name ] || name; // Boolean attributes get special treatment (#10870) if ( jQuery.expr.match.bool.test( name ) ) { // Set corresponding property to false elem[ propName ] = false; } elem.removeAttribute( name ); } } }, attrHooks: { type: { set: function( elem, value ) { if ( !jQuery.support.radioValue && value === "radio" && jQuery.nodeName(elem, "input") ) { // Setting the type on a radio button after the value resets the value in IE6-9 // Reset value to default in case type is set after value during creation var val = elem.value; elem.setAttribute( "type", value ); if ( val ) { elem.value = val; } return value; } } } }, propFix: { "for": "htmlFor", "class": "className" }, prop: function( elem, name, value ) { var ret, hooks, notxml, nType = elem.nodeType; // don't get/set properties on text, comment and attribute nodes if ( !elem || nType === 3 || nType === 8 || nType === 2 ) { return; } notxml = nType !== 1 || !jQuery.isXMLDoc( elem ); if ( notxml ) { // Fix name and attach hooks name = jQuery.propFix[ name ] || name; hooks = jQuery.propHooks[ name ]; } if ( value !== undefined ) { return hooks && "set" in hooks && (ret = hooks.set( elem, value, name )) !== undefined ? ret : ( elem[ name ] = value ); } else { return hooks && "get" in hooks && (ret = hooks.get( elem, name )) !== null ? ret : elem[ name ]; } }, propHooks: { tabIndex: { get: function( elem ) { return elem.hasAttribute( "tabindex" ) || rfocusable.test( elem.nodeName ) || elem.href ? elem.tabIndex : -1; } } } }); // Hooks for boolean attributes boolHook = { set: function( elem, value, name ) { if ( value === false ) { // Remove boolean attributes when set to false jQuery.removeAttr( elem, name ); } else { elem.setAttribute( name, name ); } return name; } }; jQuery.each( jQuery.expr.match.bool.source.match( /\w+/g ), function( i, name ) { var getter = jQuery.expr.attrHandle[ name ] || jQuery.find.attr; jQuery.expr.attrHandle[ name ] = function( elem, name, isXML ) { var fn = jQuery.expr.attrHandle[ name ], ret = isXML ? undefined : /* jshint eqeqeq: false */ // Temporarily disable this handler to check existence (jQuery.expr.attrHandle[ name ] = undefined) != getter( elem, name, isXML ) ? name.toLowerCase() : null; // Restore handler jQuery.expr.attrHandle[ name ] = fn; return ret; }; }); // Support: IE9+ // Selectedness for an option in an optgroup can be inaccurate if ( !jQuery.support.optSelected ) { jQuery.propHooks.selected = { get: function( elem ) { var parent = elem.parentNode; if ( parent && parent.parentNode ) { parent.parentNode.selectedIndex; } return null; } }; } jQuery.each([ "tabIndex", "readOnly", "maxLength", "cellSpacing", "cellPadding", "rowSpan", "colSpan", "useMap", "frameBorder", "contentEditable" ], function() { jQuery.propFix[ this.toLowerCase() ] = this; }); // Radios and checkboxes getter/setter jQuery.each([ "radio", "checkbox" ], function() { jQuery.valHooks[ this ] = { set: function( elem, value ) { if ( jQuery.isArray( value ) ) { return ( elem.checked = jQuery.inArray( jQuery(elem).val(), value ) >= 0 ); } } }; if ( !jQuery.support.checkOn ) { jQuery.valHooks[ this ].get = function( elem ) { // Support: Webkit // "" is returned instead of "on" if a value isn't specified return elem.getAttribute("value") === null ? "on" : elem.value; }; } }); var rkeyEvent = /^key/, rmouseEvent = /^(?:mouse|contextmenu)|click/, rfocusMorph = /^(?:focusinfocus|focusoutblur)$/, rtypenamespace = /^([^.]*)(?:\.(.+)|)$/; function returnTrue() { return true; } function returnFalse() { return false; } function safeActiveElement() { try { return document.activeElement; } catch ( err ) { } } /* * Helper functions for managing events -- not part of the public interface. * Props to Dean Edwards' addEvent library for many of the ideas. */ jQuery.event = { global: {}, add: function( elem, types, handler, data, selector ) { var handleObjIn, eventHandle, tmp, events, t, handleObj, special, handlers, type, namespaces, origType, elemData = data_priv.get( elem ); // Don't attach events to noData or text/comment nodes (but allow plain objects) if ( !elemData ) { return; } // Caller can pass in an object of custom data in lieu of the handler if ( handler.handler ) { handleObjIn = handler; handler = handleObjIn.handler; selector = handleObjIn.selector; } // Make sure that the handler has a unique ID, used to find/remove it later if ( !handler.guid ) { handler.guid = jQuery.guid++; } // Init the element's event structure and main handler, if this is the first if ( !(events = elemData.events) ) { events = elemData.events = {}; } if ( !(eventHandle = elemData.handle) ) { eventHandle = elemData.handle = function( e ) { // Discard the second event of a jQuery.event.trigger() and // when an event is called after a page has unloaded return typeof jQuery !== core_strundefined && (!e || jQuery.event.triggered !== e.type) ? jQuery.event.dispatch.apply( eventHandle.elem, arguments ) : undefined; }; // Add elem as a property of the handle fn to prevent a memory leak with IE non-native events eventHandle.elem = elem; } // Handle multiple events separated by a space types = ( types || "" ).match( core_rnotwhite ) || [""]; t = types.length; while ( t-- ) { tmp = rtypenamespace.exec( types[t] ) || []; type = origType = tmp[1]; namespaces = ( tmp[2] || "" ).split( "." ).sort(); // There *must* be a type, no attaching namespace-only handlers if ( !type ) { continue; } // If event changes its type, use the special event handlers for the changed type special = jQuery.event.special[ type ] || {}; // If selector defined, determine special event api type, otherwise given type type = ( selector ? special.delegateType : special.bindType ) || type; // Update special based on newly reset type special = jQuery.event.special[ type ] || {}; // handleObj is passed to all event handlers handleObj = jQuery.extend({ type: type, origType: origType, data: data, handler: handler, guid: handler.guid, selector: selector, needsContext: selector && jQuery.expr.match.needsContext.test( selector ), namespace: namespaces.join(".") }, handleObjIn ); // Init the event handler queue if we're the first if ( !(handlers = events[ type ]) ) { handlers = events[ type ] = []; handlers.delegateCount = 0; // Only use addEventListener if the special events handler returns false if ( !special.setup || special.setup.call( elem, data, namespaces, eventHandle ) === false ) { if ( elem.addEventListener ) { elem.addEventListener( type, eventHandle, false ); } } } if ( special.add ) { special.add.call( elem, handleObj ); if ( !handleObj.handler.guid ) { handleObj.handler.guid = handler.guid; } } // Add to the element's handler list, delegates in front if ( selector ) { handlers.splice( handlers.delegateCount++, 0, handleObj ); } else { handlers.push( handleObj ); } // Keep track of which events have ever been used, for event optimization jQuery.event.global[ type ] = true; } // Nullify elem to prevent memory leaks in IE elem = null; }, // Detach an event or set of events from an element remove: function( elem, types, handler, selector, mappedTypes ) { var j, origCount, tmp, events, t, handleObj, special, handlers, type, namespaces, origType, elemData = data_priv.hasData( elem ) && data_priv.get( elem ); if ( !elemData || !(events = elemData.events) ) { return; } // Once for each type.namespace in types; type may be omitted types = ( types || "" ).match( core_rnotwhite ) || [""]; t = types.length; while ( t-- ) { tmp = rtypenamespace.exec( types[t] ) || []; type = origType = tmp[1]; namespaces = ( tmp[2] || "" ).split( "." ).sort(); // Unbind all events (on this namespace, if provided) for the element if ( !type ) { for ( type in events ) { jQuery.event.remove( elem, type + types[ t ], handler, selector, true ); } continue; } special = jQuery.event.special[ type ] || {}; type = ( selector ? special.delegateType : special.bindType ) || type; handlers = events[ type ] || []; tmp = tmp[2] && new RegExp( "(^|\\.)" + namespaces.join("\\.(?:.*\\.|)") + "(\\.|$)" ); // Remove matching events origCount = j = handlers.length; while ( j-- ) { handleObj = handlers[ j ]; if ( ( mappedTypes || origType === handleObj.origType ) && ( !handler || handler.guid === handleObj.guid ) && ( !tmp || tmp.test( handleObj.namespace ) ) && ( !selector || selector === handleObj.selector || selector === "**" && handleObj.selector ) ) { handlers.splice( j, 1 ); if ( handleObj.selector ) { handlers.delegateCount--; } if ( special.remove ) { special.remove.call( elem, handleObj ); } } } // Remove generic event handler if we removed something and no more handlers exist // (avoids potential for endless recursion during removal of special event handlers) if ( origCount && !handlers.length ) { if ( !special.teardown || special.teardown.call( elem, namespaces, elemData.handle ) === false ) { jQuery.removeEvent( elem, type, elemData.handle ); } delete events[ type ]; } } // Remove the expando if it's no longer used if ( jQuery.isEmptyObject( events ) ) { delete elemData.handle; data_priv.remove( elem, "events" ); } }, trigger: function( event, data, elem, onlyHandlers ) { var i, cur, tmp, bubbleType, ontype, handle, special, eventPath = [ elem || document ], type = core_hasOwn.call( event, "type" ) ? event.type : event, namespaces = core_hasOwn.call( event, "namespace" ) ? event.namespace.split(".") : []; cur = tmp = elem = elem || document; // Don't do events on text and comment nodes if ( elem.nodeType === 3 || elem.nodeType === 8 ) { return; } // focus/blur morphs to focusin/out; ensure we're not firing them right now if ( rfocusMorph.test( type + jQuery.event.triggered ) ) { return; } if ( type.indexOf(".") >= 0 ) { // Namespaced trigger; create a regexp to match event type in handle() namespaces = type.split("."); type = namespaces.shift(); namespaces.sort(); } ontype = type.indexOf(":") < 0 && "on" + type; // Caller can pass in a jQuery.Event object, Object, or just an event type string event = event[ jQuery.expando ] ? event : new jQuery.Event( type, typeof event === "object" && event ); // Trigger bitmask: & 1 for native handlers; & 2 for jQuery (always true) event.isTrigger = onlyHandlers ? 2 : 3; event.namespace = namespaces.join("."); event.namespace_re = event.namespace ? new RegExp( "(^|\\.)" + namespaces.join("\\.(?:.*\\.|)") + "(\\.|$)" ) : null; // Clean up the event in case it is being reused event.result = undefined; if ( !event.target ) { event.target = elem; } // Clone any incoming data and prepend the event, creating the handler arg list data = data == null ? [ event ] : jQuery.makeArray( data, [ event ] ); // Allow special events to draw outside the lines special = jQuery.event.special[ type ] || {}; if ( !onlyHandlers && special.trigger && special.trigger.apply( elem, data ) === false ) { return; } // Determine event propagation path in advance, per W3C events spec (#9951) // Bubble up to document, then to window; watch for a global ownerDocument var (#9724) if ( !onlyHandlers && !special.noBubble && !jQuery.isWindow( elem ) ) { bubbleType = special.delegateType || type; if ( !rfocusMorph.test( bubbleType + type ) ) { cur = cur.parentNode; } for ( ; cur; cur = cur.parentNode ) { eventPath.push( cur ); tmp = cur; } // Only add window if we got to document (e.g., not plain obj or detached DOM) if ( tmp === (elem.ownerDocument || document) ) { eventPath.push( tmp.defaultView || tmp.parentWindow || window ); } } // Fire handlers on the event path i = 0; while ( (cur = eventPath[i++]) && !event.isPropagationStopped() ) { event.type = i > 1 ? bubbleType : special.bindType || type; // jQuery handler handle = ( data_priv.get( cur, "events" ) || {} )[ event.type ] && data_priv.get( cur, "handle" ); if ( handle ) { handle.apply( cur, data ); } // Native handler handle = ontype && cur[ ontype ]; if ( handle && jQuery.acceptData( cur ) && handle.apply && handle.apply( cur, data ) === false ) { event.preventDefault(); } } event.type = type; // If nobody prevented the default action, do it now if ( !onlyHandlers && !event.isDefaultPrevented() ) { if ( (!special._default || special._default.apply( eventPath.pop(), data ) === false) && jQuery.acceptData( elem ) ) { // Call a native DOM method on the target with the same name name as the event. // Don't do default actions on window, that's where global variables be (#6170) if ( ontype && jQuery.isFunction( elem[ type ] ) && !jQuery.isWindow( elem ) ) { // Don't re-trigger an onFOO event when we call its FOO() method tmp = elem[ ontype ]; if ( tmp ) { elem[ ontype ] = null; } // Prevent re-triggering of the same event, since we already bubbled it above jQuery.event.triggered = type; elem[ type ](); jQuery.event.triggered = undefined; if ( tmp ) { elem[ ontype ] = tmp; } } } } return event.result; }, dispatch: function( event ) { // Make a writable jQuery.Event from the native event object event = jQuery.event.fix( event ); var i, j, ret, matched, handleObj, handlerQueue = [], args = core_slice.call( arguments ), handlers = ( data_priv.get( this, "events" ) || {} )[ event.type ] || [], special = jQuery.event.special[ event.type ] || {}; // Use the fix-ed jQuery.Event rather than the (read-only) native event args[0] = event; event.delegateTarget = this; // Call the preDispatch hook for the mapped type, and let it bail if desired if ( special.preDispatch && special.preDispatch.call( this, event ) === false ) { return; } // Determine handlers handlerQueue = jQuery.event.handlers.call( this, event, handlers ); // Run delegates first; they may want to stop propagation beneath us i = 0; while ( (matched = handlerQueue[ i++ ]) && !event.isPropagationStopped() ) { event.currentTarget = matched.elem; j = 0; while ( (handleObj = matched.handlers[ j++ ]) && !event.isImmediatePropagationStopped() ) { // Triggered event must either 1) have no namespace, or // 2) have namespace(s) a subset or equal to those in the bound event (both can have no namespace). if ( !event.namespace_re || event.namespace_re.test( handleObj.namespace ) ) { event.handleObj = handleObj; event.data = handleObj.data; ret = ( (jQuery.event.special[ handleObj.origType ] || {}).handle || handleObj.handler ) .apply( matched.elem, args ); if ( ret !== undefined ) { if ( (event.result = ret) === false ) { event.preventDefault(); event.stopPropagation(); } } } } } // Call the postDispatch hook for the mapped type if ( special.postDispatch ) { special.postDispatch.call( this, event ); } return event.result; }, handlers: function( event, handlers ) { var i, matches, sel, handleObj, handlerQueue = [], delegateCount = handlers.delegateCount, cur = event.target; // Find delegate handlers // Black-hole SVG instance trees (#13180) // Avoid non-left-click bubbling in Firefox (#3861) if ( delegateCount && cur.nodeType && (!event.button || event.type !== "click") ) { for ( ; cur !== this; cur = cur.parentNode || this ) { // Don't process clicks on disabled elements (#6911, #8165, #11382, #11764) if ( cur.disabled !== true || event.type !== "click" ) { matches = []; for ( i = 0; i < delegateCount; i++ ) { handleObj = handlers[ i ]; // Don't conflict with Object.prototype properties (#13203) sel = handleObj.selector + " "; if ( matches[ sel ] === undefined ) { matches[ sel ] = handleObj.needsContext ? jQuery( sel, this ).index( cur ) >= 0 : jQuery.find( sel, this, null, [ cur ] ).length; } if ( matches[ sel ] ) { matches.push( handleObj ); } } if ( matches.length ) { handlerQueue.push({ elem: cur, handlers: matches }); } } } } // Add the remaining (directly-bound) handlers if ( delegateCount < handlers.length ) { handlerQueue.push({ elem: this, handlers: handlers.slice( delegateCount ) }); } return handlerQueue; }, // Includes some event props shared by KeyEvent and MouseEvent props: "altKey bubbles cancelable ctrlKey currentTarget eventPhase metaKey relatedTarget shiftKey target timeStamp view which".split(" "), fixHooks: {}, keyHooks: { props: "char charCode key keyCode".split(" "), filter: function( event, original ) { // Add which for key events if ( event.which == null ) { event.which = original.charCode != null ? original.charCode : original.keyCode; } return event; } }, mouseHooks: { props: "button buttons clientX clientY offsetX offsetY pageX pageY screenX screenY toElement".split(" "), filter: function( event, original ) { var eventDoc, doc, body, button = original.button; // Calculate pageX/Y if missing and clientX/Y available if ( event.pageX == null && original.clientX != null ) { eventDoc = event.target.ownerDocument || document; doc = eventDoc.documentElement; body = eventDoc.body; event.pageX = original.clientX + ( doc && doc.scrollLeft || body && body.scrollLeft || 0 ) - ( doc && doc.clientLeft || body && body.clientLeft || 0 ); event.pageY = original.clientY + ( doc && doc.scrollTop || body && body.scrollTop || 0 ) - ( doc && doc.clientTop || body && body.clientTop || 0 ); } // Add which for click: 1 === left; 2 === middle; 3 === right // Note: button is not normalized, so don't use it if ( !event.which && button !== undefined ) { event.which = ( button & 1 ? 1 : ( button & 2 ? 3 : ( button & 4 ? 2 : 0 ) ) ); } return event; } }, fix: function( event ) { if ( event[ jQuery.expando ] ) { return event; } // Create a writable copy of the event object and normalize some properties var i, prop, copy, type = event.type, originalEvent = event, fixHook = this.fixHooks[ type ]; if ( !fixHook ) { this.fixHooks[ type ] = fixHook = rmouseEvent.test( type ) ? this.mouseHooks : rkeyEvent.test( type ) ? this.keyHooks : {}; } copy = fixHook.props ? this.props.concat( fixHook.props ) : this.props; event = new jQuery.Event( originalEvent ); i = copy.length; while ( i-- ) { prop = copy[ i ]; event[ prop ] = originalEvent[ prop ]; } // Support: Cordova 2.5 (WebKit) (#13255) // All events should have a target; Cordova deviceready doesn't if ( !event.target ) { event.target = document; } // Support: Safari 6.0+, Chrome < 28 // Target should not be a text node (#504, #13143) if ( event.target.nodeType === 3 ) { event.target = event.target.parentNode; } return fixHook.filter? fixHook.filter( event, originalEvent ) : event; }, special: { load: { // Prevent triggered image.load events from bubbling to window.load noBubble: true }, focus: { // Fire native event if possible so blur/focus sequence is correct trigger: function() { if ( this !== safeActiveElement() && this.focus ) { this.focus(); return false; } }, delegateType: "focusin" }, blur: { trigger: function() { if ( this === safeActiveElement() && this.blur ) { this.blur(); return false; } }, delegateType: "focusout" }, click: { // For checkbox, fire native event so checked state will be right trigger: function() { if ( this.type === "checkbox" && this.click && jQuery.nodeName( this, "input" ) ) { this.click(); return false; } }, // For cross-browser consistency, don't fire native .click() on links _default: function( event ) { return jQuery.nodeName( event.target, "a" ); } }, beforeunload: { postDispatch: function( event ) { // Support: Firefox 20+ // Firefox doesn't alert if the returnValue field is not set. if ( event.result !== undefined ) { event.originalEvent.returnValue = event.result; } } } }, simulate: function( type, elem, event, bubble ) { // Piggyback on a donor event to simulate a different one. // Fake originalEvent to avoid donor's stopPropagation, but if the // simulated event prevents default then we do the same on the donor. var e = jQuery.extend( new jQuery.Event(), event, { type: type, isSimulated: true, originalEvent: {} } ); if ( bubble ) { jQuery.event.trigger( e, null, elem ); } else { jQuery.event.dispatch.call( elem, e ); } if ( e.isDefaultPrevented() ) { event.preventDefault(); } } }; jQuery.removeEvent = function( elem, type, handle ) { if ( elem.removeEventListener ) { elem.removeEventListener( type, handle, false ); } }; jQuery.Event = function( src, props ) { // Allow instantiation without the 'new' keyword if ( !(this instanceof jQuery.Event) ) { return new jQuery.Event( src, props ); } // Event object if ( src && src.type ) { this.originalEvent = src; this.type = src.type; // Events bubbling up the document may have been marked as prevented // by a handler lower down the tree; reflect the correct value. this.isDefaultPrevented = ( src.defaultPrevented || src.getPreventDefault && src.getPreventDefault() ) ? returnTrue : returnFalse; // Event type } else { this.type = src; } // Put explicitly provided properties onto the event object if ( props ) { jQuery.extend( this, props ); } // Create a timestamp if incoming event doesn't have one this.timeStamp = src && src.timeStamp || jQuery.now(); // Mark it as fixed this[ jQuery.expando ] = true; }; // jQuery.Event is based on DOM3 Events as specified by the ECMAScript Language Binding // http://www.w3.org/TR/2003/WD-DOM-Level-3-Events-20030331/ecma-script-binding.html jQuery.Event.prototype = { isDefaultPrevented: returnFalse, isPropagationStopped: returnFalse, isImmediatePropagationStopped: returnFalse, preventDefault: function() { var e = this.originalEvent; this.isDefaultPrevented = returnTrue; if ( e && e.preventDefault ) { e.preventDefault(); } }, stopPropagation: function() { var e = this.originalEvent; this.isPropagationStopped = returnTrue; if ( e && e.stopPropagation ) { e.stopPropagation(); } }, stopImmediatePropagation: function() { this.isImmediatePropagationStopped = returnTrue; this.stopPropagation(); } }; // Create mouseenter/leave events using mouseover/out and event-time checks // Support: Chrome 15+ jQuery.each({ mouseenter: "mouseover", mouseleave: "mouseout" }, function( orig, fix ) { jQuery.event.special[ orig ] = { delegateType: fix, bindType: fix, handle: function( event ) { var ret, target = this, related = event.relatedTarget, handleObj = event.handleObj; // For mousenter/leave call the handler if related is outside the target. // NB: No relatedTarget if the mouse left/entered the browser window if ( !related || (related !== target && !jQuery.contains( target, related )) ) { event.type = handleObj.origType; ret = handleObj.handler.apply( this, arguments ); event.type = fix; } return ret; } }; }); // Create "bubbling" focus and blur events // Support: Firefox, Chrome, Safari if ( !jQuery.support.focusinBubbles ) { jQuery.each({ focus: "focusin", blur: "focusout" }, function( orig, fix ) { // Attach a single capturing handler while someone wants focusin/focusout var attaches = 0, handler = function( event ) { jQuery.event.simulate( fix, event.target, jQuery.event.fix( event ), true ); }; jQuery.event.special[ fix ] = { setup: function() { if ( attaches++ === 0 ) { document.addEventListener( orig, handler, true ); } }, teardown: function() { if ( --attaches === 0 ) { document.removeEventListener( orig, handler, true ); } } }; }); } jQuery.fn.extend({ on: function( types, selector, data, fn, /*INTERNAL*/ one ) { var origFn, type; // Types can be a map of types/handlers if ( typeof types === "object" ) { // ( types-Object, selector, data ) if ( typeof selector !== "string" ) { // ( types-Object, data ) data = data || selector; selector = undefined; } for ( type in types ) { this.on( type, selector, data, types[ type ], one ); } return this; } if ( data == null && fn == null ) { // ( types, fn ) fn = selector; data = selector = undefined; } else if ( fn == null ) { if ( typeof selector === "string" ) { // ( types, selector, fn ) fn = data; data = undefined; } else { // ( types, data, fn ) fn = data; data = selector; selector = undefined; } } if ( fn === false ) { fn = returnFalse; } else if ( !fn ) { return this; } if ( one === 1 ) { origFn = fn; fn = function( event ) { // Can use an empty set, since event contains the info jQuery().off( event ); return origFn.apply( this, arguments ); }; // Use same guid so caller can remove using origFn fn.guid = origFn.guid || ( origFn.guid = jQuery.guid++ ); } return this.each( function() { jQuery.event.add( this, types, fn, data, selector ); }); }, one: function( types, selector, data, fn ) { return this.on( types, selector, data, fn, 1 ); }, off: function( types, selector, fn ) { var handleObj, type; if ( types && types.preventDefault && types.handleObj ) { // ( event ) dispatched jQuery.Event handleObj = types.handleObj; jQuery( types.delegateTarget ).off( handleObj.namespace ? handleObj.origType + "." + handleObj.namespace : handleObj.origType, handleObj.selector, handleObj.handler ); return this; } if ( typeof types === "object" ) { // ( types-object [, selector] ) for ( type in types ) { this.off( type, selector, types[ type ] ); } return this; } if ( selector === false || typeof selector === "function" ) { // ( types [, fn] ) fn = selector; selector = undefined; } if ( fn === false ) { fn = returnFalse; } return this.each(function() { jQuery.event.remove( this, types, fn, selector ); }); }, trigger: function( type, data ) { return this.each(function() { jQuery.event.trigger( type, data, this ); }); }, triggerHandler: function( type, data ) { var elem = this[0]; if ( elem ) { return jQuery.event.trigger( type, data, elem, true ); } } }); var isSimple = /^.[^:#\[\.,]*$/, rparentsprev = /^(?:parents|prev(?:Until|All))/, rneedsContext = jQuery.expr.match.needsContext, // methods guaranteed to produce a unique set when starting from a unique set guaranteedUnique = { children: true, contents: true, next: true, prev: true }; jQuery.fn.extend({ find: function( selector ) { var i, ret = [], self = this, len = self.length; if ( typeof selector !== "string" ) { return this.pushStack( jQuery( selector ).filter(function() { for ( i = 0; i < len; i++ ) { if ( jQuery.contains( self[ i ], this ) ) { return true; } } }) ); } for ( i = 0; i < len; i++ ) { jQuery.find( selector, self[ i ], ret ); } // Needed because $( selector, context ) becomes $( context ).find( selector ) ret = this.pushStack( len > 1 ? jQuery.unique( ret ) : ret ); ret.selector = this.selector ? this.selector + " " + selector : selector; return ret; }, has: function( target ) { var targets = jQuery( target, this ), l = targets.length; return this.filter(function() { var i = 0; for ( ; i < l; i++ ) { if ( jQuery.contains( this, targets[i] ) ) { return true; } } }); }, not: function( selector ) { return this.pushStack( winnow(this, selector || [], true) ); }, filter: function( selector ) { return this.pushStack( winnow(this, selector || [], false) ); }, is: function( selector ) { return !!winnow( this, // If this is a positional/relative selector, check membership in the returned set // so $("p:first").is("p:last") won't return true for a doc with two "p". typeof selector === "string" && rneedsContext.test( selector ) ? jQuery( selector ) : selector || [], false ).length; }, closest: function( selectors, context ) { var cur, i = 0, l = this.length, matched = [], pos = ( rneedsContext.test( selectors ) || typeof selectors !== "string" ) ? jQuery( selectors, context || this.context ) : 0; for ( ; i < l; i++ ) { for ( cur = this[i]; cur && cur !== context; cur = cur.parentNode ) { // Always skip document fragments if ( cur.nodeType < 11 && (pos ? pos.index(cur) > -1 : // Don't pass non-elements to Sizzle cur.nodeType === 1 && jQuery.find.matchesSelector(cur, selectors)) ) { cur = matched.push( cur ); break; } } } return this.pushStack( matched.length > 1 ? jQuery.unique( matched ) : matched ); }, // Determine the position of an element within // the matched set of elements index: function( elem ) { // No argument, return index in parent if ( !elem ) { return ( this[ 0 ] && this[ 0 ].parentNode ) ? this.first().prevAll().length : -1; } // index in selector if ( typeof elem === "string" ) { return core_indexOf.call( jQuery( elem ), this[ 0 ] ); } // Locate the position of the desired element return core_indexOf.call( this, // If it receives a jQuery object, the first element is used elem.jquery ? elem[ 0 ] : elem ); }, add: function( selector, context ) { var set = typeof selector === "string" ? jQuery( selector, context ) : jQuery.makeArray( selector && selector.nodeType ? [ selector ] : selector ), all = jQuery.merge( this.get(), set ); return this.pushStack( jQuery.unique(all) ); }, addBack: function( selector ) { return this.add( selector == null ? this.prevObject : this.prevObject.filter(selector) ); } }); function sibling( cur, dir ) { while ( (cur = cur[dir]) && cur.nodeType !== 1 ) {} return cur; } jQuery.each({ parent: function( elem ) { var parent = elem.parentNode; return parent && parent.nodeType !== 11 ? parent : null; }, parents: function( elem ) { return jQuery.dir( elem, "parentNode" ); }, parentsUntil: function( elem, i, until ) { return jQuery.dir( elem, "parentNode", until ); }, next: function( elem ) { return sibling( elem, "nextSibling" ); }, prev: function( elem ) { return sibling( elem, "previousSibling" ); }, nextAll: function( elem ) { return jQuery.dir( elem, "nextSibling" ); }, prevAll: function( elem ) { return jQuery.dir( elem, "previousSibling" ); }, nextUntil: function( elem, i, until ) { return jQuery.dir( elem, "nextSibling", until ); }, prevUntil: function( elem, i, until ) { return jQuery.dir( elem, "previousSibling", until ); }, siblings: function( elem ) { return jQuery.sibling( ( elem.parentNode || {} ).firstChild, elem ); }, children: function( elem ) { return jQuery.sibling( elem.firstChild ); }, contents: function( elem ) { return elem.contentDocument || jQuery.merge( [], elem.childNodes ); } }, function( name, fn ) { jQuery.fn[ name ] = function( until, selector ) { var matched = jQuery.map( this, fn, until ); if ( name.slice( -5 ) !== "Until" ) { selector = until; } if ( selector && typeof selector === "string" ) { matched = jQuery.filter( selector, matched ); } if ( this.length > 1 ) { // Remove duplicates if ( !guaranteedUnique[ name ] ) { jQuery.unique( matched ); } // Reverse order for parents* and prev-derivatives if ( rparentsprev.test( name ) ) { matched.reverse(); } } return this.pushStack( matched ); }; }); jQuery.extend({ filter: function( expr, elems, not ) { var elem = elems[ 0 ]; if ( not ) { expr = ":not(" + expr + ")"; } return elems.length === 1 && elem.nodeType === 1 ? jQuery.find.matchesSelector( elem, expr ) ? [ elem ] : [] : jQuery.find.matches( expr, jQuery.grep( elems, function( elem ) { return elem.nodeType === 1; })); }, dir: function( elem, dir, until ) { var matched = [], truncate = until !== undefined; while ( (elem = elem[ dir ]) && elem.nodeType !== 9 ) { if ( elem.nodeType === 1 ) { if ( truncate && jQuery( elem ).is( until ) ) { break; } matched.push( elem ); } } return matched; }, sibling: function( n, elem ) { var matched = []; for ( ; n; n = n.nextSibling ) { if ( n.nodeType === 1 && n !== elem ) { matched.push( n ); } } return matched; } }); // Implement the identical functionality for filter and not function winnow( elements, qualifier, not ) { if ( jQuery.isFunction( qualifier ) ) { return jQuery.grep( elements, function( elem, i ) { /* jshint -W018 */ return !!qualifier.call( elem, i, elem ) !== not; }); } if ( qualifier.nodeType ) { return jQuery.grep( elements, function( elem ) { return ( elem === qualifier ) !== not; }); } if ( typeof qualifier === "string" ) { if ( isSimple.test( qualifier ) ) { return jQuery.filter( qualifier, elements, not ); } qualifier = jQuery.filter( qualifier, elements ); } return jQuery.grep( elements, function( elem ) { return ( core_indexOf.call( qualifier, elem ) >= 0 ) !== not; }); } var rxhtmlTag = /<(?!area|br|col|embed|hr|img|input|link|meta|param)(([\w:]+)[^>]*)\/>/gi, rtagName = /<([\w:]+)/, rhtml = /<|&#?\w+;/, rnoInnerhtml = /<(?:script|style|link)/i, manipulation_rcheckableType = /^(?:checkbox|radio)$/i, // checked="checked" or checked rchecked = /checked\s*(?:[^=]|=\s*.checked.)/i, rscriptType = /^$|\/(?:java|ecma)script/i, rscriptTypeMasked = /^true\/(.*)/, rcleanScript = /^\s*\s*$/g, // We have to close these tags to support XHTML (#13200) wrapMap = { // Support: IE 9 option: [ 1, "" ], thead: [ 1, "", "
" ], col: [ 2, "", "
" ], tr: [ 2, "", "
" ], td: [ 3, "", "
" ], _default: [ 0, "", "" ] }; // Support: IE 9 wrapMap.optgroup = wrapMap.option; wrapMap.tbody = wrapMap.tfoot = wrapMap.colgroup = wrapMap.caption = wrapMap.thead; wrapMap.th = wrapMap.td; jQuery.fn.extend({ text: function( value ) { return jQuery.access( this, function( value ) { return value === undefined ? jQuery.text( this ) : this.empty().append( ( this[ 0 ] && this[ 0 ].ownerDocument || document ).createTextNode( value ) ); }, null, value, arguments.length ); }, append: function() { return this.domManip( arguments, function( elem ) { if ( this.nodeType === 1 || this.nodeType === 11 || this.nodeType === 9 ) { var target = manipulationTarget( this, elem ); target.appendChild( elem ); } }); }, prepend: function() { return this.domManip( arguments, function( elem ) { if ( this.nodeType === 1 || this.nodeType === 11 || this.nodeType === 9 ) { var target = manipulationTarget( this, elem ); target.insertBefore( elem, target.firstChild ); } }); }, before: function() { return this.domManip( arguments, function( elem ) { if ( this.parentNode ) { this.parentNode.insertBefore( elem, this ); } }); }, after: function() { return this.domManip( arguments, function( elem ) { if ( this.parentNode ) { this.parentNode.insertBefore( elem, this.nextSibling ); } }); }, // keepData is for internal use only--do not document remove: function( selector, keepData ) { var elem, elems = selector ? jQuery.filter( selector, this ) : this, i = 0; for ( ; (elem = elems[i]) != null; i++ ) { if ( !keepData && elem.nodeType === 1 ) { jQuery.cleanData( getAll( elem ) ); } if ( elem.parentNode ) { if ( keepData && jQuery.contains( elem.ownerDocument, elem ) ) { setGlobalEval( getAll( elem, "script" ) ); } elem.parentNode.removeChild( elem ); } } return this; }, empty: function() { var elem, i = 0; for ( ; (elem = this[i]) != null; i++ ) { if ( elem.nodeType === 1 ) { // Prevent memory leaks jQuery.cleanData( getAll( elem, false ) ); // Remove any remaining nodes elem.textContent = ""; } } return this; }, clone: function( dataAndEvents, deepDataAndEvents ) { dataAndEvents = dataAndEvents == null ? false : dataAndEvents; deepDataAndEvents = deepDataAndEvents == null ? dataAndEvents : deepDataAndEvents; return this.map( function () { return jQuery.clone( this, dataAndEvents, deepDataAndEvents ); }); }, html: function( value ) { return jQuery.access( this, function( value ) { var elem = this[ 0 ] || {}, i = 0, l = this.length; if ( value === undefined && elem.nodeType === 1 ) { return elem.innerHTML; } // See if we can take a shortcut and just use innerHTML if ( typeof value === "string" && !rnoInnerhtml.test( value ) && !wrapMap[ ( rtagName.exec( value ) || [ "", "" ] )[ 1 ].toLowerCase() ] ) { value = value.replace( rxhtmlTag, "<$1>" ); try { for ( ; i < l; i++ ) { elem = this[ i ] || {}; // Remove element nodes and prevent memory leaks if ( elem.nodeType === 1 ) { jQuery.cleanData( getAll( elem, false ) ); elem.innerHTML = value; } } elem = 0; // If using innerHTML throws an exception, use the fallback method } catch( e ) {} } if ( elem ) { this.empty().append( value ); } }, null, value, arguments.length ); }, replaceWith: function() { var // Snapshot the DOM in case .domManip sweeps something relevant into its fragment args = jQuery.map( this, function( elem ) { return [ elem.nextSibling, elem.parentNode ]; }), i = 0; // Make the changes, replacing each context element with the new content this.domManip( arguments, function( elem ) { var next = args[ i++ ], parent = args[ i++ ]; if ( parent ) { // Don't use the snapshot next if it has moved (#13810) if ( next && next.parentNode !== parent ) { next = this.nextSibling; } jQuery( this ).remove(); parent.insertBefore( elem, next ); } // Allow new content to include elements from the context set }, true ); // Force removal if there was no new content (e.g., from empty arguments) return i ? this : this.remove(); }, detach: function( selector ) { return this.remove( selector, true ); }, domManip: function( args, callback, allowIntersection ) { // Flatten any nested arrays args = core_concat.apply( [], args ); var fragment, first, scripts, hasScripts, node, doc, i = 0, l = this.length, set = this, iNoClone = l - 1, value = args[ 0 ], isFunction = jQuery.isFunction( value ); // We can't cloneNode fragments that contain checked, in WebKit if ( isFunction || !( l <= 1 || typeof value !== "string" || jQuery.support.checkClone || !rchecked.test( value ) ) ) { return this.each(function( index ) { var self = set.eq( index ); if ( isFunction ) { args[ 0 ] = value.call( this, index, self.html() ); } self.domManip( args, callback, allowIntersection ); }); } if ( l ) { fragment = jQuery.buildFragment( args, this[ 0 ].ownerDocument, false, !allowIntersection && this ); first = fragment.firstChild; if ( fragment.childNodes.length === 1 ) { fragment = first; } if ( first ) { scripts = jQuery.map( getAll( fragment, "script" ), disableScript ); hasScripts = scripts.length; // Use the original fragment for the last item instead of the first because it can end up // being emptied incorrectly in certain situations (#8070). for ( ; i < l; i++ ) { node = fragment; if ( i !== iNoClone ) { node = jQuery.clone( node, true, true ); // Keep references to cloned scripts for later restoration if ( hasScripts ) { // Support: QtWebKit // jQuery.merge because core_push.apply(_, arraylike) throws jQuery.merge( scripts, getAll( node, "script" ) ); } } callback.call( this[ i ], node, i ); } if ( hasScripts ) { doc = scripts[ scripts.length - 1 ].ownerDocument; // Reenable scripts jQuery.map( scripts, restoreScript ); // Evaluate executable scripts on first document insertion for ( i = 0; i < hasScripts; i++ ) { node = scripts[ i ]; if ( rscriptType.test( node.type || "" ) && !data_priv.access( node, "globalEval" ) && jQuery.contains( doc, node ) ) { if ( node.src ) { // Hope ajax is available... jQuery._evalUrl( node.src ); } else { jQuery.globalEval( node.textContent.replace( rcleanScript, "" ) ); } } } } } } return this; } }); jQuery.each({ appendTo: "append", prependTo: "prepend", insertBefore: "before", insertAfter: "after", replaceAll: "replaceWith" }, function( name, original ) { jQuery.fn[ name ] = function( selector ) { var elems, ret = [], insert = jQuery( selector ), last = insert.length - 1, i = 0; for ( ; i <= last; i++ ) { elems = i === last ? this : this.clone( true ); jQuery( insert[ i ] )[ original ]( elems ); // Support: QtWebKit // .get() because core_push.apply(_, arraylike) throws core_push.apply( ret, elems.get() ); } return this.pushStack( ret ); }; }); jQuery.extend({ clone: function( elem, dataAndEvents, deepDataAndEvents ) { var i, l, srcElements, destElements, clone = elem.cloneNode( true ), inPage = jQuery.contains( elem.ownerDocument, elem ); // Support: IE >= 9 // Fix Cloning issues if ( !jQuery.support.noCloneChecked && ( elem.nodeType === 1 || elem.nodeType === 11 ) && !jQuery.isXMLDoc( elem ) ) { // We eschew Sizzle here for performance reasons: http://jsperf.com/getall-vs-sizzle/2 destElements = getAll( clone ); srcElements = getAll( elem ); for ( i = 0, l = srcElements.length; i < l; i++ ) { fixInput( srcElements[ i ], destElements[ i ] ); } } // Copy the events from the original to the clone if ( dataAndEvents ) { if ( deepDataAndEvents ) { srcElements = srcElements || getAll( elem ); destElements = destElements || getAll( clone ); for ( i = 0, l = srcElements.length; i < l; i++ ) { cloneCopyEvent( srcElements[ i ], destElements[ i ] ); } } else { cloneCopyEvent( elem, clone ); } } // Preserve script evaluation history destElements = getAll( clone, "script" ); if ( destElements.length > 0 ) { setGlobalEval( destElements, !inPage && getAll( elem, "script" ) ); } // Return the cloned set return clone; }, buildFragment: function( elems, context, scripts, selection ) { var elem, tmp, tag, wrap, contains, j, i = 0, l = elems.length, fragment = context.createDocumentFragment(), nodes = []; for ( ; i < l; i++ ) { elem = elems[ i ]; if ( elem || elem === 0 ) { // Add nodes directly if ( jQuery.type( elem ) === "object" ) { // Support: QtWebKit // jQuery.merge because core_push.apply(_, arraylike) throws jQuery.merge( nodes, elem.nodeType ? [ elem ] : elem ); // Convert non-html into a text node } else if ( !rhtml.test( elem ) ) { nodes.push( context.createTextNode( elem ) ); // Convert html into DOM nodes } else { tmp = tmp || fragment.appendChild( context.createElement("div") ); // Deserialize a standard representation tag = ( rtagName.exec( elem ) || ["", ""] )[ 1 ].toLowerCase(); wrap = wrapMap[ tag ] || wrapMap._default; tmp.innerHTML = wrap[ 1 ] + elem.replace( rxhtmlTag, "<$1>" ) + wrap[ 2 ]; // Descend through wrappers to the right content j = wrap[ 0 ]; while ( j-- ) { tmp = tmp.lastChild; } // Support: QtWebKit // jQuery.merge because core_push.apply(_, arraylike) throws jQuery.merge( nodes, tmp.childNodes ); // Remember the top-level container tmp = fragment.firstChild; // Fixes #12346 // Support: Webkit, IE tmp.textContent = ""; } } } // Remove wrapper from fragment fragment.textContent = ""; i = 0; while ( (elem = nodes[ i++ ]) ) { // #4087 - If origin and destination elements are the same, and this is // that element, do not do anything if ( selection && jQuery.inArray( elem, selection ) !== -1 ) { continue; } contains = jQuery.contains( elem.ownerDocument, elem ); // Append to fragment tmp = getAll( fragment.appendChild( elem ), "script" ); // Preserve script evaluation history if ( contains ) { setGlobalEval( tmp ); } // Capture executables if ( scripts ) { j = 0; while ( (elem = tmp[ j++ ]) ) { if ( rscriptType.test( elem.type || "" ) ) { scripts.push( elem ); } } } } return fragment; }, cleanData: function( elems ) { var data, elem, events, type, key, j, special = jQuery.event.special, i = 0; for ( ; (elem = elems[ i ]) !== undefined; i++ ) { if ( Data.accepts( elem ) ) { key = elem[ data_priv.expando ]; if ( key && (data = data_priv.cache[ key ]) ) { events = Object.keys( data.events || {} ); if ( events.length ) { for ( j = 0; (type = events[j]) !== undefined; j++ ) { if ( special[ type ] ) { jQuery.event.remove( elem, type ); // This is a shortcut to avoid jQuery.event.remove's overhead } else { jQuery.removeEvent( elem, type, data.handle ); } } } if ( data_priv.cache[ key ] ) { // Discard any remaining `private` data delete data_priv.cache[ key ]; } } } // Discard any remaining `user` data delete data_user.cache[ elem[ data_user.expando ] ]; } }, _evalUrl: function( url ) { return jQuery.ajax({ url: url, type: "GET", dataType: "script", async: false, global: false, "throws": true }); } }); // Support: 1.x compatibility // Manipulating tables requires a tbody function manipulationTarget( elem, content ) { return jQuery.nodeName( elem, "table" ) && jQuery.nodeName( content.nodeType === 1 ? content : content.firstChild, "tr" ) ? elem.getElementsByTagName("tbody")[0] || elem.appendChild( elem.ownerDocument.createElement("tbody") ) : elem; } // Replace/restore the type attribute of script elements for safe DOM manipulation function disableScript( elem ) { elem.type = (elem.getAttribute("type") !== null) + "/" + elem.type; return elem; } function restoreScript( elem ) { var match = rscriptTypeMasked.exec( elem.type ); if ( match ) { elem.type = match[ 1 ]; } else { elem.removeAttribute("type"); } return elem; } // Mark scripts as having already been evaluated function setGlobalEval( elems, refElements ) { var l = elems.length, i = 0; for ( ; i < l; i++ ) { data_priv.set( elems[ i ], "globalEval", !refElements || data_priv.get( refElements[ i ], "globalEval" ) ); } } function cloneCopyEvent( src, dest ) { var i, l, type, pdataOld, pdataCur, udataOld, udataCur, events; if ( dest.nodeType !== 1 ) { return; } // 1. Copy private data: events, handlers, etc. if ( data_priv.hasData( src ) ) { pdataOld = data_priv.access( src ); pdataCur = data_priv.set( dest, pdataOld ); events = pdataOld.events; if ( events ) { delete pdataCur.handle; pdataCur.events = {}; for ( type in events ) { for ( i = 0, l = events[ type ].length; i < l; i++ ) { jQuery.event.add( dest, type, events[ type ][ i ] ); } } } } // 2. Copy user data if ( data_user.hasData( src ) ) { udataOld = data_user.access( src ); udataCur = jQuery.extend( {}, udataOld ); data_user.set( dest, udataCur ); } } function getAll( context, tag ) { var ret = context.getElementsByTagName ? context.getElementsByTagName( tag || "*" ) : context.querySelectorAll ? context.querySelectorAll( tag || "*" ) : []; return tag === undefined || tag && jQuery.nodeName( context, tag ) ? jQuery.merge( [ context ], ret ) : ret; } // Support: IE >= 9 function fixInput( src, dest ) { var nodeName = dest.nodeName.toLowerCase(); // Fails to persist the checked state of a cloned checkbox or radio button. if ( nodeName === "input" && manipulation_rcheckableType.test( src.type ) ) { dest.checked = src.checked; // Fails to return the selected option to the default selected state when cloning options } else if ( nodeName === "input" || nodeName === "textarea" ) { dest.defaultValue = src.defaultValue; } } jQuery.fn.extend({ wrapAll: function( html ) { var wrap; if ( jQuery.isFunction( html ) ) { return this.each(function( i ) { jQuery( this ).wrapAll( html.call(this, i) ); }); } if ( this[ 0 ] ) { // The elements to wrap the target around wrap = jQuery( html, this[ 0 ].ownerDocument ).eq( 0 ).clone( true ); if ( this[ 0 ].parentNode ) { wrap.insertBefore( this[ 0 ] ); } wrap.map(function() { var elem = this; while ( elem.firstElementChild ) { elem = elem.firstElementChild; } return elem; }).append( this ); } return this; }, wrapInner: function( html ) { if ( jQuery.isFunction( html ) ) { return this.each(function( i ) { jQuery( this ).wrapInner( html.call(this, i) ); }); } return this.each(function() { var self = jQuery( this ), contents = self.contents(); if ( contents.length ) { contents.wrapAll( html ); } else { self.append( html ); } }); }, wrap: function( html ) { var isFunction = jQuery.isFunction( html ); return this.each(function( i ) { jQuery( this ).wrapAll( isFunction ? html.call(this, i) : html ); }); }, unwrap: function() { return this.parent().each(function() { if ( !jQuery.nodeName( this, "body" ) ) { jQuery( this ).replaceWith( this.childNodes ); } }).end(); } }); var curCSS, iframe, // swappable if display is none or starts with table except "table", "table-cell", or "table-caption" // see here for display values: https://developer.mozilla.org/en-US/docs/CSS/display rdisplayswap = /^(none|table(?!-c[ea]).+)/, rmargin = /^margin/, rnumsplit = new RegExp( "^(" + core_pnum + ")(.*)$", "i" ), rnumnonpx = new RegExp( "^(" + core_pnum + ")(?!px)[a-z%]+$", "i" ), rrelNum = new RegExp( "^([+-])=(" + core_pnum + ")", "i" ), elemdisplay = { BODY: "block" }, cssShow = { position: "absolute", visibility: "hidden", display: "block" }, cssNormalTransform = { letterSpacing: 0, fontWeight: 400 }, cssExpand = [ "Top", "Right", "Bottom", "Left" ], cssPrefixes = [ "Webkit", "O", "Moz", "ms" ]; // return a css property mapped to a potentially vendor prefixed property function vendorPropName( style, name ) { // shortcut for names that are not vendor prefixed if ( name in style ) { return name; } // check for vendor prefixed names var capName = name.charAt(0).toUpperCase() + name.slice(1), origName = name, i = cssPrefixes.length; while ( i-- ) { name = cssPrefixes[ i ] + capName; if ( name in style ) { return name; } } return origName; } function isHidden( elem, el ) { // isHidden might be called from jQuery#filter function; // in that case, element will be second argument elem = el || elem; return jQuery.css( elem, "display" ) === "none" || !jQuery.contains( elem.ownerDocument, elem ); } // NOTE: we've included the "window" in window.getComputedStyle // because jsdom on node.js will break without it. function getStyles( elem ) { return window.getComputedStyle( elem, null ); } function showHide( elements, show ) { var display, elem, hidden, values = [], index = 0, length = elements.length; for ( ; index < length; index++ ) { elem = elements[ index ]; if ( !elem.style ) { continue; } values[ index ] = data_priv.get( elem, "olddisplay" ); display = elem.style.display; if ( show ) { // Reset the inline display of this element to learn if it is // being hidden by cascaded rules or not if ( !values[ index ] && display === "none" ) { elem.style.display = ""; } // Set elements which have been overridden with display: none // in a stylesheet to whatever the default browser style is // for such an element if ( elem.style.display === "" && isHidden( elem ) ) { values[ index ] = data_priv.access( elem, "olddisplay", css_defaultDisplay(elem.nodeName) ); } } else { if ( !values[ index ] ) { hidden = isHidden( elem ); if ( display && display !== "none" || !hidden ) { data_priv.set( elem, "olddisplay", hidden ? display : jQuery.css(elem, "display") ); } } } } // Set the display of most of the elements in a second loop // to avoid the constant reflow for ( index = 0; index < length; index++ ) { elem = elements[ index ]; if ( !elem.style ) { continue; } if ( !show || elem.style.display === "none" || elem.style.display === "" ) { elem.style.display = show ? values[ index ] || "" : "none"; } } return elements; } jQuery.fn.extend({ css: function( name, value ) { return jQuery.access( this, function( elem, name, value ) { var styles, len, map = {}, i = 0; if ( jQuery.isArray( name ) ) { styles = getStyles( elem ); len = name.length; for ( ; i < len; i++ ) { map[ name[ i ] ] = jQuery.css( elem, name[ i ], false, styles ); } return map; } return value !== undefined ? jQuery.style( elem, name, value ) : jQuery.css( elem, name ); }, name, value, arguments.length > 1 ); }, show: function() { return showHide( this, true ); }, hide: function() { return showHide( this ); }, toggle: function( state ) { if ( typeof state === "boolean" ) { return state ? this.show() : this.hide(); } return this.each(function() { if ( isHidden( this ) ) { jQuery( this ).show(); } else { jQuery( this ).hide(); } }); } }); jQuery.extend({ // Add in style property hooks for overriding the default // behavior of getting and setting a style property cssHooks: { opacity: { get: function( elem, computed ) { if ( computed ) { // We should always get a number back from opacity var ret = curCSS( elem, "opacity" ); return ret === "" ? "1" : ret; } } } }, // Don't automatically add "px" to these possibly-unitless properties cssNumber: { "columnCount": true, "fillOpacity": true, "fontWeight": true, "lineHeight": true, "opacity": true, "order": true, "orphans": true, "widows": true, "zIndex": true, "zoom": true }, // Add in properties whose names you wish to fix before // setting or getting the value cssProps: { // normalize float css property "float": "cssFloat" }, // Get and set the style property on a DOM Node style: function( elem, name, value, extra ) { // Don't set styles on text and comment nodes if ( !elem || elem.nodeType === 3 || elem.nodeType === 8 || !elem.style ) { return; } // Make sure that we're working with the right name var ret, type, hooks, origName = jQuery.camelCase( name ), style = elem.style; name = jQuery.cssProps[ origName ] || ( jQuery.cssProps[ origName ] = vendorPropName( style, origName ) ); // gets hook for the prefixed version // followed by the unprefixed version hooks = jQuery.cssHooks[ name ] || jQuery.cssHooks[ origName ]; // Check if we're setting a value if ( value !== undefined ) { type = typeof value; // convert relative number strings (+= or -=) to relative numbers. #7345 if ( type === "string" && (ret = rrelNum.exec( value )) ) { value = ( ret[1] + 1 ) * ret[2] + parseFloat( jQuery.css( elem, name ) ); // Fixes bug #9237 type = "number"; } // Make sure that NaN and null values aren't set. See: #7116 if ( value == null || type === "number" && isNaN( value ) ) { return; } // If a number was passed in, add 'px' to the (except for certain CSS properties) if ( type === "number" && !jQuery.cssNumber[ origName ] ) { value += "px"; } // Fixes #8908, it can be done more correctly by specifying setters in cssHooks, // but it would mean to define eight (for every problematic property) identical functions if ( !jQuery.support.clearCloneStyle && value === "" && name.indexOf("background") === 0 ) { style[ name ] = "inherit"; } // If a hook was provided, use that value, otherwise just set the specified value if ( !hooks || !("set" in hooks) || (value = hooks.set( elem, value, extra )) !== undefined ) { style[ name ] = value; } } else { // If a hook was provided get the non-computed value from there if ( hooks && "get" in hooks && (ret = hooks.get( elem, false, extra )) !== undefined ) { return ret; } // Otherwise just get the value from the style object return style[ name ]; } }, css: function( elem, name, extra, styles ) { var val, num, hooks, origName = jQuery.camelCase( name ); // Make sure that we're working with the right name name = jQuery.cssProps[ origName ] || ( jQuery.cssProps[ origName ] = vendorPropName( elem.style, origName ) ); // gets hook for the prefixed version // followed by the unprefixed version hooks = jQuery.cssHooks[ name ] || jQuery.cssHooks[ origName ]; // If a hook was provided get the computed value from there if ( hooks && "get" in hooks ) { val = hooks.get( elem, true, extra ); } // Otherwise, if a way to get the computed value exists, use that if ( val === undefined ) { val = curCSS( elem, name, styles ); } //convert "normal" to computed value if ( val === "normal" && name in cssNormalTransform ) { val = cssNormalTransform[ name ]; } // Return, converting to number if forced or a qualifier was provided and val looks numeric if ( extra === "" || extra ) { num = parseFloat( val ); return extra === true || jQuery.isNumeric( num ) ? num || 0 : val; } return val; } }); curCSS = function( elem, name, _computed ) { var width, minWidth, maxWidth, computed = _computed || getStyles( elem ), // Support: IE9 // getPropertyValue is only needed for .css('filter') in IE9, see #12537 ret = computed ? computed.getPropertyValue( name ) || computed[ name ] : undefined, style = elem.style; if ( computed ) { if ( ret === "" && !jQuery.contains( elem.ownerDocument, elem ) ) { ret = jQuery.style( elem, name ); } // Support: Safari 5.1 // A tribute to the "awesome hack by Dean Edwards" // Safari 5.1.7 (at least) returns percentage for a larger set of values, but width seems to be reliably pixels // this is against the CSSOM draft spec: http://dev.w3.org/csswg/cssom/#resolved-values if ( rnumnonpx.test( ret ) && rmargin.test( name ) ) { // Remember the original values width = style.width; minWidth = style.minWidth; maxWidth = style.maxWidth; // Put in the new values to get a computed value out style.minWidth = style.maxWidth = style.width = ret; ret = computed.width; // Revert the changed values style.width = width; style.minWidth = minWidth; style.maxWidth = maxWidth; } } return ret; }; function setPositiveNumber( elem, value, subtract ) { var matches = rnumsplit.exec( value ); return matches ? // Guard against undefined "subtract", e.g., when used as in cssHooks Math.max( 0, matches[ 1 ] - ( subtract || 0 ) ) + ( matches[ 2 ] || "px" ) : value; } function augmentWidthOrHeight( elem, name, extra, isBorderBox, styles ) { var i = extra === ( isBorderBox ? "border" : "content" ) ? // If we already have the right measurement, avoid augmentation 4 : // Otherwise initialize for horizontal or vertical properties name === "width" ? 1 : 0, val = 0; for ( ; i < 4; i += 2 ) { // both box models exclude margin, so add it if we want it if ( extra === "margin" ) { val += jQuery.css( elem, extra + cssExpand[ i ], true, styles ); } if ( isBorderBox ) { // border-box includes padding, so remove it if we want content if ( extra === "content" ) { val -= jQuery.css( elem, "padding" + cssExpand[ i ], true, styles ); } // at this point, extra isn't border nor margin, so remove border if ( extra !== "margin" ) { val -= jQuery.css( elem, "border" + cssExpand[ i ] + "Width", true, styles ); } } else { // at this point, extra isn't content, so add padding val += jQuery.css( elem, "padding" + cssExpand[ i ], true, styles ); // at this point, extra isn't content nor padding, so add border if ( extra !== "padding" ) { val += jQuery.css( elem, "border" + cssExpand[ i ] + "Width", true, styles ); } } } return val; } function getWidthOrHeight( elem, name, extra ) { // Start with offset property, which is equivalent to the border-box value var valueIsBorderBox = true, val = name === "width" ? elem.offsetWidth : elem.offsetHeight, styles = getStyles( elem ), isBorderBox = jQuery.support.boxSizing && jQuery.css( elem, "boxSizing", false, styles ) === "border-box"; // some non-html elements return undefined for offsetWidth, so check for null/undefined // svg - https://bugzilla.mozilla.org/show_bug.cgi?id=649285 // MathML - https://bugzilla.mozilla.org/show_bug.cgi?id=491668 if ( val <= 0 || val == null ) { // Fall back to computed then uncomputed css if necessary val = curCSS( elem, name, styles ); if ( val < 0 || val == null ) { val = elem.style[ name ]; } // Computed unit is not pixels. Stop here and return. if ( rnumnonpx.test(val) ) { return val; } // we need the check for style in case a browser which returns unreliable values // for getComputedStyle silently falls back to the reliable elem.style valueIsBorderBox = isBorderBox && ( jQuery.support.boxSizingReliable || val === elem.style[ name ] ); // Normalize "", auto, and prepare for extra val = parseFloat( val ) || 0; } // use the active box-sizing model to add/subtract irrelevant styles return ( val + augmentWidthOrHeight( elem, name, extra || ( isBorderBox ? "border" : "content" ), valueIsBorderBox, styles ) ) + "px"; } // Try to determine the default display value of an element function css_defaultDisplay( nodeName ) { var doc = document, display = elemdisplay[ nodeName ]; if ( !display ) { display = actualDisplay( nodeName, doc ); // If the simple way fails, read from inside an iframe if ( display === "none" || !display ) { // Use the already-created iframe if possible iframe = ( iframe || jQuery("