C++ LengthSquared函数代码示例

void EnemyShip::changeVelocity(const float& dt, Engine::ParticleSystem& system)
{
	if (target != nullptr)
	{
		Vector2 toTarget = target->getPosition() - shipPosition;
		
		if (LengthSquared(toTarget) != 0)
		{
			Vector2 normalizedtoTarget = PerpCCW(Normalized(toTarget));

			shipRotation = atan2f(normalizedtoTarget.getY(), normalizedtoTarget.getX());

			if (LengthSquared(toTarget) >= 0)
			{
				velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, -(acceleration * dt) * dt));
				system.AddParticle(new THRUSTPARTICLEUP);
			}
			else
			{
				velocity = velocity + (Engine::Matrix2::rotation(shipRotation) * Engine::Vector2(0, (acceleration * dt) * dt));
				system.AddParticle(new THRUSTPARTICLEDOWN);
			}
		}
	}
}

SVector2 ArriveBehavior::Update(float deltaTime)
{
    if (mActive)
    {
        SVector2 positionToDestination = mpAgent->GetDestination() - mpAgent->GetPosition();
        SVector2 posToDestNorm = Normalize(positionToDestination);

        float maxSpeed = mpAgent->GetMaxSpeed();

        SVector2 desiredVelocity = posToDestNorm * maxSpeed;

        float distSq = LengthSquared(positionToDestination);
        float velocitySq = LengthSquared(mpAgent->GetVelocity());
        float slowingRadiusSq = 160000.0f;
        float stopRadiusSq = 25.0f;

        if (distSq < velocitySq)
        {
            if (distSq < slowingRadiusSq && distSq > stopRadiusSq )
            {
                //desiredVelocity = -velocitySq/(2*sqrt(distSq));
                desiredVelocity = posToDestNorm * maxSpeed * distSq/(slowingRadiusSq*slowingRadiusSq*slowingRadiusSq*slowingRadiusSq);// * maxSpeed/(distSq*distSq);
            }
            else if (distSq <= stopRadiusSq)
            {
                mpAgent->SetPosition(mpAgent->GetDestination());
                desiredVelocity = SVector2(0.0f, 0.0f);
            }
        }
        return desiredVelocity - mpAgent->GetVelocity();
    }

    return SVector2(0.0f, 0.0f);

}

void ColourNormalFit::Permute3()
{
  const Vec3 scale  = Vec3( 1.0f / 0.5f);
  const Vec3 offset = Vec3(-1.0f * 0.5f);
  const Vec3 scalei = Vec3( 1.0f * 0.5f);
  
  // cache some values
  int const count = m_colours->GetCount();
  Vec3 const* values = m_colours->GetPoints();
  Scr3 const* freq = m_colours->GetWeights();
  
  cQuantizer3<5,6,5> q = cQuantizer3<5,6,5>();
  Scr3 berror = Scr3(DEVIANCE_MAXSUM);
  
  Vec3 c_start = m_start;
  Vec3 c_end   = m_end;
  Scr3 l_start = LengthSquared(Normalize(scale * (offset + m_start)));
  Scr3 l_end   = LengthSquared(Normalize(scale * (offset + m_end)));
  Vec3 q_start = Reciprocal(q.grid + Vec3(1.0f));
  Vec3 q_end   = q_start;

  // adjust offset towards sphere-boundary
  if (!(l_start < Scr3(1.0f)))
    q_start = Vec3(0.0f) - q_start;
  if (!(l_end   < Scr3(1.0f)))
    q_end   = Vec3(0.0f) - q_end;
  
  int trie = 0x3F;
  do {
    // permute end-points +-1 towards sphere-boundary
    Vec3 p_start = q_start & Vec3(!(trie & 0x01), !(trie & 0x02), !(trie & 0x04));
    Vec3 p_end   = q_end   & Vec3(!(trie & 0x08), !(trie & 0x10), !(trie & 0x20));
    
    p_start = q.SnapToLattice(c_start + p_start);
    p_end   = q.SnapToLattice(c_end   + p_end);

    // create a codebook
    // resolve "metric * (value - code)" to "metric * value - metric * code"
    Vec3 codes[3]; Codebook3n(codes, p_start, p_end);

    Scr3 merror = Scr3(DEVIANCE_BASE);
    for (int i = 0; i < count; ++i) {
      // find the closest code
      Vec3 value = Normalize(scale * (offset + values[i]));
      Scr3 dist; MinDeviance3<false>(dist, i, value, codes);
      
      // accumulate the error
      AddDeviance(dist, merror, freq[i]);
    }
    
    if (berror > merror) {
      berror = merror;

      m_start = p_start;
      m_end   = p_end;
    }

  } while(--trie);
}

bool Collisions::sphereToBox(RigidBody *sphere, RigidBody *box, CollisionInfo &info)
{
    //translation
    vec2 nearest;
    vec2 clamp_box = box->collider.dims/2;
    vec2 sphere_in_box_world = sphere->position-box->position;
    nearest.x = clamp(sphere_in_box_world.x,-clamp_box.x,clamp_box.x);
    nearest.y = clamp(sphere_in_box_world.y,-clamp_box.y,clamp_box.y);

    float dist = LengthSquared(sphere_in_box_world-nearest);
    float radius = sphere->collider.radius*sphere->collider.radius;

    if(dist < radius)
    {
        float realDist = sqrt(dist);
        info.intersection = (box->position + nearest) + (sphere_in_box_world*(realDist-sphere->collider.radius)/realDist)/2;
        info.normal = Normalize(nearest - sphere_in_box_world);
        info.type = SPHERE_TO_BOX;

        // Side of box collision
        if(abs(nearest.x) < abs(nearest.y))
            info.boxSideCol = SIDE_EDGE;
        else
            info.boxSideCol = UPPER_EGDE;

        return true;
    }
    else
        return false;
}

float Vector::Length()
{
	float r = (float)sqrt(LengthSquared());
	if ( r < 0.0f )
		r = -r;
	return r;
}

float4 float4::normalize() const
{
	f32 lsqr = LengthSquared();
	if(NearZero(lsqr)) { return ZERO; };
	f32 recip = InvSqrt(lsqr);
	
	return float4(vec[0]*recip, vec[1]*recip, vec[2]*recip, vec[3]*recip);
};

        bool Collides(const CollisionRadius& other) const
        {
            T radiusSquared = RadiusSquared() + other.RadiusSquared();

            auto rawDistance = _position - other._position;

            return rawDistance.LengthSquared() <= radiusSquared;
        }

/*------------------------------------------------------------------------------
normalize this quaternion
------------------------------------------------------------------------------*/
Quaternion& Quaternion::Normalize() {
	float lengthSquared = LengthSquared();
	float invLength = 0;
	if( lengthSquared ) {
		invLength = 1.0f / sqrtf( lengthSquared );
	}
	*this *= invLength;
	return *this;
}

Vector2 Vector2::NormalizeFast() const
{
    float len = LengthSquared();

    if (len > 0)
        return Mul(invsqrt(len));

    return Vector2();
}

void Vector4::FastNormalize()
{
	// No error checking, plus fast inverse square root
	float recLen = FastInvSqRt( LengthSquared() );
	x *= recLen;
	y *= recLen;
	z *= recLen;
	w *= recLen;
}

//--------------------------------------------------------------------
// Computes the average curvature per unit surface distance in the face
//--------------------------------------------------------------------
float ComputeFaceCurvature(Point3 *n, Point3 *v, Point3 bc)
{
	Point3 nc = (n[0]+n[1]+n[2])/3.0f;
	Point3 dn0 = n[0]-nc;
	Point3 dn1 = n[1]-nc;
	Point3 dn2 = n[2]-nc;
	Point3 c = (v[0] + v[1] + v[2]) /3.0f;
	Point3 v0 = v[0]-c;
	Point3 v1 = v[1]-c;
	Point3 v2 = v[2]-c;
	float d0 = DotProd(dn0,v0)/LengthSquared(v0);
	float d1 = DotProd(dn1,v1)/LengthSquared(v1);
	float d2 = DotProd(dn2,v2)/LengthSquared(v2);
	float ad0 = (float)fabs(d0);
	float ad1 = (float)fabs(d1);
	float ad2 = (float)fabs(d2);
	return (ad0>ad1)? (ad0>ad2?d0:d2): ad1>ad2?d1:d2;
}

void PlayerTurret::rotateTurret()
{
	Vector2 mousetoposition = Vector2((float)Input::GetMouseX(), (float)Input::GetMouseY()) - parentPosition;

	if (LengthSquared(mousetoposition) != 0)
	{
		rotationNormal = Engine::Normalized(mousetoposition);
	}
}

  float SSEVector3::Length() const
  {
    float result[ 4 ];
    float lengthSquared = LengthSquared();

    // Store in all floats, do not multiply fourth value: 0111 1111
    const int mask = 0x7F;
    _mm_store_ss( result, _mm_sqrt_ss( _mm_dp_ps( vec, vec, mask ) ) );
    return result[ 0 ];
  }

Vector3 Vector3::Normalize(const Vector3& v)
{
	float lengthSq = LengthSquared(v);
	if (lengthSq == 0.0f)
	{
		return v;
	}

	return v / static_cast<float>(Math::Sqrt(lengthSq));
}

	void magnet::update()
	{
		if(!is_activated())
		{
			return;
		}

		auto mag_pos = body_->GetWorldPoint(local_point_);

		auto ce = body_->GetContactList();
		//			std::set< b2Body* > processed;
		while(ce)
		{
			auto contact = ce->contact;
			if(contact->IsTouching())
			{
				auto fixA = contact->GetFixtureA();
				auto fixB = contact->GetFixtureB();

				//					b2Body* obj = nullptr;
				b2Fixture* fix = nullptr;
				if(fixA == sensor_)
				{
					//						obj = fixB->GetBody();
					fix = fixB;
				}
				else if(fixB == sensor_)
				{
					//						obj = fixA->GetBody();
					fix = fixA;
				}

				//					if(obj != nullptr && processed.find(obj) == processed.end())
				if(fix)
				{
					b2MassData md;
					fix->GetMassData(&md);
					auto metallic_mass = md.mass; // TODO: composition
					auto obj_pos = fix->GetBody()->GetWorldPoint(md.center);
					auto vec = mag_pos - obj_pos;
					auto sqr_dist = vec.LengthSquared();
					auto magnitude = strength_ * metallic_mass / sqr_dist;

					vec *= magnitude / std::sqrt(sqr_dist);
					fix->GetBody()->ApplyForce(vec, obj_pos, true);
					body_->ApplyForce(-vec, mag_pos, true);

					//						processed.insert(obj);
				}
			}

			ce = ce->next;
		}
	}