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

LScrMatrix4D LScrMatrix4D::diag3() const
{
  Matrix3D mat;
  for (int i=1; i<=3; ++i)
    for (int j=1; j<=3; ++j)
      mat.aij(i,j) = aij(i,j);

  Matrix3D evecs;
  Vector4D evals;
  mat.diag(evecs, evals);

  LScrMatrix4D rval;
  for (int i=1; i<=3; ++i)
    for (int j=1; j<=3; ++j)
      rval.aij(i,j) = evecs.aij(i,j);

  rval.aij(4,1) = evals.x();
  rval.aij(4,2) = evals.y();
  rval.aij(4,3) = evals.z();

  if ( rval.aij(4,1)>rval.aij(4,2) )
    swapcols(rval, 1, 2);
  if ( rval.aij(4,1)>rval.aij(4,3) )
    swapcols(rval, 1, 3);
  if ( rval.aij(4,2)>rval.aij(4,3) )
    swapcols(rval, 2, 3);

  return rval;
}

		/**
		 * Method is used to add heightfield actor to physics scene.
		 * @param	entity is actor logic object.
		 * @param	heightData is terrain height map data.
		 * @param	params is terrain parameters: heightmap size, row/column scale, max. height.
		 * @param	filterGroup is actor own id.
		 * @param	filterMask is mask to filter pairs that trigger a contact callback.
		 */
		void PhysicsManager::addHeightFieldActor(SceneEntity* entity, unsigned char* heightData, const Vector4D& params, PxU32 filterGroup, PxU32 filterMask)
		{
			int terrainSize = static_cast<int>(params.x());

			PxHeightFieldSample* samples = new PxHeightFieldSample[terrainSize*terrainSize];
			PxHeightFieldDesc descriptor;
			descriptor.nbColumns = static_cast<int>(terrainSize);
			descriptor.nbRows = static_cast<int>(terrainSize);
	
			for(int i = 0; i < terrainSize*terrainSize; ++i)
				samples[i].height = static_cast<PxI16>(heightData[i]);

			descriptor.samples.data = samples;
			descriptor.samples.stride = sizeof(PxHeightFieldSample);

			PxHeightField* heightField = physicsSDK->createHeightField(descriptor);
			PxHeightFieldGeometry geometry(heightField, PxMeshGeometryFlags(),params.w()*0.00390625f,params.y(),params.z());
			PxTransform transformation = PxTransform(PxVec3(-terrainSize*0.5f*params.y(),0.0f,-terrainSize*0.5f*params.z()),PxQuat::createIdentity());

			PxRigidStatic* heightFieldActor = physicsSDK->createRigidStatic(transformation);
			PxShape* aHeightFieldShape = heightFieldActor->createShape(geometry,*materials[0].second);
			heightFieldActor->setName(entity->entityName.c_str());
			setupFiltering(heightFieldActor,filterGroup,filterMask);
			scene->addActor(*heightFieldActor);
			
			StaticActor* s = new StaticActor();
			s->entityLogic = entity;
			s->entityPhysics = heightFieldActor;
			staticActors.push_back(s);
		}

		// Get the distance from this vector to the other one squared.
		// NJS: note, VC wasn't inlining it correctly in several deeply nested inlines due to being an 'out of line' .  
		// may be able to tidy this up after switching to VC7
		vec_t DistToSqr( const Vector4D &vOther ) const {
			Vector4D delta;

			delta.x = x - vOther.x;
			delta.y = y - vOther.y;
			delta.z = z - vOther.z;
			delta.w = w - vOther.w;

			return delta.LengthSqr();
		}

    /// Computes normal vector for object
    /// \param point point in space, where w = 1 and point is near object (almost hit or hit)
    /// \return normal vector
    virtual Vector4D Normal(const Point4D& point) const {
        static double e = 0.00005;
        Vector4D n;

        n[0] = dist(MakeVector4(point[0] + e, point[1], point[2])) - dist(MakeVector4(point[0] - e, point[1], point[2]));
        n[1] = dist(MakeVector4(point[0], point[1] + e, point[2])) - dist(MakeVector4(point[0], point[1] - e, point[2]));
        n[2] = dist(MakeVector4(point[0], point[1], point[2] + e)) - dist(MakeVector4(point[0], point[1], point[2] - e));
      
        return n.Norm();
    }

void MolArrayMap::convertf(qlib::Array<float> &refary)
{
  int i;
  const_iterator iter = begin();
  for (i=0; iter!=end(); ++iter, ++i) {
    Vector4D pos = iter->first.pA->getPos();
    refary[i*3] = (float)pos.x();
    refary[i*3+1] = (float)pos.y();
    refary[i*3+2] = (float)pos.z();
  }
}

void LScrVector4D::setStrValue(const LString &val)
{
  Vector4D vec;
  if (!Vector4D::fromStringS(val, vec)) {
    LString msg = LString::format("cannot convert \"%s\" to vector", val.c_str());
    MB_THROW(qlib::RuntimeException, msg);
    return;
  }

  for (int i=1; i<=4; ++i)
    ai(i) = vec.ai(i);
}

void MolSurfBuilder::drawArc(const Vector4D &n, double rad, const Vector4D &cen,
             const Vector4D &vst, double theta2)
{
  const Vector4D &e1 = n;
  const Vector4D e2 = vst.normalize();
  const Vector4D e3 = e1.cross(e2);

  Matrix4D xfmat = Matrix4D::makeTransMat(cen);

  xfmat.aij(1,1) = e2.x();
  xfmat.aij(2,1) = e2.y();
  xfmat.aij(3,1) = e2.z();

  xfmat.aij(1,2) = e3.x();
  xfmat.aij(2,2) = e3.y();
  xfmat.aij(3,2) = e3.z();

  xfmat.aij(1,3) = e1.x();
  xfmat.aij(2,3) = e1.y();
  xfmat.aij(3,3) = e1.z();

  m_pdl->pushMatrix();
  m_pdl->multMatrix(xfmat);
  /*m_pdl->cylinder(0.05, Vector4D(0,0,0), e1);
    m_pdl->cylinder(0.05, Vector4D(0,0,0), e2);
    m_pdl->cylinder(0.05, Vector4D(0,0,0), e3);*/

  /*
    m_pdl->color_3d(1, 0, 0);
    m_pdl->cylinder(0.05, Vector4D(0,0,0), Vector4D(1,0,0));
    m_pdl->color_3d(0, 1, 0);
    m_pdl->cylinder(0.05, Vector4D(0,0,0), Vector4D(0,1,0));
    m_pdl->color_3d(0, 0, 1);
    m_pdl->cylinder(0.05, Vector4D(0,0,0), Vector4D(0,0,1));
   */

  const double arclen = qlib::abs(theta2 * rad);
  int ndiv = int(arclen/0.1);
  if (ndiv<5)
    ndiv = 5;
  const double dth = theta2/double(ndiv);
  //MB_DPRINTLN("arclen: %f, ndiv: %d, dth: %f", arclen, ndiv, dth);

  int i;
  double th = 0.0;
  m_pdl->setLighting(false);
  m_pdl->startLineStrip();
  for (i=0; i<ndiv+1; ++i) {
    m_pdl->vertex(rad*::cos(th), rad*::sin(th), 0.0);
    th += dth;
  }
  m_pdl->end();
  m_pdl->setLighting(true);
  m_pdl->popMatrix();
}

Quaternions Quaternions::qFromAngleAxis(float theta, Vector4D axis)
{
	Vector4D axisn = axis.normalize();
	Quaternions q;
	float a = theta * (float)DEGREES_TO_RADIANS;
	q.t = cos(a / 2.0f);
	float s = sin(a / 2.0f);
	q.x = axisn.getX() * s;
	q.y = axisn.getY() * s;
	q.z = axisn.getZ() * s;

	qClean(q);
	return qNormalize(q);
}

void RGB2HSV(const Vector4D &normalizedRGB, float &H, float &s, float &v)
{
    float fmax = max(normalizedRGB.x, max(normalizedRGB.y, normalizedRGB.z));
    float fmin = min(normalizedRGB.x, min(normalizedRGB.y, normalizedRGB.z));

    v = fmax;

    if (fmax <= 0.0f || normalizedRGB.LengthSqr() <= 0.0f)
        s = 0.0f;
    else
        s = (fmax - fmin) / fmax;

    if (fmax == fmin || (normalizedRGB.x == normalizedRGB.y && normalizedRGB.x == normalizedRGB.z))
    {
        H = 0; //-1.0f;
    }
    else if (normalizedRGB.x >= fmax)
        H = 60.0f * ((normalizedRGB.y - normalizedRGB.z) / (fmax - fmin));
    else if (normalizedRGB.y >= fmax)
        H = 60.0f * (2 + (normalizedRGB.z - normalizedRGB.x) / (fmax - fmin));
    else if (normalizedRGB.z >= fmax)
        H = 60.0f * (4 + (normalizedRGB.x - normalizedRGB.y) / (fmax - fmin));
    if (H < 0)
        H += 360.0f;
}

void MolSurfBuilder::drawDisk(const Vector4D &cen, const Vector4D &norm, double rad)
{
  const double thik = 0.05;
  const Vector4D start = cen - norm.scale(thik/2.0);
  const Vector4D end   = cen + norm.scale(thik/2.0);
  m_pdl->cylinderCap(rad, start, end);
}

void Vector4DMultiplyPosition( const VMatrix& src1, Vector const& src2, Vector4D& dst )
{
	// Make sure it works if src2 == dst
	Vector tmp;
	Vector const&v = ( &src2 == &dst.AsVector3D() ) ? static_cast<const Vector>(tmp) : src2;

	if (&src2 == &dst.AsVector3D())
	{
		VectorCopy( src2, tmp );
	}

	dst[0] = src1[0][0] * v[0] + src1[0][1] * v[1] + src1[0][2] * v[2] + src1[0][3];
	dst[1] = src1[1][0] * v[0] + src1[1][1] * v[1] + src1[1][2] * v[2] + src1[1][3];
	dst[2] = src1[2][0] * v[0] + src1[2][1] * v[1] + src1[2][2] * v[2] + src1[2][3];
	dst[3] = src1[3][0] * v[0] + src1[3][1] * v[1] + src1[3][2] * v[2] + src1[3][3];
}

//------------------------------------------------------------------------------------------------------
//function that multiplies two Matrix objects together
//------------------------------------------------------------------------------------------------------
Matrix4D& Matrix4D::operator*(Matrix4D& rhs)
{

	//variable to keep track of each matrix element of final result
	int count = 0;
	
	//the final matrix result object and two Vector4D objects 
	//are needed to calculate each row and column multiplication
	Matrix4D result;
	Vector4D<float> leftRow;
	Vector4D<float> topColumn;
	
	//loop through each of the top matrix columns
	for(int i = 0; i < 4; i++)
	{

		//assign the elements from top to bottom 
		topColumn.X = rhs[i * 4];
		topColumn.Y = rhs[i * 4 + 1];
		topColumn.Z = rhs[i * 4 + 2];
		topColumn.W = rhs[i * 4 + 3];

		//loop through each of the left matrix rows
		for(int j = 0; j < 4; j++)
		{

			//assign the elements from left to right 
			leftRow.X = m_matrix[j];
			leftRow.Y = m_matrix[j + 4];
			leftRow.Z = m_matrix[j + 8];
			leftRow.W = m_matrix[j + 12];
						
			//use dot product to produce each matrix element result
			result[count++] = leftRow.DotProduct(topColumn);

		}

	}

	//assign result to Matrix object and return reference
	//of lhs matrix to allow for multiplication chaining
	return (*this = result);

}

Vector4D	Vector4D :: getRandomVector ( float len )
{
	Vector4D	v;

	for ( ; ; )
	{
		v.x = rnd ();
		v.y = rnd ();
		v.z = rnd ();
		v.w = rnd ();

		if ( v.lengthSq () < EPS )
			continue;

		v *= len / v.length ();

		return v;
	}
}

	vec_t NormalizeVector(Vector4D& v) {
		vec_t l = v.Length();
		if (l != 0.0f) {
			v /= l;
		}
		else {
			v.x = v.y = v.z = v.w = 0.0f;
		}
		return l;
	}

//-----------------------------------------------------------------------------
// Converts a color + alpha into a vector4
//-----------------------------------------------------------------------------
void CBaseVSShader::ColorVarsToVector( int colorVar, int alphaVar, Vector4D &color )
{
	color.Init( 1.0, 1.0, 1.0, 1.0 ); 
	if ( colorVar != -1 )
	{
		IMaterialVar* pColorVar = s_ppParams[colorVar];
		if ( pColorVar->GetType() == MATERIAL_VAR_TYPE_VECTOR )
		{
			pColorVar->GetVecValue( color.Base(), 3 );
		}
		else
		{
			color[0] = color[1] = color[2] = pColorVar->GetFloatValue();
		}
	}
	if ( alphaVar != -1 )
	{
		float flAlpha = s_ppParams[alphaVar]->GetFloatValue();
		color[3] = clamp( flAlpha, 0.0f, 1.0f );
	}
}

qlib::Vector4D RendGroup::getCenter() const
{
  // Calc COM of renderers in this group
  Vector4D resvec;
  int nsum = 0;
  ObjectPtr pObj = getClientObj();
  Object::RendIter iter = pObj->beginRend();
  Object::RendIter eiter = pObj->endRend();
  for (;iter!=eiter;++iter) {
    RendererPtr pRend = iter->second;
    if (!pRend->getGroupName().equals(getName()))
      continue;
    if (pRend->hasCenter()) {
      resvec += pRend->getCenter();
      ++nsum;
    }
  }
  if (nsum>0)
    return resvec.divide(nsum);
  else
    return qlib::Vector4D();
}

Vector4D ElePotMap::convToOrth(const Vector4D &index) const
{
  Vector4D tv = index;

  tv.x() = tv.x() * m_gx;
  tv.y() = tv.y() * m_gy;
  tv.z() = tv.z() * m_gz;

  tv += m_origPos;

  return tv;
}

/** perform interpolation */
bool CubicSpline::interpolate(double par, Vector4D *vec,
                              Vector4D *dvec /*= NULL*/,
                              Vector4D *ddvec /*= NULL*/)
{
  // check parameter value f
  int ncoeff = (int)::floor(par);
  if (ncoeff<0)
    ncoeff = 0;
  if (ncoeff>=(m_nPoints-1))
    ncoeff = m_nPoints-2;

  const Vector4D &coeff0 = m_pCoeff0[ncoeff];
  const Vector4D &coeff1 = m_pCoeff1[ncoeff];
  const Vector4D &coeff2 = m_pCoeff2[ncoeff];
  const Vector4D &coeff3 = m_pCoeff3[ncoeff];

  double f = par - (double)ncoeff;

  Vector4D tmp;
  tmp = coeff3.scale(f) + coeff2;
  tmp = tmp.scale(f) + coeff1;
  tmp = tmp.scale(f) + coeff0;
  *vec = tmp;

  if (dvec != NULL) {
    // calculate tangential vector
    tmp = coeff3.scale(3.0*f) + coeff2.scale(2.0);
    tmp = tmp.scale(f) + coeff1;
    *dvec = tmp;
  }

  if (ddvec != NULL) {
    // calculate curvature vector
    tmp = coeff3.scale(6.0*f) + coeff2.scale(2.0);
    *ddvec = tmp;
  }
  return true;
}

bool Serialize( CUtlBuffer &buf, const Vector4D &src )
{
	if ( buf.IsText() )
	{
		SerializeFloats( buf, 4, src.Base() );
	}
	else
	{
		buf.PutFloat( src.x );
		buf.PutFloat( src.y );
		buf.PutFloat( src.z );
		buf.PutFloat( src.w );
	}
	return buf.IsValid();
}