Ejemplo n.º 1
0
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);
}
Ejemplo n.º 3
0
		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
Ejemplo n.º 4
0
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 );
}
Ejemplo n.º 5
0
/**
*@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;
	}
	
}
Ejemplo n.º 6
0
/**
*@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;
	

}
Ejemplo n.º 7
0
Plane::Plane(scalar_t a, scalar_t b, scalar_t c, scalar_t d) {
    N = Vector3d(a, b, c);
    D = d;
}
Ejemplo n.º 8
0
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;
}