void CMeshSurfSphereDlg::onApply(void) { //Delete old object m_pGLPreviewWin->releaseObject(); //Check input parameters if (m_fR<=0) return; if (m_nArcDiv<2) m_nArcDiv=2; int nzone1, nzone2; FREAL zoneangle1, zoneangle2; GET_ANGULAR_SECTIONS(m_fA1, m_fA2, 360, nzone1, zoneangle1); GET_ANGULAR_SECTIONS(m_fB1, m_fB2, 180, nzone2, zoneangle2); zoneangle2 = 90; if (nzone1==0 || nzone2==0) return; int nArcDiv=m_nArcDiv/nzone1; if (nArcDiv<2) nArcDiv=2; //Create the first quater cyl object CQuadObj *pobj; const FREAL a1=m_fA1; const FREAL a2=a1+zoneangle1; FREAL b1; if (m_fB1<90) b1=0; else b1=90; const FREAL b2=b1+zoneangle2; CQuadObj *p1; if (b1==0) p1=meshQuarterSphere(m_fR, a1, a2, b1, b2, nArcDiv); else{ p1=meshQuarterSphere(m_fR, a1, a2, 0, 90, nArcDiv); p1->mirrorXYPlane(); } if (nzone2==2){ CQuadObj *p2 = new CQuadObj(*p1); CQuadObj *p3 = new CQuadObj; assert(p2!=NULL && p3!=NULL); p2->mirrorXYPlane(); MergeQuadObj(*p1, *p2, *p3); delete p1; delete p2; p1 = p3; } //Make copies of the first part if (p1==NULL) return; pobj = p1; for (int i=1; i<nzone1; i++){ CQuadObj *pobj0=pobj; CQuadObj *pdup = new CQuadObj(*p1); pobj = new CQuadObj; FAST_ROTATE_Z_AXIS(pdup->m_pVertex, pdup->m_nVertexCount, zoneangle1); MergeQuadObj(*pobj0, *pdup, *pobj); delete p1; p1 = pdup; } if (pobj!=p1) delete p1; //Dont forget to translate m_vCenter; if (!(m_vCenter==Vector3d(0,0,0))){ const double tx = m_vCenter.x; const double ty = m_vCenter.y; const double tz = m_vCenter.z; pobj->applyTranslation(tx, ty, tz); } //Set bounding box AxisAlignedBox box; pobj->ComputeBoundingBox(box.minp, box.maxp); pobj->SetBoundingBox(box); char namebuff[256]; sprintf(namebuff, "Sphere%d", 0); pobj->SetObjectName(namebuff); //Set the new object for the preview window m_pGLPreviewWin->setObject(pobj); m_pGLPreviewWin->updateGL(); }
void VPythonFile::draw_axes(const double shaft_width) { draw_arrow(Vector3d(0.0), Vector3d(1.0, 0.0, 0.0), "red", shaft_width); draw_arrow(Vector3d(0.0), Vector3d(0.0, 1.0, 0.0), "green", shaft_width); draw_arrow(Vector3d(0.0), Vector3d(0.0, 0.0, 1.0), "blue", shaft_width); }
sphereSubzoneToQuadObj(3, obj3, nx3, tmpObj2); MergeQuadObj(tmpObj1, tmpObj2, *pQuarterObj); } return pQuarterObj; } //==================================================================== int CMeshSurfSphereDlg::m_nArcDiv = 2; double CMeshSurfSphereDlg::m_fR = 0.5f; double CMeshSurfSphereDlg::m_fA1 = 0; double CMeshSurfSphereDlg::m_fA2 = 90; double CMeshSurfSphereDlg::m_fB1 = 0; double CMeshSurfSphereDlg::m_fB2 = 90; Vector3d CMeshSurfSphereDlg::m_vCenter = Vector3d(0,0,0); CMeshSurfSphereDlg::CMeshSurfSphereDlg(QWidget* parent, const char* name): CMeshPrimitiveDialog(parent, name, false, Qt::WStyle_Tool) { setCaption(name); QValueList<int> slist; slist.append(80); slist.append(200); m_pSplitter->setSizes(slist); //generate the symbol table prepareNameTable(); appendToListview(); m_pGLPreviewWin->setNameTable(m_table, m_nLen, 0); //0: brick type
Vector3d Vector3d::crossProduct( Vector3d v ) { return Vector3d( this->y * v.z - v.y * this->z, this->z * v.x - v.z * this->x, this->x * v.y - v.x * this->y ); }
/** *@brief 更新 * @param st 刻み幅 */ void RobotArm::update(double st) { if(pauseFalg || stopFalg || !serbo) return; //std::cout << targetPoint.end_time << "\t" << targetPoint.time << std::endl; if(targetPoint.end_time < targetPoint.time) { if(targetPoint.type == Point) { Vector3d pos = calcKinematics(theta); double dPx = Kp*(targetPoint.target_pos(0)-pos(0)); double dPy = Kp*(targetPoint.target_pos(1)-pos(1)); double dPz = Kp*(targetPoint.target_pos(2)-pos(2)); Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz)); updatePos(dthe(0), dthe(1), dthe(2), 0); theta[3] = targetPoint.target_theta; } else { for(int i=0;i < 4;i++) { theta[i] = targetPoint.target_joint_pos[i]; } if (!judgeSoftLimitJoint(theta))stop(); } setTargetPos(); return; } else { if(targetPoint.type == Point) { double td = calcVel(targetPoint.target_theta, targetPoint.start_theta, targetPoint.end_time, targetPoint.time, theta[3]); dt = st; MinTime = dt; double dx = targetPoint.target_pos(0)-targetPoint.start_pos(0); double dy = targetPoint.target_pos(1)-targetPoint.start_pos(1); double dz = targetPoint.target_pos(2)-targetPoint.start_pos(2); double ST = sqrt(dx*dx+dy*dy+dz*dz); if(ST < 0.001) { updatePos(0, 0, 0, td); targetPoint.time += dt; return; } double A = 2*M_PI*ST/(targetPoint.end_time*targetPoint.end_time); double s = A*targetPoint.end_time/(2*M_PI)*(targetPoint.time - targetPoint.end_time/(2*M_PI)*sin(2*M_PI/targetPoint.end_time*targetPoint.time)); double Px = s*dx/ST + targetPoint.start_pos(0); double Py = s*dy/ST + targetPoint.start_pos(1); double Pz = s*dz/ST + targetPoint.start_pos(2); double ds = A*targetPoint.end_time/(2*M_PI)*(1 - cos(2*M_PI/targetPoint.end_time*targetPoint.time)); Vector3d pos = calcKinematics(theta); double dPx = ds*dx/ST + Kp*(Px-pos(0)); double dPy = ds*dy/ST + Kp*(Py-pos(1)); double dPz = ds*dz/ST + Kp*(Pz-pos(2)); //ofs << ds << "\t" << s << std::endl; //std::cout << pos << std::endl; Vector3d dthe = calcJointVel(Vector3d(dPx, dPy, dPz)); updatePos(dthe(0), dthe(1), dthe(2), td); } else { double dthe[4]; for(int i=0;i < 4;i++) { dthe[i] = calcVel(targetPoint.target_joint_pos[i], targetPoint.start_joint_pos[i], targetPoint.end_time, targetPoint.time, theta[i]); } updatePos(dthe[0], dthe[1], dthe[2], dthe[3]); } targetPoint.time += dt; } }
/** *@brief サインスマート製4自由度ロボットアーム制御クラスのコンストラクタ */ RobotArm::RobotArm() { jl = new Vector3d[4]; pl = new Vector3d[4]; l[0] = ArmLength0; l[1] = ArmLength1; l[2] = ArmLength2; l[3] = ArmLength3; lh = HandLength; lf = FingerLength; m[0] = ArmMath0; m[1] = ArmMath1; m[2] = ArmMath2; m[3] = ArmMath3; mh = HandMath; mf = FingerMath; wi = ArmWidth; wf = FingerWidth; hi = ArmHeight; hf = FingerHeight; rh = HandRadius; jl[0](0) = 0; jl[0](1) = 0; jl[0](2) = 0; pl[0](0) = jl[0](0); pl[0](1) = jl[0](1); pl[0](2) = jl[0](2)+l[0]/2; jl[1](0) = pl[0](0); jl[1](1) = pl[0](1); jl[1](2) = pl[0](2)+l[0]/2; pl[1](0) = jl[1](0); pl[1](1) = jl[1](1); pl[1](2) = jl[1](2)+l[1]/2; jl[2](0) = pl[1](0); jl[2](1) = pl[1](1); jl[2](2) = pl[1](2)+l[1]/2; pl[2](0) = jl[2](0); pl[2](1) = jl[2](1); pl[2](2) = jl[2](2)+l[2]/2; jl[3](0) = pl[2](0); jl[3](1) = pl[2](1); jl[3](2) = pl[2](2)+l[2]/2; pl[3](0) = jl[3](0); pl[3](1) = jl[3](1)+l[3]/2; pl[3](2) = jl[3](2); jh(0) = pl[3](0); jh(1) = pl[3](1)+l[3]/2; jh(2) = pl[3](2); ph(0) = jh(0); //pyh = jyh+lh/2; ph(1) = jh(1); ph(2) = jh(2); jf(0) = ph(0); //jyf = pyh+lh/2; jf(1) = ph(1); jf(2) = ph(2); pf(0) = jf(0); pf(1) = jf(1); pf(2) = jf(2)-lf/2; hw = 0.02; setAngle(0, 0, 0, 0); dt = 0.01; //endTime = -1; //time = 0; /*targetPoint(0) = 0; targetPoint(1) = 0; targetPoint(2) = 0; startPoint(0) = 0; startPoint(1) = 0; startPoint(2) = 0;*/ setOffset(0,0,0,0); Kp = 10; Kjp = 10; openGripper(); maxSpeedCartesian = Vector3d(1000, 1000, 1000); maxSpeedJoint[0] = 1000; maxSpeedJoint[1] = 1000; maxSpeedJoint[2] = 1000; maxSpeedJoint[3] = 1000; softUpperLimitCartesian = Vector3d(1000, 1000, 1000); softLowerLimitCartesian = Vector3d(-1000, -1000, -1000); pauseFalg = false; stopFalg = false; //homePosition = Vector3d(0, 0, 0); softUpperLimitJoint[0] = MOTOR_UPPER__LIMIT_0; softUpperLimitJoint[1] = MOTOR_UPPER__LIMIT_1; softUpperLimitJoint[2] = MOTOR_UPPER__LIMIT_2; softUpperLimitJoint[3] = MOTOR_UPPER__LIMIT_3; softLowerLimitJoint[0] = MOTOR_LOWER__LIMIT_0; softLowerLimitJoint[1] = MOTOR_LOWER__LIMIT_1; softLowerLimitJoint[2] = MOTOR_LOWER__LIMIT_2; softLowerLimitJoint[3] = MOTOR_LOWER__LIMIT_3; serbo = true; manifactur = "SainSmart"; type = "DIY 4-Axis Servos Control Palletizing Robot Arm"; axisNum = 4; cmdCycle = 50; isGripper = false; //speedPoint = 10; //speedJointPos = 1; MaxSpeedJoint[0] = 2; MaxSpeedJoint[1] = 2; MaxSpeedJoint[2] = 2; MaxSpeedJoint[3] = 2; MaxSpeedCartesianTrans = 0.5; MaxSpeedCartesianRot = 2; MinTime = dt; jointOffset[0] = MOTOR_OFFSET_0; jointOffset[1] = MOTOR_OFFSET_1; jointOffset[2] = MOTOR_OFFSET_2; jointOffset[3] = MOTOR_OFFSET_3; }
Plane::Plane(scalar_t a, scalar_t b, scalar_t c, scalar_t d) { N = Vector3d(a, b, c); D = d; }
void World::render_world() { //set all pixels: alpha to 255, color to 0 for (int l = 0; l < vp.width()*vp.height()*4; ++l){ if((l+1)%4 == 0)//alpha _pixels[l] = 255; else _pixels[l] = 0; } //setup ambient light Directional_ray amb_ray;//used for shading Vector3d amb_dir = Vector3d(1.f,1.f,0.f); amb_dir.normalize_vector(); amb_ray.set_direction(amb_dir); //the tracing ray Directional_ray ray; Directional_ray spec_light; //The place to hold pixel data Scene_info info; //start point of ray Point3d cur_orig; float x_off = vp.ref_point().get_x(); float y_off = vp.ref_point().get_y(); float z_off = vp.ref_point().get_z(); float ax_step = vp.A().get_x(); float bx_step = vp.B().get_x(); float ay_step = vp.A().get_y(); float by_step = vp.B().get_y(); float az_step = vp.A().get_z(); float bz_step = vp.B().get_z(); float zw = 100.0; // hard-coded vp.set_pixel_size(2.0); Vector3d dir(0.f,0.f,-1.f); ray.set_direction(dir); //setup environment light Vector3d environment_dir(3.f,4.f,-5.f); environment_dir.normalize_vector(); spec_light.set_direction(environment_dir); std::cout<<"Start ray tracing"<<std::endl; std::cout<<_objects.size()<<" objects involve(s)."<<std::endl; //build the pixel buffer int red_sum = 0; int green_sum = 0; int blue_sum = 0; int rec_ctr = MAX_RECURSE_NUM; float factor = TRACE_FACTOR; srand (time(NULL)); for (int i = 0; i < vp.height(); ++i) { std::cout<<"Processing line "<<i<<std::endl; for (int j = 0; j < vp.width(); ++j) { red_sum = 0; green_sum = 0; blue_sum = 0; //reset pixel color _pixels[i*vp.width()*4 + j*4 + 0] = 0; _pixels[i*vp.width()*4 + j*4 + 1] = 0; _pixels[i*vp.width()*4 + j*4 + 2] = 0; for (int sample_count = 0; sample_count<SAMPLE_PER_PIXEL; ++sample_count){ //rand sample point float yr = (rand() % (int)(vp.pixel_size()*100) )*0.01; float xr = (rand() % (int)(vp.pixel_size()*100) )*0.01; float zr = (rand() % (int)(vp.pixel_size()*100) )*0.01; //reset path tracer factor = TRACE_FACTOR; rec_ctr = MAX_RECURSE_NUM; //TODO: dont forget to change this back if( !PERSPECTIVE ){ cur_orig.set_point(y_off + vp.pixel_size() * (i - vp.height() / 2.0 + 0.5) * ay_step + vp.pixel_size() * (j - vp.width() / 2.0 + 0.5) * by_step+yr, x_off + vp.pixel_size() * (i - vp.height() / 2.0 + 0.5) * ax_step + vp.pixel_size() * (j - vp.width() / 2.0 + 0.5) * bx_step+xr, z_off + vp.pixel_size() * (i - vp.height() / 2.0 + 0.5) * az_step + vp.pixel_size() * (j - vp.width() / 2.0 + 0.5) * bz_step+zr); } else{ ray.set_destination(Point3d(0.f,0.f,-10800.f)); Vector3d dir = ray.get_dest() - cur_orig; dir.normalize_vector(); ray.set_direction(dir); cur_orig.set_point(y_off+vp.pixel_size() * (j - vp.width() / 2.0 + 0.5)+yr, x_off+vp.pixel_size() * (i - vp.height() / 2.0 + 0.5)+xr, z_off+zw+zr); } //set the origin and dir of the tracing ray ray.set_origin(cur_orig); ray.set_direction(dir); //"z-buffer" float min_distance = 3.4e38;//set as the max value of float int cur_obj_idx = -1; trace_scene(info, ray, spec_light, rec_ctr, cur_obj_idx, min_distance, i, j, amb_dir, factor, cur_orig); red_sum += _pixels[i*vp.width()*4 + j*4 + 0]; green_sum += _pixels[i*vp.width()*4 + j*4 + 1]; blue_sum += _pixels[i*vp.width()*4 + j*4 + 2]; } //calculate final color if(SAMPLE_PER_PIXEL != 0){ _pixels[i*vp.width()*4 + j*4 + 0] = red_sum/SAMPLE_PER_PIXEL; _pixels[i*vp.width()*4 + j*4 + 1] = green_sum/SAMPLE_PER_PIXEL; _pixels[i*vp.width()*4 + j*4 + 2] = blue_sum/SAMPLE_PER_PIXEL; } } } std::cout<<"Ray tracing finished!"<<std::endl; }