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 HGear::GetGripperPositions(std::list<GripData> *list, bool just_for_endof){
gp_Pnt o = m_pos.Location();
double acting_radius = (double)(m_module * m_num_teeth)/2;
gp_Pnt px(o.XYZ() + m_pos.XDirection().XYZ() * acting_radius);
gp_Pnt py(o.XYZ() + m_pos.YDirection().XYZ() * acting_radius);
gp_Dir z_dir = m_pos.XDirection() ^ m_pos.YDirection();
gp_Pnt pz(o.XYZ() + z_dir.XYZ() * acting_radius);
gp_Pnt pxyz(o.XYZ() + m_pos.XDirection().XYZ() * acting_radius + m_pos.YDirection().XYZ() * acting_radius + z_dir.XYZ() * acting_radius);
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));
}
