implemented obj loader as inequalities

data/roms/centers.txt

@@ -46,3 +46,46 @@ center =
 radius =
     0.4302
 
+
+comlArm
+center =
+    0.0026
+    0.1401
+   -0.1185
+
+
+radius =
+
+    0.8326
+
+comlLeg
+center =
+   -0.0170
+   -0.1324
+    0.3960
+
+
+radius =
+    0.2233
+
+comrArm
+center =
+   -0.0006
+    0.0944
+   -0.1467
+
+
+radius =
+    0.8344
+
+
+comrLeg
+center =
+    0.0025
+    0.1338
+    0.3987
+
+
+radius =
+    0.2238
+

data/roms/centers.txt~

+This file contains the coordinate and radius
+of the chebyschev ball associated to each limb's ROM
+
+rleg
+center =
+    0.1362
+   -0.1887
+   -0.1498
+
+
+radius =
+    0.4237
+
+lleg
+center =
+
+    0.0092
+    0.0357
+   -0.2253
+
+
+radius =
+    0.4023
+
+
+rarm
+center =
+
+    0.1014
+   -0.3514
+    0.5129
+
+
+radius =
+    0.4308
+
+
+larm
+center =
+
+    0.1070
+    0.3497
+    0.5331
+
+
+radius =
+    0.4302
+
+
+comlArm
+center =
+    0.0026
+    0.1401
+   -0.1185
+
+
+radius =
+
+    0.8326
+
+comlLeg
+center =
+   -0.0170
+   -0.1324
+    0.3960
+
+
+radius =
+    0.2233
+
+comrArm
+center =
+   -0.0006
+    0.0944
+   -0.1467
+
+
+radius =
+    0.8344
+
+
+comrLeg
+center =
+    0.0025
+    0.1338
+    0.3987
+
+
+radius =
+    0.2238
+
+
+xc7 =
+
+    0.0025
+    0.1338
+    0.3987
+
+
+Rc7 =
+
+    0.2238
+
+Polyhedron in R^3 with representations:
+    H-rep (irredundant) : Inequalities 528 | Equalities   0
+    V-rep (irredundant) : Vertices 266 | Rays   0
+Functions : none
+
+The @polytope class will be deprecated, use @Polyhedron instead.
+>> 

data/roms/comlArmSimplified.mtl

+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/roms/comlLegSimplified.mtl

+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/roms/comrArmSimplified.mtl

+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

data/roms/comrLegSimplified.mtl

+# Blender MTL File: 'None'
+# Material Count: 1
+
+newmtl None
+Ns 0
+Ka 0.000000 0.000000 0.000000
+Kd 0.8 0.8 0.8
+Ks 0.8 0.8 0.8
+d 1
+illum 2

script/loadobj.py

+#do the loading of the obj file
+import numpy as np
+from collections import namedtuple
+ObjectData = namedtuple("ObjectData", "V T N F")
+
+def toFloat(stringArray):
+	res= np.zeros(len(stringArray))
+	for i in range(0,len(stringArray)):
+		res[i] = float(stringArray[i])
+	return res
+
+def load_obj(filename) :
+ V = [] #vertex
+ T = [] #texcoords
+ N = [] #normals
+ F = [] #face indexies
+
+ fh = open(filename)
+ for line in fh :
+  if line[0] == '#' : continue
+
+  line = line.strip().split(' ')
+  if line[0] == 'v' : #vertex
+   V.append(toFloat(line[1:]))
+  elif line[0] == 'vt' : #tex-coord
+   T.append(line[1:])
+  elif line[0] == 'vn' : #normal vector
+   N.append(toFloat(line[1:]))
+  elif line[0] == 'f' : #face
+   face = line[1:]
+   for i in range(0, len(face)) :
+    face[i] = face[i].split('/')
+    # OBJ indexies are 1 based not 0 based hence the -1
+    # convert indexies to integer
+    for j in range(0, len(face[i])): 
+		if j!=1:
+			face[i][j] = int(face[i][j]) - 1
+   F.append(face)
+
+ return ObjectData(V, T, N, F)
+ 
+def inequality(v, n): 
+	#the plan has for equation ax + by + cz = d, with a b c coordinates of the normal
+	#inequality is then ax + by +cz -d <= 0
+	return [n[0], n[1], n[2], -np.array(v).dot(np.array(n))]
+	
+def asInequalities(obj):
+	#for each face, find first three points and deduce plane
+	#inequality is given by normal
+	res=[]
+	for f in range(0, len(obj.F)):
+		face = obj.F[f]
+		v = obj.V[face[0][0]]
+		# assume normals are in obj
+		n = obj.N[face[0][2]]
+		res.append(inequality(v,n))
+	return np.array(res)
+	
+def is_inside(inequalities, pt):
+	p4 = [pt[0], pt[1], pt[2], 1]
+	for i in range(0, len(inequalities)):
+		if inequalities[i].dot(p4) > 0:
+			return 0
+	return 1
+
+
+def test_inequality():
+	n = np.array([0,-1,0])
+	v = np.array([0,1,1])
+	print(inequality(v,n))
+	if inequality(v,n) != [0,-1,0,1]:
+		print("error in inequality")
+	else:
+		print("test of inequality successful")
+
+obj = load_obj('../data/roms/comlArmSimplified.obj')
+ineq = asInequalities(obj)
+print(is_inside(ineq, [0,0,0]))
+print(is_inside(ineq, [4,0,0]))