def _write_frame(self, frames_grp: h5py.Group, resp: List[bytes], frame_num: int) -> \
            Tuple[h5py.Group, h5py.Group, dict, bool]:
        frame, objs, tr, done = super()._write_frame(frames_grp=frames_grp, resp=resp, frame_num=frame_num)
        num_objects = len(self.object_ids)
        # Physics data.
        velocities = np.empty(dtype=np.float32, shape=(num_objects, 3))
        angular_velocities = np.empty(dtype=np.float32, shape=(num_objects, 3))
        # Collision data.
        collision_ids = np.empty(dtype=np.int32, shape=(0, 2))
        collision_relative_velocities = np.empty(dtype=np.float32, shape=(0, 3))
        collision_contacts = np.empty(dtype=np.float32, shape=(0, 2, 3))
        # Environment Collision data.
        env_collision_ids = np.empty(dtype=np.int32, shape=(0, 1))
        env_collision_contacts = np.empty(dtype=np.float32, shape=(0, 2, 3))

        sleeping = True

        for r in resp[:-1]:
            r_id = OutputData.get_data_type_id(r)
            if r_id == "rigi":
                ri = Rigidbodies(r)
                ri_dict = dict()
                for i in range(ri.get_num()):
                    ri_dict.update({ri.get_id(i): {"vel": ri.get_velocity(i),
                                                   "ang": ri.get_angular_velocity(i)}})
                    # Check if any objects are sleeping that aren't in the abyss.
                    if not ri.get_sleeping(i) and tr[ri.get_id(i)]["pos"][1] >= -1:
                        sleeping = False
                # Add the Rigibodies data.
                for o_id, i in zip(self.object_ids, range(num_objects)):
                    velocities[i] = ri_dict[o_id]["vel"]
                    angular_velocities[i] = ri_dict[o_id]["ang"]
            elif r_id == "coll":
                co = Collision(r)
                collision_ids = np.append(collision_ids, [co.get_collider_id(), co.get_collidee_id()])
                collision_relative_velocities = np.append(collision_relative_velocities, co.get_relative_velocity())
                for i in range(co.get_num_contacts()):
                    collision_contacts = np.append(collision_contacts, (co.get_contact_normal(i),
                                                                        co.get_contact_point(i)))
            elif r_id == "enco":
                en = EnvironmentCollision(r)
                env_collision_ids = np.append(env_collision_ids, en.get_object_id())
                for i in range(en.get_num_contacts()):
                    env_collision_contacts = np.append(env_collision_contacts, (en.get_contact_normal(i),
                                                                                en.get_contact_point(i)))
        objs.create_dataset("velocities", data=velocities.reshape(num_objects, 3), compression="gzip")
        objs.create_dataset("angular_velocities", data=angular_velocities.reshape(num_objects, 3), compression="gzip")
        collisions = frame.create_group("collisions")
        collisions.create_dataset("object_ids", data=collision_ids.reshape((-1, 2)), compression="gzip")
        collisions.create_dataset("relative_velocities", data=collision_relative_velocities.reshape((-1, 3)),
                                  compression="gzip")
        collisions.create_dataset("contacts", data=collision_contacts.reshape((-1, 2, 3)), compression="gzip")
        env_collisions = frame.create_group("env_collisions")
        env_collisions.create_dataset("object_ids", data=env_collision_ids, compression="gzip")
        env_collisions.create_dataset("contacts", data=env_collision_contacts.reshape((-1, 2, 3)),
                                      compression="gzip")
        return frame, objs, tr, sleeping
Example #2
0
    def get_object_target_collision(self, obj_id: int, target_id: int, resp: List[bytes]):

        contact_points = []
        contact_normals = []

        for r in resp[:-1]:
            r_id = OutputData.get_data_type_id(r)
            if r_id == "coll":
                co = Collision(r)
                coll_ids = [co.get_collider_id(), co.get_collidee_id()]
                if [obj_id, target_id] == coll_ids or [target_id, obj_id] == coll_ids:
                    contact_points = [co.get_contact_point(i) for i in range(co.get_num_contacts())]
                    contact_normals = [co.get_contact_normal(i) for i in range(co.get_num_contacts())]

        return (contact_points, contact_normals)
    def trial(self, d_f, s_f, b_f, d_c, s_c, b_c, collision_types):
        """
        Collide a chair with a fridge.

        :param d_f: The dynamic friction of the fridge.
        :param s_f: The static friction of the fridge.
        :param b_f: The bounciness of the fridge.
        :param d_c: The dynamic friction of the chair.
        :param s_c: The static friction of the chair.
        :param b_c: The bounciness of the chair.
        :param collision_types: The types of collisions to listen for.
        """

        print("###\n\nNew Trial\n\n###\n")

        fridge_id = 0
        chair_id = 1

        # Destroy all objects currently in the scene.
        init_commands = [{"$type": "destroy_all_objects"}]
        # Create the avatar.
        init_commands.extend(
            TDWUtils.create_avatar(position={
                "x": 1,
                "y": 2.5,
                "z": 5
            },
                                   look_at=TDWUtils.VECTOR3_ZERO))
        # Add the objects.
        # Set the masses and physic materials.
        # Apply a force to the chair.
        # Receive collision data (note that by setting "stay" to True, you will receive a LOT of data;
        # see "Performance Optimizations" documentation.)
        init_commands.extend([
            self.get_add_object("fridge_large", object_id=fridge_id),
            self.get_add_object("chair_billiani_doll",
                                object_id=chair_id,
                                position={
                                    "x": 4,
                                    "y": 0,
                                    "z": 0
                                }), {
                                    "$type": "set_mass",
                                    "id": fridge_id,
                                    "mass": 40
                                }, {
                                    "$type": "set_mass",
                                    "id": chair_id,
                                    "mass": 20
                                }, {
                                    "$type": "set_physic_material",
                                    "id": fridge_id,
                                    "dynamic_friction": d_f,
                                    "static_friction": s_f,
                                    "bounciness": b_f
                                }, {
                                    "$type": "set_physic_material",
                                    "id": chair_id,
                                    "dynamic_friction": d_c,
                                    "static_friction": s_c,
                                    "bounciness": b_c
                                }, {
                                    "$type": "apply_force_to_object",
                                    "force": {
                                        "x": -200,
                                        "y": 0,
                                        "z": 0
                                    },
                                    "id": chair_id
                                }, {
                                    "$type": "send_collisions",
                                    "collision_types": collision_types,
                                    "enter": True,
                                    "exit": True,
                                    "stay": True
                                }
        ])
        self.communicate(init_commands)

        # Iterate through 500 frames.
        # Every frame, listen for collisions, and parse the output data.
        for i in range(500):
            resp = self.communicate([])
            if len(resp) > 1:
                for r in resp[:-1]:
                    r_id = OutputData.get_data_type_id(r)
                    # There was a collision between two objects.
                    if r_id == "coll":
                        collision = Collision(r)
                        print("Collision between two objects:")
                        print("\tEvent: " + collision.get_state())
                        print("\tCollider: " +
                              str(collision.get_collider_id()))
                        print("\tCollidee: " +
                              str(collision.get_collidee_id()))
                        print("\tRelative velocity: " +
                              str(collision.get_relative_velocity()))
                        print("\tContacts:")
                        for j in range(collision.get_num_contacts()):
                            print(
                                str(collision.get_contact_normal(j)) + "\t" +
                                str(collision.get_contact_point(j)))
                    # There was a collision between an object and the environment.
                    elif r_id == "enco":
                        collision = EnvironmentCollision(r)
                        print(
                            "Collision between an object and the environment:")
                        print("\tEvent: " + collision.get_state())
                        print("\tCollider: " + str(collision.get_object_id()))
                        print("\tContacts:")
                        for j in range(collision.get_num_contacts()):
                            print(
                                str(collision.get_contact_normal(j)) + "\t" +
                                str(collision.get_contact_point(j)))
                    else:
                        raise Exception(r_id)