void CCuboid::GetGripperPositions(std::list<GripData> *list, bool just_for_endof)
{
gp_Pnt o = m_pos.Location();
gp_Pnt px(o.XYZ() + m_pos.XDirection().XYZ() * m_x);
gp_Pnt py(o.XYZ() + m_pos.YDirection().XYZ() * m_y);
gp_Dir z_dir = m_pos.XDirection() ^ m_pos.YDirection();
gp_Pnt pz(o.XYZ() + z_dir.XYZ() * m_z);
gp_Pnt m2(o.XYZ() + m_pos.XDirection().XYZ() * m_x + m_pos.YDirection().XYZ() * m_y/2);
gp_Pnt m3(o.XYZ() + m_pos.XDirection().XYZ() * m_x/2 + m_pos.YDirection().XYZ() * m_y);
gp_Pnt m8(o.XYZ() + m_pos.YDirection().XYZ() * m_y/2 + z_dir.XYZ() * m_z);
gp_Pnt pxy(o.XYZ() + m_pos.XDirection().XYZ() * m_x + m_pos.YDirection().XYZ() * m_y);
gp_Pnt pxz(o.XYZ() + m_pos.XDirection().XYZ() * m_x + z_dir.XYZ() * m_z);
gp_Pnt pyz(o.XYZ() + m_pos.YDirection().XYZ() * m_y + z_dir.XYZ() * m_z);
gp_Pnt pxyz(o.XYZ() + m_pos.XDirection().XYZ() * m_x + m_pos.YDirection().XYZ() * m_y + z_dir.XYZ() * m_z);
list->push_back(GripData(GripperTypeTranslate,o.X(),o.Y(),o.Z(),NULL));
list->push_back(GripData(GripperTypeRotateObject,px.X(),px.Y(),px.Z(),NULL));
list->push_back(GripData(GripperTypeRotateObject,py.X(),py.Y(),py.Z(),NULL));
list->push_back(GripData(GripperTypeRotateObject,pz.X(),pz.Y(),pz.Z(),NULL));
list->push_back(GripData(GripperTypeScale,pxyz.X(),pxyz.Y(),pxyz.Z(),NULL));
list->push_back(GripData(GripperTypeRotate,pxy.X(),pxy.Y(),pxy.Z(),NULL));
list->push_back(GripData(GripperTypeRotate,pxz.X(),pxz.Y(),pxz.Z(),NULL));
list->push_back(GripData(GripperTypeRotate,pyz.X(),pyz.Y(),pyz.Z(),NULL));
list->push_back(GripData(GripperTypeObjectScaleX,m2.X(),m2.Y(),m2.Z(),NULL));
list->push_back(GripData(GripperTypeObjectScaleY,m3.X(),m3.Y(),m3.Z(),NULL));
list->push_back(GripData(GripperTypeObjectScaleZ,m8.X(),m8.Y(),m8.Z(),NULL));
}
void CCylinder::GetGripperPositions(std::list<GripData> *list, bool just_for_endof)
{
gp_Pnt o = m_pos.Location();
gp_Pnt px(o.XYZ() + m_pos.XDirection().XYZ() * m_radius);
gp_Dir z_dir = m_pos.XDirection() ^ m_pos.YDirection();
gp_Pnt pyz(o.XYZ() + m_pos.YDirection().XYZ() * m_radius + z_dir.XYZ() * m_height);
gp_Pnt pmxz(o.XYZ() + m_pos.XDirection().XYZ() * (-m_radius) + z_dir.XYZ() * m_height);
gp_Pnt pz(o.XYZ() + z_dir.XYZ() * m_height);
list->push_back(GripData(GripperTypeTranslate,o.X(),o.Y(),o.Z(),NULL));
list->push_back(GripData(GripperTypeObjectScaleXY,px.X(),px.Y(),px.Z(),NULL));
list->push_back(GripData(GripperTypeRotateObject,pyz.X(),pyz.Y(),pyz.Z(),NULL));
list->push_back(GripData(GripperTypeRotateObject,pmxz.X(),pmxz.Y(),pmxz.Z(),NULL));
list->push_back(GripData(GripperTypeObjectScaleZ,pz.X(),pz.Y(),pz.Z(),NULL));
}
bool CCone::Stretch2(const double *p, const double* shift, gp_Ax2& new_pos, double& new_r1, double& new_r2, double& new_height)
{
gp_Pnt vp = make_point(p);
gp_Vec vshift = make_vector(shift);
gp_Pnt o = m_pos.Location();
gp_Pnt px(o.XYZ() + m_pos.XDirection().XYZ() * m_r1);
gp_Dir z_dir = m_pos.XDirection() ^ m_pos.YDirection();
gp_Pnt py(o.XYZ() + m_pos.YDirection().XYZ() * m_r1);
gp_Pnt pyz(o.XYZ() + m_pos.YDirection().XYZ() * m_r2 + z_dir.XYZ() * m_height);
gp_Pnt pmxz(o.XYZ() + m_pos.XDirection().XYZ() * (-m_r2) + z_dir.XYZ() * m_height);
gp_Pnt pz(o.XYZ() + z_dir.XYZ() * m_height);
gp_Pnt pxz(o.XYZ() + m_pos.XDirection().XYZ() * m_r2 + z_dir.XYZ() * m_height);
bool make_a_new_cone = false;
if(px.IsEqual(vp, wxGetApp().m_geom_tol)){
px = px.XYZ() + vshift.XYZ();
double new_x = gp_Vec(px.XYZ()) * gp_Vec(m_pos.XDirection()) - gp_Vec(o.XYZ()) * gp_Vec(m_pos.XDirection());
double new_y = gp_Vec(px.XYZ()) * gp_Vec(m_pos.YDirection()) - gp_Vec(o.XYZ()) * gp_Vec(m_pos.YDirection());
new_r1 = sqrt(new_x * new_x + new_y * new_y);
if(fabs(new_r1 - m_r2) > 0.000000001){
make_a_new_cone = true;
}
}
else if(pxz.IsEqual(vp, wxGetApp().m_geom_tol)){
pxz = pxz.XYZ() + vshift.XYZ();
double new_x = gp_Vec(pxz.XYZ()) * gp_Vec(m_pos.XDirection()) - gp_Vec(o.XYZ()) * gp_Vec(m_pos.XDirection());
double new_y = gp_Vec(pxz.XYZ()) * gp_Vec(m_pos.YDirection()) - gp_Vec(o.XYZ()) * gp_Vec(m_pos.YDirection());
new_r2 = sqrt(new_x * new_x + new_y * new_y);
if(fabs(m_r1 - new_r2) > 0.000000001){
make_a_new_cone = true;
}
}
else if(pz.IsEqual(vp, wxGetApp().m_geom_tol)){
pz = pz.XYZ() + vshift.XYZ();
new_height = gp_Vec(pz.XYZ()) * gp_Vec(z_dir) - gp_Vec(o.XYZ()) * gp_Vec(z_dir);
if(new_height > 0){
make_a_new_cone = true;
}
}
return make_a_new_cone;
}
