### [tools] add getSurfacesExtremumPoints : script to manipulate affordance...

`[tools] add getSurfacesExtremumPoints : script to manipulate affordance objects and convert them to points list`
parent 6f0bf141
 import numpy as np from numpy import arange, array, append, cross, dot, zeros from numpy.linalg import norm from scipy.spatial import ConvexHull def normal(points): p1 = array(points) p2 = array(points) p3 = array(points) normal = cross((p2 - p1),(p3 - p1)) normal /= norm(normal) return normal.tolist() def cutList2D(l): return [el[:2] for el in l] #precision handling def roundPoints (points, precision): return [[round(x,precision) for x in p] for p in points] def removeDuplicates (points): pList = [] for p in points: if p not in pList: pList.append(p) return pList def computeAxisAngleRotation(u, c): ux = u ; uy = u ; uz = u s = np.sqrt(1- c*c) return [[c+ux*ux*(1-c), ux*uy*(1-c)-uz*s, ux*uz*(1-c)+uy*s], [uy*ux*(1-c)+uz*s, c+uy*uy*(1-c), uy*uz*(1-c)-ux*s], [uz*ux*(1-c)-uy*s, uz*uy*(1-c)+ux*s, c+uz*uz*(1-c)]] def getSurfaceRotation (surface): n = surface cosx = np.dot(surface,[0,0,1]) #print "normal = ",n axisx = np.cross(surface,[0,0,1]) #print "axisx = ",axisx n_axisx = norm(axisx) #print "axisx norm = ",n_axisx if n_axisx > 0: axisx /= n_axisx return computeAxisAngleRotation(axisx, cosx) def getPtsRotation (points): return getSurfaceRotation((points,normal(points))) def getSurfaceTranslation (surface): return [sum(x)/len(x) for x in zip(*surface)] def getPtsTranslation (points): return getSurfaceTranslation((points,normal(points))) #into xy plane, back to its position def allignSurface (surface): R = getSurfaceRotation(surface) t = getSurfaceTranslation(surface) translatedPts = [(array(p)-array(t)).tolist() for p in surface] rotatedPts = [np.dot(R,p).tolist() for p in translatedPts] return [(array(p)+array(t)).tolist() for p in rotatedPts] def allignPoints (points): return allignSurface((points, normal(points))) def pointsTransform (points,R,t): translatedPts = [(array(pt)-array(t)).tolist() for pt in points] rotatedPts = [np.dot(R,pt).tolist() for pt in translatedPts] return [(array(pt)+array(t)).tolist() for pt in rotatedPts] def getSurfaceExtremumPoints(el): pts = removeDuplicates(el+el) apts = allignPoints(pts) #print pts hull = ConvexHull(cutList2D(apts))#,False,'QbB') return [pts[idx] for idx in hull.vertices] #get contact surfaces (pts and normal) def contactSurfaces(afftool): l = afftool.getAffordancePoints("Support") return [(getSurfaceExtremumPoints(el)) for el in l] #return [(getSurfaceExtremumPoints(el), normal(el)) for el in l] ######################################################################### #get surface information #surfaces = contactSurfaces(afftool)
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!