Ejemplo n.º 1
0
    def make_outline(self):
        left = []
        right = []
        for i in xrange(len(self.path)):
            s1l = None
            s1r = None
            s2l = None
            s2r = None
            if i > 0:
                d = vectorops.unit(
                    vectorops.sub(self.path[i], self.path[i - 1]))
                ofs = (self.gutter * d[1], -self.gutter * d[0])
                s1l = (d, ofs)
                s1r = (d, (-ofs[0], -ofs[1]))
            if i + 1 < len(self.path):
                d = vectorops.unit(
                    vectorops.sub(self.path[i + 1], self.path[i]))
                ofs = (self.gutter * d[1], -self.gutter * d[0])
                s2l = (d, ofs)
                s2r = (d, (-ofs[0], -ofs[1]))
            if i == 0:
                left.append(tuple(vectorops.add(self.path[i], s2l[1])))
                right.append(tuple(vectorops.add(self.path[i], s2r[1])))
            elif i + 1 == len(self.path):
                left.append(tuple(vectorops.add(self.path[i], s1l[1])))
                right.append(tuple(vectorops.add(self.path[i], s1r[1])))
            else:
                #figure out intersections
                if vectorops.dot(s1r[0], s2r[1]) < 0:
                    #inner junction, find line intersection
                    right.append(
                        tuple(
                            vectorops.add(
                                self.path[i],
                                ray_ray_intersection(s1r[1], s1r[0], s2r[1],
                                                     s2r[0]))))
                else:
                    #outer junction
                    right.append(tuple(vectorops.add(self.path[i], s1r[1])))
                    right.append(tuple(vectorops.add(self.path[i], s2r[1])))
                if vectorops.dot(s1l[0], s2l[1]) < 0:
                    #inner junction, find line intersection
                    left.append(
                        tuple(
                            vectorops.add(
                                self.path[i],
                                ray_ray_intersection(s1l[1], s1l[0], s2l[1],
                                                     s2l[0]))))
                else:
                    #outer junction
                    left.append(tuple(vectorops.add(self.path[i], s1l[1])))
                    left.append(tuple(vectorops.add(self.path[i], s2l[1])))

        self.outline = left + list(reversed(right))
Ejemplo n.º 2
0
def sample():
    """Returns a uniformly distributed rotation matrix."""
    import random
    q = [random.gauss(0,1),random.gauss(0,1),random.gauss(0,1),random.gauss(0,1)]
    q = vectorops.unit(q)
    theta = math.acos(q[3])*2.0
    if abs(theta) < 1e-8:
        m = [0,0,0]
    else:
        m = vectorops.mul(vectorops.unit(q[0:3]),theta)
    return from_moment(m)
Ejemplo n.º 3
0
def sample():
    """Returns a uniformly distributed rotation matrix."""
    import random
    q = [
        random.gauss(0, 1),
        random.gauss(0, 1),
        random.gauss(0, 1),
        random.gauss(0, 1)
    ]
    q = vectorops.unit(q)
    theta = math.acos(q[3]) * 2.0
    if abs(theta) < 1e-8:
        m = [0, 0, 0]
    else:
        m = vectorops.mul(vectorops.unit(q[0:3]), theta)
    return from_moment(m)
Ejemplo n.º 4
0
def vector_rotation(v1,v2):
    """Finds the minimal-angle matrix that rotates v1 to v2.  v1 and v2
    are assumed to be nonzero"""
    a1 = vectorops.unit(v1)
    a2 = vectorops.unit(v2)
    cp = vectorops.cross(a1,a2)
    dp = vectorops.dot(a1,a2)
    if abs(vectorops.norm(cp)) < 1e-4:
        if dp < 0:
            R0 = canonical(a1)
            #return a rotation 180 degrees about the canonical y axis
            return rotation(R0[3:6],math.pi)
        else:
            return identity()
    else:
        angle = math.acos(max(min(dp,1.0),-1.0))
        axis = vectorops.mul(cp,1.0/vectorops.norm(cp))
        return rotation(axis,angle)
Ejemplo n.º 5
0
def vector_rotation(v1, v2):
    """Finds the minimal-angle matrix that rotates v1 to v2.  v1 and v2
    are assumed to be nonzero"""
    a1 = vectorops.unit(v1)
    a2 = vectorops.unit(v2)
    cp = vectorops.cross(a1, a2)
    dp = vectorops.dot(a1, a2)
    if abs(vectorops.norm(cp)) < 1e-4:
        if dp < 0:
            R0 = canonical(a1)
            #return a rotation 180 degrees about the canonical y axis
            return rotation(R0[3:6], math.pi)
        else:
            return identity()
    else:
        angle = math.acos(max(min(dp, 1.0), -1.0))
        axis = vectorops.mul(cp, 1.0 / vectorops.norm(cp))
        return rotation(axis, angle)
Ejemplo n.º 6
0
def quaternion(R):
    """Given a Klamp't rotation representation, produces the corresponding
    unit quaternion (w,x,y,z)."""
    tr = trace(R) + 1.0
    a11, a21, a31, a12, a22, a32, a13, a23, a33 = R

    #If the trace is nonzero, it's a nondegenerate rotation
    if tr > 1e-5:
        s = math.sqrt(tr)
        w = s * 0.5
        s = 0.5 / s
        x = (a32 - a23) * s
        y = (a13 - a31) * s
        z = (a21 - a12) * s
        return vectorops.unit((w, x, y, z))
    else:
        #degenerate it's a rotation of 180 degrees
        nxt = [1, 2, 0]
        #check for largest diagonal entry
        i = 0
        if a22 > a11: i = 1
        if a33 > max(a11, a22): i = 2
        j = nxt[i]
        k = nxt[j]
        M = matrix(R)

        q = [0.0] * 4
        s = math.sqrt((M[i][i] - (M[j][j] + M[k][k])) + 1.0)
        q[i] = s * 0.5

        if abs(s) < 1e-7:
            raise ValueError(
                "Could not solve for quaternion... Invalid rotation matrix?")
        else:
            s = 0.5 / s
            q[3] = (M[k][j] - M[j][k]) * s
            q[j] = (M[i][j] + M[j][i]) * s
            q[k] = (M[i][k] + M[i][k]) * s
        w, x, y, z = q[3], q[0], q[1], q[2]
        return vectorops.unit([w, x, y, z])
Ejemplo n.º 7
0
def quaternion(R):
    """Given a Klamp't rotation representation, produces the corresponding
    unit quaternion (w,x,y,z)."""
    tr = trace(R) + 1.0;
    a11,a21,a31,a12,a22,a32,a13,a23,a33 = R

    #If the trace is nonzero, it's a nondegenerate rotation
    if tr > 1e-5:
        s = math.sqrt(tr)
        w = s * 0.5
        s = 0.5 / s
        x = (a32 - a23) * s
        y = (a13 - a31) * s
        z = (a21 - a12) * s
        return vectorops.unit((w,x,y,z))
    else:
        #degenerate it's a rotation of 180 degrees
        nxt = [1, 2, 0]
        #check for largest diagonal entry
        i = 0
        if a22 > a11: i = 1
        if a33 > max(a11,a22): i = 2
        j = nxt[i]
        k = nxt[j]
        M = matrix(R)

        q = [0.0]*4
        s = math.sqrt((M[i][i] - (M[j][j] + M[k][k])) + 1.0);
        q[i] = s * 0.5
    
        if abs(s)<1e-7:
            raise ValueError("Could not solve for quaternion... Invalid rotation matrix?")
        else:
            s = 0.5 / s;
            q[3] = (M[k][j] - M[j][k]) * s;
            q[j] = (M[i][j] + M[j][i]) * s;
            q[k] = (M[i][k] + M[i][k]) * s;
        w,x,y,z = q[3],q[0],q[1],q[2]
        return vectorops.unit([w,x,y,z])
Ejemplo n.º 8
0
def axis_angle(R):
    """Returns the (axis,angle) pair representing R"""
    m = moment(R)
    return (vectorops.unit(m),vectorops.norm(m))
Ejemplo n.º 9
0
def axis_angle(R):
    """Returns the (axis,angle) pair representing R"""
    m = moment(R)
    return (vectorops.unit(m), vectorops.norm(m))
Ejemplo n.º 10
0
def expmap_from_euler(ai, aj, ak, axes="sxyz"):
    matrix = transformations.euler_matrix(ai, aj, ak, axes)
    R = so3.from_matrix(matrix)
    moment = so3.moment(R)
    axis_angle_parameters = vectorops.unit(moment) + [vectorops.norm(moment)]
    return axis_angle_parameters