text: import platform
  import pytest
  import dartpy as dart
  import numpy as np
  
  
  def collision_groups_tester(cd):
      size = [1, 1, 1]
      pos1 = [0, 0, 0]
      pos2 = [0.5, 0, 0]
  
      simple_frame1 = dart.dynamics.SimpleFrame()
      simple_frame2 = dart.dynamics.SimpleFrame()
  
      sphere1 = dart.dynamics.SphereShape(1)
      sphere2 = dart.dynamics.SphereShape(1)
  
      simple_frame1.setShape(sphere1)
      simple_frame2.setShape(sphere2)
  
      group = cd.createCollisionGroup()
      group.addShapeFrame(simple_frame1)
      group.addShapeFrame(simple_frame2)
      assert group.getNumShapeFrames() is 2
  
      #
      #    ( s1,s2 )              collision!
      # ---+---|---+---+---+---+--->
      #   -1   0  +1  +2  +3  +4
      #
      assert group.collide()
  
      #
      #    (  s1   )   (  s2   )  no collision
      # ---+---|---+---+---+---+--->
      #   -1   0  +1  +2  +3  +4
      #
      simple_frame2.setTranslation([3, 0, 0])
      assert not group.collide()
  
      option = dart.collision.CollisionOption()
      result = dart.collision.CollisionResult()
  
      group.collide(option, result)
      assert not result.isCollision()
      assert result.getNumContacts() is 0
  
      option.enableContact = True
      simple_frame2.setTranslation([1.99, 0, 0])
  
      group.collide(option, result)
      assert result.isCollision()
      assert result.getNumContacts() is not 0
  
      # Repeat the same test with BodyNodes instead of SimpleFrames
  
      group.removeAllShapeFrames()
      assert group.getNumShapeFrames() is 0
  
      skel1 = dart.dynamics.Skeleton()
      skel2 = dart.dynamics.Skeleton()
  
      [joint1, body1] = skel1.createFreeJointAndBodyNodePair(None)
      [joint2, body2] = skel2.createFreeJointAndBodyNodePair(None)
  
      shape_node1 = body1.createShapeNode(sphere1)
      shape_node1.createVisualAspect()
      shape_node1.createCollisionAspect()
  
      shape_node2 = body2.createShapeNode(sphere2)
      shape_node2.createVisualAspect()
      shape_node2.createCollisionAspect()
  
      group.addShapeFramesOf(body1)
      group.addShapeFramesOf(body2)
  
      assert group.getNumShapeFrames() is 2
  
      assert group.collide()
  
      joint2.setPosition(3, 3)
      assert not group.collide()
  
      # Repeat the same test with BodyNodes and two groups
  
      joint2.setPosition(3, 0)
  
      group.removeAllShapeFrames()
      assert group.getNumShapeFrames() is 0
      group2 = cd.createCollisionGroup()
  
      group.addShapeFramesOf(body1)
      group2.addShapeFramesOf(body2)
  
      assert group.getNumShapeFrames() is 1
      assert group2.getNumShapeFrames() is 1
  
      assert group.collide(group2)
  
      joint2.setPosition(3, 3)
      assert not group.collide(group2)
  
  
  def test_collision_groups():
      cd = dart.collision.FCLCollisionDetector()
      collision_groups_tester(cd)
  
      cd = dart.collision.DARTCollisionDetector()
      collision_groups_tester(cd)
  
      if hasattr(dart.collision, "BulletCollisionDetector"):
          cd = dart.collision.BulletCollisionDetector()
          collision_groups_tester(cd)
  
      if hasattr(dart.collision, "OdeCollisionDetector"):
          cd = dart.collision.OdeCollisionDetector()
          collision_groups_tester(cd)
  
  
  # TODO: Add more collision detectors
  @pytest.mark.parametrize("cd", [dart.collision.FCLCollisionDetector()])
  def test_filter(cd):
      # Create two bodies skeleton. The two bodies are placed at the same position
      # with the same size shape so that they collide by default.
      skel = dart.dynamics.Skeleton()
  
      shape = dart.dynamics.BoxShape(np.zeros(3))
  
      _, body0 = skel.createRevoluteJointAndBodyNodePair()
      shape_node0 = body0.createShapeNode(shape)
      shape_node0.createVisualAspect()
      shape_node0.createCollisionAspect()
  
      _, body1 = skel.createRevoluteJointAndBodyNodePair(body0)
      shape_node1 = body1.createShapeNode(shape)
      shape_node1.createVisualAspect()
      shape_node1.createCollisionAspect()
  
      # Create a world and add the created skeleton
      world = dart.simulation.World()
      world.addSkeleton(skel)
  
      # Set a new collision detector
      constraint_solver = world.getConstraintSolver()
      constraint_solver.setCollisionDetector(cd)
  
      # Get the collision group from the constraint solver
      group = constraint_solver.getCollisionGroup()
      assert group.getNumShapeFrames() == 2
  
      # Create BodyNodeCollisionFilter
      option = constraint_solver.getCollisionOption()
      body_node_filter = dart.collision.BodyNodeCollisionFilter()
      option.collisionFilter = body_node_filter
  
      skel.enableSelfCollisionCheck()
      skel.enableAdjacentBodyCheck()
      assert skel.isEnabledSelfCollisionCheck()
      assert skel.isEnabledAdjacentBodyCheck()
      assert group.collide()
      assert group.collide(option)
  
      skel.enableSelfCollisionCheck()
      skel.disableAdjacentBodyCheck()
      assert skel.isEnabledSelfCollisionCheck()
      assert not skel.isEnabledAdjacentBodyCheck()
      assert group.collide()
      assert not group.collide(option)
  
      skel.disableSelfCollisionCheck()
      skel.enableAdjacentBodyCheck()
      assert not skel.isEnabledSelfCollisionCheck()
      assert skel.isEnabledAdjacentBodyCheck()
      assert group.collide()
      assert not group.collide(option)
  
      skel.disableSelfCollisionCheck()
      skel.disableAdjacentBodyCheck()
      assert not skel.isEnabledSelfCollisionCheck()
      assert not skel.isEnabledAdjacentBodyCheck()
      assert group.collide()
      assert not group.collide(option)
  
      # Test collision body filtering
      skel.enableSelfCollisionCheck()
      skel.enableAdjacentBodyCheck()
      body_node_filter.addBodyNodePairToBlackList(body0, body1)
      assert not group.collide(option)
      body_node_filter.removeBodyNodePairFromBlackList(body0, body1)
      assert group.collide(option)
      body_node_filter.addBodyNodePairToBlackList(body0, body1)
      assert not group.collide(option)
      body_node_filter.removeAllBodyNodePairsFromBlackList()
      assert group.collide(option)
  
  
  def test_raycast():
      cd = dart.collision.BulletCollisionDetector()
  
      simple_frame = dart.dynamics.SimpleFrame()
      sphere = dart.dynamics.SphereShape(1)
      simple_frame.setShape(sphere)
  
      group = cd.createCollisionGroup()
      group.addShapeFrame(simple_frame)
      assert group.getNumShapeFrames() == 1
  
      option = dart.collision.RaycastOption()
      option.mEnableAllHits = False
  
      result = dart.collision.RaycastResult()
      assert not result.hasHit()
  
      ray_hit = dart.collision.RayHit()
  
      result.clear()
      simple_frame.setTranslation(np.zeros(3))
      assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
      assert result.hasHit()
      assert len(result.mRayHits) == 1
      ray_hit = result.mRayHits[0]
      assert np.isclose(ray_hit.mPoint, [-1, 0, 0]).all()
      assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
      assert ray_hit.mFraction == pytest.approx(0.25)
  
      result.clear()
      simple_frame.setTranslation(np.zeros(3))
      assert group.raycast([2, 0, 0], [-2, 0, 0], option, result)
      assert result.hasHit()
      assert len(result.mRayHits) == 1
      ray_hit = result.mRayHits[0]
      assert np.isclose(ray_hit.mPoint, [1, 0, 0]).all()
      assert np.isclose(ray_hit.mNormal, [1, 0, 0]).all()
      assert ray_hit.mFraction == pytest.approx(0.25)
  
      result.clear()
      simple_frame.setTranslation([1, 0, 0])
      assert group.raycast([-2, 0, 0], [2, 0, 0], option, result)
      assert result.hasHit()
      assert len(result.mRayHits) == 1
      ray_hit = result.mRayHits[0]
      assert np.isclose(ray_hit.mPoint, [0, 0, 0]).all()
      assert np.isclose(ray_hit.mNormal, [-1, 0, 0]).all()
      assert ray_hit.mFraction == pytest.approx(0.5)
  
  
  if __name__ == "__main__":
      pytest.main()
  