void InitPlaneByVecs(vector<CP_LineSegment3D> &vRefPlane, const CP_Vector3D *pVec0, const CP_Vector3D *pVec1 )
{
CP_Vector3D vecDiagonal = *pVec0 + *pVec1;
CP_Point3D pts[4] = {CP_Point3D(0, 0, 0),
CP_Point3D(pVec0->m_x, pVec0->m_y, pVec0->m_z),
CP_Point3D(vecDiagonal.m_x, vecDiagonal.m_y, vecDiagonal.m_z),
CP_Point3D(pVec1->m_x, pVec1->m_y, pVec1->m_z)
};
for (int i = 0; i < 4; ++i) {
vRefPlane.push_back(CP_LineSegment3D(pts[i], pts[(i+1)%4]));
}
}
CP_Point3D operator * (const CP_Point3D& p, const CMatrix4& mat)
{
double x, y, z, w;
const double *mt = &mat.mat[0][0];
x = p.m_x*mt[0] + p.m_y*mt[4] + p.m_z*mt[8] + mt[12];
y = p.m_x*mt[1] + p.m_y*mt[5] + p.m_z*mt[9] + mt[13];
z = p.m_x*mt[2] + p.m_y*mt[6] + p.m_z*mt[10] + mt[14];
w = p.m_x*mt[3] + p.m_y*mt[7] + p.m_z*mt[11] + mt[15];
if (fabs (w) < TOLER)
return CP_Point3D (0, 0, 0);
else
return CP_Point3D (x/w, y/w, z/w);
}
void cf_bodyCylinderCreate(CP_Body& b, double cx, double cy, double cz, double h, double r)
{
CP_Face* tf;
CP_Edge* te;
CP_Vertex* tv1, *tv0;
CP_Plane* plane;
CP_LineSegment3D* lineSegment;
CP_Point3D cp1, cp0, origin, cp00;
CP_Vector3D vx, vy;
CP_Circle * circle;
//两个顶点
cp0 = CP_Point3D(cx + r, cy, cz);
cp1 = CP_Point3D(cx + r, cy, cz + h);
tv0 = new CP_Vertex(&cp0);
b.mf_addVertex(tv0);
tv1 = new CP_Vertex(&cp1);
b.mf_addVertex(tv1);
//下底圆心
origin = CP_Point3D(cx, cy, cz);
//上底圆心
cp00 = CP_Point3D(cx, cy, cz+h);
//3条边
vx = cp0 - origin;
vy = CP_Point3D(cx, cy+r, cz) - origin;
CP_Circle3D * circle0 = new CP_Circle3D(origin, vx, vy, r);
tv0 = (CP_Vertex *)b.mf_getVertex(0);
tv1= (CP_Vertex *)b.mf_getVertex(0);
te = new CP_Edge(circle0, tv0, tv1);
b.mf_addEdge(te);
tv0->mf_addAdjacentEdge(te);
tv1->mf_addAdjacentEdge(te);
CP_Circle3D * circle1 = new CP_Circle3D(cp00, vx, vy, r);
tv0 = (CP_Vertex *)b.mf_getVertex(1);
tv1 = (CP_Vertex *)b.mf_getVertex(1);
te = new CP_Edge(circle1, tv0, tv1);
b.mf_addEdge(te);
tv0->mf_addAdjacentEdge(te);
tv1->mf_addAdjacentEdge(te);
lineSegment=new CP_LineSegment3D(cp0, cp1);
tv0 = (CP_Vertex *)b.mf_getVertex(0);
tv1 = (CP_Vertex *)b.mf_getVertex(1);
te=new CP_Edge(lineSegment, tv0, tv1);
b.mf_addEdge(te);
tv0->mf_addAdjacentEdge(te);
tv1->mf_addAdjacentEdge(te);
//3个面
//下底面
//CP_Circle3D(const CP_Point3D& c, const CP_Vector3D& vx, const CP_Vector3D& vy, double r);
CP_Circle3D lowCircle = CP_Circle3D(origin, vx, vy, r);
circle = new CP_Circle(origin, vx, vy, r);
tf = new CP_Face(&b, circle, true);
b.mf_addFace(tf);
CP_Loop * loop = new CP_Loop(tf);
tf->mf_addLoop(loop);
CP_Circle2D* circle2D = new CP_Circle2D();
CP_Edge* edge = b.mf_getEdge(0);
CP_Coedge* coedge = new CP_Coedge(circle2D, loop, edge, false);
loop->mf_addCoedge(coedge);
edge->mf_addCoedge(coedge);
//上底面
CP_Circle3D upCircle = CP_Circle3D(cp00, vx, vy, r);
circle = new CP_Circle(cp00, vx, vy, r);
tf = new CP_Face(&b, circle, true);
b.mf_addFace(tf);
loop = new CP_Loop(tf);
tf->mf_addLoop(loop);
circle2D = new CP_Circle2D();
edge = b.mf_getEdge(1);
coedge = new CP_Coedge(circle2D, loop, edge, true);
loop->mf_addCoedge(coedge);
edge->mf_addCoedge(coedge);
//圆柱面
CP_Cylinder* cylinder = new CP_Cylinder(origin, vx, vy, r, h);
tf = new CP_Face(&b, cylinder, true);
b.mf_addFace(tf);
cf_bodycylinderCreate_loop(b, *loop);
} // 函数cf_bodyCylinderCreate结束
