//==============================================================================
// プレイヤー判定スキップ
//==============================================================================
void CGame::PushBackBattleArea(void)
{
	// 判定
	CPlayer*	pPlayerCurrent = nullptr;		// 対象オブジェクト
	for (int cntPlayer = 0; cntPlayer < PLAYER_MAX; ++cntPlayer)
	{
		// 対象オブジェクトを取得
		pPlayerCurrent = Player[cntPlayer];

		// 対象のステートを確認
		if (NeedsSkipPlayer(pPlayerCurrent))
		{
			continue;
		}

		// 押し戻し
		VECTOR3	vectorPlayerToCenter = Ground->Pos() - pPlayerCurrent->Pos();
		vectorPlayerToCenter.y = 0.0f;
		float	distanceFromCenter = vectorPlayerToCenter.x * vectorPlayerToCenter.x + vectorPlayerToCenter.y * vectorPlayerToCenter.y + vectorPlayerToCenter.z * vectorPlayerToCenter.z;
		if (distanceFromCenter > (RADIUS_AREA_BATTLE - RADIUS_PUSH_CHARACTER) * (RADIUS_AREA_BATTLE - RADIUS_PUSH_CHARACTER))
		{
			float	distancePushBack = sqrtf(distanceFromCenter) - (RADIUS_AREA_BATTLE - RADIUS_PUSH_CHARACTER);
			vectorPlayerToCenter.Normalize();
   pPlayerCurrent->AddPos(vectorPlayerToCenter * distancePushBack);
   pPlayerCurrent->AddDestPos(vectorPlayerToCenter * distancePushBack);
		}
	}
}

/// Computes the angular velocity of a body given the current quaternion orientation and the quaternion velocity
VECTOR3 QUAT::to_omega(const QUAT& q, const QUAT& qd)
{
  VECTOR3 omega;
  omega.x() = 2 * (-q.x * qd.w + q.w * qd.x - q.z * qd.y + q.y * qd.z);
  omega.y() = 2 * (-q.y * qd.w + q.z * qd.x + q.w * qd.y - q.x * qd.z);
  omega.z() = 2 * (-q.z * qd.w - q.y * qd.x + q.x * qd.y + q.w * qd.z);
  return omega;
}

//==============================================================================
// 着弾地点判定
//==============================================================================
void CGame::HitBulletToField(void)
{
	VECTOR3		nor;
	float		height;
	CBillboard* mark;
	VECTOR3		markPos;
	CBallistic* ballistic = Player[CManager::netData.charNum]->GetBallistic();
	CPolygon3D* landing = ballistic->GetLanding();

	for(int cnt = 0; cnt < MARK_MAX; ++cnt)
	{
		// 初期化
		nor = VECTOR3(0.0f,0.0f,0.0f);
		height = 0.0f;

		// マーク情報
		mark = ballistic->GetMark(cnt);
		markPos = mark->Pos();

		// 高さ判定
		height = Ground->GetHeight(markPos - VECTOR3(0.0f, BULLET_SIZE * 0.5f, 0.0f), &nor);
		if(height >= markPos.y)
		{
			// 回転を求める
			VECTOR3	vectorUp(0.0f, 1.0f, 0.0f);		// 上方向ベクトル
			VECTOR3	vectorAxisRotation;				// 回転軸
			float	rotation = 0.0f;				// 回転量
			VECTOR3::Cross(&vectorAxisRotation, nor, vectorUp);
			if (vectorAxisRotation.x < FLT_EPSILON && vectorAxisRotation.x > -FLT_EPSILON)
			{
				if (vectorAxisRotation.z < FLT_EPSILON && vectorAxisRotation.z > -FLT_EPSILON)
				{
					if (vectorAxisRotation.y < FLT_EPSILON && vectorAxisRotation.y > -FLT_EPSILON)
					{
						vectorAxisRotation.y = 1.0f;
					}
				}
			}
			vectorAxisRotation.Normalize();
			rotation = VECTOR3::Dot(nor, vectorUp);
			if (rotation <= 1.0f && rotation >= -1.0f)
			{
				rotation = RAD_TO_DEG * acosf(rotation);
			}
			else
			{
				rotation = 0.0f;
			}

			// 着弾マークに設定する
			landing->SetPos(VECTOR3(markPos.x, height + 0.5f, markPos.z));
			landing->SetAxisRotation(vectorAxisRotation);
			landing->SetRotationAxis(rotation);
			break;
		}
	}
}

float FieldData::getFieldMag(float point[3])
{
	VECTOR3 pos(point[0],point[1],point[2]);
	VECTOR3 vel;

	int rc = pField->getFieldValue( pos, (float)timeStep, vel);
	if (rc < 0) 
		return -1.f;

	return vel.GetMag();
}

/**
 * This matrix is used in the relationships omega' = 2*L*qd and
 * alpha' = 2*L*qdd, where omega'/alpha' are the angular velocity/acceleration
 * of a rigid body in the body's frame and qd/qdd are the first/second time
 * derivatives of the Euler (unit quaternion) parameters.
 * 
 * The matrix L is defined as:
 * -e1  e0  e3 -e2
 * -e2 -e3  e0  e1
 * -e3  e2 -e1  e0
 */
VECTOR3 QUAT::L_mult(REAL qx, REAL qy, REAL qz, REAL qw) const
{
  const double& e0 = w;
  const double& e1 = x;
  const double& e2 = y;
  const double& e3 = z;

  VECTOR3 v;
  v.x() = -e1*qw + e0*qx + e3*qy - e2*qz; 
  v.y() = -e2*qw - e3*qx + e0*qy + e1*qz;
  v.z() = -e3*qw + e2*qx - e1*qy + e0*qz;
  return v;
}

/**
 * This matrix is used in the relationships omega = 2*G*qd and
 * alpha = 2*G*qdd, where omega/alpha are the angular velocity/acceleration
 * of a rigid body in the game frame and qd/qdd are the first/second time
 * derivatives of the Euler (unit quaternion) parameters.
 */
VECTOR3 QUAT::G_mult(REAL qx, REAL qy, REAL qz, REAL qw) const
{
  const double e0 = qw;
  const double e1 = qx;
  const double e2 = qy;
  const double e3 = qz;

  VECTOR3 r;
  r.x() = -x*e0 + w*e1 - z*e2 + y*e3;
  r.y() = -y*e0 + z*e1 + w*e2 - x*e3;
  r.z() = -z*e0 - y*e1 + x*e2 + w*e3;
  return r;
}

/**
 * The local axis for this joint does not take the orientation of the 
 * inboard link into account; thus, if the orientation of the inboard link 
 * changes, then the local axis remains constant.
 * \param axis a unit vector
 * \sa get_axis_global()
 * \sa set_axis_global()
 */
void PRISMATICJOINT::set_axis(const VECTOR3& axis) 
{
  // check that axis is ok 
  if (std::fabs(axis.norm() - (REAL) 1.0) > EPS)
    throw UndefinedAxisException(); 
 
  // normalize the axis, in case caller did not 
  VECTOR3 naxis = VECTOR3::normalize(axis); 

  // transform axis to joint frame
  _u = POSE3::transform_vector(get_pose(), naxis);

  // setup v1i and v1j 
  VECTOR3::determine_orthonormal_basis(_u, _v1i, _v1j);

  // set _ui
  VECTOR3 outboard_origin(0.0, 0.0, 0.0, _Fb);
  _ui = POSE3::transform_point(_F, outboard_origin);

  // set _uj
  _uj = POSE3::transform_vector(_Fb, _v1i); 

  // set the joint axis in the inner link frame
  update_spatial_axes(); 
}        

void get_color_entropy(float& r,float& g,float& b,float& a,VECTOR3 p,int* grid_res)
{
	if(!entropies)
		return;
	int x=p.x(); int y=p.y(); int z=p.z();
	int idx=x+y*grid_res[0]+z*grid_res[0]*grid_res[1];
	float val=entropies[idx];
	r=val;
	if(val<0.5)
		g=2*val;
	else
		g=2-2*val;
	b=1-val;
	a=val;

}

//////////////////////////////////////////////////////////////////////////
// get value of node id at time step t
// input
//		id:			node Id
//		t:			time step in check
// output
//		nodeData:	vector value at this node
// return
//		1:			operation successful
//		-1:			invalid id
//////////////////////////////////////////////////////////////////////////
int Solution::GetValue(int id, float t, VECTOR3& nodeData)
{
  float adjusted_t = t - m_MinT;
	if((id < 0) || (id >= m_nNodeNum) || (adjusted_t < 0.0) || (adjusted_t > (float)(m_nTimeSteps-1)))
		return -1;

	if(!isTimeVarying())
		nodeData = m_pDataArray[(int)adjusted_t][id];
	else
	{
		int lowT, highT;
		float ratio;
		lowT = (int)floor(adjusted_t);
		ratio = adjusted_t - (float)floor(adjusted_t);
		highT = lowT + 1;
		if(lowT >= (m_nTimeSteps-1))
		{
			highT = lowT;
			ratio = 0.0;
		}
		nodeData.Set(Lerp(m_pDataArray[lowT][id][0], m_pDataArray[highT][id][0], ratio),
					 Lerp(m_pDataArray[lowT][id][1], m_pDataArray[highT][id][1], ratio),
					 Lerp(m_pDataArray[lowT][id][2], m_pDataArray[highT][id][2], ratio));
	}

	return 1;
}

// get the min and max value for all time steps
int Solution::GetMinMaxValueAll(VECTOR3& minVal, VECTOR3& maxVal)
{
	minVal = m_pMinValue[0];
	maxVal = m_pMaxValue[0];

	for(int tFor = 1; tFor < m_nTimeSteps; tFor++)
	{
		if(minVal.GetMag() > m_pMinValue[tFor].GetMag())
			minVal = m_pMinValue[tFor];

		if(maxVal.GetMag() < m_pMaxValue[tFor].GetMag())
			maxVal = m_pMaxValue[tFor];
	}

	return 1;
}

void IMainGame::mFunction_GameOverAnimationInit(BOOL hasPlayerWon)
{
	static std::default_random_engine rndEngine;
	static std::uniform_real_distribution<float> unitDist(-1.0f, 1.0f);

	if (hasPlayerWon)
	{
		mMainGameState = GameState::MainGame::GS_DeathExplode;
		//clear bullets
		mBulletMgr.KillAllBullet();
		//player WIN
		mIsPlayerVictorious = TRUE;
		//set camera to look at chicken
		gCamera.SetLookAt(mChickenBoss.GetPosition());
		//..explode fireworks
		for (int i = 0;i < 2000;++i)
		{
			//shoot direction (add some random offset)
			VECTOR3 dir = { unitDist(rndEngine),unitDist(rndEngine) ,unitDist(rndEngine) };
			//Y direction offset ( a whole column of bullets)
			dir.Normalize();
			mBulletMgr.SpawnBullet(mChickenBoss.GetPosition(), dir, VECTOR3(1, 0,0));
		}
	}
	else
	{
		mMainGameState = GameState::MainGame::GS_DeathExplode;
		//clear bullets
		mBulletMgr.KillAllBullet();
		//player LOSE
		mIsPlayerVictorious = FALSE;
		//..explode fireworks
		for (int i = 0;i < 2000;++i)
		{
			//shoot direction (add some random offset)
			VECTOR3 dir = { unitDist(rndEngine),unitDist(rndEngine) ,unitDist(rndEngine) };
			//Y direction offset ( a whole column of bullets)
			dir.Normalize();
			mBulletMgr.SpawnBullet(mPlayer.GetPosition(), dir, VECTOR3(1, 0, 0));
		}

		//move to another position to watch the explosion
		gCamera.SetPosition(mPlayer.GetPosition() + VECTOR3(300.0f,300.0f,300.0f));
		gCamera.SetLookAt(mPlayer.GetPosition());
	}
}

//==============================================================================
// オブジェクトの地形による押し戻し
//==============================================================================
void CGame::PushBackObjectByField(CObject* pObject, float offsetY)
{
	// 地形とのあたり判定
	VECTOR3	NormalGround;		// 地形の法線
	float	HeightGround;		// 地形の高さ
	HeightGround = Ground->GetHeight(pObject->Pos(), &NormalGround) + offsetY;

	//********************************************************
	// 2015_02_12 姿勢制御用の処理を追加 ここから
	//********************************************************
	// 回転を求める
	VECTOR3	vectorUp(0.0f, 1.0f, 0.0f);		// 上方向ベクトル
	VECTOR3	vectorAxisRotation;				// 回転軸
	float	rotation = 0.0f;				// 回転量
	VECTOR3::Cross(&vectorAxisRotation, NormalGround, vectorUp);
	if (vectorAxisRotation.x < FLT_EPSILON && vectorAxisRotation.x > -FLT_EPSILON)
	{
		if (vectorAxisRotation.z < FLT_EPSILON && vectorAxisRotation.z > -FLT_EPSILON)
		{
			if (vectorAxisRotation.y < FLT_EPSILON && vectorAxisRotation.y > -FLT_EPSILON)
			{
				vectorAxisRotation.y = 1.0f;
			}
		}
	}
	vectorAxisRotation.Normalize();
	rotation = VECTOR3::Dot(NormalGround, vectorUp);
	if (rotation <= 1.0f && rotation >= -1.0f)
	{
		rotation = RAD_TO_DEG * acosf(rotation);
	}
	else
	{
		rotation = 0.0f;
	}

	// キャラクターに設定する
 pObject->SetPosY(HeightGround);
 pObject->SetDestPosY(HeightGround);
	pObject->SetAxisRotation(vectorAxisRotation);
	pObject->SetRotationAxis(rotation);
	//********************************************************
	// 2015_02_12 姿勢制御用の処理を追加 ここまで
	//********************************************************
}

TRIANGLE::TRIANGLE(VECTOR3 v1, VECTOR3 v2, VECTOR3 v3, VECTOR3 norm, COLOR col)
{
    vertex[0] = v1;
    vertex[1] = v2;
    vertex[2] = v3;

    if(norm.isNull())
        computeNormal();
    else
        normal = norm;

    color = col;
}

int GetListOrder()
{
	register int order=0;

	glPushMatrix();
	glLoadIdentity();
	object.trackball.apply_inverse_transform();
	GLdouble m[16];
	glGetDoublev(GL_MODELVIEW_MATRIX, m);
	matrix4f tm(m[0], m[4], m[8],  m[12],
			   m[1], m[5], m[9],  m[13],
			   m[2], m[6], m[10], m[14],
			   m[3], m[7], m[11], m[15]);
	
	tm = tm.inverse();
	tm = tm.transpose();
	
	vec3f splat_normal(0, 0, 1);
	tm.mult_matrix_vec(splat_normal);
	splat_normal.normalize();

	float max = -1;
	float dm;
	for (int i=0; i<SORTED_LIST_NUM; i++){
		VECTOR3 s(splat_normal[0], splat_normal[1], splat_normal[2]);
		VECTOR3 t = viewer.eyes[i];
		t.Normalize();
		dm = dot(t, s);

		if (dm>max){
			order = i;
			max = dm;
		}
	}

	glPopMatrix();
	return order;
}

int vtCStreamLine::AdvectOneStep(TIME_DIR time_dir,
                                 INTEG_ORD integ_ord,
                                 TIME_DEP time_dep,
                                 PointInfo& seedInfo,
                                 VECTOR3& finalP)
{
	int istat;
	PointInfo thisParticle;
	VECTOR3 vel;
	float curTime = m_fCurrentTime;
	float dt = m_fInitialStepSize;

	finalP.Set(-1, -1, -1);
	thisParticle = seedInfo;

	// the first point
	istat = m_pField->at_phys(seedInfo.fromCell, seedInfo.phyCoord, 
	                          seedInfo, m_fCurrentTime, vel);
	if(istat == OUT_OF_BOUND)
		return OUT_OF_BOUND;
	if((fabs(vel[0]) < m_fStationaryCutoff) && 
	   (fabs(vel[1]) < m_fStationaryCutoff) && 
	   (fabs(vel[2]) < m_fStationaryCutoff))
		return CRITICAL_POINT;

	float error;
	if(integ_ord == SECOND)
		istat = runge_kutta2(time_dir, time_dep, thisParticle, &curTime, dt);
	else if(integ_ord == FOURTH)
		istat = runge_kutta4(time_dir, time_dep, thisParticle, &curTime, dt);
	else if(integ_ord == RK45)
		istat = runge_kutta45(time_dir, time_dep, thisParticle, &curTime, dt, 
		                      &error);
	else 
		return OUT_OF_BOUND;

	if(istat == OUT_OF_BOUND)			// out of boundary
			return OUT_OF_BOUND;
	m_pField->at_phys(thisParticle.fromCell, thisParticle.phyCoord, 
	                  thisParticle, m_fCurrentTime, vel);
	if((fabs(vel[0]) < m_fStationaryCutoff) && 
	   (fabs(vel[1]) < m_fStationaryCutoff) && 
	   (fabs(vel[2]) < m_fStationaryCutoff))
		return CRITICAL_POINT;
	else
		finalP = thisParticle.phyCoord;

	return OKAY;
}

//load seeds
float* get_grid_vec_data(int* grid_res)//get vec data at each grid point
{
	osuflow->GetFlowField()->getDimension(grid_res[0],grid_res[1],grid_res[2]);

	float * vectors=new float[grid_res[0]*grid_res[1]*grid_res[2]*3];
	for(int k=0; k<grid_res[2];k++)
	{
		for(int j=0; j<grid_res[1];j++)
		{
			for(int i=0; i<grid_res[0];i++)
			{
				VECTOR3 data;
				osuflow->GetFlowField()->at_vert(i,j,k,0,data);//t=0, static data
				int idx=i+j*grid_res[0]+k*grid_res[0]*grid_res[1];

				data.Normalize();
				vectors[idx*3+0]=data.x();
				vectors[idx*3+1]=data.y();
				vectors[idx*3+2]=data.z();
			}
		}
	}
	return vectors;
}

bool TRIANGLE::intersect(POINT origin, VECTOR3 direction, double &depth) const
{
    //algorithm used:  http://en.wikipedia.org/wiki/M%C3%B6ller%E2%80%93Trumbore_intersection_algorithm
    //QVector3D d = direction;
    VECTOR3 e1 = vertex[1] - vertex[0];
    VECTOR3 e2 = vertex[2] - vertex[0];
    VECTOR3 p = direction.cross(e2);// QVector3D::crossProduct(d, e2);
    double det = e1.dot(p);// QVector3D::dotProduct(e1, p);
    if(det > -EPSILON && det < EPSILON)
        return false;
    double inv_det = 1.f / det;

    VECTOR3 t = VECTOR3(origin) - vertex[0];

    double u = (t.dot(p) * inv_det); // QVector3D::dotProduct(t, p) * inv_det);
    if(u < 0.f || u > 1.f)
        return false;

    //QVector3D q = QVector3D::crossProduct(t, e1);
    VECTOR3 q = t.cross(e1);

    double v = direction.dot(q) * inv_det; // (QVector3D::dotProduct(d, q) * inv_det);
    if(v < 0.f || (u + v) > 1.f)
        return false;

    double dist = e2.dot(q) * inv_det; // QVector3D::dotProduct(e2, q) * inv_det;
    if(dist > EPSILON)
    {
        //DEBUG PRINT
        //std::cout << "getRayIntersection - Treffer: " << dist << std::endl;
        depth = dist;
        return true;
    }

    return false;
}

/**
 * Note that the derivative is not generally a unit quaternion.
 * \param q the current orientation
 * \param w the angular velocity (in the global frame)
 * Uses the matrix:
 *      |  -q.x  +q.w  -q.z  +q.y  |
 * G =  |  -q.y  +q.z  +q.w  -q.x  |
 *      |  -q.z  -q.y  +q.x  +q.w  |
 */
QUAT QUAT::deriv(const QUAT& q, const VECTOR3& w)
{
  QUAT qd;

  qd.w = .5 * (-q.x * w.x() - q.y * w.y() - q.z * w.z());
  qd.x = .5 * (+q.w * w.x() + q.z * w.y() - q.y * w.z());
  qd.y = .5 * (-q.z * w.x() + q.w * w.y() + q.x * w.z());
  qd.z = .5 * (+q.y * w.x() - q.x * w.y() + q.w * w.z());

  return qd;
}

//////////////////////////////////////////////////////////////////////////
// get the physical coordinate of the vertex
//
// input:
// verIdx: index of vertex
// output:
// pos: physical coordinate of vertex
//////////////////////////////////////////////////////////////////////////
ReturnStatus CurvilinearGrid::at_vertex(int verIdx, VECTOR3& pos)
{
	int totalVer = xdim() * ydim() * zdim();
	if ((verIdx < 0) || (verIdx >= totalVer))
		return OUT_OF_BOUND;
	/*
	zidx = verIdx / (xdim() * ydim());
	yidx = verIdx % (xdim() * ydim());
	yidx = verIdx / xdim();
	xidx = verIdx - zidx * xdim() * ydim() - yidx * xdim();
	*/
	float xpos = m_pVertex[verIdx][0];
	float ypos = m_pVertex[verIdx][1];
	float zpos = m_pVertex[verIdx][2];

	// pos.set((float)xidx, (float)yidx, (float)zidx);
	pos.set(xpos, ypos, zpos);
	return SUCCESS;
}

/**
 * This matrix is used in the relationships qd = 1/2*L^T*omega and
 * qdd = 1/2*L^T*alpha' - 1/4*omega'^2*q, where omega'/alpha' are the angular
 * velocity/acceleration of a rigid body in the body frame and qd/qdd are the
 * first/second time derivatives of the Euler (unit quaternion) parameters.
 * 
 * The matrix L is defined as:
 * -e1  e0  e3 -e2
 * -e2 -e3  e0  e1
 * -e3  e2 -e1  e0
 */
QUAT QUAT::L_transpose_mult(const VECTOR3& v) const
{
  double de0 = -x*v.x() - y*v.y() - z*v.z();
  double de1 = +w*v.x() - z*v.y() + y*v.z();
  double de2 = +z*v.x() + w*v.y() - x*v.z();
  double de3 = -y*v.x() + x*v.y() + w*v.z();

  QUAT q;
  q.w = de0;
  q.x = de1;
  q.y = de2;
  q.z = de3;
  return q;
}