//Normalize of 4D vector (on self)
void Vector4::Normalize(){ 
	float len = Magnitude();
	w/=len;
	x/=len;
	y/=len;
	z/=len;
}

void CComputerBauer::HandleGolfer(const float elapsed)
{
	CChicken* c=(CChicken*)SearchNextChicken();
	if (c==NULL)return;

	// um 0.07 RAD weiter nach links drehen um noch richtig zu zielen
	float w=GetWinkel(ang.y,c->pos)-0.07f;
	float entf=Magnitude(pos-c->pos);

	if (abs(w)>0.025f)w=RotateTo(w,elapsed*((entf<13.0f)?2.25f:1.0f));

	if (entf>1.5f)speed.z=4.4f;
	else speed.z=0.0f;

	if ((w<0.1f)&&(entf<2.0f))
	{	// Zuschlagen
		ang.x=-0.2f;

		CGolfer* g=(CGolfer*)object;
		if (g->Schlag())
		{
			g->SendNotify(3);
		}
	}
}

	void Vector3::Normalise()
	{
		float fMag = Magnitude();
		m_fX /= fMag;
		m_fY /= fMag;
		m_fZ /= fMag;
	}

void CComputerBauer::HandleGartenkralle(const float elapsed)
{
	CChicken* c=(CChicken*)SearchNextChicken();
	if (c==NULL)return;

	// um 0.07 RAD weiter nach links drehen um noch richtig zu zielen
	float w=GetWinkel(ang.y,c->pos)-0.07f;
	float entf=Magnitude(pos-c->pos);

	if (abs(w)>0.02f)w=RotateTo(w,elapsed*((entf<13.0f)?2.25f:1.0f));

	if (entf>1.3f)speed.z=4.0f;
	else speed.z=0.0f;

	if ((w<0.1f)&&(entf<1.8f))
	{	// Zustechen
		ang.x=-0.45f;

		CGartenkralle* g=(CGartenkralle*)object;
		if (g->Stoss())
		{
			g->SendNotify(2);
			g->EndStoss();
		}
	}
}

void CComputerBauer::HandleDrescher(const float elapsed)
{
	CChicken* c=SearchNextChicken();
	CDrescher* d=(CDrescher*)object;

	if (c==NULL)
	{	// Ist kein Huhn in der Nähe, einfach stumpfsinnig in einem kleinen Kreis weiterfahren
		d->rot=0.35f;	
		return;
	}

	d->acc.z=5.5f;

	float w=GetWinkel(d->ang.y,c->pos);
	if (abs(w)<0.1f)
	{	// Wenn Winkeldiffenenz zu klein, Kurs einfach beibehalten ohne zu lenken
		d->rot=0.0f;
		return;
	}
	// Drehen
	if ((abs(w)>g_PI/2.0f)&&(Magnitude(pos-c->pos)<12.0f)&&(world->GetDistanceFromWall(pos)>5.0f))
	{	// Wenn Ziel hinter Mähdrescher ist, weiterfahren bis es mit der Kurve erwischt werden kann
		d->rot=0.0f;
		return;
	}

	// Sonst Mähdrescher in Richtung des Huhns drehen
	if (w<0.0f)d->rot=-1.0f;
	else d->rot=+1.0f;
}

void ZCharacterObject::OnKnockback(rvector& dir, float fForce)
{
    AddVelocity(dir * fForce);

    // ³Ë¹é ÃÖ´ë¼Óµµ¿¡ ¸ÂÃá´Ù
    rvector vel = GetVelocity();
    if (Magnitude(vel) > MAX_KNOCKBACK_VELOCITY) {
        Normalize(vel);
        vel *= MAX_KNOCKBACK_VELOCITY;
        SetVelocity(vel);
    }


    // Ÿ°Ý°¨ - ²ÞƲ
    rvector dir1 = m_Direction;
    rvector dir2 = dir;
    Normalize(dir2);

    float cosAng1 = DotProduct(dir1, dir2);
    float fMaxValue = m_fTremblePower;

    if (cosAng1 < 0.f)	{
        fMaxValue = -fMaxValue;
    }
    Tremble(fMaxValue, 50, 100);
}

/* Normalize */
void Vector4::Normalize(void) {
	float mag = Magnitude();

	x /= mag;
	y /= mag;
	z /= mag;
}

void		mlVector3D::Normalise()
{
	mlFloat maxValue = 0.0f;

	if(mlFabs(x) > maxValue) maxValue = mlFabs(x);
	if(mlFabs(y) > maxValue) maxValue = mlFabs(y);
	if(mlFabs(z) > maxValue) maxValue = mlFabs(z);
	
	if(maxValue == 0.0f)
		return;
	
	x = x / maxValue;
	y = y / maxValue;
	z = z / maxValue;

	mlFloat mag = Magnitude();
	
	if(mag == 0.0f)
		return;

	mlFloat invMag = 1.0f / mag;
	x *= invMag;
	y *= invMag;
	z *= invMag;
}

//use the center plane of the dihedral angle as the reference plane
inline void getReferencePlanesForQuadPair(
    const Vector3d &p1, const Vector3d &p2, const Vector3d &p3, const Vector3d &p4, const Vector3d &p5,
    Vector3d &facenorm0, Vector3d &facenorm1, double3x3& mat, double &xlen)
{
    Vector3d Y0, Y1;
    Vector3d &Z0 = facenorm0;
    Vector3d &Z1 = facenorm1;
    Vector3d &X = *((Vector3d*)(&mat.x[0]));
    Vector3d &Y = *((Vector3d*)(&mat.x[3]));
    Vector3d &Z = *((Vector3d*)(&mat.x[6]));
    const Vector3d p23 = (p2+p3)*0.5;	//quad center
    const Vector3d p45 = (p4+p5)*0.5;	//quad center
    X = p1;
    xlen = Magnitude(X);
    X /= xlen;	//normalize
    Y0 = p23;
    Z0 = CrossProd(X, Y0);
    Y1 = p45;
    Z1 = CrossProd(Y1, X);
    Z0.Normalize();
    Z1.Normalize();
    Z = Z0+Z1;
    Z.Normalize();
    Y = CrossProd(Z, X);
}

//////////////////////////////////////////////////////////////////////////
//	ZEmblemList
//////////////////////////////////////////////////////////////////////////
void ZEmblemList::Draw()
{
	rvector camera_pos = RealSpace2::RCameraPosition;
	rvector t_vec;
	rvector t_pos;

	for( iterator iter = begin(); iter != end(); ++iter )
	{
		ZClothEmblem* pCurr	= *iter;

		if( pCurr == NULL) continue;

		t_pos.x = pCurr->mWorldMat._41;
		t_pos.y = pCurr->mWorldMat._42;
		t_pos.z = pCurr->mWorldMat._43;

		t_vec = camera_pos - t_pos;
		pCurr->m_fDist = Magnitude(t_vec);
	}

	sort(e_clothemblem_sort_float);

	for( iterator iter = begin(); iter != end(); ++iter )
	{
		ZClothEmblem* pCurr	= *iter;
		if(pCurr != 0)	pCurr->render();
	}
}

// rotate about u-axis 
Matrix4D Rotate(const Matrix4D &m,const Vector3D& u, float theta)
{
	float x, y, z,w;
	x = u.x;
	y = u.y;
	z = u.z;

	w=Magnitude(u);
	float ux, uy, uz,ud,xtheta,ytheta;
	ux = x / w;
	uy = y / w;
	uz = z / w;

	ud = pow(pow(uy, 2) + pow(uz, 2), 0.5);
	xtheta = acos(uy / ud);
	ytheta = acos(ud);

	Matrix4D temp;
	temp = Translate(x, y, z)*m;
	temp = RotateX(ytheta)*temp;
	temp = RotateY(ytheta)*temp;
	temp = RotateZ(theta)*temp;
	temp = Inverse(RotateY(ytheta))*temp;
	temp = Inverse(RotateX(xtheta))*temp;
	temp = Inverse(Translate(x, y, z))*temp;

	return temp;

}

const float Distance(	const TVector2f& _krA,
							const TVector2f& _krB)
{
	const float kfDistance = Magnitude(Subtract(TVector2f(), _krA, _krB));

	return(kfDistance);
}

Vector3<T> Vector3<T>::Normalize() const
{
  float magnitude = Magnitude();
  return { static_cast<T>(x / magnitude),
           static_cast<T>(y / magnitude),
           static_cast<T>(z / magnitude) };
}

//Normilization
void Vector2Math::Normalize()
{
	float Length = Magnitude();
	x /= Length;
	y /= Length;

}

void Vector3::Normalized()
{
	float length = Magnitude();
	this->x /= length;
	this->y /= length;
	this->z /= length;
}

PredicThirteen::geodetic_t PredicThirteen::calc(PredicThirteen::tle_t t){
    printTle(&t);
    DEBUG_PRINTLN("----------------------------------------");
    PredicThirteen::vector_t zero_vector={0,0,0,0};
    PredicThirteen::vector_t pos = zero_vector;
    PredicThirteen::vector_t vel = zero_vector;
    PredicThirteen::geodetic_t geo = {0,0,0,0};

    jul_utc = daynum +723244000000LL;
    jul_epoch=Julian_Date_of_Epoch(t.epoch_year, t.epoch_day);
    printUint64("TestNum", (uint64_t) 11118562939LL);
    printUint64("daynum", daynum);
    printUint64("jul_utc",jul_utc);
    printUint64("jul_epoch",jul_epoch);
    tsince =   ((jul_utc - jul_epoch)/1000000.0) * minday;


    select_ephemeris(&t);

    SGP4(tsince, &t, &pos, &vel);
    Convert_Sat_State(&pos, &vel);   
    Magnitude(&vel);        
    printVar("jul_utc", jul_utc);
    Calculate_LatLonAlt(jul_utc, &pos, &geo);
    printVector(&pos);
    printVector(&vel);
    printGeo(&geo);
    LAT = geo.lat;
    LON = geo.lon;
    Serial.print("LAT: ");
    Serial.println(LAT);
    Serial.print("LON: ");
    Serial.println(LON);
    return geo;
}

void RWeaponTracks::AddVertex(RLVertex* pVert)
{
	if (false == IsTimeToAddVertex()) return;	// 높은 frame rate에 영향받지 않도록 시간계산해서 궤적을 업데이트함

	CheckShift();

	RLVertex* pV = &m_pVert[m_current_vertex_size];
	RWeaponSNode* pN = &m_pNode[m_current_node_size];

	pV->p = pVert->p;
	pV->color = pVert->color;
	pN->up = pVert->p;
	pN->color[0] = pVert->color;

	pV++;
	pVert++;

	pV->p = pVert->p;
	pV->color = pVert->color;
	pN->down = pVert->p;
	pN->color[1] = pVert->color;

	// calc_length
	if( m_current_node_size )
		pN->len =  Magnitude( m_pNode[m_current_node_size].up - m_pNode[m_current_node_size-1].up);
	else 
		pN->len = 0.f;

	m_current_vertex_size += 2;
	m_current_node_size++;
}

Vector2& Vector2::Normalize()
{
	float mag = Magnitude();
	x /= mag;
	y /= mag;
	return *this;
}

	void Vector3::Normalise()
	{
		float magnitude = 1.0f / Magnitude();
		x *= magnitude;
		y *= magnitude;
		z *= magnitude;
	}

NaGeVector3D NaGeVector3D::Unit()
{
	NaGeVector3D result;
	double x, y, z;
	if(!IsNull())
	{
		x = GetX()/Magnitude();
		y = GetY()/Magnitude();
		z = GetZ()/Magnitude();
		result.SetX(x);
		result.SetY(y);
		result.SetZ(z);
		return result;
	}
	else
		return *this;
}