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

bool colliderParticeCirclePeg(Ball &ball, Peg &peg, GLfloat time)
{
	
	
	GLfloat sum_radii, sum_radii_sq, n_mag_sq;
	Point2f normal;
	
	normal = ball.position - peg.position;
	//GLfloat distance = normal.magitude();
	
	sum_radii = ball.radius + peg.radius;
	sum_radii_sq = sum_radii * sum_radii;
	
	//std::cout << "x " << normal._x << std::endl;
	n_mag_sq = normal.squareMagnitude();
//	std::cout << " n_maq_sq:" << sum_radii_sq  << std::endl;
//	std::cout << "sum_radii_sq:" << sum_radii_sq << std::endl;
	if (n_mag_sq <= sum_radii_sq) 
	{
		std::cout << "x " << normal._x << std::endl;
		normal.normalize();
		std::cout << "normal x " << normal._x << std::endl;
		GLfloat velocityDotN = dotProduct(normal, ball.velocity);
		if(velocityDotN < 0)
		{
			ball.velocity._x -= 2.0f * velocityDotN * normal._x;
			ball.velocity._y -= 2.0f * velocityDotN * normal._y;
			
			ball.position._x += 2.0f * (sum_radii - peg.radius + ball.radius) * normal._x * time;
			ball.position._y += 2.0f * (sum_radii - peg.radius + ball.radius) * normal._y * time;
			
		}
		
		return true;
	}
	else 
	{
		return false;	
	}
	
}

//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;
//.........这里部分代码省略.........

//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();
//.........这里部分代码省略.........