Vector3F LinearSpline::getPositionAtTime(unsigned int length, unsigned int time)
{
	float splineLength = 0;
	std::vector<float> segmentLengths;
	for (size_t i = 1; i < mNodes.size(); ++i) {
		float segmentLength = (mNodes[i - 1]->position.value() - mNodes[i]->position.value()).length();
		splineLength += segmentLength;
		segmentLengths.push_back(splineLength);
	}
	float step = splineLength / length;
	float segment = time * step;
	size_t segmentIndex = 0;
	while (segment > segmentLengths[segmentIndex])
		segmentIndex++;

	float segmentRemains = segment - (segmentIndex > 0 ? segmentLengths[segmentIndex - 1] : 0);

	Vector3F segmentToUse = mNodes[segmentIndex + 1]->position.value() - mNodes[segmentIndex]->position.value();
	float segmentToUseLength = segmentToUse.length();

	float segmentPercentage = segmentRemains / segmentToUseLength;

	Vector3F segmentFraction = segmentToUse * segmentPercentage;

	return mNodes[segmentIndex]->position.value() + segmentFraction;
}

void BaseView::tumble(int dx, int dy, int portWidth)
{
	Vector3F side  = m_space.getSide();
	Vector3F up    = m_space.getUp();
	Vector3F front = m_space.getFront();
	Vector3F eye = m_space.getTranslation();	
	Vector3F toEye = eye - m_centerOfInterest;
	float dist = toEye.length();
	const float scaleing = dist * 2.f / (float)portWidth;
	eye -= side * (float)dx * scaleing;
	eye += up * (float)dy * scaleing;
	
	toEye = eye - m_centerOfInterest;
	toEye.normalize();
	
	eye = m_centerOfInterest + toEye * dist;
	m_space.setTranslation(eye);
	
	front = toEye;
	
	side = up.cross(front);
	side.y = 0.f;
	side.normalize();
	
	up = front.cross(side);
	up.normalize();
	
	m_space.setOrientations(side, up, front);
	
	m_invSpace = m_space;
	m_invSpace.inverse();
}

void TransformManipulator::spin(const Vector3F & d)
{
	Matrix44F ps;
	parentSpace(ps);
	Matrix44F invps = ps;
	invps.inverse();
	
	const Vector3F worldP = ps.transform(translation());
	const Vector3F rotUp = ps.transformAsNormal(hitPlaneNormal());
	
	Vector3F toa = m_currentPoint - worldP;
	Vector3F tob = toa + d;
	
	toa.normalize();
	tob.normalize();
	float ang = toa.angleBetween(tob, toa.cross(rotUp).reversed());
	
	Vector3F angles;
	
	if(m_rotateAxis == AY) angles.set(0.f, ang, 0.f);
	else if(m_rotateAxis == AZ) angles.set(0.f, 0.f, ang);
	else angles.set(ang, 0.f, 0.f);
	
	m_subject->rotate(angles);
	setRotationAngles(m_subject->rotationAngles());
}

void MeshManipulator::smoothSurface(const Ray * r)
{
	VertexAdjacency adj = m_topo->getAdjacency(m_intersect->m_componentIdx);
	
	Vector3F *p = &m_mesh->vertices()[m_intersect->m_componentIdx];
    Vector3F d = adj.center() - *p;
	*p += d * .7f;
	
	Plane pl(m_intersect->m_hitN, m_intersect->m_hitP);

    Vector3F hit;
    float t;
	if(!pl.rayIntersect(*r, hit, t, 1)) return;
	
	d = hit - *p;
	float minD = d.length();
	float curD;
	
	VertexAdjacency::VertexNeighbor *neighbor;
    for(neighbor = adj.firstNeighbor(); !adj.isLastNeighbor(); neighbor = adj.nextNeighbor()) {
        d = hit - *(neighbor->v->m_v);
        curD = d.length();
		if(curD < minD) {
			minD = curD;
			m_intersect->m_componentIdx = neighbor->v->getIndex();
		}
    }
}

void Plane::ProjectToPlane( const Vector3F& _vector, Vector3F* projected ) const
{
	// A || B = B x (Ax B / |B|) / |B|
	Vector3F vector = _vector;
	vector.Normalize();

	Vector3F AcB;
	CrossProduct( vector, n, &AcB );
	CrossProduct( n, AcB, projected );
	projected->Normalize();


	/*
	GLASSERT( Equal( n.Length(), 1.0f, EPSILON ) );

	Vector3F vector = _vector;
	vector.Normalize();

	Vector3F pn;	
	CrossProduct( n, vector, &pn );
	CrossProduct( pn, n, projected );
	*/
	
	#ifdef DEBUG
	{
		float len = projected->Length();
		GLASSERT( Equal( len, 1.0f, EPSILON ) );

		Vector3F test = { 0.0f, 0.0f, Z( 0.0f, 0.0f ) };
		test = test + (*projected);
		float z = Z( projected->x, projected->y );
		GLASSERT( Equal( test.z, z, 0.0001f ) );
	}
	#endif
}

/*---------------------------------------------------------------------*//**
	全シェイプを含めたバウンディングボックスを計算する
**//*---------------------------------------------------------------------*/
void ShapeModel::getAllShapesBoundingBox(Vector3F* vBbCenter, f32* rBb)
{
	if(_vShapesBbCenter == 0L)	// 未計算
	{
		Vector3F vBbMinWk;
		Vector3F vBbMaxWk;

		// 全シェイプ分を計算する
		Vector3F vBbMinS;
		Vector3F vBbMaxS;
		_sarrShape[0]->getBoundingBox(&vBbMinWk, &vBbMaxWk);
		for(int i = 1; i < (int)_numShape; i++)
		{
			_sarrShape[i]->getBoundingBox(&vBbMinS, &vBbMaxS);
			if(vBbMinWk._v[0] > vBbMinS._v[0])	{	vBbMinWk._v[0] = vBbMinS._v[0];	}
			if(vBbMinWk._v[1] > vBbMinS._v[1])	{	vBbMinWk._v[1] = vBbMinS._v[1];	}
			if(vBbMinWk._v[2] > vBbMinS._v[2])	{	vBbMinWk._v[2] = vBbMinS._v[2];	}
			if(vBbMaxWk._v[0] < vBbMaxS._v[0])	{	vBbMaxWk._v[0] = vBbMaxS._v[0];	}
			if(vBbMaxWk._v[1] < vBbMaxS._v[1])	{	vBbMaxWk._v[1] = vBbMaxS._v[1];	}
			if(vBbMaxWk._v[2] < vBbMaxS._v[2])	{	vBbMaxWk._v[2] = vBbMaxS._v[2];	}
		}

		// 取得済みとして保存する
		_vShapesBbCenter = new Vector3F((vBbMinWk.x() + vBbMaxWk.x()) * 0.5f, (vBbMinWk.y() + vBbMaxWk.y()) * 0.5f, (vBbMinWk.z() + vBbMaxWk.z()) * 0.5f);
		_rShapesBb = (vBbMaxWk - vBbMinWk).length() * 0.5f;
	}

	if(vBbCenter != 0L)	{	vBbCenter->copy(_vShapesBbCenter);	}
	if(rBb != 0L)		{	*rBb = _rShapesBb;					}
}

	float AABB::GetDistanceSqr(Vector3F const& v) const
	{
		Vector3F vNear = v;
		vNear.CheckMax(min);
		vNear.CheckMin(max);
		return (vNear + v).LengthSquared();
	}

Vector3F AccCorner::computeNormal() const
{
	if(isOnBoundary())
		return *_centerNormal * (2.f / 3.f) +  _edgeNormals[0] * (1.f / 6.f) +  _edgeNormals[valence() - 1] * (1.f / 6.f);
		
	float e = 4.f;
	float c = 1.f;
	float sum = 0.f;
	Vector3F res;
	res.setZero();
	Vector3F q;
	
	for(int i = 0; i < valence(); i++) {
		q = _edgeNormals[i];
		res += q * e;
		sum += e;
		q = _cornerNormals[i];
		res += q * c;
		sum += c;
	}
	
	sum += valence() * valence();
	res += *_centerNormal * valence() * valence();
	return res / sum;
}

void Lilith3D::IntersectRayFromScreen( int x, int y, int flags, LilithObjectList* vec )
{
	double p0x, p0y, p0z, p1x, p1y, p1z;

	double modelView[ 16 ];
	glGetDoublev( GL_MODELVIEW_MATRIX, modelView );
	double projection[ 16 ];
	glGetDoublev( GL_PROJECTION_MATRIX, projection );
	int viewport[ 4 ];
	glGetIntegerv( GL_VIEWPORT, viewport );

	gluUnProject( (double) x, (double) ( viewport[3]-y ), 0,
							  modelView, projection, viewport,
							  &p0x, &p0y, &p0z );

	gluUnProject( (double) x, (double) ( viewport[3]-y ), 1,
							  modelView, projection, viewport,
							  &p1x, &p1y, &p1z );

	//GLOUTPUT( "ret0=%d ret1=%d\n", ret0, ret1 );

	Vector3F point = { (float) p0x, (float) p0y, (float) p0z };
	Vector3F dir   = { (float) ( p1x-p0x ), (float) ( p1y-p0y ), float( p1z-p0z ) };

	dir.Normalize();
	Ray ray;
	ray.origin = point;
	ray.direction = dir;
	ray.length = FAR_PLANE_DISTANCE;

	IntersectRay( ray, flags, vec );
}

void Matrix::AxisRotate(const Vector3F& mvAxis, float angle)
{
	Vector3F mvNormalizedAxis = mvAxis;
	mvNormalizedAxis.Normalize();

	float c = cosf(angle);
	float s = sinf(angle);
	float x = mvNormalizedAxis.x, y = mvNormalizedAxis.y, z = mvNormalizedAxis.z;

	_11 = x * x * (1 - c) + c;
	_21 = x * y * (1 - c) - (z * s);
	_31 = x * z * (1 - c) + (y * s);
	_41 = 0;

	_12 = y * x * (1 - c) + (z * s);
	_22 = y * y * (1 - c) + c;
	_32 = y * z * (1 - c) - (x * s);
	_42 = 0;

	_13 = z * x * (1 - c) - (y * s);
	_23 = z * y * (1 - c) + (x * s);
	_33 = z * z * (1 - c) + c;
	_43 = 0;

	_14 = 0;
	_24 = 0;
	_34 = 0;
	_44 = 1;
}

//----------------------------------------------------------------------------
void NodeBillboard::UpdateWorldData(Double appTime, Bool updateControllers)
{
    // Compute billboard's world transforms based on its parent's world
    // transform and its local transforms. Notice that you should not call
    // Node::UpdateWorldData since that function updates its children. The
    // children of a NodeBillboard cannot be updated until the billboard is
    // aligned with the camera.
    Spatial::UpdateWorldData(appTime, updateControllers);

    if (mspCamera)
    {
        // Inverse-transform the camera to the model space of the billboard.
        Vector3F camLocation = World.ApplyInverse(mspCamera->GetLocation());

        // To align the billboard, the projection of the camera to the
        // xz-plane of the billboard's model space determines the angle of
        // rotation about the billboard's model y-axis. If the projected
        // camera is on the model axis (x = 0 and z = 0), ATan2 returns zero
        // (rather than NaN), so there is no need to trap this degenerate
        // case and handle it separately.
        Float angle = MathF::ATan2(camLocation.X(), camLocation.Z());
        Matrix34F orientation(Vector3F::UNIT_Y, angle);
        World.SetRotate(World.GetMatrix() * orientation);
    }

    // update the children now that the billboard orientation is known
    for (UInt i = 0; i < mChildren.GetQuantity(); i++)
    {
        Spatial* pChild = mChildren[i];
        if (pChild)
        {
            pChild->UpdateGS(appTime, false, updateControllers);
        }
    }
}

void Sculptor::movePointsToward(const Vector3F & d, const float & fac, bool normalize, Vector3F * vmod)
{
	if(m_active->numSelected() < 1) return;
	Array<int, VertexP> * vs = m_active->vertices;
	
	Vector3F tod;
	float wei;
	vs->begin();
	while(!vs->end()) {
		
		VertexP * l = vs->value();
		wei = *l->index->t4;
		
		const Vector3F p0(*(l->index->t1));
		
		tod = d - *(l->index->t1);
		if(normalize) tod.normalize();
		*(l->index->t1) += tod * fac * wei * m_strength;
		if(vmod) {
			*(l->index->t1) += *vmod * wei * m_strength;
		}
	
		m_tree->displace(l, *(l->index->t1), p0);
		vs->next();
	}
}

int CameraComponent::DoTick(U32 delta)
{
	float EASE = 0.2f;

	switch (mode) {

		case PAN:
		{
			Vector3F d = (dest - camera->PosWC());
			Vector3F c = camera->PosWC();

			if (d.Length() < 0.01f) {
				camera->SetPosWC(dest.x, dest.y, dest.z);
				mode = DONE;
			}
			else {
				camera->SetPosWC(c.x + EASE*d.x, c.y + EASE*d.y, c.z + EASE*d.z);
			}
		}
		break;

		case TRACK:
		{
			Chit* chit = Context()->chitBag->GetChit(targetChitID);
			if (chit) {

				Vector3F pos = chit->Position();
				pos.y = 0;

				// Scoot the camera to always focus on the target. Removes
				// errors that occur from rotation, drift, etc.
				const Vector3F* eye3 = camera->EyeDir3();
				Vector3F at;
				int result = IntersectRayPlane(camera->PosWC(), eye3[0], 1, 0.0f, &at);
				if (result == INTERSECT) {
					Vector3F t = (camera->PosWC() - at);

					//camera->SetPosWC( pos + t );
					Vector3F c = camera->PosWC();
					Vector3F d = (pos + t) - c;

					// If grid moving, the EASE contributes to jitter.
					if (GET_SUB_COMPONENT(chit, MoveComponent, GridMoveComponent)) {
						EASE = 1.0f;
					}
					camera->SetPosWC(c.x + EASE*d.x, pos.y + t.y, c.z + EASE*d.z);
				}
			}
		}
		break;

		case DONE:
		break;

		default:
		GLASSERT(0);
	}
	return 0;
}

void Plane::set(const Vector3F & nor, const Vector3F & pop)
{
    Vector3F nn = nor.normal();
	m_a = nn.x;
	m_b = nn.y;
	m_c = nn.z;
	m_d = - pop.dot(nn);
}

const Vector3F &Turret::getCompassRotation (void)
{
	static Vector3F rot;

	rot.set (0, 0, turretRotation + getRot().z);

	return rot;
}

Vector3F Vector3F::perpendicular() const
{
	Vector3F ref(0,1,0);
	Vector3F n = normal();
	if(n.y < -0.9f || n.y > 0.9f) ref = Vector3F(1,0,0);
	Vector3F per = cross(ref);
	per.normalize();
	return per;
}

Vector3F AccInterior::computeNormal() const
{
	Vector3F res = _cornerNormals[0] * _valence;
	res += _cornerNormals[1] * 2.f;
	res += _cornerNormals[3] * 2.f;
	res += _cornerNormals[2];
	return res / (_valence + 5.f);
	res.normalize();
	return res;
}

float MlRachis::pushToSurface(const Vector3F & wv, const Matrix33F & space)
{
	Vector3F ov = space.transform(wv);
	ov.normalize();
	ov.y = 0.f;
	ov.x += 0.05f;
	ov.normalize();
	float a = acos(Vector3F::ZAxis.dot(ov));
	if(ov.x < 0.f) a = 0.f;
	return a;
}

void GeometryCorrectionTable::apply_correction(const Vector3F& dir, MatrixFr& loop) {
    Float edge_len = dir.norm();
    assert(edge_len > 1e-12);
    Vector3F edge_dir = dir.normalized();
    if (fabs(edge_dir[2]) < 1e-3) {
        apply_correction_to_in_plane_edge(edge_dir, loop);
    } else {
        apply_correction_to_out_plane_edge(edge_dir, loop);
    }
    apply_z_correction(edge_dir, loop);
}

void AccCorner::addCornerNeighborBetween(int a, int b, Vector3F * positions, Vector3F * normals)
{
	_cornerIndices.push_back(a);
	_cornerIndices.push_back(b);
	_tagCornerIndices.push_back(0);
	_tagCornerIndices.push_back(0);
	_cornerPositions.push_back(positions[a] * 0.5f + positions[b] * 0.5f);
	Vector3F an = normals[a] * 0.5f + normals[b] * 0.5f;
	an.normalize();
	_cornerNormals.push_back(an);
}