C++ Vector2D::Normalize方法代码示例

mitk::Point2D mitk::PlanarDoubleEllipse::ApplyControlPointConstraints(unsigned int index, const Point2D& point)
{
  if (index == 2 && !m_ConstrainCircle)
  {
    Point2D centerPoint = this->GetControlPoint(0);
    Vector2D outerMajorVector = this->GetControlPoint(1) - centerPoint;

    Vector2D minorDirection;
    minorDirection[0] = outerMajorVector[1];
    minorDirection[1] = -outerMajorVector[0];
    minorDirection.Normalize();

    double outerMajorRadius = outerMajorVector.GetNorm();
    double innerMajorRadius = (this->GetControlPoint(3) - centerPoint).GetNorm();
    ScalarType radius = std::max(outerMajorRadius - innerMajorRadius, std::min(centerPoint.EuclideanDistanceTo(point), outerMajorRadius));

    return centerPoint + minorDirection * radius;
  }
  else if (index == 3 && !m_ConstrainThickness)
  {
    Point2D centerPoint = this->GetControlPoint(0);
    Vector2D outerMajorVector = this->GetControlPoint(1) - centerPoint;

    double outerMajorRadius = outerMajorVector.GetNorm();
    double outerMinorRadius = (this->GetControlPoint(2) - centerPoint).GetNorm();
    ScalarType radius = std::max(outerMajorRadius - outerMinorRadius, std::min(centerPoint.EuclideanDistanceTo(point), outerMajorRadius));

    outerMajorVector.Normalize();

    return centerPoint - outerMajorVector * radius;
  }

  return point;
}

void TextureTest::ProcessEvent(SDL_Event* event)
{
    switch(event->type)
    {
        case SDL_MOUSEBUTTONDOWN:
        {
            if(event->button.button == 4)
            {

            }
            else if(event->button.button == 5)
            {

            }
            break;
        }
        case SDL_KEYDOWN:
        {
            if(event->key.keysym.sym == SDLK_UP)
            {
                Vector2D oldSteer = v->GetVelocity();
                Vector2D temp = v->GetVelocity();
                temp.Normalize();
                v->SetVelocity(oldSteer+temp*16);
                //v->SetHeading(v->GetVelocity());
            }
            if(event->key.keysym.sym == SDLK_DOWN)
            {
                Vector2D oldSteer = v->GetVelocity();
                Vector2D temp = v->GetVelocity();
                temp.Normalize();
                v->SetVelocity(oldSteer-temp*163);
            }
            if(event->key.keysym.sym == SDLK_LEFT)
            {
                Matrix2D mat;
                //Vector2D vec = v->GetVelocity();
                Vector2D vecH = v->GetVelocity();
                //Vec2DRotateAroundO(vecH,angle);
                mat.Rotate(angle);
//
               //mat.Rotate(angle);
                mat.TransformVector(vecH);
//              / mat.TransformVector(vecH);
//                //v->SetVelocity(vec);
//                v->RotateHeadingToFacePosition(vecH);
                //v->SetSteeringForce(vecH);
                v->SetVelocity(vecH);
                angle=0.07;

            }
            break;
        }
        //case SDL_KEYUP
    }
}

void Physics::TestCollision(Circle *a, Circle *b) {
	Vector2D n = (b->GetCentroidPosition() - a->GetCentroidPosition());
	double depth = a->radius + b->radius - n.GetLength();
	if (depth < 0) return;
	n.Normalize();
	constraints.push_back(new Contact(a, b, a->GetCentroidPosition() + n * a->radius, n, depth));
}

		Vector2D Slerp(const Vector2D &a, const Vector2D &b, Float t) {
			 Float dot = Math::WrapFloat(Dot(a, b), -1.0f, 1.0f);
			 Float theta = acos(dot) * t;
			 Vector2D relativeVec = b - (a * dot);
			 relativeVec.Normalize();
			 return ((a*cos(theta)) + (relativeVec*sin(theta)));
		}

void Shape::resolveCollision( Shape & shape )
{
    Vector2D delta = position - shape.position;
    float d = delta.Length();
    Vector2D mtd = delta * ( ( ( getRadius() + shape.getRadius() ) - d ) / d );

    float im1 = 1 / getMass();
    float im2 = 1 / shape.getMass();

    // speed
    Vector2D v = velocity - shape.velocity;
    float vn = v * (mtd.Normalize());

    // intersecting but moving away
    if ( vn > 0.0f ) return;

    // impulse
    float i = (-(1.0f + RESTITUTION) * vn ) / ( im1 + im2 );
    Vector2D impulse = mtd * i;

    // momentum
    velocity += impulse * im1;
    shape.velocity -= impulse * im1;

}

void Vector2D::ProjToLine (const Vector2D &rclPt, const Vector2D &rclLine)
{
  double l  = rclLine.Length();
  double t1 = (rclPt * rclLine) / l;
  Vector2D clNormal = rclLine;
  clNormal.Normalize();
  clNormal.Scale(t1);  
  *this = clNormal;
}

mitk::Point2D mitk::PlanarCircle::ApplyControlPointConstraints(unsigned int index, const Point2D &point)
{
    if ( this->GetPlaneGeometry() ==  nullptr )
    {
        return point;
    }

    Point2D indexPoint;
    this->GetPlaneGeometry()->WorldToIndex( point, indexPoint );

    BoundingBox::BoundsArrayType bounds = this->GetPlaneGeometry()->GetBounds();
    if ( indexPoint[0] < bounds[0] ) {
        indexPoint[0] = bounds[0];
    }
    if ( indexPoint[0] > bounds[1] ) {
        indexPoint[0] = bounds[1];
    }
    if ( indexPoint[1] < bounds[2] ) {
        indexPoint[1] = bounds[2];
    }
    if ( indexPoint[1] > bounds[3] ) {
        indexPoint[1] = bounds[3];
    }

    Point2D constrainedPoint;
    this->GetPlaneGeometry()->IndexToWorld( indexPoint, constrainedPoint );

    if(m_MinMaxRadiusContraintsActive)
    {
        if( index != 0)
        {
            const Point2D &centerPoint = this->GetControlPoint(0);
            double euclideanDinstanceFromCenterToPoint1 = centerPoint.EuclideanDistanceTo(point);

            Vector2D vectorProjectedPoint;
            vectorProjectedPoint = point - centerPoint;
            vectorProjectedPoint.Normalize();

            if( euclideanDinstanceFromCenterToPoint1 > m_MaxRadius )
            {
                vectorProjectedPoint *= m_MaxRadius;
                constrainedPoint = centerPoint;
                constrainedPoint += vectorProjectedPoint;
            }
            else if( euclideanDinstanceFromCenterToPoint1 < m_MinRadius )
            {
                vectorProjectedPoint *= m_MinRadius;
                constrainedPoint = centerPoint;
                constrainedPoint += vectorProjectedPoint;
            }
        }
    }

    return constrainedPoint;
}

TEST(vec2d, norm)
{
	Vector2D testingVector = Vector2D();

	testingVector.x = 1;
	testingVector.y = 0;
	testingVector.Magnitude();
	testingVector.Normalize();
	EXPECT_FLOAT_EQ(1.f, testingVector.x);
	EXPECT_FLOAT_EQ(0.f, testingVector.y);
}

void mitk::PlanarEllipse::GeneratePolyLine()
{
    // clear the PolyLine-Contrainer, it will be reconstructed soon enough...
    this->ClearPolyLines();

    const Point2D &centerPoint = GetControlPoint( 0 );
    const Point2D &boundaryPoint1 = GetControlPoint( 1 );
    const Point2D &boundaryPoint2 = GetControlPoint( 2 );

    Vector2D dir = boundaryPoint1 - centerPoint; dir.Normalize();
    vnl_matrix_fixed<float, 2, 2> rot;

    // differentiate between clockwise and counterclockwise rotation
    int start = 0;
    int end = 64;
    if (dir[1]<0)
    {
        dir[0] = -dir[0];
        start = -32;
        end = 32;
    }
    // construct rotation matrix to align ellipse with control point vector
    rot[0][0] = dir[0];
    rot[1][1] = rot[0][0];
    rot[1][0] = sin(acos(rot[0][0]));
    rot[0][1] = -rot[1][0];

    double radius1 = centerPoint.EuclideanDistanceTo( boundaryPoint1 );
    double radius2 = centerPoint.EuclideanDistanceTo( boundaryPoint2 );

    // Generate poly-line with 64 segments
    for ( int t = start; t < end; ++t )
    {
        double alpha = (double) t * vnl_math::pi / 32.0;

        // construct the new polyline point ...
        vnl_vector_fixed< float, 2 > vec;
        vec[0] = radius1 * cos( alpha );
        vec[1] = radius2 * sin( alpha );
        vec = rot*vec;

        Point2D polyLinePoint;
        polyLinePoint[0] = centerPoint[0] + vec[0];
        polyLinePoint[1] = centerPoint[1] + vec[1];

        // ... and append it to the PolyLine.
        // No extending supported here, so we can set the index of the PolyLineElement to '0'
        AppendPointToPolyLine( 0, PolyLineElement( polyLinePoint, 0 ) );
    }

    AppendPointToPolyLine( 1, PolyLineElement( centerPoint, 0 ) );
    AppendPointToPolyLine( 1, PolyLineElement( GetControlPoint( 3 ), 0 ) );
}

void mitk::GizmoInteractor::RotateAroundAxis(StateMachineAction*,
                                               InteractionEvent* interactionEvent)
{
  auto positionEvent = dynamic_cast<const InteractionPositionEvent*>(interactionEvent);
  if(positionEvent == NULL)
  {
    return;
  }

  Vector2D originalVector = m_InitialClickPosition2D - m_InitialGizmoCenter2D;
  Vector2D currentVector = positionEvent->GetPointerPositionOnScreen() - m_InitialGizmoCenter2D;

  originalVector.Normalize();
  currentVector.Normalize();

  double angle_rad = std::atan2(currentVector[1], currentVector[0]) -
                     std::atan2(originalVector[1], originalVector[0]);

  ApplyRotationToManipulatedObject(vtkMath::DegreesFromRadians(angle_rad));
  RenderingManager::GetInstance()->ForceImmediateUpdateAll();
}

void IFSMCowFlee::Calculate(MovingEntity* entity, Instance* instance){

	Rabbit* target = instance->GetRabbit();

	Vector2D newHeading = (entity->GetPosition() - target->GetPosition());
	newHeading.Normalize();

	entity->SetHeading(newHeading);

	entity->Move(0.0f);

}

bool Sector::Intersects(const Vector2D& position) const {
    Vector2D l(this->GetPosition() - position);
    if(l.GetLength() > GetRadius()) return false;
    if(Math::IsEqual(l.GetLength(), 0.0)) return true;

    Vector2D sF = Vector2D::GetFacingVector(this->GetArcCenter(), this->GetPosition());
    Vector2D sF_n(sF.Normalize());
    Vector2D P_to_S(l.Normalize());

    double angle = std::acos(Vector2D::DotProduct(sF_n, P_to_S));

    return (angle <= GetTheta() / 2.0);
}

void DumbTank::Update(float deltaTime, SDL_Event e)
{
	//This is a dumb tank. Do NOT copy this approach.

	//Did we see a tank?
	if(mTanksICanSee.size() == 0)
	{
		ChangeState(TANKSTATE_IDLE);

		//If there are no visible tanks, then keep moving.

		//Check if we reached position before turning.
		if(mPosition.y < mPosition1.y && mHeading.y != -1.0f)
		{
			mHeading = Vector2D(0.0f, -1.0f);
			mRotationAngle = 180.0f;
			mVelocity = Vector2D();
			return;
		}
		else if(mPosition.y > mPosition2.y && mHeading.y != 1.0f)
		{
			mHeading = Vector2D(0.0f, 1.0f);
			mRotationAngle = 0.0f;
			mVelocity = Vector2D();
			return;
		}
		else
		{
			//Move if we are facing the correct direction.
			mCurrentSpeed -= kSpeedIncrement*deltaTime;
			if(mCurrentSpeed < -GetMaxSpeed())
				mCurrentSpeed = -GetMaxSpeed();
		}
	}
	else
	{
		//Rotate man to face enemy tank.
		Vector2D toTarget = mTanksICanSee[0]->GetCentralPosition()-GetCentralPosition();
		toTarget.Normalize();
		double dot = toTarget.Dot(mManFireDirection);
		if(dot < 0.95f)
			RotateManByRadian(kManTurnRate, -1, deltaTime);

		//Otherwise stop moving and fire at the visible tank.
		mVelocity = Vector2D();
		ChangeState(TANKSTATE_MANFIRE);
	}

	BaseTank::Update(deltaTime, e);
}

void BaseTank::Rebound(Vector2D position)
{
	//DEBUG: Alert on colliding.
	//cout << "Collision" << endl;

	//We need to rebound, but which direction?
	Vector2D newHeading = GetCentralPosition()-position;
	newHeading.Normalize();

	//Flip the y coordinate because of the 0,0 position of SDL.
	newHeading.y *= -1.0f;

	//Set new velocity.
	mVelocity = newHeading*-kReboundSpeed;

	//Cut the speed.
	mCurrentSpeed = 0.0f;
}

void IFSMCowFindGun::Calculate(MovingEntity* entity, Instance* instance){

	Gun* target = instance->GetGun();
	if (entity->GetPosition().DistanceBetween(target->GetPosition()) <= CATCH_DISTANCE)
	{
		//switch states
		printf("[Cow] found the Gun!\n");
		entity->SetState(new IFSMCowHide());
		instance->ResetEntities(false, false, false, true);
		return;
	}

	Vector2D newHeading = (target->GetPosition() - entity->GetPosition());
	newHeading.Normalize();

	entity->SetHeading(newHeading);
	entity->Move(0.0f);

}