void CGraphicsCamera::RotateXY( double xInc, double yInc, CWindowFilter& filter )
{
	m_rot.x += xInc;
	if( m_rot.x > M_PI*2 )
		m_rot.x = 0.0;
	else if( m_rot.x < -M_PI*2 )
		m_rot.x = 0.0;
	m_rot.y += yInc;
	if( m_rot.y > M_PI*2 )
		m_rot.y = 0.0;
	else if( m_rot.y < -M_PI*2 )
		m_rot.y = 0.0;
	filter.SetRotations( m_rot );

	//get the quaternion representation of this rotation
	CVector3D right = m_up.cross( CVector3D( m_pos, m_focalPt ).normalize() );
	CQuaternion xquat( xInc, CVector3D( right ).normalize() );
	CQuaternion yquat( yInc, m_up.normalize() );
	CQuaternion total_quat = xquat + yquat;
	total_quat.normalize();

	double m[4][4] = {0.};
	total_quat.QuatToMat( m );
	CVector3D v1( m_pos, m_focalPt );
	//multiply our view vector by the rotation matrix
	v1 = v1*m;
	//update our position
	m_pos = CPoint3D( v1.x + m_focalPt.x, v1.y + m_focalPt.y, v1.z + m_focalPt.z );
	//update our camera's up vector
	m_up = m_up*m;

}

// internal. Based on Equation 15
CVector3D RNSpline::GetEndVelocity(int index)
{
	if (index >= NodeCount || index < 1)
		return CVector3D(0.0f, 0.0f, 0.0f);
	CVector3D temp = CVector3D(Node[index].Position - Node[index-1].Position) * 3.0f * (1.0f / Node[index-1].Distance.ToFloat());
	return (temp - Node[index-1].Velocity) * 0.5f;
}

CVector3D CCamera::GetFocus() const
{
	// Basically the same as GetWorldCoordinates

	CHFTracer tracer(g_Game->GetWorld()->GetTerrain());
	int x, z;

	CVector3D origin, dir, delta, terrainPoint, waterPoint;

	origin = m_Orientation.GetTranslation();
	dir = m_Orientation.GetIn();

	bool gotTerrain = tracer.RayIntersect(origin, dir, x, z, terrainPoint);

	CPlane plane;
	plane.Set(CVector3D(0.f, 1.f, 0.f),										// upwards normal
		CVector3D(0.f, g_Renderer.GetWaterManager()->m_WaterHeight, 0.f));	// passes through water plane

	bool gotWater = plane.FindRayIntersection( origin, dir, &waterPoint );

	// Clamp the water intersection to within the map's bounds, so that
	// we'll always return a valid position on the map
	ssize_t mapSize = g_Game->GetWorld()->GetTerrain()->GetVerticesPerSide();
	if (gotWater)
	{
		waterPoint.X = clamp(waterPoint.X, 0.f, (float)((mapSize-1)*TERRAIN_TILE_SIZE));
		waterPoint.Z = clamp(waterPoint.Z, 0.f, (float)((mapSize-1)*TERRAIN_TILE_SIZE));
	}

	if (gotTerrain)
	{
		if (gotWater)
		{
			// Intersecting both heightmap and water plane; choose the closest of those
			if ((origin - terrainPoint).LengthSquared() < (origin - waterPoint).LengthSquared())
				return terrainPoint;
			else
				return waterPoint;
		}
		else
		{
			// Intersecting heightmap but parallel to water plane
			return terrainPoint;
		}
	}
	else
	{
		if (gotWater)
		{
			// Only intersecting water plane
			return waterPoint;
		}
		else
		{
			// Not intersecting terrain or water; just return 0,0,0.
			return CVector3D(0.f, 0.f, 0.f);
		}
	}
}

//! polygon orientation - clockwise or anti-clockwise
CPolygonOrientation
CMathGeom3D::
PolygonOrientation(const CPoint3D &point1, const CPoint3D &point2,
                   const CPoint3D &point3, const CPoint3D &eye)
{
  CVector3D off(point1, eye);

  CVector3D d1d2 =
    CVector3D(point1, point2).crossProduct(CVector3D(point2, point3));

  double dotprod = d1d2.dotProduct(off);

  return (CPolygonOrientation) CMathGen::sign(dotprod);
}

///////////////////////////////////////////////////////////////////
// Scissor rectangle of water patches
CBoundingBoxAligned TerrainRenderer::ScissorWater(const CMatrix3D &viewproj)
{
	CBoundingBoxAligned scissor;
	for (size_t i = 0; i < m->visiblePatches.size(); ++i)
	{
		CPatchRData* data = m->visiblePatches[i];
		const CBoundingBoxAligned& waterBounds = data->GetWaterBounds();
		if (waterBounds.IsEmpty())
			continue;

		CVector4D v1 = viewproj.Transform(CVector4D(waterBounds[0].X, waterBounds[1].Y, waterBounds[0].Z, 1.0f));
		CVector4D v2 = viewproj.Transform(CVector4D(waterBounds[1].X, waterBounds[1].Y, waterBounds[0].Z, 1.0f));
		CVector4D v3 = viewproj.Transform(CVector4D(waterBounds[0].X, waterBounds[1].Y, waterBounds[1].Z, 1.0f));
		CVector4D v4 = viewproj.Transform(CVector4D(waterBounds[1].X, waterBounds[1].Y, waterBounds[1].Z, 1.0f));
		CBoundingBoxAligned screenBounds;
		#define ADDBOUND(v1, v2, v3, v4) \
			if (v1[2] >= -v1[3]) \
				screenBounds += CVector3D(v1[0], v1[1], v1[2]) * (1.0f / v1[3]); \
			else \
			{ \
				float t = v1[2] + v1[3]; \
				if (v2[2] > -v2[3]) \
				{ \
					CVector4D c2 = v1 + (v2 - v1) * (t / (t - (v2[2] + v2[3]))); \
					screenBounds += CVector3D(c2[0], c2[1], c2[2]) * (1.0f / c2[3]); \
				} \
				if (v3[2] > -v3[3]) \
				{ \
					CVector4D c3 = v1 + (v3 - v1) * (t / (t - (v3[2] + v3[3]))); \
					screenBounds += CVector3D(c3[0], c3[1], c3[2]) * (1.0f / c3[3]); \
				} \
				if (v4[2] > -v4[3]) \
				{ \
					CVector4D c4 = v1 + (v4 - v1) * (t / (t - (v4[2] + v4[3]))); \
					screenBounds += CVector3D(c4[0], c4[1], c4[2]) * (1.0f / c4[3]); \
				} \
			}
		ADDBOUND(v1, v2, v3, v4);
		ADDBOUND(v2, v1, v3, v4);
		ADDBOUND(v3, v1, v2, v4);
		ADDBOUND(v4, v1, v2, v3);
		#undef ADDBOUND
		if (screenBounds[0].X >= 1.0f || screenBounds[1].X <= -1.0f || screenBounds[0].Y >= 1.0f || screenBounds[1].Y <= -1.0f)
			continue;
		scissor += screenBounds;
	}
	return CBoundingBoxAligned(CVector3D(clamp(scissor[0].X, -1.0f, 1.0f), clamp(scissor[0].Y, -1.0f, 1.0f), -1.0f),
				  CVector3D(clamp(scissor[1].X, -1.0f, 1.0f), clamp(scissor[1].Y, -1.0f, 1.0f), 1.0f));
}

CVector3D CVector3D::GetVerticalVector(int m) const
{

	if(!IS_ZERO(dx))
		return CVector3D(-(m*dy+dz)/dx,m,1);
	else
		if(!IS_ZERO(dy))
			return CVector3D(1,-(dx+m*dz)/dy,m);
		else
			if(!IS_ZERO(dz))
				return CVector3D(m,1,-(m*dx+dy)/dz);
			else
				return CVector3D(0,0,0);

}

CVector3D CCamera::GetWorldCoordinates(int px, int py, float h) const
{
	CPlane plane;
	plane.Set(CVector3D(0.f, 1.f, 0.f), CVector3D(0.f, h, 0.f)); // upwards normal, passes through h

	CVector3D origin, dir, delta, currentTarget;

	BuildCameraRay(px, py, origin, dir);

	if (plane.FindRayIntersection(origin, dir, &currentTarget))
		return currentTarget;

	// No intersection with the infinite plane - nothing sensible can be returned,
	// so just choose an arbitrary point on the plane
	return CVector3D(0.f, h, 0.f);
}

void CCinemaPath::MoveToPointAt(float t, float nodet, const CVector3D& startRotation)
{
	CCamera *camera = g_Game->GetView()->GetCamera();
	t = (this->*DistModePtr)(t);

	CVector3D pos = GetPosition(t);

	if (m_LookAtTarget)
	{
		if (m_TimeElapsed <= m_TargetSpline.MaxDistance.ToFloat())
			camera->LookAt(pos, m_TargetSpline.GetPosition(m_TimeElapsed / m_TargetSpline.MaxDistance.ToFloat()), CVector3D(0, 1, 0));
		else
			camera->LookAt(pos, m_TargetSpline.GetAllNodes().back().Position, CVector3D(0, 1, 0));
	}
	else
	{
		CVector3D nodeRotation = Node[m_CurrentNode + 1].Rotation;
		CQuaternion start, end;
		start.FromEulerAngles(DEGTORAD(startRotation.X), DEGTORAD(startRotation.Y), DEGTORAD(startRotation.Z));
		end.FromEulerAngles(DEGTORAD(nodeRotation.X), DEGTORAD(nodeRotation.Y), DEGTORAD(nodeRotation.Z));
		start.Slerp(start, end, nodet);

		camera->m_Orientation.SetIdentity();
		camera->m_Orientation.Rotate(start);
		camera->m_Orientation.Translate(pos);
	}
	camera->UpdateFrustum();
}

void CTestParticleSet::setupTestParticles( ) 
{
	RemoveAll( );

	// Check for invalid ranges.
	if( m_xRange.m_y < m_xRange.m_x ||
		m_yRange.m_y < m_yRange.m_x ||
		m_zRange.m_y < m_zRange.m_x ||
		IS_LESS_THAN_OR_EQUAL( m_xRange.m_z, 0 ) ||
		IS_LESS_THAN_OR_EQUAL( m_yRange.m_z, 0 ) ||
		IS_LESS_THAN_OR_EQUAL( m_zRange.m_z, 0 ) )
		return;

	for( double i=m_xRange.m_x; i<=m_xRange.m_y; i+=m_xRange.m_z )
	{
		for( double j=m_yRange.m_x; j<=m_yRange.m_y; j+=m_yRange.m_z )
		{
			for( double k=m_zRange.m_x; k<=m_zRange.m_y; k+=m_zRange.m_z )
			{
				// A planet.
				CPlanet newPlanet;
				newPlanet.setPositionOptimized( CVector3D( i, j, k ) );
				newPlanet.setRadius( 0.1 );
				newPlanet.setDrawAsPoint( true );

				Add( newPlanet );
			}
		}
	}
}

void Utility::ScreenToWorld(CVector3D *out,CVector3D spos,CMatrix mProj,CMatrix mView,int w,int h){
	CMatrix	mVP;
	mVP.Viewport(0.0f,0.0f, static_cast<float>(w), static_cast<float>(h));
	CVector4D o = (mView.getInverse() * mProj.getInverse() * mVP.getInverse()) * CVector4D(spos.x,spos.y,spos.z,1);
	*out = CVector3D(o.x/o.w,o.y/o.w,o.z/o.w);
	
}

CVector3D CConeVecProjectionGeometry3D::getProjectionDirection(int _iProjectionIndex, int _iDetectorIndex) const
{
	const SConeProjection& p = m_pProjectionAngles[_iProjectionIndex];
	int u = _iDetectorIndex % m_iDetectorColCount;
	int v = _iDetectorIndex / m_iDetectorColCount;

	return CVector3D(p.fDetSX + (u+0.5)*p.fDetUX + (v+0.5)*p.fDetVX - p.fSrcX, p.fDetSY + (u+0.5)*p.fDetUY + (v+0.5)*p.fDetVY - p.fSrcY, p.fDetSZ + (u+0.5)*p.fDetUZ + (v+0.5)*p.fDetVZ - p.fSrcZ);
}

	virtual CVector3D GetProjectileLaunchPoint()
	{
		if (!m_Unit)
			return CVector3D();

		if (m_Unit->GetModel().ToCModel())
		{
			// Ensure the prop transforms are correct
			m_Unit->GetModel().ValidatePosition();

			CModelAbstract* ammo = m_Unit->GetModel().ToCModel()->FindFirstAmmoProp();
			if (ammo)
				return ammo->GetTransform().GetTranslation();
		}

		return CVector3D();
	}

CVector3D CBoundingBox::GetPosition()
{
	return CVector3D(
		(m_vMin[0] + m_vMax[0]) / 2.0,
		(m_vMin[1] + m_vMax[1]) / 2.0,
		(m_vMin[2] + m_vMax[2]) / 2.0
	);
}

//void CGraphicsCamera::Rotate
void CGraphicsCamera::Rotate( const CPoint3D& euler_angles )
{
	CQuaternion q( euler_angles.x, euler_angles.y, euler_angles.z );
	double m[4][4] = {0. };
	q.QuatToMat( m );
	CVector3D v1( m_pos, m_focalPt );
	double dist = v1.length();
	v1 = CVector3D( 0., 0., 1. );
	v1*dist;
	
	m_pos = CPoint3D( v1.x + m_focalPt.x, v1.y + m_focalPt.y, v1.z + m_focalPt.z );
	m_up = CVector3D( 0., 1., 0. );
	Rotate( CVector3D::X_AXIS, euler_angles.x );
	Rotate( CVector3D::Y_AXIS, euler_angles.y );
	Rotate( CVector3D::Z_AXIS, euler_angles.z );
	//m_up = m_up*m;
}

CGraphicsRigidDiaphragmLoad::CGraphicsRigidDiaphragmLoad( const CRigidDiaphragmLoad* pRDL ):
m_etDir( DY ),
m_loc( CPoint3D() ),
m_textPoint ( CPoint() ),
m_axis( CVector3D() ),
m_rot( 0.f ),
m_pRDL( pRDL )
{
}

///////////////////////////////////////////////////////////////////
// Culls patches and decals against a frustum.
bool TerrainRenderer::CullPatches(const CFrustum* frustum)
{
	m->filteredPatches.clear();
	for (std::vector<CPatchRData*>::iterator it = m->visiblePatches.begin(); it != m->visiblePatches.end(); ++it)
	{
		if (frustum->IsBoxVisible(CVector3D(0, 0, 0), (*it)->GetPatch()->GetWorldBounds()))
			m->filteredPatches.push_back(*it);
	}

	m->filteredDecals.clear();
	for (std::vector<CDecalRData*>::iterator it = m->visibleDecals.begin(); it != m->visibleDecals.end(); ++it)
	{
		if (frustum->IsBoxVisible(CVector3D(0, 0, 0), (*it)->GetDecal()->GetWorldBounds()))
			m->filteredDecals.push_back(*it);
	}

	return !m->filteredPatches.empty() || !m->filteredDecals.empty();
}

void Init(void)
{
	//フレーム制御初期化
	CFPS::Init();

	//ボタンの設定
	CInput::SetButton(0, CInput::eButton1, ' ');
	CInput::SetButton(0, CInput::eButton2, 'Q');
	CInput::SetButton(0, CInput::eButton3, 'E');
	CInput::SetButton(0, CInput::eUp, 'W');
	CInput::SetButton(0, CInput::eDown, 'S');
	CInput::SetButton(0, CInput::eLeft, 'A');
	CInput::SetButton(0, CInput::eRight, 'D');
	CInput::SetButton(0, CInput::eMouseL, MK_LBUTTON);
	CInput::SetButton(0, CInput::eMouseR, MK_RBUTTON);

	//カメラ初期化
	CCamera::getCamera()->LookAt(CVector3D(0, 2, 0), CVector3D(0, 2, 1), CVector3D(0.0, 1.0, 0.0));
}

CVector3D CBoundingBox::GetCorner(int i, int j, int k)
{
	CVector3D vPos = GetPosition();
	double dbLength = GetLength();
	return vPos + CVector3D(
		(i - 0.5) * dbLength,
		(j - 0.5) * dbLength,
		(k - 0.5) * dbLength
	);
}

bool
CMathGeom3D::
PointPlaneDistance(const CPoint3D &point, const CPlane3D &plane, double *dist)
{
  const CVector3D &normal = plane.getNormal();
  const CPoint3D  &ppoint = plane.getPoint();

  *dist = CVector3D(ppoint, point).dotProduct(normal)/normal.length();

  return true;
}

void COBB::Transeform(CMatrix &mtx,bool trans) {
	for (int i=0;i<3;i++) {
		m_axis[i] = mtx * m_axis[i];
	}
	if(trans) {
		CVector4D v = CVector4D(m_base_center.x,m_base_center.y,m_base_center.z,1.0);
		v = mtx * v;
		m_center = CVector3D(v.x,v.y,v.z);
	}
	m_mat = mtx;
}