1class shapes_index():
2    SPHERE = 0
3    ELLIPSOID = 1
4    BOX = 2
5    CYLINDER = 3
6    CONVEXHULL = 4
7    TRIANGLEMESH = 5
8    BARREL = 6
9    CAPSULE = 7
10    CONE = 8
11    ROUNDEDBOX = 9
12    ROUNDEDCYL = 10
13    ROUNDEDCONE = 11
14    BEZIER = 12
15
16
17def import_shapes(filepath):
18
19    shapes_data = []
20
21    with open(filepath, 'r') as FH:
22        lines = FH.readlines()
23
24        # Read number of bodies, visual assets, joints, and TSDA elements (first line)
25        line = lines[0].strip().split(',')
26        num_bodies = int(line[0])
27        num_assets = int(line[1])
28        ##print ("File: ", filepath, "  Num bodies: ", num_bodies, "  Num assets: ", num_assets)
29
30        # Read only visual assets
31        for il in range(1 + num_bodies, 1 + num_bodies + num_assets):
32            line = lines[il].strip().split(',')
33
34            # Extract information common to all assets
35            body_id, active,  x, y, z, e0, e1, e2, e3, r, g, b, shape_type = line[:13:]
36            data = [int(shape_type), int(body_id), active,  float(x), float(y), float(z), float(e0), float(e1), float(e2), float(e3), float(r), float(g), float(b)]
37
38            # Read asset-specific data
39            if int(shape_type) == shapes_index.TRIANGLEMESH:
40                try:
41                    path = line[13]
42                    data.extend([path.strip('"')])
43                except:
44                    print("Record: ", lines[il])
45                    raise Exception("Failed while trying to parse trimesh data.")
46            elif int(shape_type) == shapes_index.SPHERE:
47                try:
48                    rad = line[13]
49                    data.extend([float(rad)])
50                except:
51                    print("Record: ", lines[il])
52                    raise Exception("Failed while trying to parse sphere data.")
53            elif int(shape_type) == shapes_index.BOX:
54                try:
55                    size_x, size_y, size_z = line[13:16:]
56                    data.extend([float(size_x), float(size_y), float(size_z)])
57                except:
58                    print("Record: ", lines[il])
59                    raise Exception("Failed while trying to parse box data.")
60            elif int(shape_type) == shapes_index.ELLIPSOID:
61                try:
62                    size_x, size_y, size_z = line[13:16:]
63                    data.extend([float(size_x), float(size_y), float(size_z)])
64                except:
65                    print("Record: ", lines[il])
66                    raise Exception("Failed while trying to parse ellipsoid data.")
67            elif int(shape_type) == shapes_index.CYLINDER:
68                try:
69                    rad, p1x, p1y, p1z, p2x, p2y, p2z = line[13:20:]
70                    data.extend([float(rad), float(p1x), float(p1y), float(p1z), float(p2x), float(p2y), float(p2z)])
71                except:
72                    print("Record: ", lines[il])
73                    raise Exception("Failed while trying to parse cyilinder data.")
74            else:
75                print('Unsupported shape type')
76                continue
77
78            # Append to list of shape data
79            shapes_data.append(data)
80
81
82    return shapes_data
83
84if __name__ == "__main__":
85    data = import_shapes('data1.dat')
86    print(data)