def test_gp_Quaternion(self): ''' Test Interpolate method of qp_QuaternionSLerp. This method takes a by ref parameter q. ''' vX = gp_Vec(12, 0, 0) vY = gp_Vec(0, 12, 0) v45 = (gp_Vec(1, 1, 1).Normalized() * 12) q = gp_Quaternion() q1 = gp_Quaternion(vX, vX) q2 = gp_Quaternion(vX, vY) interp = gp_QuaternionSLerp(q1, q2) interp.Init(q1, q2) for i in range(10): i__ = i / 10. interp.Interpolate(i__, q) if i == 0: self.assertEqual(q.X(), 0.) self.assertEqual(q.Y(), 0.) self.assertEqual(q.Z(), 0.) self.assertEqual(q.W(), 1.) else: self.assertEqual(q.X(), 0.) self.assertEqual(q.Y(), 0.) assert q.Z() > 0. assert q.W() < 1.
def axisAndAngle(vec1, alpha1, vec2, alpha2, front=False): q1 = gp_Quaternion(gp_Vec(*vec1), alpha1) if front: q = q1 else: q2 = gp_Quaternion(gp_Vec(*vec2), alpha2) q = q2 * q1 axis = gp_Vec(0.0, 0.0, 0.0) angle = gp_Quaternion.GetVectorAndAngle(q, axis) return (axis, angle)
def rotate(event=None): display.EraseAll() origin = gp_Vec(0, 0, 0) origin_pt = as_pnt(origin) vX = gp_Vec(12, 0, 0) vY = gp_Vec(0, 12, 0) vZ = gp_Vec(0, 0, 12) v45 = (gp_Vec(1, 1, 1).Normalized() * 12) q1 = gp_Quaternion(vX, vY) p1 = as_pnt(origin + vX) p2 = as_pnt(origin + vY) p3 = as_pnt(origin + (q1 * vY)) p4 = as_pnt(origin + (q1 * v45)) # RED e1 = make_edge(origin_pt, p1) e2 = make_edge(origin_pt, p2) e3 = make_edge(origin_pt, as_pnt(v45)) # GREEN -> transformed e4 = make_edge(origin_pt, p3) e5 = make_edge(origin_pt, p4) display.DisplayShape([e1, e2, e3]) display.DisplayColoredShape([e4, e5], 'GREEN') #display.DisplayMessage(p1, 'e1') #display.DisplayMessage(p2, 'e2') #display.DisplayMessage(v45.as_pnt(), 'e3') #display.DisplayMessage(p3, 'q1*vY') #display.DisplayMessage(p4, 'q1*v45') display.DisplayVector((q1 * vY).Normalized(), as_pnt(origin + q1 * vY / 2.)) display.DisplayVector((q1 * v45).Normalized(), as_pnt(origin + q1 * v45 / 2.)) display.FitAll()
def x3d_display(shape, vertex_shader=None, fragment_shader=None, export_edges=True, color=(1, 1, 0), specular_color=(1, 1, 1), shininess=0.4, transparency=0.4, line_color=(0, 0, 0), line_width=2., mesh_quality=.3): exporter = X3DExporter(shape, vertex_shader, fragment_shader, export_edges, color, specular_color, shininess, transparency, line_color, line_width, mesh_quality) exporter.compute() x3d_str = exporter.to_x3dfile_string() x3d_str = '\n'.join(x3d_str.splitlines()[N_HEADER_LINES:]) bb = BoundBox._fromTopoDS(shape) d = max(bb.xlen, bb.ylen, bb.zlen) c = bb.center vec = gp_Vec(0, 0, d / 1.5 / tan(FOV / 2)) quat = gp_Quaternion(*ROT) vec = quat * (vec) + c.wrapped return add_x3d_boilerplate(x3d_str, d=(vec.X(), vec.Y(), vec.Z()), center=(c.x, c.y, c.z))
def viewpoint(axis, angle, center, dist): q = gp_Quaternion(axis, angle) viewDir = q.Multiply(gp_Vec(0, 0, 1)) viewpoint = center.wrapped + (viewDir * dist) return { "viewpoint": gp_Vec2list(viewpoint), "axis": gp_Vec2list(axis), "angle": angle }
def interpolate(event=None): display.EraseAll() origin = gp_Vec() vX = gp_Vec(12, 0, 0) vY = gp_Vec(0, 12, 0) v45 = (gp_Vec(1, 1, 1).Normalized() * 12) q = gp_Quaternion() interp = gp_QuaternionSLerp(gp_Quaternion(vX, vX), gp_Quaternion(vX, vY)) for i in frange(0, 1.0, 0.01): interp.Interpolate(i, q) # displace the white edges a little from the origin so not to obstruct the other edges v = gp_Vec(0, -24*i, 0) q_v_ = q * v45 p = gp_Pnt((q_v_ + v).XYZ()) v__as_pnt = gp_Pnt((origin + v).XYZ()) e = make_edge(v__as_pnt, p) display.DisplayColoredShape(e, 'WHITE') msg = 'v45->q1*v45 @{0}'.format(i / 10.) #display.DisplayMessage(p, msg) display.FitAll()
def interpolate(event=None): display.EraseAll() origin = gp_Vec() vX = gp_Vec(12, 0, 0) vY = gp_Vec(0, 12, 0) v45 = (gp_Vec(1, 1, 1).Normalized() * 12) q = gp_Quaternion() interp = gp_QuaternionSLerp(gp_Quaternion(vX, vX), gp_Quaternion(vX, vY)) for i in frange(0, 1.0, 0.01): interp.Interpolate(i, q) # displace the white edges a little from the origin so not to obstruct the other edges v = gp_Vec(0, -24 * i, 0) q_v_ = q * v45 p = gp_Pnt((q_v_ + v).XYZ()) v__as_pnt = gp_Pnt((origin + v).XYZ()) e = make_edge(v__as_pnt, p) display.DisplayColoredShape(e, 'WHITE') msg = 'v45->q1*v45 @{0}'.format(i / 10.) display.DisplayMessage(p, msg) display.FitAll()
def vstep(step_str): step = int(step_str) positions = dpos_data[nbobjs*step:nbobjs*step+nbobjs, 2:] builder = BRep_Builder() comp = TopoDS_Compound() builder.MakeCompound(comp) for _id in range(positions.shape[0]): q0, q1, q2, q3, q4, q5, q6 = [float(x) for x in positions[_id,:]] obj = obj_by_id[_id+1] q = Quaternion((q3, q4, q5, q6)) for shape_name, avatar in zip(io.instances()[obj], avatars(obj)): offset = get_offset(obj, shape_name) p = q.rotate(offset[0]) r = q*Quaternion(offset[1]) tr = gp_Trsf() qocc = gp_Quaternion(r[1], r[2], r[3], r[0]) tr.SetRotation(qocc) xyz = gp_XYZ(q0 + p[0], q1 + p[1], q2 + p[2]) vec = gp_Vec(xyz) tr.SetTranslationPart(vec) loc = TopLoc_Location(tr) display.Context.SetLocation(avatar, loc) moved_shape = BRepBuilderAPI_Transform(avatar.GetObject().Shape(), tr, True).Shape() builder.Add(comp, moved_shape) display.Context.UpdateCurrentViewer() write_step((step_str, comp))
def vstep(step_str): step = int(step_str) positions = dpos_data[nbobjs * step:nbobjs * step + nbobjs, 2:] builder = BRep_Builder() comp = TopoDS_Compound() builder.MakeCompound(comp) for _id in range(positions.shape[0]): q0, q1, q2, q3, q4, q5, q6 = [float(x) for x in positions[_id, :]] obj = obj_by_id[_id + 1] q = Quaternion((q3, q4, q5, q6)) for shape_name, avatar in zip(io.instances()[obj], avatars(obj)): offset = get_offset(obj, shape_name) p = q.rotate(offset[0]) r = q * Quaternion(offset[1]) tr = gp_Trsf() qocc = gp_Quaternion(r[1], r[2], r[3], r[0]) tr.SetRotation(qocc) xyz = gp_XYZ(q0 + p[0], q1 + p[1], q2 + p[2]) vec = gp_Vec(xyz) tr.SetTranslationPart(vec) loc = TopLoc_Location(tr) display.Context.SetLocation(avatar, loc) moved_shape = BRepBuilderAPI_Transform( avatar.GetObject().Shape(), tr, True).Shape() builder.Add(comp, moved_shape) display.Context.UpdateCurrentViewer() write_step((step_str, comp))
def rotate(event=None): display.EraseAll() origin = gp_Vec(0, 0, 0) origin_pt = as_pnt(origin) vX = gp_Vec(12, 0, 0) vY = gp_Vec(0, 12, 0) vZ = gp_Vec(0, 0, 12) v45 = (gp_Vec(1, 1, 1).Normalized() * 12) q1 = gp_Quaternion(vX, vY) p1 = as_pnt(origin + vX) p2 = as_pnt(origin + vY) p3 = as_pnt(origin + (q1 * vY)) p4 = as_pnt(origin + (q1 * v45)) # RED e1 = make_edge(origin_pt, p1) e2 = make_edge(origin_pt, p2) e3 = make_edge(origin_pt, as_pnt(v45)) # GREEN -> transformed e4 = make_edge(origin_pt, p3) e5 = make_edge(origin_pt, p4) display.DisplayShape([e1, e2, e3]) display.DisplayColoredShape([e4, e5], 'GREEN') display.DisplayMessage(p1, 'e1') display.DisplayMessage(p2, 'e2') display.DisplayMessage(as_pnt(v45), 'e3') display.DisplayMessage(p3, 'q1*vY') display.DisplayMessage(p4, 'q1*v45') display.DisplayVector((q1 * vY).Normalized(), as_pnt(origin + q1 * vY / 2.)) display.DisplayVector((q1 * v45).Normalized(), as_pnt(origin + q1 * v45 / 2.)) display.FitAll()
def align_mutiHoles(shp, matched_hole_pairs): # matching points pntsA = [i[0].projLocation for i in matched_hole_pairs] pntsB = [i[1].projLocation for i in matched_hole_pairs] normal = gp_Vec(matched_hole_pairs[0][1].direction) pntsA_noDuplicates = list(set(pntsA)) pntsB_noDuplicates = list(set(pntsB)) if len(pntsA_noDuplicates) != len(pntsA) or len(pntsB_noDuplicates) != len( pntsB): logging.warn( "[WARN] There are duplicated pairs(i.e. [A,B], [B, A]) in the given matched_hole_pairs" ) assert len(pntsA_noDuplicates) == len( pntsB_noDuplicates), "[Error] data are not pairwise" assert len(pntsA_noDuplicates) >= 2 or len( pntsB_noDuplicates ) >= 2, "[FATEL] input hole pairs should be more than 2" # Create the 3rd point on the project plane in the perpandicular diretion from the 1st to the 2nd point # [TODO] More test # Now 3rd point was added in the middle, so there will be sysmetry problem? if (len(pntsA_noDuplicates) == 2 and len(pntsB_noDuplicates) == 2): logging.warn( "[WARN] only 2 hole pairs found, add a dummpy pair to constrain rotating along the normal project plane" ) # distance between two points, which is used as a scale dist = pntsA_noDuplicates[0].Distance(pntsA_noDuplicates[1]) / 2.0 vecA = gp_Vec(pntsA_noDuplicates[0], pntsA_noDuplicates[1]) # Translation vector from orignal mid-points to the 3rd point(dummy points) translateVecA = vecA.Crossed(normal).Normalized().Scaled(dist) dummyPntA = centerOfMass_pnts(pntsA_noDuplicates).Translated( translateVecA) pntsA_noDuplicates.append(dummyPntA) vecB = gp_Vec(pntsB_noDuplicates[0], pntsB_noDuplicates[1]) translateVecB = vecB.Crossed(normal).Normalized().Scaled(dist) dummyPntB = centerOfMass_pnts(pntsB_noDuplicates).Translated( translateVecB) pntsB_noDuplicates.append(dummyPntB) # center of Mass cenA = centerOfMass_pnts(pntsA_noDuplicates) cenB = centerOfMass_pnts(pntsB_noDuplicates) # translation vector from centerOfMass to origin mvVecA2O = gp_Vec(cenA, gp_Pnt(0, 0, 0)) mvVecB2O = gp_Vec(cenB, gp_Pnt(0, 0, 0)) # translate all points, with rigid movement to move solid's center of Mass to origin newPntsA = [i.Translated(mvVecA2O) for i in pntsA_noDuplicates] newPntsB = [i.Translated(mvVecB2O) for i in pntsB_noDuplicates] if newPntsB[1].X() * newPntsA[1].X() < 0: ttmp = newPntsB[0] newPntsB[0] = newPntsB[1] newPntsB[1] = ttmp # matching 2 points set by SVD, transform from B to A npMatA = [gpVec2npMat((gp_Vec(gp_Pnt(0, 0, 0), i))) for i in newPntsA] npMatB = [gpVec2npMat(gp_Vec(gp_Pnt(0, 0, 0), i)) for i in newPntsB] varMat = np.asmatrix(np.zeros((3, 3))) for i in range(0, len(npMatA)): varMat += npMatB[i] * npMatA[i].transpose() linAlg = np.linalg U, s, Vh = linAlg.svd(varMat, full_matrices=True) R = Vh.transpose() * U.transpose() # if R has determinant -1, then R is a rotation plus a reflection if linAlg.det(R) < 0: # [ToDo] Don't know why third column or row should multiply -1 reverseMat = np.matrix([(1, 0, 0), (0, 1, 0), (0, 0, -1)]) logging.info('det(R) is < 0, change the sign of last column of Vh') R = reverseMat * (Vh.transpose()) * U.transpose() # q = quaternion_from_matrix(R) R = np.array(R) RgpMat = gp_Mat(R[0][0], R[0][1], R[0][2], R[1][0], R[1][1], R[1][2], R[2][0], R[2][1], R[2][2]) # RgpMat = gp_Mat(gp_XYZ(R[0][0], R[1][0], R[2][0]), gp_XYZ(R[0][1], R[1][1], R[2][1]), gp_XYZ(R[0][2], R[1][2], R[2][2])) q = gp_Quaternion(RgpMat) # gp_Extrinsic_XYZ = 2 # q.GetEulerAngles(2) # qq = gp_Quaternion() # qq.SetVectorAndAngle(gp_Vec(gp_Dir(gp_XYZ(0,0,1))), q.GetEulerAngles(2)[2]) trsf = gp_Trsf() trsf.SetTranslation(mvVecB2O) toploc = TopLoc_Location(trsf) shp.Move(toploc) trsf = gp_Trsf() trsf.SetRotation(q) toploc = TopLoc_Location(trsf) shp.Move(toploc) trsf = gp_Trsf() trsf.SetTranslation(mvVecA2O.Reversed()) toploc = TopLoc_Location(trsf) shp.Move(toploc)