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 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)
