C++ math::Vector3类代码示例

bool RaySphere(const Vector3& rayStart, const Vector3& rayDir,
               const Vector3& sphereCenter, float sphereRadius,
               float& t)
{
  ++Application::mStatistics.mRaySphereTests;
  //project sphere center on to line
  Math::Vector3 sphereVec = sphereCenter - rayStart;
  float floatingPointEpsilon = 0.0001f;
  float tClosestToRay = sphereVec.Dot(rayDir);
  if(tClosestToRay >= 0.0f)
  {
    Math::Vector3 pointClosestToSphere = rayStart + (rayDir * tClosestToRay);
    
    if((sphereCenter - pointClosestToSphere).LengthSq() <= (sphereRadius * sphereRadius))
    {
      //may be inside sphere, so we must calculate the length of ray that is inside the sphere and subtract
      //we can use cosine for this.
      //                                             adjacent                           hypotenuse
      float lenInSphere;
      if((sphereCenter - pointClosestToSphere).LengthSq() > floatingPointEpsilon)
      {
        lenInSphere = Math::ArcCos((sphereCenter - pointClosestToSphere).Length() / sphereRadius);
      }
      else
      {
        lenInSphere = sphereRadius;
      }
      //since raydir should be unit length, we can simply subtract this from our closest t
      //we max to make sure that we don't subtract past the start of the ray
      t = Math::Max(tClosestToRay - lenInSphere, 0.0f);
      return true;
    }
  }
  return false;
}

float DistFromPlane(const Vector3& point, const Vector3& normal, float planeDistance)
{
  //compute the vector from plane's point to poi
  Math::Vector3 vectorToPoint = point - (normal * planeDistance);

  //project this vector on to the normal to find the distance from the plane
  return vectorToPoint.Dot(normal);
}

void TCurveBase::normal(Math::Vector3 &normal, const Math::Vector3 &point) const
{
    Math::Vector2 d;

    derivative(point.project_xy(), d);

    normal = Math::Vector3(d.x(), d.y(), -1.0);
    normal.normalize();
}

 void parse (StringParser& p, Math::Vector3<T>& out) {
   size_t start = p.pos ();
   p.expect (typeid (Math::Vector3<T>), start, '(');
   parse (p, out.x ());
   p.expect (typeid (Math::Vector3<T>), start, ',');
   parse (p, out.y ());
   p.expect (typeid (Math::Vector3<T>), start, ',');
   parse (p, out.z ());
   p.expect (typeid (Math::Vector3<T>), start, ')');
 }

void TLens::set_thickness(double thickness, unsigned int index)
{
    double diff = thickness - get_thickness(index);

    for (unsigned int i = index; i < _surfaces.size(); i++)
    {
        Math::Vector3 p = _surfaces[i].get_local_position();
        p.z() += diff;
        _surfaces[i].set_local_position(p);
    }

    _last_pos += diff;
}

bool RayTriangle(const Vector3& rayStart, const Vector3& rayDir,
                 const Vector3& triP0, const Vector3& triP1, const Vector3& triP2,
                 float& t, float triExpansionEpsilon)
{
  ++Application::mStatistics.mRayTriangleTests;
  Math::Vector3 normal = (triP1 - triP0).Cross(triP2 - triP0).Normalized();
  if(RayPlane(rayStart, rayDir, Math::Vector4(normal.x, normal.y, normal.z, normal.Dot(triP0)), t))
  {
    Math::Vector3 pointOnPlane = (rayDir * t) + rayStart;
    Math::Vector3 barycentric;
    return BarycentricCoordinates(pointOnPlane, triP0, triP1, triP2, barycentric.x, barycentric.y, barycentric.z, triExpansionEpsilon);
  }
  return false;
}

 void Box::createDipoleGeometry (DipoleGeometry& dipoleGeometry) const {
   Math::Vector3<ldouble> size = this->size () / dipoleGeometry.gridUnit ();
   size_t mat = dipoleGeometry.materials ().size ();
   dipoleGeometry.materials ().push_back (material ());
   for (uint32_t z = 0; z < size.z (); z++) {
     for (uint32_t y = 0; y < size.y (); y++) {
       for (uint32_t x = 0; x < size.x (); x++) {
         ASSERT (mat <= 255);
         dipoleGeometry.addDipole (x, y, z, static_cast<uint8_t> (mat));
       }
     }
   }
   dipoleGeometry.moveToCenter ();
 }

//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
//see http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?f=9&t=4152&hilit=raytest
//for reference
void 
PhysicsZone::rayCast(Math::Ray _ray, Math::Real _maxDistance, I_CollisionVisitor& _visitor)
{
    Math::Vector3 offset = _ray.getDirection();
    offset.normalize();
    offset *= _maxDistance;
    Math::Point3 endpoint = _ray.getOrigin() + offset;

    //(m_pZone, _ray.getOrigin().m_array, endpoint.m_array, rayCastFilter, &rayCastQuery, rayCastPrefilter);

    btVector3 tquatFrom = btVector3(_ray.getOrigin().m_x,_ray.getOrigin().m_y,_ray.getOrigin().m_z);
    btVector3 tquatTo = btVector3(_ray.getOrigin().m_x,_ray.getOrigin().m_y,_ray.getOrigin().m_z);

	RayResultCallback	resultCallback(tquatFrom,tquatTo);
   
	m_pZone->rayTest(tquatFrom,tquatTo,resultCallback);
}

void GameProjectile::launch( const math::Vector3& direction )
{
	setVelocity( direction.normalize() * m_launchVelocity );
	m_removable = false;
	m_hasHit = false;
	m_age = 0;
	m_movedDistance = 0;
}

//-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~-~
// set the transformation of a rigid body
void 
PhysicsActor::TransformCallback(const NewtonBody* _body, const Zen::Math::Real* _matrix)
{
    void* pBody = NewtonBodyGetUserData(_body);
    if (pBody != NULL)
    {
        PhysicsActor* pShape = static_cast<PhysicsActor*>(pBody);

        // Only use the transform callback if the state is active
        if (pShape->m_activationState != 0)
        {
            Math::Matrix4 transform;
            for(int x = 0; x < 16; x++)
            {
                transform.m_array[x] = _matrix[x];
            }
            TransformEventData evenData(*pShape, transform);
            pShape->onTransformEvent(evenData);
        }
    }

#if 0
    // HACK Keep the object to a constant velocity
    Math::Vector3 velocity;

    NewtonBodyGetVelocity(_body, velocity.m_array);

    velocity.normalize();
    velocity = velocity * 50;

    NewtonBodySetVelocity(_body, velocity.m_array);
#endif

    //std::cout << "TransformCallback()" << std::endl;
#if 0
	RenderPrimitive* primitive;

	// get the graphic object form the rigid body
	primitive = (RenderPrimitive*) NewtonBodyGetUserData (body);

	// set the transformation matrix for this rigid body
	dMatrix& mat = *((dMatrix*)matrix);
	primitive->SetMatrix (mat);
#endif
}

void Node::lookAt( const math::Vector3& target, const math::Vector3& up )
{
	assert( Math::abs(up.length()-1.f) < 1e-3f ); // Up direction must be normalized

	// src parent->world space
	Matrix4x4 parentToWorld = Matrix4x4(1);
	const Node* parent = this->parent();
	while ( parent )
	{
		parentToWorld = parent->transform() * parentToWorld;
		parent = parent->parent();
	}

	// src->world space
	Matrix4x4 sourceToWorld = parentToWorld * transform();

	// src -> target (world space)
	Vector3 sourceRotZ = target - sourceToWorld.translation();
	if ( sourceRotZ.lengthSquared() > Float::MIN_VALUE )
	{
		// src->target direction (world space)
		sourceRotZ = sourceRotZ.normalize();

		// src rotation (world space)
		Vector3 sourceRotX = up.cross( sourceRotZ );
		if ( sourceRotX.lengthSquared() > Float::MIN_VALUE )
			sourceRotX = sourceRotX.normalize();
		else
			sourceRotX = Vector3(1,0,0);
		Vector3 sourceRotY = sourceRotZ.cross( sourceRotX );
		Matrix3x3 sourceRot;
		sourceRot.setColumn( 0, sourceRotX );
		sourceRot.setColumn( 1, sourceRotY );
		sourceRot.setColumn( 2, sourceRotZ );

		// src world space rotation back to src parent space
		Matrix3x3 parentToWorldRot = parentToWorld.rotation();
		Matrix3x3 rot = sourceRot * parentToWorldRot.inverse();
		setRotation( rot );
	}
}

  DipoleGeometry::DipoleGeometry (ldouble gridUnit,
                                  Math::Vector3<ldouble> periodicity1,
                                  Math::Vector3<ldouble> periodicity2)
    : box_ (0, 0, 0), matCount_ (0), validNvCount_ (0), origin_ (0, 0, 0),
      orientation_ (EMSim::Rotation<ldouble>::none ()),
      orientationInverse_ (EMSim::Rotation<ldouble>::none ()),
      gridUnit_ (gridUnit),
      periodicity1_ (periodicity1),
      periodicity2_ (periodicity2)
  {
    ASSERT (gridUnit >= 0);

    bool periodicity1IsZero = periodicity1.x () == 0 && periodicity1.y () == 0 && periodicity1.z () == 0;
    bool periodicity2IsZero = periodicity2.x () == 0 && periodicity2.y () == 0 && periodicity2.z () == 0;
    if (periodicity1IsZero) {
      ASSERT (periodicity2IsZero);
      periodicityDimension_ = 0;
    } else {
      if (periodicity2IsZero) {
        periodicityDimension_ = 1;
      } else {
        periodicityDimension_ = 2;
      }
    }

    gridUnitVol_ = gridUnit * gridUnit * gridUnit;
  }

 template <typename T> inline Math::Vector3<T> operator* (SymMatrix3<T> m, Math::Vector3<T> v) {
   return Math::Vector3<T> (
                            m.aa()*v.x() + m.ab()*v.y() + m.ac()*v.z(),
                            m.ab()*v.x() + m.bb()*v.y() + m.bc()*v.z(),
                            m.ac()*v.x() + m.bc()*v.y() + m.cc()*v.z()
                            );
 }

DebugShape& DebugDrawer::DrawSphere(const Sphere& sphere)
{
  // To access the camera's position for the horizon disc calculation use mApplication->mCamera.mTranslation
  DebugShape& shape = GetNewShape();
  int it;

  std::vector<LineSegment> segments[4];

  segments[0] = makeCircle(sphere.mCenter, Math::Vector3(1.0f, 0.0f, 0.0f), sphere.mRadius);

  
  segments[1] = makeCircle(sphere.mCenter, Math::Vector3(0.0f, 1.0f, 0.0f), sphere.mRadius);


  segments[2] = makeCircle(sphere.mCenter, Math::Vector3(0.0f, 0.0f, 1.0f), sphere.mRadius);

  if(mApplication)
  {
    //horizon circle
    Math::Vector3 dVec = sphere.mCenter - mApplication->mCamera.mTranslation;
    Math::Vector3 dVecNorm = dVec.Normalized();
    float radSquared = sphere.mRadius * sphere.mRadius;
    float lLen = Math::Sqrt(dVec.LengthSq() - radSquared);
    float rPrime = (sphere.mRadius * lLen) / dVec.Length();
    float z = radSquared - (rPrime * rPrime);
    Math::Vector3 newCenter = mApplication->mCamera.mTranslation + (dVec - (dVecNorm * z));
    segments[3] = makeCircle(newCenter, dVecNorm, rPrime);
  }
  
  
  

  for(int i = 0; i < 4; ++i)
  {
    shape.mSegments.insert(shape.mSegments.end(), segments[i].begin(), segments[i].end());
  }

  return shape;
}

            /*!
             * Alias for glVertex(x,y,z).
             */
            inline void tglVertex(const math::Vector3& v)
            {
#if defined(TAU_SCALAR_DOUBLE)
                glVertex3d(v.getX(), v.getY(), v.getZ());
#else
                glVertex3f(v.getX(), v.getY(), v.getZ());
#endif
            }