C++ Point2f::magnitude方法代码示例

//Circle to Edge Collision Function aka Poly Collision for predicted_Trajectory
bool Peg::polyPegCollision_p(Ball &ball)
{
	GLfloat collisionDist = INFINITY;
	//bool isClosestEdge; 
	
	//Variables for edge collision
	Point2f vecFromVert;
	Point2f tangent, normal;
	GLfloat tangentComponent, normalComponent, edgeLenght;
	
	//Variables for Corner collision 
	
	int cornerIndex = -1, prevCornerIndex, nextCornerIndex;
	Point2f cornerNormal_1, cornerNormal_2;
	GLfloat cornerDist;
	
	//Variable for Rebound
	Point2f rebound;
	GLfloat orthogBallVelMag;
	GLfloat transPolyVelDotReb;
	
	//Calculate transformed co-ordinates from ball position
	Point2f orig(ball.position_p._x - position._x, ball.position_p._y - position._y);
	Point2f trans(cosf(rotation) * orig._x + sinf(rotation) * orig._y, 
	- sinf(rotation) * orig._x + cosf(rotation) * orig._y);
	
	//Calculate transformed co-ordinates from ball velocity
	
	Point2f transVel(cosf(rotation) * ball.position_p._x + sinf(rotation) * ball.position_p._y, 
	- sinf(rotation) * ball.position_p._x + cosf(rotation) * ball.position_p._y);
	
	//Calculate transformed co-ordinates from peg velocity...errr oh wait...it is zero!!
	Point2f zeroVeloc(0,0);
	Point2f transPegPolyVel(cosf(rotation) * zeroVeloc._x + sinf(rotation) * zeroVeloc._y, 
	- sinf(rotation) * zeroVeloc._x + cosf(rotation) * zeroVeloc._y);
	
	if(type == circle)
	{
		rebound = trans;
		collisionDist = rebound.magnitude()- ball.radius - vertices[0].magnitude();
		rebound.normalize();
	}
	else
	{
		//check Edge collision
		for(int i = 0; i < subs; i++)
		{
			//isClosestEdge = false;
			//Calculate vectFromVert 
			vecFromVert = trans - vertices[i];
			
			//Calculate tangent between two vertices
			tangent = vertices[i+1] - vertices[i];
			edgeLenght = tangent.magnitude();
			
			tangent.normalize();
			
			//calculate normal vector
			normal._x = tangent._y;
			normal._y = -tangent._x;
			
			//caluclate components
			tangentComponent = dotProduct(tangent, vecFromVert);
			normalComponent = dotProduct(normal, vecFromVert);
			
			if(normalComponent >= 0.0 && tangentComponent >= 0 && tangentComponent <= edgeLenght)
			{
			 //	isClosestEdge = true;
				collisionDist = normalComponent - ball.radius;
				rebound._x = normal._x;
				rebound._y = normal._y;
				
			}
			
		}
	}
	//if Edge don't collision and check corner collision
	
	if(collisionDist == INFINITY)
	{
		for(int i = 0; i < subs; i++)
		{
			Point2f cornerToBall;
			cornerToBall = trans - vertices[i];
			cornerDist = cornerToBall.magnitude() - ball.radius;
			
			if(cornerDist < collisionDist)
			{
				cornerIndex = i;
				nextCornerIndex = i + 1;
				if(i == 0)
					prevCornerIndex = subs - 1;
				else
					prevCornerIndex = i - 1;
				
				collisionDist = cornerDist;
				
				cornerToBall.normalize();
				rebound._x = cornerToBall._x;
//.........这里部分代码省略.........

bool collisionBoundingBox(Ball &ball, bool debugVisual, Peg &peg)
{
	GLfloat collisionDist = INFINITY;
	GLfloat cornerDist;
	int cornerIndex = -1;
	
	Point2f *vertices = new Point2f[5];
	vertices[0].setXY(peg.radius + peg.position._x, peg.radius + peg.position._y);
	vertices[1].setXY(peg.radius + peg.position._x, -peg.radius + peg.position._y);
	vertices[2].setXY(-peg.radius + peg.position._x, -peg.radius + peg.position._y);
	vertices[3].setXY(-peg.radius + peg.position._x, peg.radius +  peg.position._y);
	vertices[4].setXY(peg.radius + peg.position._x, peg.radius + peg.position._y);
	
	Point2f * orthogVecs = new Point2f[4];
	orthogVecs[0].setXY(1.0, 0.0);
	orthogVecs[0].setXY(0.0, -1.0);
	orthogVecs[0].setXY(-1.0, 0.0);
	orthogVecs[0].setXY(0.0, 1.0);
	
	//Vector parallel to hit box edge remainder;
	Point2f hitRemaidner;
	
	//Component orthogonal to hit box edge
	GLfloat orthogDist;
	
	//Component paraell to hit box edge
	GLfloat hitRemainderDist;
	
	//Caluclate the ball's position in transformed co-ordinates
	Point2f orig(ball.position._x - peg.position._x, ball.position._y - peg.position._y);
	
	Point2f trans(cosf(peg.rotation) * orig._x + sinf(peg.rotation) * orig._y, 
	-sin(peg.rotation) * orig._x + cosf(peg.rotation) * orig._y);
	
	for(int i = 0; i < 4; i++)
	{
		orthogDist = dotProduct(orthogVecs[i], trans);
		hitRemaidner._x = trans._x - orthogDist * orthogVecs[i]._x;
		hitRemaidner._y = trans._y - orthogDist * orthogVecs[i]._y;
		hitRemainderDist = hitRemaidner.magnitude();
		//std::cout << "hitRemainderDist : "<<  hitRemainderDist << std::endl;
		//std::cout << "peg.Radius : "<<  peg.radius << std::endl;
		if(orthogDist >= 0.0 && hitRemainderDist <= peg.radius)
		{
			
			glColor3f(1.0f, 0.0f, 0.0f);
			collisionDist = orthogDist - (peg.radius + ball.radius);
		}
		else
		{
			glColor3f(1.0f, 1.0f, 1.0f);
		}
		
		if(debugVisual)
		{
			glBegin(GL_LINES);
			glVertex2f(vertices[i]._x, vertices[i]._y);
			glVertex2f(vertices[i+1]._x, vertices[i+1]._y);
			glEnd();
		}		
		
	}
	/*check Corners */
	if(collisionDist == INFINITY)
	{
		for(int i = 0; i < 4; i++)
		{
			cornerDist = sqrt((trans._x - vertices[i]._x) 
			* (trans._x - vertices[i]._x) + (trans._y - vertices[i]._y)
			* (trans._y - vertices[i]._y)) - ball.radius;
			
			if(cornerDist < collisionDist)
			{
				cornerIndex = i;
				collisionDist = cornerDist;
			}			
		}
		
		if(debugVisual)
		{
			glColor3f(1.0f, 0.0f, 0.0f);
			glPointSize(3);
			glBegin(GL_POINTS);
			glVertex2f(vertices[cornerIndex]._x, vertices[cornerIndex]._y);
			glEnd();
		}
		
	}
//	std::cout << "collisionDist : "<<  collisionDist << std::endl;
	if(collisionDist > 0.0)
		return false;
	else
		return true;

}

//Circle to Edge Collision Function aka Poly Collision
bool Peg::polyPegCollision(Ball &ball, bool visualDebug)
{
	GLfloat collisionDist = INFINITY;
	bool isClosestEdge; 
	
	//Variables for edge collision
	Point2f vecFromVert;
	Point2f tangent, normal;
	GLfloat tangentComponent, normalComponent, edgeLenght;
	
	//Variables for Corner collision 
	
	int cornerIndex = -1, prevCornerIndex, nextCornerIndex;
	Point2f cornerNormal_1, cornerNormal_2;
	GLfloat cornerDist;
	
	//Variable for Rebound
	Point2f rebound;
	GLfloat orthogBallVelMag;
	GLfloat transPolyVelDotReb;
	
	//Calculate transformed co-ordinates from ball position
	Point2f orig(ball.position._x - position._x, ball.position._y - position._y);
	Point2f trans(cosf(rotation) * orig._x + sinf(rotation) * orig._y, 
	- sinf(rotation) * orig._x + cosf(rotation) * orig._y);
	
	//Calculate transformed co-ordinates from ball velocity
	
	Point2f transVel(cosf(rotation) * ball.velocity._x + sinf(rotation) * ball.velocity._y, 
	- sinf(rotation) * ball.velocity._x + cosf(rotation) * ball.velocity._y);
	
	//Calculate transformed co-ordinates from peg velocity...errr oh wait...it is zero!!
	Point2f zeroVeloc(0,0);
	Point2f transPegPolyVel(cosf(rotation) * zeroVeloc._x + sinf(rotation) * zeroVeloc._y, 
	- sinf(rotation) * zeroVeloc._x + cosf(rotation) * zeroVeloc._y);
	
	if(type == circle)
	{
		rebound = trans;
		collisionDist = rebound.magnitude()- (ball.radius * 2) - vertices[0].magnitude();
		rebound.normalize();
	}
	else
	{
		//check Edge collision
		for(int i = 0; i < subs; i++)
		{
			isClosestEdge = false;
			//Calculate vectFromVert 
			vecFromVert = trans - vertices[i];
			
			//Calculate tangent between two vertices
			tangent = vertices[i+1] - vertices[i];
			edgeLenght = tangent.magnitude();
			
			tangent.normalize();
			
			//calculate normal vector
			normal._x = tangent._y;
			normal._y = -tangent._x;
			
			//caluclate components
			tangentComponent = dotProduct(tangent, vecFromVert);
			normalComponent = dotProduct(normal, vecFromVert);
			
			if(normalComponent >= 0.0 && tangentComponent >= 0 && tangentComponent <= edgeLenght)
			{
				isClosestEdge = true;
				collisionDist = normalComponent - ball.radius;
				rebound._x = normal._x;
				rebound._y = normal._y;
				
			}
			
			//Visual Debug
			if(visualDebug)
			{
				//highlight edge with teal color if closest Edge is choiced
				glPushMatrix();
				if(isClosestEdge)
					glColor3f(0,1,1);
				else
					glColor3f(1,1,1);
				
				
				glTranslatef(position.getX(), position.getY(), 0.0);
				glRotatef(180.0f * rotation / PI_F, 0.0f, 0.0f, 1.0f);	
				
				Point2f vertices1 = vertices[i];// + position;
				Point2f vertices2 = vertices[i+1];// + position;
				glBegin(GL_LINES);
				glVertex2f(
				vertices1._x,
				vertices1._y
				);
				glVertex2f(
				vertices2._x, 
				vertices2._y);
				glEnd();
//.........这里部分代码省略.........