Exemplo n.º 1
0
def make_hcs_3d_scaled (a, b, c):
    """build a 3D homogeneus coordiate system from three points, and derive scale from distance between points"""
     # create orthonormal basis 
    u = normalised(b-a)
    v = normalised(c-a)
    nu = vector.norm(u)
    nv = vector.norm(v)
    if tol_eq(nu,0) and tol_eq(nv,0):
        # all points equal, no rotation
        u = vector.vector([1.0,0.0,0.0])
        v = vector.vector([0.0,1.0,0.0])
    elif tol_eq(nu, 0):
        # determine u perpendicular from v
        u,dummy = perp_3d(v)[0]
    elif tol_eq(nv, 0):
        # determine v perpendicular from u
        dummy,v = perp_3d(u)[0]
    # make the basis vectors orthogonal
    w = vector.cross(u,v)
    v = vector.cross(w,u)
    # scale again
    if not tol_eq(vector.norm(b-a),0.0):
        u = u / vector.norm(b-a)
    if not tol_eq(vector.norm(c-a),0.0):
        v = v / vector.norm(c-a)
    # note: w is not scaled
    # create matix with basis vectors + translation as columns
    hcs = Mat([ 
        [u[0],v[0], w[0], a[0]], 
        [u[1],v[1], w[1], a[1]],
        [u[2],v[2], w[2], a[2]], 
        [0.0, 0.0, 0.0, 1.0]    ])
    return hcs 
Exemplo n.º 2
0
def make_hcs_3d (a, b, c, righthanded=True):
    """build a 3D homogeneous coordiate system from three points. The origin is point a. The x-axis is
    b-a, the y axis is c-a, or as close as possible after orthogonormalisation."""
    # create orthonormal basis 
    u = normalised(b-a)
    v = normalised(c-a)
    nu = vector.norm(u)
    nv = vector.norm(v)
    if tol_eq(nu,0.0) and tol_eq(nv,0.0):
        # all points equal, no rotation
        u = vector.vector([1.0,0.0,0.0])
        v = vector.vector([0.0,1.0,0.0])
    elif tol_eq(nu, 0.0):
        # determine u perpendicular from v
        u,dummy = perp_3d(v)
    elif tol_eq(nv, 0.0):
        # determine v perpendicular from u
        dummy,v = perp_3d(u)
    # ensure that u and v are different
    if tol_eq(vector.norm(u-v),0.0):
        dummy,v = perp_3d(u)
    # make the basis vectors orthogonal
    w = vector.cross(u,v)
    v = vector.cross(w,u)
    # flip basis if lefthanded desired
    if righthanded==False:
        w = -w
    # create matix with basis vectors + translation as columns
    hcs = Mat([ 
        [u[0],v[0], w[0], a[0]], 
        [u[1],v[1], w[1], a[1]],
        [u[2],v[2], w[2], a[2]], 
        [0.0, 0.0, 0.0, 1.0]    ])
    return hcs 
Exemplo n.º 3
0
def sss_int(p1, r1, p2, r2, p3, r3):
    """Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively. 
       Returns a list of zero, one or two solution points.
    """
    solutions = []
    # plane though p1, p2, p3
    n = vector.cross(p2 - p1, p3 - p1)
    n = n / vector.norm(n)
    # intersect circles in plane
    cp1 = vector.vector([0.0, 0.0])
    cp2 = vector.vector([vector.norm(p2 - p1), 0.0])
    cpxs = cc_int(cp1, r1, cp2, r2)
    if len(cpxs) == 0:
        return []
    # px, rx, nx is circle
    px = p1 + (p2 - p1) * cpxs[0][0] / vector.norm(p2 - p1)
    rx = abs(cpxs[0][1])
    # plane of intersection cicle
    nx = p2 - p1
    nx = nx / vector.norm(nx)
    # print "px,rx,nx:",px,rx,nx
    # py = project p3 on px,nx
    dy3 = vector.dot(p3 - px, nx)
    py = p3 - (nx * dy3)
    if tol_gt(dy3, r3):
        return []
    ry = math.sin(math.acos(abs(dy3 / r3))) * r3
    # print "py,ry:",py,ry
    cpx = vector.vector([0.0, 0.0])
    cpy = vector.vector([vector.norm(py - px), 0.0])
    cp4s = cc_int(cpx, rx, cpy, ry)
    for cp4 in cp4s:
        p4 = px + (py - px) * cp4[0] / vector.norm(py - px) + n * cp4[1]
        solutions.append(p4)
    return solutions
Exemplo n.º 4
0
    def step(self, myOrder, dT=0.01):
        # initialize variables:
        tanks = self.ship.get_tanks()
        desired_vel_c, desired_ang_vel_c = myOrder.getorder()
        myShip = self.ship

        acc = []
        ang_acc = []

        for engine, orientation in myShip.engines:
            f = orientation * engine.thrust
            r = Vector(0, 0, myShip.get_mod_pos(engine))
            tq = cross(r, f)

            a = f / myShip.mass
            tqx, tqy, tqz = tq.getval()
            moixy, moiz = self.moi
            alpha = Vector(tqx / moixy, tqy / moixy, tqz / moiz)

            acc.append(a)
            ang_acc.append(alpha)

        vel_pblm = vec_to_pbl_matrix(desired_vel_c, acc)
        ang_pblm = vec_to_pbl_matrix(desired_ang_vel_c, ang_acc)

        pblmtx = Matrix(
            len(vel_pblm.mat) + len(ang_pblm.mat),
            len(myShip.engines) + 1)
        pblmtx.mat = vel_pblm.mat
        pblmtx.mat.extend(ang_pblm.mat)
        l, r = pblmtx.deaug()

        engine_burntimes = nnls(l, r)

        i = 0

        deltav = Vector(0, 0, 0)
        deltaw = Vector(0, 0, 0)
        for engine, ort in myShip.engines:
            if max(engine_burntimes) == 0:
                this_burn_time = 0
            else:
                this_burn_time = dT * engine_burntimes[i] / max(
                    engine_burntimes)
            myShip.burn(engine, tanks, this_burn_time)
            deltav += this_burn_time * acc[i]
            deltaw += this_burn_time * ang_acc[i]
            i += 1

        # assuming mostly constant-acceleration.(reasonable assumption under small enough dt)
        euler_vec = self.angvel * dT + 1 / 2 * deltaw * dT**2
        self.ort = euler_rot(self.ort, euler_vec).unit()

        self.pos += self.vel * dT + 1 / 2 * deltav.norm() * dT**2 * self.ort

        # translate deltav (in ship coordinate space) into world coordinate space.
        self.vel += deltav.norm() * self.ort
        self.angvel += deltaw

        myOrder.update_order(deltav, deltaw, dT)
Exemplo n.º 5
0
def is_right_handed(p1, p2, p3, p4):
    """return True if tetrahedron p1 p2 p3 p4 is right handed"""
    u = p2 - p1
    v = p3 - p1
    uv = vector.cross(u, v)
    w = p4 - p1
    return vector.dot(uv, w) > 0
Exemplo n.º 6
0
def is_right_handed(p1,p2,p3,p4):
    """return True if tetrahedron p1 p2 p3 p4 is right handed"""
    u = p2-p1
    v = p3-p1
    uv = vector.cross(u,v)
    w = p4-p1
    #return vector.dot(uv,w) > 0 
    return tol_gt(vector.dot(uv,w), 0) 
Exemplo n.º 7
0
def is_coplanar(p1,p2,p3,p4):
    """return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
    u = p2-p1
    v = p3-p1
    uv = vector.cross(u,v)
    w = p4-p1
    #return vector.dot(uv,w) == 0 
    return tol_eq(vector.dot(uv,w), 0) 
Exemplo n.º 8
0
	def getarea(self):
		sumA = 0.0
		for i in range(0, self.totalSize()-2):
			v1 = self.pts[i+1] - self.pts[0]
			v2 = self.pts[i+2] - self.pts[0]
			sumA += 0.5*vector.mag(vector.cross(v1, v2))

		return sumA
Exemplo n.º 9
0
def is_left_handed(p1, p2, p3, p4):
    """return True if tetrahedron p1 p2 p3 p4 is left handed"""
    u = p2 - p1
    v = p3 - p1
    uv = vector.cross(u, v)
    w = p4 - p1
    #return vector.dot(uv,w) < 0
    return tol_lt(vector.dot(uv, w), 0)
Exemplo n.º 10
0
def is_coplanar(p1, p2, p3, p4):
    """return True if tetrahedron p1 p2 p3 p4 is co-planar (not left- or right-handed)"""
    u = p2 - p1
    v = p3 - p1
    uv = vector.cross(u, v)
    w = p4 - p1
    #return vector.dot(uv,w) == 0
    return tol_eq(vector.dot(uv, w), 0)
Exemplo n.º 11
0
def vec2vec2quat(a, b):
    n = vector.cross(a, b)
    fac = vector.dot(a, b) / vector.norm(a) / vector.norm(b)
    fac = min(max(fac, -1),
              1)  # protect against possible slight numerical errors

    ang = math.acos(fac)
    return angvec2quat(ang, n)
Exemplo n.º 12
0
 def __init__(self, color, pos, points):
     super().__init__(color, pos)
     self.mPoints = []
     self.mPoints3d = []
     for p in points:
         v = p - self.mPos
         self.mPoints.append(v)
         self.mPoints3d.append(vector.Vector3(v.x, v.y, 0))
     self.mArea = vector.cross(self.mPoints3d[1] - self.mPoints3d[0], self.mPoints3d[2] - self.mPoints3d[0]).magnitude / 2.0
Exemplo n.º 13
0
    def update(self, delta, all_boids, attractors, obstacles):
        """This is main function which figures out all the conditions and determine resultant change in the boid location per cycle. It calls all the function given above and find resultant change in location and applies it to the boid."""
        #start with determining nearby boids.
        self.determine_nearby_boids(all_boids)

        # obstacles = self.nearby_obj(objs, _MIN_OBSTACLE_DISTANCE)
        self.nearby_obj(obstacles, _MIN_OBSTACLE_DISTANCE)
        #initializing all the change vectors
        # cohesion_factor = self.average_position()
        # alignment_factor = self.average_velocity()
        # seperation_factor = self.avoid_collisions(self.neighbours, _BOID_COLLISION_DISTANCE)
        p = self.all_in_one()
        cohesion_factor = p[0]
        alignment_factor = p[1]
        seperation_factor = p[2]
        collision_factor = self.avoid_collisions(self.obj_nearby,
                                                 _MIN_OBSTACLE_DISTANCE)
        attraction_factor = self.attraction(attractors)
        bound_factor = self.know_bound()

        self.force_factors = [[_FACTOR_COHESION, cohesion_factor],
                              [_FACTOR_ALIGNMENT, alignment_factor],
                              [_FACTOR_BOID_AVOIDANCE, seperation_factor],
                              [_FACTOR_OBSTACLE_AVOID, collision_factor],
                              [_FACTOR_ATTRACT, attraction_factor],
                              [_FACTOR_BOUND, bound_factor]]
        # print(_FACTOR_COHESION)
        # print(cohesion_factor)
        # y = _FACTOR_COHESION*cohesion_factor[0]
        for x in self.force_factors:
            self.velocity[0] += x[1][0] * x[0]
            self.velocity[1] += x[0] * x[1][1] * 0.2
            self.velocity[2] += x[0] * x[1][2]

        velocity2 = vector.limit_magnitude(self.velocity, _SPEED_MAX,
                                           _SPEED_MIN, True)

        #changing the boids position in accordance with velocity.
        for i in range(0, len(self.position)):
            self.position[i] += delta * velocity2[i]
            # if (self.position[i] > self.bound[i]) or (self.position[i]< -self.bound[i]):
            # 	self.position[i] = -self.position[i]

        # for i in range(3):
        # 	if (self.position[i] > self.bound[i]) or (self.position[i]< -self.bound[i]):
        # 		self.position = vector.limit_magnitude(self.position, self.bound[i], -self.bound[i], True)

        self.force = self.mass * vector.magnitude(
            self.velocity[0] - velocity2[0], self.velocity[1] - velocity2[1],
            self.velocity[2] - velocity2[2]) / (self.dt * 50)
        self.velocity = velocity2
        cross = vector.cross(self.velocity, self.position)
        self.ang_mom = self.mass * vector.magnitude(cross[0], cross[1],
                                                    cross[2]) / 1000
        self.energy = self.mass * (vector.magnitude(
            self.velocity[0], self.velocity[1], self.velocity[2])**2) / 4000000
Exemplo n.º 14
0
 def set(self, verts):
     rightMost = 0
     highestXCoord = verts[0].x
     for i in range(len(verts)):
         x = verts[i].x
         if x > highestXCoord:
             highestXCoord = x
             rightMost = i
         elif x == highestXCoord:
             if verts[i].y < verts[rightMost].y:
                 rightMost = i
     
     hull = [0] * Polygon.MAX_POLY_VERTEX_COUNT
     outCount = 0
     indexHull = rightMost
     nextHullIndex = 0
     
     while True:
         hull[outCount] = indexHull
         nextHullIndex = 0
         for i in range(len(verts)):
             if nextHullIndex == indexHull:
                 nextHullIndex = i
                 continue
             
             e1 = verts[nextHullIndex] - verts[hull[outCount]]
             e2 = verts[i] - verts[hull[outCount]]
             c = cross(e1, e2)
             if  c < 0:
                 nextHullIndex = i
                 
             if c == 0 and e2.lengthSq() > e1.lengthSq():
                 nextHullIndex = i
                 
         outCount += 1
         
         indexHull = nextHullIndex
         
         if nextHullIndex == rightMost:
             self.vertexCount = outCount
             break
     
     #copy vertices into shape's vertices
     #print(verts)
     #print("len verts:", len(verts))
     #print("vertexCount:", self.vertexCount)
     
     for i in range(self.vertexCount):
         #print("i:", i)
         self.vertices.append(Vec2( verts[hull[i]]))
         
     # Compute face normals    
     for i in range(self.vertexCount):
         face = self.vertices[(i+1)%self.vertexCount] - self.vertices[i]
         self.normals.append(Vec2(face.y, -face.x))
         self.normals[i].normalize()
def identity_3(v1, v2, v3):

    #v1 ×(v2 × v3) = (v1 · v3)v2 − (v1 · v2)v3

    v4 = vector.cross(v2, v3)
    v4 = vector.cross(v1, v4) #lhs
    v4 = [round(elm, 10) for elm in v4] #lhs rounded
    s5 = vector.dot(v1, v3)
    s6 = vector.dot(v1, v2)
    s6 = -s6
    v5 = vector.scalar_mult(v2, s5)
    v6 = vector.scalar_mult(v3, s6)
    v7 = vector.sum(v5, v6) #rhs
    v7 = [round(elm, 10) for elm in v7] #rhs rounded

    if v4[0]==v7[0] and v4[1]==v7[1] and v4[2]==v7[2]:
        print('v1 ×(v2 × v3) = (v1 · v3)v2 − (v1 · v2)v3')
    else:
        print('v1 ×(v2 × v3) does not equal (v1 · v3)v2 − (v1 · v2)v3')
Exemplo n.º 16
0
 def intersect(self, ray):
     v1 = ray.orig - self.a
     v2 = self.b - self.a
     v3 = Vector(-ray.dir.y, ray.dir.x)
     if dot(v2, v3) == 0:
         return None
     ts = dot(v1, v3) / dot(v2, v3)
     tr = cross(v2, v1) / dot(v2, v3)
     if tr >= 0.0 and 0.0 <= ts <= 1.0:
         return self.a + v2 * ts
     else:
         return None
Exemplo n.º 17
0
 def intersect(self, ray):
     v1 = ray.orig - self.a
     v2 = self.b - self.a
     v3 = Vector(-ray.dir.y, ray.dir.x)
     if dot(v2, v3) == 0:
         return None
     ts = dot(v1, v3) / dot(v2, v3)
     tr = cross(v2, v1) / dot(v2, v3)
     if tr >= 0.0 and 0.0 <= ts <= 1.0:
         return self.a + v2 * ts
     else:
         return None
Exemplo n.º 18
0
def sss_int(p1, r1, p2, r2, p3, r3):
    """Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively. 
       Returns a list of zero, one or two solution points.
    """
    solutions = []
    # intersect circles in plane
    cp1 = vector.vector([0.0,0.0]) 
    cp2 = vector.vector([vector.norm(p2-p1), 0.0])
    cpxs = cc_int(cp1, r1, cp2, r2)
    if len(cpxs) == 0:
        return []
    # determine normal of plane though p1, p2, p3
    n = vector.cross(p2-p1, p3-p1)
    if not tol_eq(vector.norm(n),0.0):  
        n = n / vector.norm(n)
    else:
        # traingle p1, p2, p3 is degenerate
        # check for 2d solutions
        if len(cpxs) == 0:
            return []
        # project cpxs back to 3d and check radius r3 
        cp4 = cpxs[0]
        u = normalised(p2-p1)
        v,w = perp_3d(u)
        p4 = p1 + cp4[0] * u + cp4[1] * v
        if tol_eq(vector.norm(p4-p3), r3):
            return [p4]
        else:
            return []
    # px, rx, nx is circle 
    px = p1 + (p2-p1) * cpxs[0][0] / vector.norm(p2-p1)
    rx = abs(cpxs[0][1])
    nx = p2-p1
    nx = nx / vector.norm(nx)
    # py is projection of p3 on px,nx
    dy3 = vector.dot(p3-px, nx)
    py = p3 - (nx * dy3)
    if tol_gt(dy3, r3):
        return []
    # ry is radius of circle in py
    if tol_eq(r3,0.0):
        ry = 0.0 
    else:   
        ry = math.sin(math.acos(min(1.0,abs(dy3/r3))))*r3
    # determine intersection of circle px, rx and circle py, ry, projected relative to line py-px 
    cpx = vector.vector([0.0,0.0]) 
    cpy = vector.vector([vector.norm(py-px), 0.0])
    cp4s = cc_int(cpx, rx, cpy, ry)
    for cp4 in cp4s:
        p4 = px + (py-px) * cp4[0] / vector.norm(py-px) + n * cp4[1] 
        solutions.append(p4)  
    return solutions
Exemplo n.º 19
0
def sss_int(p1, r1, p2, r2, p3, r3):
    """Intersect three spheres, centered in p1, p2, p3 with radius r1,r2,r3 respectively. 
       Returns a list of zero, one or two solution points.
    """
    solutions = []
    # intersect circles in plane
    cp1 = vector.vector([0.0, 0.0])
    cp2 = vector.vector([vector.norm(p2 - p1), 0.0])
    cpxs = cc_int(cp1, r1, cp2, r2)
    if len(cpxs) == 0:
        return []
    # determine normal of plane though p1, p2, p3
    n = vector.cross(p2 - p1, p3 - p1)
    if not tol_eq(vector.norm(n), 0.0):
        n = n / vector.norm(n)
    else:
        # traingle p1, p2, p3 is degenerate
        # check for 2d solutions
        if len(cpxs) == 0:
            return []
        # project cpxs back to 3d and check radius r3
        cp4 = cpxs[0]
        u = normalised(p2 - p1)
        v, w = perp_3d(u)
        p4 = p1 + cp4[0] * u + cp4[1] * v
        if tol_eq(vector.norm(p4 - p3), r3):
            return [p4]
        else:
            return []
    # px, rx, nx is circle
    px = p1 + (p2 - p1) * cpxs[0][0] / vector.norm(p2 - p1)
    rx = abs(cpxs[0][1])
    nx = p2 - p1
    nx = nx / vector.norm(nx)
    # py is projection of p3 on px,nx
    dy3 = vector.dot(p3 - px, nx)
    py = p3 - (nx * dy3)
    if tol_gt(dy3, r3):
        return []
    # ry is radius of circle in py
    if tol_eq(r3, 0.0):
        ry = 0.0
    else:
        ry = math.sin(math.acos(min(1.0, abs(dy3 / r3)))) * r3
    # determine intersection of circle px, rx and circle py, ry, projected relative to line py-px
    cpx = vector.vector([0.0, 0.0])
    cpy = vector.vector([vector.norm(py - px), 0.0])
    cp4s = cc_int(cpx, rx, cpy, ry)
    for cp4 in cp4s:
        p4 = px + (py - px) * cp4[0] / vector.norm(py - px) + n * cp4[1]
        solutions.append(p4)
    return solutions
Exemplo n.º 20
0
 def rayTest(self, R):
     """Returns a list of sorted Raycollisions or none if its empty."""
     v = self.point2 - self.origin
     w = self.point1 - self.origin
     l = self.point1 - self.point2
     norm = vector.cross(v, w).normalized()
     P = Plane(self.material, self.origin, norm, 1000, 1000)
     z = P.rayTest(R)
     if z == []:
         return []
     A = (vector.cross(v, w) / 2).magnitude()
     PBC = (vector.cross(z[0].point - self.point2, l) / 2 / A).magnitude()
     PBA = (vector.cross(z[0].point - self.origin, v) / 2 / A).magnitude()
     PAC = (vector.cross(z[0].point - self.origin, w) / 2 / A).magnitude()
     n1 = self.origin_norm * PBC
     n2 = self.point1_norm * PBA
     n3 = self.point2_norm * PAC
     new_norm = (n1 + n2 + n3)
     if PBC + PBA + PAC <= 1.0000001:
         return [Raycollision(R, self, z[0].distance, z[0].point, new_norm)]
     else:
         return []
Exemplo n.º 21
0
 def intersect(self, ray):
     oc = self.center - ray.orig
     # let P be projection of C on the line
     # then l = |OP|, h = |CP|
     l = dot(oc, ray.dir)
     h = cross(oc, ray.dir)
     if self.radius < abs(h):
         return None
     q = sqrt(self.radius ** 2 - h ** 2)
     # find t: o + d * t == intersection
     t = l - q
     if t < 0:
         t = l + q
     if t < 0:
         return None
     else:
         return ray.orig + t * ray.dir
Exemplo n.º 22
0
 def intersect(self, ray):
     oc = self.center - ray.orig
     # let P be projection of C on the line
     # then l = |OP|, h = |CP|
     l = dot(oc, ray.dir)
     h = cross(oc, ray.dir)
     if self.radius < abs(h):
         return None
     q = sqrt(self.radius**2 - h**2)
     # find t: o + d * t == intersection
     t = l - q
     if t < 0:
         t = l + q
     if t < 0:
         return None
     else:
         return ray.orig + t * ray.dir
def main():

    # Create two random vectors

    v1 = [random.random(), random.random(), random.random()]
    v2 = [random.random(), random.random(), random.random()]
    v3 = [random.random(), random.random(), random.random()]

    #line break in output screen, clearer to read

    print()


    # Print out the vectors to terminal window

    print("v1 = ", v1)
    print("v2 = ", v2)
    print("v3 = ", v3)


    #line break in output screen, clearer to read

    print()

    #print basic vector manipulations

    #vector sum, scalar product and vector (cross) product

    print("v1 + v2 =", vector.sum(v1, v2))
    print('v1 · v2 =', vector.dot(v1, v2))
    print('v1 x v2 =', vector.cross(v1, v2))

    #line break in output screen, clearer to read

    print()


    #testing if the vector identites hold

    identity_1(v1, v2)
    identity_2(v1, v2, v3)
    identity_3(v1, v2, v3)
Exemplo n.º 24
0
    def compare_square_normals(self, square, dim):
        """
        Increments the score of two squares based on how parallel their normal
        vectors are.
        :param square:
        :param dim: the dimensions of the image
        :return: nothing
        """

        # Using 1-cross product due to larger change of sin when parallel
        temp_cross = cross(self.normal, square.normal)

        edge = False
        for point in square.corners:
            if point[0][0] < 1 or point[0][0] > dim[1] - 2 or point[0][1] < 1 \
                    or point[0][1] > dim[0] - 2:
                # Contours on the edge don't improve scores
                edge = True

        if not edge:
            # Increment the score
            self.score += 1 - abs(distance(temp_cross) / (distance(square.normal)*distance(self.normal)))
Exemplo n.º 25
0
 def computeMass(self, density, body):
     centroid = Vec2()
     area = 0 
     triangleArea = 0
     inertia = 0
     k_inv6 = 1.0 / 6
     
     #count centroid and inertia
     for i in range(self.vertexCount):
         p1 = self.vertices[i]
         p2 = self.vertices[ (i+1) % self.vertexCount]
         
         D = cross(p1, p2)
         triangleArea = 0.5 * D
         
         area += triangleArea
         #print(area)
         centroid += (p1 * D)
         centroid += (p2 * D)
         
         intx2 = p1.x ** 2 + p2.x * p1.x + p2.x ** 2
         inty2 = p1.y ** 2 + p2.y * p1.y + p2.y ** 2
         inertia += (0.5 * k_inv6 * D) * (intx2+ inty2)
         
     centroid =  centroid * k_inv6 / area
     
     #translate vertices to centroid
     for i in range(self.vertexCount):
         self.vertices[i] -= centroid
         
     body.mass = density * area
     body.invMass = 1 / body.mass if  body.mass else 0
     body.inertia = inertia * density
     body.invInertia = 1 / body.inertia if  body.inertia else 0
     
     self.body = body
Exemplo n.º 26
0
    def compare_square_normals(self, square, dim):
        """
        Increments the score of two squares based on how parallel their normal
        vectors are.
        :param square:
        :param dim: the dimensions of the image
        :return: nothing
        """

        # Using 1-cross product due to larger change of sin when parallel
        temp_cross = cross(self.normal, square.normal)

        edge = False
        for point in square.corners:
            if point[0][0] < 1 or point[0][0] > dim[1] - 2 or point[0][1] < 1 \
                    or point[0][1] > dim[0] - 2:
                # Contours on the edge don't improve scores
                edge = True

        if not edge:
            # Increment the score
            self.score += 1 - abs(
                distance(temp_cross) /
                (distance(square.normal) * distance(self.normal)))
Exemplo n.º 27
0
 def test_vec_cross(self):
     v1 = Vector(1, 2, 3)
     v2 = Vector(4, 5, 6)
     v3 = Vector(-3, 6, -3)
     self.assertEqual(v1 % v2, v3)
     self.assertEqual(cross(v1, v2), v3)
Exemplo n.º 28
0
def distance_to_segment(point, a, b):
    if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0:
        return min(dist(point, a), dist(point, b))
    return abs(cross(point - a, normalize(b - a)))
Exemplo n.º 29
0
 def test_vec_cross(self):
     v1 = Vector( 1, 2,  3)
     v2 = Vector( 4, 5,  6)
     v3 = Vector(-3, 6, -3)
     self.assertEqual(v1 % v2, v3)
     self.assertEqual(cross(v1, v2), v3)
Exemplo n.º 30
0
def perp_3d(u):
    """Returns 2 orthonormal vectors."""
    t = transform_point(u, perp_matrix)
    v = normalised(vector.cross(u,t))
    w = normalised(vector.cross(u,v))
    return (v,w)
Exemplo n.º 31
0
 def test_mul_cross_normal(self):
     n1 = (1., 0,  0)
     n2 = (0,  1., 0)
     n3 = (0,  0,  1.)
     self.assertEqual(cross(n1, n2), n3)
Exemplo n.º 32
0
 def view_up(self):
     return (vector.cross(self.viewing_direction,
                          self.view_left)).normalized
Exemplo n.º 33
0
 def view_left(self):
     return (vector.cross(self._up_vector,
                          self.viewing_direction)).normalized
Exemplo n.º 34
0
 def test_mul_cross(self):
     v1 = [1, 2, 3]
     v2 = [4, 5, 6]
     v3 = [-3, 6, -3]
     self.assertEqual(cross(v1, v2), v3)
Exemplo n.º 35
0
 def test_mul_cross_zero(self):
     n = [0] * 3
     self.assertEqual(cross(n, n), n)
Exemplo n.º 36
0
#!/usr/bin/env python2

import vector
import math

v1 = 1,2,3
v2 = 3,4,5
n1 = vector.normalize(v1)

assert vector.add(v1, v2) == (4, 6, 8)
assert vector.subtract(v1, v2) == (-2, -2, -2)
assert abs(n1[0] - 0.27) < 0.01 and \
       abs(n1[1] - 0.53) < 0.01 and \
       abs(n1[2] - 0.80) < 0.01
assert vector.dot(v1, v2) == 26
assert vector.cross(v1, v2) == (-2, 4, -2)

print "All tests passed!"
Exemplo n.º 37
0
 def test_mul_cross_zero(self):
     n = [0] * 3
     self.assertEqual(cross(n, n), n)
Exemplo n.º 38
0
 def test_mul_cross_normal(self):
     n1 = [1., 0, 0]
     n2 = [0, 1., 0]
     n3 = [0, 0, 1.]
     self.assertEqual(cross(n1, n2), n3)
Exemplo n.º 39
0
def perp_3d(u):
    """Returns 2 orthonormal vectors."""
    t = transform_point(u, perp_matrix)
    v = normalised(vector.cross(u, t))
    w = normalised(vector.cross(u, v))
    return (v, w)
Exemplo n.º 40
0
def square2(p1, p2, p3):
    return vec.cross(p2 - p1, p3 - p1)
Exemplo n.º 41
0
 def test_vec_cross_normal(self):
     n1 = Vector(1., 0,  0)
     n2 = Vector(0,  1., 0)
     n3 = Vector(0,  0,  1.)
     self.assertEqual(n1 % n2, n3)
     self.assertEqual(cross(n1, n2), n3)
Exemplo n.º 42
0
def mul(P, Q):
    w1, v1 = P
    w2, v2 = Q
    return (w1*w2 - _v.dot(v1, v2),_v.sum(_v.mul(w1, v2), _v.mul(w2, v1), _v.cross(v1, v2)))
Exemplo n.º 43
0
	def intersectedArea(self, g, Debug=False):
		if not self.intersect(g):
			return -1

		#--- search the pivot point
		ist = 0
		poly = []
		for i in range(0, self.size()):
			if g.inside(self.pts[i]):
				poly.append(self.pts[ist])
				ist = i
				break

			lseg = lineseg.lineseg(self.pts[i], self.pts[i+1])
			isectd, psect, iseg = g.intersect(lseg)
			if isectd:
				poly.append(psect)
				ist = i
				break

		#--- Walk along boundary of intersected area
		# cag : cell as geometry
		cag = geometry(self.points(False))

		# Loop over the cell sides
		for i in range(ist+1, self.size()+1):
			p = self.pts[i]

			# f: a side of this cell
			f = lineseg.lineseg(self.pts[i-1], self.pts[i])

			# Check if the side f intersect with given geometry g
			#   isectd: boolean if g and f intersected or not
			#	psect : intersected point
			#	iseg  : index of intersected segment of g
			isectd, psect, iseg = g.intersect(f)

			if isectd:
				# Check if psect has been added to poly already
				if not iselem(psect, poly):
					poly.append(psect)

				# Walk along the geometry g to find another intersect point of
				# this cell and g. The geometry points inbetween the two
				# intersected points are added to the poly
				for i in range(iseg+1, g.size()):
					pchk = g.pts[i]
					if cag.inside(pchk) and \
					   not iselem(pchk, poly):
						poly.append(pchk)

			if g.inside(p) and not iselem(p, poly):
				poly.append(p)

		if Debug:
			print('\nPoints in this poly:')
			for l in poly:
				print(l)

		# Estimate the area
		if len(poly) == 3:
			v1 = poly[1] - poly[0]
			v2 = poly[2] - poly[1]
			return 0.5*vector.mag(vector.cross(v1, v2))
		else:
			sumA = 0.0
			for i in range(0, len(poly)-2):
				v1 = poly[i+1] - poly[0]
				v2 = poly[i+2] - poly[0]
				sumA += 0.5*vector.mag(vector.cross(v1, v2))

			return sumA
Exemplo n.º 44
0
#!/usr/bin/env python2

import vector
import math

v1 = [36.886, 53.177, 21.887]
v2 = [38.323, 52.817, 21.996]
v3 = [38.493, 51.553, 22.830]
v4 = [39.483, 50.748, 22.463]

n1 = vector.normalize(v1)
n2 = vector.normalize(v2)

v1 = n1 = 1,2,3
v2 = n2 = 3,4,5

print vector.add(v1, v2)
print vector.subtract(v1, v2)
print vector.normalize(v1)
print vector.dot(n1, n2)
print vector.cross(n1, n2)
print vector.torsion(v1, v2, v3, v4)
Exemplo n.º 45
0
 def test_vec_cross_zero(self):
     n = Vector(0, 0, 0)
     self.assertEqual(n % n, n)
     self.assertEqual(cross(n, n), n)
Exemplo n.º 46
0
 def test_mul_cross(self):
     v1 = [ 1, 2,  3]
     v2 = [ 4, 5,  6]
     v3 = [-3, 6, -3]
     self.assertEqual(cross(v1, v2), v3)
Exemplo n.º 47
0
 def applyImpulse(self, impulse, contactVector):
     self.velocity += impulse * self.invMass
     self.angularVelocity += self.invInertia * cross(contactVector, impulse)
Exemplo n.º 48
0
def distance_to_segment(point, a, b):
    if dot(b - a, point - a) <= 0 or dot(a - b, point - b) <= 0:
        return min(dist(point, a), dist(point, b))
    return abs(cross(point - a, normalize(b - a)))
Exemplo n.º 49
0
def FitPointsCompass(debug, points, current, norm):
    # ensure current and norm are float
    current = lmap(float, current)
    norm = lmap(float, norm)

    zpoints = [[], [], [], [], [], []]
    for i in range(6):
        zpoints[i] = lmap(lambda x : x[i], points)
        
    # determine if we have 0D, 1D, 2D, or 3D set of points
    point_fit, point_dev, point_max_dev = PointFit(points)
    if point_max_dev < 9:
        debug('0d fit, insufficient data %.1f %.1f < 9' % (point_dev, point_max_dev))
        return False

    line, plane = LinearFit(points)
    line_fit, line_dev, line_max_dev = line
    plane_fit, plane_dev, plane_max_dev = plane

    # initial guess average min and max for bias, and average range for radius
    minc = [1000, 1000, 1000]
    maxc = [-1000, -1000, -1000]
    for p in points:
        minc = lmap(min, p[:3], minc)
        maxc = lmap(max, p[:3], maxc)

    guess = lmap(lambda a, b : (a+b)/2, minc, maxc)
    diff = lmap(lambda a, b : b-a, minc, maxc)
    guess.append((diff[0]+diff[1]+diff[2])/3)
    #debug('initial guess', guess)

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
    initial.append(current[3])
    #debug('initial 1d fit', initial)

    # attempt 'normal' fit along normal vector
    '''
    def f_sphere1(beta, x):
        bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
        return r0
    sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
    if not sphere1d_fit or sphere1d_fit[1] < 0:
        print('FitLeastSq sphere1d failed!!!! ', len(points))
        return False
    sphere1d_fit = lmap(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
    debug('sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit))
    '''

    def f_new_sphere1(beta, x):
        bias = lmap(lambda x, n: x + beta[0]*n, initial[:3], norm)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[1] - vector.norm(y), m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = 1 # weight deviation
        def dip(y, z):
            n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
            return n
        r1 = lmap(lambda y, z : fac*beta[1]*(beta[2]-dip(y, z)), m, g)
        return r0 + r1
    new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints, debug, 2)
    if not new_sphere1d_fit or new_sphere1d_fit[1] < 0 or abs(new_sphere1d_fit[2]) > 1:
        debug('FitLeastSq new_sphere1 failed!!!! ', len(points), new_sphere1d_fit)
        new_sphere1d_fit = current
    else:
        new_sphere1d_fit = lmap(lambda x, a: x + new_sphere1d_fit[0]*a, initial[:3], norm) + [new_sphere1d_fit[1], math.degrees(math.asin(new_sphere1d_fit[2]))]
    new_sphere1d_fit = [new_sphere1d_fit, ComputeDeviation(points, new_sphere1d_fit), 1]
        #print('new sphere1 fit', new_sphere1d_fit)

    if line_max_dev < 2:
        debug('line fit found, insufficient data %.1f %.1f' % (line_dev, line_max_dev))
        return False
    
    # 2d sphere fit across normal vector
    u = vector.cross(norm, [norm[1]-norm[2], norm[2]-norm[0], norm[0]-norm[1]])
    v = vector.cross(norm, u)
    u = vector.normalize(u)
    v = vector.normalize(v)

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
    initial.append(current[3])
    #debug('initial 2d fit', initial)
    
    '''
    def f_sphere2(beta, x):
        bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
        return r0
    sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
    if not sphere2d_fit or sphere2d_fit[2] < 0:
        print('FitLeastSq sphere2d failed!!!! ', len(points))
        new_sphere2d_fit = initial
    else:
        sphere2d_fit = lmap(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
    debug('sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit))
    '''

    def f_new_sphere2(beta, x):
        bias = lmap(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[2] - vector.norm(y), m)
        #r0 = lmap(lambda y : 1 - vector.norm(y)/beta[2], m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = 1 # weight deviation
        def dip(y, z):
            n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
            return n
        r1 = lmap(lambda y, z : fac*beta[2]*(beta[3]-dip(y, z)), m, g)
        return r0 + r1
    new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2, zpoints, debug, 2)
    if not new_sphere2d_fit or new_sphere2d_fit[2] < 0 or abs(new_sphere2d_fit[3]) >= 1:
        debug('FitLeastSq sphere2 failed!!!! ', len(points), new_sphere2d_fit)
        return False
    new_sphere2d_fit = lmap(lambda x, a, b: x + new_sphere2d_fit[0]*a + new_sphere2d_fit[1]*b, initial[:3], u, v) + [new_sphere2d_fit[2], math.degrees(math.asin(new_sphere2d_fit[3]))]
    new_sphere2d_fit = [new_sphere2d_fit, ComputeDeviation(points, new_sphere2d_fit), 2]

    if plane_max_dev < 1.2:
        ang = math.degrees(math.asin(vector.norm(vector.cross(plane_fit[1], norm))))
        
        debug('plane fit found, 2D fit only', ang, plane_fit, plane_dev, plane_max_dev)
        if ang > 30:
            debug('angle of plane not aligned to normal: no 2d fit')
            new_sphere2d_fit = False

        return [new_sphere1d_fit, new_sphere2d_fit, False]

    # ok to use best guess for 3d fit
    initial = guess
    '''
    def f_sphere3(beta, x):
        bias = beta[:3]
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
        return r0
    sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
    if not sphere3d_fit or sphere3d_fit[3] < 0:
        print('FitLeastSq sphere failed!!!! ', len(points))
        return False
    debug('sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit))
    '''
    def f_new_sphere3(beta, x):
        b = numpy.matrix(lmap(lambda a, b : a - b, x[:3], beta[:3]))
        m = list(numpy.array(b.transpose()))
        r0 = lmap(lambda y : beta[3] - vector.norm(y), m)
        #r0 = lmap(lambda y : 1 - vector.norm(y)/beta[3], m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = 1 # weight deviation
        def dip(y, z):
            n = min(max(vector.dot(y, z)/vector.norm(y), -1), 1)
            return n
        r1 = lmap(lambda y, z : fac*beta[3]*(beta[4]-dip(y, z)), m, g)

        return r0 + r1
    new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, debug, 2)
    if not new_sphere3d_fit or new_sphere3d_fit[3] < 0 or abs(new_sphere3d_fit[4]) >= 1:
        debug('FitLeastSq sphere3 failed!!!! ', len(points))
        return False
    new_sphere3d_fit[4] = math.degrees(math.asin(new_sphere3d_fit[4]))
    new_sphere3d_fit = [new_sphere3d_fit, ComputeDeviation(points, new_sphere3d_fit), 3]
    #debug('new sphere3 fit', new_sphere3d_fit)
    
    return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
Exemplo n.º 50
0
def FitPoints(points, current, norm):
    if len(points) < 5:
        return False

    # ensure current and norm are float
    current = map(float, current)
    norm = map(float, norm)

    zpoints = [[], [], [], [], [], []]
    for i in range(6):
        zpoints[i] = map(lambda x: x[i], points)

    # determine if we have 0D, 1D, 2D, or 3D set of points
    point_fit, point_dev, point_max_dev = PointFit(points)
    if point_max_dev < 9:
        if debug:
            print '0d fit, insufficient data', point_dev, point_max_dev, '< 9'
        return False

    line, plane = LinearFit(points)
    line_fit, line_dev, line_max_dev = line
    plane_fit, plane_dev, plane_max_dev = plane

    # initial guess average min and max for bias, and average range for radius
    minc = [1000, 1000, 1000]
    maxc = [-1000, -1000, -1000]
    for p in points:
        minc = map(min, p[:3], minc)
        maxc = map(max, p[:3], maxc)

    guess = map(lambda a, b: (a + b) / 2, minc, maxc)
    diff = map(lambda a, b: b - a, minc, maxc)
    guess.append((diff[0] + diff[1] + diff[2]) / 3)
    if debug:
        print 'initial guess', guess

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(
        current[:3], vector.project(vector.sub(guess[:3], current[:3]), norm))
    initial.append(current[3])
    if debug:
        print 'initial 1d fit', initial

    # attempt 'normal' fit along normal vector
    '''
    def f_sphere1(beta, x):
        bias = map(lambda x, n: x + beta[0]*n, initial[:3], norm)
        b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y : beta[1] - vector.norm(y), m)
        return r0
    sphere1d_fit = FitLeastSq([0, initial[3]], f_sphere1, zpoints)
    if not sphere1d_fit or sphere1d_fit[1] < 0:
        print 'FitLeastSq sphere1d failed!!!! ', len(points)
        return False
    sphere1d_fit = map(lambda x, n: x + sphere1d_fit[0]*n, initial[:3], norm) + [sphere1d_fit[1]]
    if debug:
        print 'sphere1 fit', sphere1d_fit, ComputeDeviation(points, sphere1d_fit)
    '''
    def f_new_sphere1(beta, x):
        bias = map(lambda x, n: x + beta[0] * n, initial[:3], norm)
        b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y: beta[1] - vector.norm(y), m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = .03  # weight deviation as 1 degree ~ .03 mag
        r1 = map(
            lambda y, z: fac * beta[1] * (beta[2] - math.degrees(
                math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
        return r0 + r1

    new_sphere1d_fit = FitLeastSq([0, initial[3], 0], f_new_sphere1, zpoints,
                                  2)
    if not new_sphere1d_fit or new_sphere1d_fit[1] < 0:
        if debug:
            print 'FitLeastSq new_sphere1 failed!!!! ', len(points)
        new_sphere1d_fit = current
    else:
        new_sphere1d_fit = map(lambda x, a: x + new_sphere1d_fit[0] * a,
                               initial[:3], norm) + new_sphere1d_fit[1:]
    new_sphere1d_fit = [
        new_sphere1d_fit,
        ComputeDeviation(points, new_sphere1d_fit), 1
    ]
    #print 'new sphere1 fit', new_sphere1d_fit

    if line_max_dev < 3:
        if debug:
            print 'line fit found, insufficient data', line_dev, line_max_dev
        return [new_sphere1d_fit, False, False]

    # 2d sphere fit across normal vector
    u = vector.cross(norm,
                     [norm[1] - norm[2], norm[2] - norm[0], norm[0] - norm[1]])
    v = vector.cross(norm, u)
    u = vector.normalize(u)
    v = vector.normalize(v)

    # initial is the closest to guess on the uv plane containing current
    initial = vector.add(
        guess[:3], vector.project(vector.sub(current[:3], guess[:3]), norm))
    initial.append(current[3])
    if debug:
        print 'initial 2d fit', initial
    '''
    def f_sphere2(beta, x):
        bias = map(lambda x, a, b: x + beta[0]*a + beta[1]*b, initial[:3], u, v)
        b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y : beta[2] - vector.norm(y), m)
        return r0
    sphere2d_fit = FitLeastSq([0, 0, initial[3]], f_sphere2, zpoints)
    if not sphere2d_fit or sphere2d_fit[2] < 0:
        print 'FitLeastSq sphere2d failed!!!! ', len(points)
        new_sphere2d_fit = initial
    else:
        sphere2d_fit = map(lambda x, a, b: x + sphere2d_fit[0]*a + sphere2d_fit[1]*b, initial[:3], u, v) + [sphere2d_fit[2]]
    if debug:
        print 'sphere2 fit', sphere2d_fit, ComputeDeviation(points, sphere2d_fit)
    '''

    def f_new_sphere2(beta, x):
        bias = map(lambda x, a, b: x + beta[0] * a + beta[1] * b, initial[:3],
                   u, v)
        b = numpy.matrix(map(lambda a, b: a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y: beta[2] - vector.norm(y), m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = .03  # weight deviation as 1 degree ~ .03 mag
        r1 = map(
            lambda y, z: fac * beta[2] * (beta[3] - math.degrees(
                math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
        return r0 + r1

    new_sphere2d_fit = FitLeastSq([0, 0, initial[3], 0], f_new_sphere2,
                                  zpoints, 2)
    if not new_sphere2d_fit or new_sphere2d_fit[2] < 0:
        if debug:
            print 'FitLeastSq sphere failed!!!! ', len(points)
        return False

    new_sphere2d_fit = map(
        lambda x, a, b: x + new_sphere2d_fit[0] * a + new_sphere2d_fit[1] * b,
        initial[:3], u, v) + new_sphere2d_fit[2:]
    new_sphere2d_fit = [
        new_sphere2d_fit,
        ComputeDeviation(points, new_sphere2d_fit), 2
    ]
    #print 'new sphere2 fit', new_sphere2d_fit

    if plane_max_dev < 1.2:
        if debug:
            print 'plane fit found, 2D fit only', plane_fit, plane_dev, plane_max_dev
        return [new_sphere1d_fit, new_sphere2d_fit, False]

    # ok to use best guess for 3d fit
    initial = guess
    '''
    def f_sphere3(beta, x):
        bias = beta[:3]
        b = numpy.matrix(map(lambda a, b : a - b, x[:3], bias))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y : beta[3] - vector.norm(y), m)
        return r0
    sphere3d_fit = FitLeastSq(initial[:4], f_sphere3, zpoints)
    if not sphere3d_fit or sphere3d_fit[3] < 0:
        print 'FitLeastSq sphere failed!!!! ', len(points)
        return False
    if debug:
        print 'sphere3 fit', sphere3d_fit, ComputeDeviation(points, sphere3d_fit)
    '''

    def f_new_sphere3(beta, x):
        b = numpy.matrix(map(lambda a, b: a - b, x[:3], beta[:3]))
        m = list(numpy.array(b.transpose()))
        r0 = map(lambda y: beta[3] - vector.norm(y), m)
        g = list(numpy.array(numpy.matrix(x[3:]).transpose()))
        fac = .03  # weight deviation as 1 degree ~ .03 mag
        r1 = map(
            lambda y, z: fac * beta[3] * (beta[4] - math.degrees(
                math.asin(vector.dot(y, z) / vector.norm(y)))), m, g)
        return r0 + r1

    new_sphere3d_fit = FitLeastSq(initial[:4] + [0], f_new_sphere3, zpoints, 2)
    if not new_sphere3d_fit or new_sphere3d_fit[3] < 0:
        if debug:
            print 'FitLeastSq sphere failed!!!! ', len(points)
        return False
    new_sphere3d_fit = [
        new_sphere3d_fit,
        ComputeDeviation(points, new_sphere3d_fit), 3
    ]
    #print 'new sphere3 fit', new_sphere3d_fit

    return [new_sphere1d_fit, new_sphere2d_fit, new_sphere3d_fit]
Exemplo n.º 51
0
 def test_mul_cross(self):
     v1 = ( 1, 2,  3)
     v2 = ( 4, 5,  6)
     v3 = (-3, 6, -3)
     self.assertEqual(cross(v1, v2), v3)
Exemplo n.º 52
0
 def test_mul_cross_normal(self):
     n1 = [1., 0,  0]
     n2 = [0,  1., 0]
     n3 = [0,  0,  1.]
     self.assertEqual(cross(n1, n2), n3)
Exemplo n.º 53
0
 def test_mul_cross_zero(self):
     n = (0, 0, 0)
     self.assertEqual(cross(n, n), n)
Exemplo n.º 54
0
 def test_vec_cross_normal(self):
     n1 = Vector(1., 0, 0)
     n2 = Vector(0, 1., 0)
     n3 = Vector(0, 0, 1.)
     self.assertEqual(n1 % n2, n3)
     self.assertEqual(cross(n1, n2), n3)