void
bn_wrt_point_direc(mat_t out, const mat_t change, const mat_t in, const point_t point, const vect_t direc, const struct bn_tol *tol)
{
static mat_t t1;
static mat_t pt_to_origin, origin_to_pt;
static mat_t d_to_zaxis, zaxis_to_d;
static vect_t zaxis;
/* build "point to origin" matrix */
MAT_IDN(pt_to_origin);
MAT_DELTAS_VEC_NEG(pt_to_origin, point);
/* build "origin to point" matrix */
MAT_IDN(origin_to_pt);
MAT_DELTAS_VEC_NEG(origin_to_pt, point);
/* build "direc to zaxis" matrix */
VSET(zaxis, 0.0, 0.0, 1.0);
bn_mat_fromto(d_to_zaxis, direc, zaxis, tol);
/* build "zaxis to direc" matrix */
bn_mat_inv(zaxis_to_d, d_to_zaxis);
/* apply change matrix...
* t1 = change * d_to_zaxis * pt_to_origin * in
*/
bn_mat_mul4(t1, change, d_to_zaxis, pt_to_origin, in);
/* apply origin_to_pt matrix:
* out = origin_to_pt * zaxis_to_d *
* change * d_to_zaxis * pt_to_origin * in
*/
bn_mat_mul3(out, origin_to_pt, zaxis_to_d, t1);
}
static int
test_bn_mat_mul4(int argc, char *argv[])
{
mat_t m1, m2, m3, m4, expected, actual;
if (argc != 7) {
bu_exit(1, "<args> format: M1 M2 M3 M4 <expected_result> [%s]\n", argv[0]);
}
scan_mat_args(argv, 2, &m1);
scan_mat_args(argv, 3, &m2);
scan_mat_args(argv, 4, &m3);
scan_mat_args(argv, 5, &m4);
scan_mat_args(argv, 6, &expected);
bn_mat_mul4(actual, m1, m2, m3, m4);
return !mat_equal(expected, actual);
}
extern "C" void
rt_revolve_brep(ON_Brep **b, const struct rt_db_internal *ip, const struct bn_tol *tol)
{
struct rt_db_internal *tmp_internal;
struct rt_revolve_internal *rip;
struct rt_sketch_internal *eip;
BU_ALLOC(tmp_internal, struct rt_db_internal);
RT_DB_INTERNAL_INIT(tmp_internal);
rip = (struct rt_revolve_internal *)ip->idb_ptr;
RT_REVOLVE_CK_MAGIC(rip);
eip = rip->skt;
RT_SKETCH_CK_MAGIC(eip);
ON_3dPoint plane_origin;
ON_3dVector plane_x_dir, plane_y_dir;
bool full_revolve = true;
if (rip->ang < 2*ON_PI && rip->ang > 0)
full_revolve = false;
// Find plane in 3 space corresponding to the sketch.
vect_t startpoint;
VADD2(startpoint, rip->v3d, rip->r);
plane_origin = ON_3dPoint(startpoint);
plane_x_dir = ON_3dVector(eip->u_vec);
plane_y_dir = ON_3dVector(eip->v_vec);
const ON_Plane sketch_plane = ON_Plane(plane_origin, plane_x_dir, plane_y_dir);
// For the brep, need the list of 3D vertex points. In sketch, they
// are stored as 2D coordinates, so use the sketch_plane to define 3 space
// points for the vertices.
for (size_t i = 0; i < eip->vert_count; i++) {
(*b)->NewVertex(sketch_plane.PointAt(eip->verts[i][0], eip->verts[i][1]), 0.0);
}
// Create the brep elements corresponding to the sketch lines, curves
// and bezier segments. Create 2d, 3d and BrepEdge elements for each segment.
// Will need to use the bboxes of each element to
// build the overall bounding box for the face. Use bGrowBox to expand
// a single box.
struct line_seg *lsg;
struct carc_seg *csg;
struct bezier_seg *bsg;
uint32_t *lng;
for (size_t i = 0; i < (&eip->curve)->count; i++) {
lng = (uint32_t *)(&eip->curve)->segment[i];
switch (*lng) {
case CURVE_LSEG_MAGIC: {
lsg = (struct line_seg *)lng;
ON_Curve* lsg3d = new ON_LineCurve((*b)->m_V[lsg->start].Point(), (*b)->m_V[lsg->end].Point());
lsg3d->SetDomain(0.0, 1.0);
(*b)->m_C3.Append(lsg3d);
}
break;
case CURVE_CARC_MAGIC:
csg = (struct carc_seg *)lng;
if (csg->radius < 0) { {
ON_3dPoint cntrpt = (*b)->m_V[csg->end].Point();
ON_3dPoint edgept = (*b)->m_V[csg->start].Point();
ON_Plane cplane = ON_Plane(cntrpt, plane_x_dir, plane_y_dir);
ON_Circle c3dcirc = ON_Circle(cplane, cntrpt.DistanceTo(edgept));
ON_Curve* c3d = new ON_ArcCurve((const ON_Circle)c3dcirc);
c3d->SetDomain(0.0, 1.0);
(*b)->m_C3.Append(c3d);
}
} else {
// need to calculated 3rd point on arc - look to sketch.c around line 581 for
// logic
}
break;
case CURVE_BEZIER_MAGIC:
bsg = (struct bezier_seg *)lng;
{
ON_3dPointArray bezpoints = ON_3dPointArray(bsg->degree + 1);
for (int j = 0; j < bsg->degree + 1; j++) {
bezpoints.Append((*b)->m_V[bsg->ctl_points[j]].Point());
}
ON_BezierCurve bez3d = ON_BezierCurve((const ON_3dPointArray)bezpoints);
ON_NurbsCurve* beznurb3d = ON_NurbsCurve::New();
bez3d.GetNurbForm(*beznurb3d);
beznurb3d->SetDomain(0.0, 1.0);
(*b)->m_C3.Append(beznurb3d);
}
break;
default:
bu_log("Unhandled sketch object\n");
break;
}
}
vect_t endpoint;
VADD2(endpoint, rip->v3d, rip->axis3d);
const ON_Line& revaxis = ON_Line(ON_3dPoint(rip->v3d), ON_3dPoint(endpoint));
FindLoops(b, &revaxis, rip->ang);
// Create the two boundary surfaces, if it's not a full revolution
if (!full_revolve) {
// First, deduce the transformation matrices to calculate the position of the end surface
// The transformation matrices are to rotate an arbitrary point around an arbitrary axis
// Let the point A = (x, y, z), the rotation axis is p1p2 = (x2,y2,z2)-(x1,y1,z1) = (a,b,c)
// Then T1 is to translate p1 to the origin
// Rx is to rotate p1p2 around the X axis to the plane XOZ
// Ry is to rotate p1p2 around the Y axis to be coincident to Z axis
// Rz is to rotate A with the angle around Z axis (the new p1p2)
// RxInv, RyInv, T1Inv are the inverse transformation of Rx, Ry, T1, respectively.
// The whole transformation is A' = A*T1*Rx*Ry*Rz*Ry*Inv*Rx*Inv = A*R
vect_t end_plane_origin, end_plane_x_dir, end_plane_y_dir;
mat_t R;
MAT_IDN(R);
mat_t T1, Rx, Ry, Rz, RxInv, RyInv, T1Inv;
MAT_IDN(T1);
VSET(&T1[12], -rip->v3d[0], -rip->v3d[1], -rip->v3d[2]);
MAT_IDN(Rx);
fastf_t v = sqrt(rip->axis3d[1]*rip->axis3d[1]+rip->axis3d[2]*rip->axis3d[2]);
VSET(&Rx[4], 0, rip->axis3d[2]/v, rip->axis3d[1]/v);
VSET(&Rx[8], 0, -rip->axis3d[1]/v, rip->axis3d[2]/v);
MAT_IDN(Ry);
fastf_t u = MAGNITUDE(rip->axis3d);
VSET(&Ry[0], v/u, 0, -rip->axis3d[0]/u);
VSET(&Ry[8], rip->axis3d[0]/u, 0, v/u);
MAT_IDN(Rz);
fastf_t C, S;
C = cos(rip->ang);
S = sin(rip->ang);
VSET(&Rz[0], C, S, 0);
VSET(&Rz[4], -S, C, 0);
bn_mat_inv(RxInv, Rx);
bn_mat_inv(RyInv, Ry);
bn_mat_inv(T1Inv, T1);
mat_t temp;
bn_mat_mul4(temp, T1, Rx, Ry, Rz);
bn_mat_mul4(R, temp, RyInv, RxInv, T1Inv);
VEC3X3MAT(end_plane_origin, plane_origin, R);
VADD2(end_plane_origin, end_plane_origin, &R[12]);
VEC3X3MAT(end_plane_x_dir, plane_x_dir, R);
VEC3X3MAT(end_plane_y_dir, plane_y_dir, R);
// Create the start and end surface with rt_sketch_brep()
struct rt_sketch_internal sketch;
sketch = *(rip->skt);
ON_Brep *b1 = ON_Brep::New();
VMOVE(sketch.V, plane_origin);
VMOVE(sketch.u_vec, plane_x_dir);
VMOVE(sketch.v_vec, plane_y_dir);
tmp_internal->idb_ptr = (void *)(&sketch);
rt_sketch_brep(&b1, tmp_internal, tol);
(*b)->Append(*b1->Duplicate());
ON_Brep *b2 = ON_Brep::New();
VMOVE(sketch.V, end_plane_origin);
VMOVE(sketch.u_vec, end_plane_x_dir);
VMOVE(sketch.v_vec, end_plane_y_dir);
tmp_internal->idb_ptr = (void *)(&sketch);
rt_sketch_brep(&b2, tmp_internal, tol);
(*b)->Append(*b2->Duplicate());
(*b)->FlipFace((*b)->m_F[(*b)->m_F.Count()-1]);
}
bu_free(tmp_internal, "free temporary rt_db_internal");
}
