C++ point::Y方法代码示例

void twoLinkArm::ikinAbs(point x, point &q)
{
	x=x-params.x0;
	double S=(std::pow(params.l1+params.l2,2)-(std::pow(x.X(),2)+std::pow(x.Y(),2)))/(std::pow(x.X(),2)+std::pow(x.Y(),2)-std::pow(params.l1-params.l2,2));
	q[1]=-2.0*std::atan(std::sqrt(std::abs(S))); //Negative sign indicates elbow right.
	q[0]=std::atan2(x.Y(),x.X())-std::atan2(params.l2*std::sin(q[1]),params.l1+params.l2*std::cos(q[1]));
}

bool twoLinkArm::ikin(point x, point &q)
{
	x=x-params.x0;
	double S=(std::pow(params.l1+params.l2,2)-(std::pow(x.X(),2)+std::pow(x.Y(),2)))/(std::pow(x.X(),2)+std::pow(x.Y(),2)-std::pow(params.l1-params.l2,2));
	if(S<0) {q=point(); return false;}
	q[1]=-2.0*std::atan(std::sqrt(S)); //Negative sign indicates elbow right.
	q[0]=std::atan2(x.Y(),x.X())-std::atan2(params.l2*std::sin(q[1]),params.l1+params.l2*std::cos(q[1]));
	return true;
}

void twoLinkArm::computeInertiaCoriolis(point q, point qdot, mat2 &D, point &C)
{
	double c2=std::cos(q.Y());
	double d11=params.m1*std::pow(params.lc1,2)+params.m2*(std::pow(params.l1,2)+std::pow(params.lc2,2)+2.0*params.l1*params.lc2*c2)+params.I1+params.I2;
	double d12=params.m2*(std::pow(params.lc2,2)+params.l1*params.lc2*c2)+params.I2;
	double d21=d12;
	double d22=params.m2*std::pow(params.lc2,2)+params.I2;
	double h=params.m2*params.l1*params.lc2*std::sin(q.Y());
	
	D=mat2(d11,d12,d21,d22);
	C=point(h*qdot.Y()*(2*qdot.X()+qdot.Y()), h*std::pow(qdot.X(),2));
}

point TargetControl::genTarget(point currentPos, bool directionMatters)
{
	double theta;
	point targetPos;
		
	if(!directionMatters) //Direction not constrained this trial
	{	
		do 
		{
			theta=randb(0,360);
			targetPos=currentPos+point(cos(theta),sin(theta))*spawnDist;
		} 	while(targetPos.mag()>innerRadius);
		return targetPos;
	}
	
	//If direction predetermined, use it
	if (nextDirection>=0) {targetPos=currentPos+point(cos(nextDirection),sin(nextDirection))*spawnDist; nextDirection=-1; return targetPos;} 
	
	//Make sure we haven't bottomed out
	int sum=0;
	for(int k=0;k<directions;k++) 
	{
		sum+=((timesUsed[k]>=minUses)?0:1);
	}
	if (sum==0) minUses++;
	
	//Find the best direction
	double min=std::numeric_limits<double>::infinity();
	int index=-1;
	double mag;
	point bestPos;
	for(int k=0;k<directions;k++)
	{
		targetPos=currentPos+point(cos(direction[k]),sin(direction[k]))*spawnDist;
		mag=targetPos.mag();
		if((mag<min)&&(timesUsed[k]<minUses)&&(mag<innerRadius)) {min=mag; index=k; bestPos=targetPos;}
	}
	if(index != -1)
	{
		timesUsed[index]++;
		return bestPos;
	}
	
	//If no direction can produce a legal target pick randomly among the least used and set up for it
	int m=INT_MAX;
	int i;
	int * shuffle = new int[directions];
	shuffle[0]=directions;
	randperm(shuffle);
	for(int k=0;k<directions;k++)
		if(timesUsed[shuffle[k]]<m) {m=timesUsed[shuffle[k]]; i=shuffle[k];}
	timesUsed[i]++;
	nextDirection=direction[i];
	theta=atan2(currentPos.Y()+spawnDist*sin(direction[i]),currentPos.X()+spawnDist*cos(direction[i]));
	targetPos=currentPos - point(cos(theta),sin(theta))*spawnDist;
	return (targetPos.mag()<=maxRadius)?targetPos:point(0,0); 
}

mat2 twoLinkArm::jacobian(point q)
{
	double s12=std::sin(q.X()+q.Y());
	double c12=std::cos(q.X()+q.Y());
	double fJ11=-params.l1*std::sin(q.X())-params.l2*s12;
	double fJ21=params.l1*std::cos(q.X())+params.l2*c12;
	double fJ12=-params.l2*s12;
	double fJ22=params.l2*c12;
	return mat2(fJ11,fJ12,fJ21,fJ22);
}

bool Frustum::InterSect( point& p )
{
	std::vector<Plane> planes  = FormPlane();
	
	for (unsigned int i = 0;i<planes.size();i++)
	{
		if(planes[i].A*p.X()+planes[i].B*p.Y()+planes[i].C*p.Z()+planes[i].D > 0.f)
		{
			return false;
		}
	}
	return true;
}

unsigned int Frustum::InterSectClip( point& p )
{
	unsigned ret = 0;

	std::vector<Plane> planes  = FormPlane();

	for (unsigned int i = 0;i<planes.size();i++)
	{
		if(planes[i].A*p.X()+planes[i].B*p.Y()+planes[i].C*p.Z()+planes[i].D > 0.f)
		{
			ret |= 1<<i;
		}
	}
	return ret;
}

point twoLinkArm::getQDDot(point q, point qdot, point xddot)
{
	double s12=std::sin(q.X()+q.Y());
	double c12=std::cos(q.X()+q.Y());
	double fJt1dx2=-params.l1*std::cos(q.X())-params.l2*c12;
	double fJt1dy2=-params.l2*std::cos(q.X()+q.Y());
	double fJt2dx2=-params.l1*std::sin(q.X())-params.l2*s12;
	double fJt2dy2=-params.l2*s12;
	double fJt1dxdy=-params.l2*c12;
	double fJt2dxdy=-params.l2*s12;
	return jacobian(q)/(xddot-(mat2(fJt1dx2*qdot.X()+fJt1dxdy*qdot.Y(),fJt1dy2*qdot.Y()+fJt1dxdy*qdot.X(),
		fJt2dx2*qdot.X()+fJt2dxdy*qdot.Y(),fJt2dy2*qdot.Y()+fJt2dxdy*qdot.X())*qdot));
}

void DisplayWidget::calibrate(point Center, point probe1, point probe2)
{
	center=Center;
	double rot=std::atan2(probe1.Y()-center.Y(),probe1.X()-center.X())-std::atan2(0,LEFTPROBE); //In theory, any one point is enough
	screenRotation=(180l/M_PI)*rot; //OpenGL likes degrees.
	point one=(probe1-center).rotateZero(-rot);
	point two=(probe2-center).rotateZero(-rot);
	std::cout << rot << " " << one.X() << " " << one.Y() << " " << two.X() << " " << two.Y() << std::endl; //Why not?
	
	width=SCREENWIDTH/LEFTPROBE*one.X();
	height=SCREENHEIGHT/UPPROBE*two.Y();
	
	calibrationMode=false;
	std::cout << width << " " << height << std::endl; //Why not?
	
	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(-width/2l+center.X(),width/2l+center.X(),-height/2l+center.Y(),height/2l+center.Y(),MINDEPTH,MAXDEPTH);
	glTranslated(center.X(),center.Y(),0);
	glRotated(fmod(screenRotation,180),0,0,1);
	glTranslated(-center.X(),-center.Y(),0);
}

    float_type diffy(const point<float_type, dim>& op1, const point<float_type, dim>& op2)
{
    return op2.Y() - op1.Y();
}

void twoLinkArm::fkin(point q, point &x, point &x1)
{
	x1=point(params.x0.X()+params.l1*std::cos(q.X()),params.x0.Y()+params.l1*std::sin(q.X()));
	x=x1+point(params.l2*std::cos(q.X()+q.Y()),params.l2*std::sin(q.X()+q.Y()));
}

point TargetControl::genTarget(point currentPos, bool directionMatters)
{
	double theta;
	point targetPos;
		
	if(!directionMatters) //Direction not constrained this trial
	{	
		do 
		{
			theta=randb(0,360);
			targetPos=currentPos+point(cos(theta),sin(theta))*spawnDist;
		} 	while(targetPos.mag()>innerRadius);
		return targetPos;
	}
	
	//If direction predetermined, use it
	if (nextDirection>=0) {targetPos=currentPos+point(cos(nextDirection),sin(nextDirection))*spawnDist; nextDirection=-1; return targetPos;} 
	
	//Make sure we haven't bottomed out
	int sum=0;
	for(int k=0;k<directions;k++) 
	{
		sum+=((timesUsed[k]>=minUses)?0:1);
	}
	if (sum==0) minUses++;
	
	//Find the best direction
	double min=std::numeric_limits<double>::infinity();
	int index=-1;
	double mag;
	point bestPos;
	for(int k=0;k<directions;k++)
	{
		targetPos=currentPos+point(cos(direction[k]),sin(direction[k]))*spawnDist;
		mag=targetPos.mag();
		if((mag<min)&&(timesUsed[k]<minUses)&&(mag<innerRadius)) {min=mag; index=k; bestPos=targetPos;}
	}
	if(index != -1)
	{
		timesUsed[index]++;
		return bestPos;
	}
	
	//If no direction can produce a legal target pick randomly among the least used and set up for it
	int m=INT_MAX;
	int i;
	int * shuffle = new int[directions];
	randperm(shuffle,directions);
	for(int k=0;k<directions;k++)
		if(timesUsed[shuffle[k]]<m) {m=timesUsed[shuffle[k]]; i=shuffle[k];}
	timesUsed[i]++;
	nextDirection=direction[i];
	theta=atan2(currentPos.Y()+spawnDist*sin(direction[i]),currentPos.X()+spawnDist*cos(direction[i]))-3.14159265;
	targetPos=currentPos - point(cos(theta),sin(theta))*spawnDist;
	if (targetPos.mag()>innerRadius)
	{
		point V=point(spawnDist*cos(direction[i]),spawnDist*sin(direction[i]));
		point center=currentPos+V;
		double distance=center.mag();
		double xi=(pow(distance,2)+pow(innerRadius,2)-pow(spawnDist,2))/(2*distance);
		double yi=sqrt(pow(innerRadius,2)-pow(xi,2));
		point i1=center*(xi/distance)+point(-center.Y(),center.X())*(yi/distance);
		point i2=center*(xi/distance)-point(-center.Y(),center.X())*(yi/distance);
		double angle1=atan2(i1.Y()-V.Y()-currentPos.Y(),i1.X()-V.X()-currentPos.X());
		double angle2=atan2(i2.Y()-V.Y()-currentPos.Y(),i2.X()-V.X()-currentPos.X());
		point p1p=currentPos+point(cos(angle1),sin(angle1))*spawnDist;
		point p1m=currentPos+point(cos(angle2),sin(angle2))*spawnDist;
		if (p1p.mag()>p1m.mag()) targetPos=p1m;
		else targetPos=p1p;
	}
	return (targetPos.mag()<=maxRadius)?targetPos:point(0,0); 
}