Merge pull request #299 from Sergeanur/PedIK

CPedIK finished, bit of CCamera

6 files changed, 236 insertions, 22 deletions

diff --git a/src/core/Camera.cpp b/src/core/Camera.cpp
index a8295763..64a52bdc 100644
--- a/src/core/Camera.cpp
+++ b/src/core/Camera.cpp
@@ -25,16 +25,11 @@ WRAPPER void CCamera::Restore(void) { EAXJMP(0x46F990); }
 WRAPPER void CamShakeNoPos(CCamera*, float) { EAXJMP(0x46B100); }
 WRAPPER void CCamera::TakeControl(CEntity*, int16, int16, int32) { EAXJMP(0x471500); }
 WRAPPER void CCamera::TakeControlNoEntity(const CVector&, int16, int32) { EAXJMP(0x4715B0); }
-WRAPPER void CCamera::SetCamPositionForFixedMode(const CVector&, const CVector&) { EAXJMP(0x46FCC0); }
 WRAPPER void CCamera::Init(void) { EAXJMP(0x46BAD0); }
-WRAPPER void CCamera::SetRwCamera(RwCamera*) { EAXJMP(0x46FEC0); }
 WRAPPER void CCamera::Process(void) { EAXJMP(0x46D3F0); }
 WRAPPER void CCamera::LoadPathSplines(int file) { EAXJMP(0x46D1D0); }
-WRAPPER uint32 CCamera::GetCutSceneFinishTime(void) { EAXJMP(0x46B920); }
-WRAPPER void CCamera::FinishCutscene(void) { EAXJMP(0x46B560); }
 WRAPPER void CCamera::RestoreWithJumpCut(void) { EAXJMP(0x46FAE0); };
-WRAPPER void CCamera::SetZoomValueFollowPedScript(int16) { EAXJMP(0x46FF30); }
-WRAPPER void CCamera::SetZoomValueCamStringScript(int16) { EAXJMP(0x46FF90); }
+WRAPPER void CCamera::SetPercentAlongCutScene(float) { EAXJMP(0x46FE20); };
 
 bool
 CCamera::GetFading()
@@ -1394,6 +1389,68 @@ CCamera::UpdateAimingCoors(CVector const &coors)
 	m_cvecAimingTargetCoors = coors;

 }
 
+void
+CCamera::SetCamPositionForFixedMode(const CVector &Source, const CVector &UpOffSet)
+{
+	m_vecFixedModeSource = Source;

+	m_vecFixedModeUpOffSet = UpOffSet;
+}
+
+void
+CCamera::SetRwCamera(RwCamera *cam)
+{
+	m_pRwCamera = cam;

+	m_viewMatrix.Attach(&m_pRwCamera->viewMatrix, false);

+	CMBlur::MotionBlurOpen(m_pRwCamera);
+}
+
+uint32
+CCamera::GetCutSceneFinishTime(void)
+{
+	int cam = ActiveCam;

+	if (Cams[cam].Mode == CCam::MODE_FLYBY)

+		return Cams[cam].m_uiFinishTime;

+	cam = (cam + 1) % 2;

+	if (Cams[cam].Mode == CCam::MODE_FLYBY)

+		return Cams[cam].m_uiFinishTime;

+

+	return 0;
+}
+
+void
+CCamera::FinishCutscene(void)
+{
+	SetPercentAlongCutScene(100.0f);

+	m_fPositionAlongSpline = 1.0f;

+	m_bcutsceneFinished = true;
+}
+
+void
+CCamera::SetZoomValueFollowPedScript(int16 mode)
+{

+	switch (mode) {

+	case 0: m_fPedZoomValueScript = 0.25f; break;

+	case 1: m_fPedZoomValueScript = 1.5f; break;

+	case 2: m_fPedZoomValueScript = 2.9f; break;

+	default: m_fPedZoomValueScript = m_fPedZoomValueScript; break;

+	}

+

+	m_bUseScriptZoomValuePed = true;
+}
+
+void
+CCamera::SetZoomValueCamStringScript(int16 mode)
+{

+	switch (mode) {

+	case 0: m_fCarZoomValueScript = 0.05f; break;

+	case 1: m_fCarZoomValueScript = 1.9f; break;

+	case 2: m_fCarZoomValueScript = 3.9f; break;

+	default: m_fCarZoomValueScript = m_fCarZoomValueScript; break;

+	}

+

+	m_bUseScriptZoomValueCar = true;
+}
+
 STARTPATCHES
 	InjectHook(0x42C760, (bool (CCamera::*)(const CVector &center, float radius, const CMatrix *mat))&CCamera::IsSphereVisible, PATCH_JUMP);
 	InjectHook(0x46FD00, &CCamera::SetFadeColour, PATCH_JUMP);
@@ -1407,6 +1464,14 @@ STARTPATCHES
 	InjectHook(0x46FF00, &CCamera::SetWideScreenOn, PATCH_JUMP);
 	InjectHook(0x46FF10, &CCamera::SetWideScreenOff, PATCH_JUMP);
 
+	InjectHook(0x46FCC0, &CCamera::SetCamPositionForFixedMode, PATCH_JUMP);
+	InjectHook(0x46FEC0, &CCamera::SetRwCamera, PATCH_JUMP);
+	InjectHook(0x46B920, &CCamera::GetCutSceneFinishTime, PATCH_JUMP);
+	InjectHook(0x46B560, &CCamera::FinishCutscene, PATCH_JUMP);
+	InjectHook(0x46FF30, &CCamera::SetZoomValueFollowPedScript, PATCH_JUMP);
+	InjectHook(0x46FF90, &CCamera::SetZoomValueCamStringScript, PATCH_JUMP);
+
+
 	InjectHook(0x456F40, WellBufferMe, PATCH_JUMP);
 	InjectHook(0x4582F0, &CCam::GetVectorsReadyForRW, PATCH_JUMP);
 	InjectHook(0x457710, &CCam::DoAverageOnVector, PATCH_JUMP);
diff --git a/src/core/Camera.h b/src/core/Camera.h
index 99028de6..3e67903f 100644
--- a/src/core/Camera.h
+++ b/src/core/Camera.h
@@ -511,6 +511,8 @@ int     m_iModeObbeCamIsInForCar;
 	void SetNewPlayerWeaponMode(int16, int16, int16);
 	void UpdateAimingCoors(CVector const &);
 
+	void SetPercentAlongCutScene(float);
+
 	void dtor(void) { this->CCamera::~CCamera(); }
 };
 static_assert(offsetof(CCamera, m_WideScreenOn) == 0x70, "CCamera: error");
diff --git a/src/core/FileLoader.cpp b/src/core/FileLoader.cpp
index 8fb4b4d9..b49fccbb 100644
--- a/src/core/FileLoader.cpp
+++ b/src/core/FileLoader.cpp
@@ -679,7 +679,7 @@ CFileLoader::LoadMLOInstance(int id, const char *line)
 		&scale.x, &scale.y, &scale.z,
 		&rot.x, &rot.y, &rot.z,
 		&angle);
-	float rad = 2.0f * (PI / 2.0f - Atan2(angle, Sqrt(1.0f - SQR(angle))));
+	float rad = Acos(angle) * 2.0f;
 	CInstance *inst = CModelInfo::GetMloInstanceStore().alloc();
 	minfo->lastInstance++;
 
diff --git a/src/peds/Ped.cpp b/src/peds/Ped.cpp
index f42ee0c8..797262d4 100644
--- a/src/peds/Ped.cpp
+++ b/src/peds/Ped.cpp
@@ -791,7 +791,7 @@ CPed::AimGun(void)
 		}
 		Say(SOUND_PED_ATTACK);
 
-		bCanPointGunAtTarget = m_pedIK.PointGunAtPosition(&vector);
+		bCanPointGunAtTarget = m_pedIK.PointGunAtPosition(vector);
 		if (m_pLookTarget != m_pSeekTarget) {
 			SetLookFlag(m_pSeekTarget, true);
 		}
diff --git a/src/peds/PedIK.cpp b/src/peds/PedIK.cpp
index 86f1bf1e..44443e6a 100644
--- a/src/peds/PedIK.cpp
+++ b/src/peds/PedIK.cpp
@@ -1,14 +1,10 @@
 #include "common.h"
 #include "patcher.h"
+#include "Camera.h"
 #include "PedIK.h"
 #include "Ped.h"
 #include "General.h"
-
-WRAPPER bool CPedIK::PointGunInDirection(float phi, float theta) { EAXJMP(0x4ED9B0); }
-WRAPPER bool CPedIK::PointGunAtPosition(CVector *position) { EAXJMP(0x4ED920); }
-WRAPPER void CPedIK::ExtractYawAndPitchLocal(RwMatrixTag*, float*, float*) { EAXJMP(0x4ED2C0); }
-WRAPPER void CPedIK::ExtractYawAndPitchWorld(RwMatrixTag*, float*, float*) { EAXJMP(0x4ED140); }
-WRAPPER bool CPedIK::RestoreLookAt(void) { EAXJMP(0x4ED810); }
+#include "RwHelper.h"
 
 LimbMovementInfo CPedIK::ms_torsoInfo = { DEGTORAD(50.0f), DEGTORAD(-50.0f), DEGTORAD(15.0f), DEGTORAD(45.0f), DEGTORAD(-45.0f), DEGTORAD(7.0f) };
 LimbMovementInfo CPedIK::ms_headInfo = { DEGTORAD(90.0f), DEGTORAD(-90.0f), DEGTORAD(10.0f), DEGTORAD(45.0f), DEGTORAD(-45.0f), DEGTORAD(5.0f) };
@@ -165,12 +161,11 @@ bool
 CPedIK::LookInDirection(float phi, float theta)
 {
 	bool success = true;
-	AnimBlendFrameData *frameData = m_ped->m_pFrames[PED_HEAD];
-	RwFrame *frame = frameData->frame;
+	RwFrame *frame = m_ped->GetNodeFrame(PED_HEAD);
 	RwMatrix *frameMat = RwFrameGetMatrix(frame);
 
-	if (!(frameData->flag & AnimBlendFrameData::IGNORE_ROTATION)) {
-		frameData->flag |= AnimBlendFrameData::IGNORE_ROTATION;
+	if (!(m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION)) {
+		m_ped->m_pFrames[PED_HEAD]->flag |= AnimBlendFrameData::IGNORE_ROTATION;
 		CPedIK::ExtractYawAndPitchLocal(frameMat, &m_headOrient.phi, &m_headOrient.theta);
 	}
 
@@ -223,6 +218,151 @@ CPedIK::LookAtPosition(CVector const &pos)
 	return LookInDirection(phiToFace, thetaToFace);
 }
 
+bool
+CPedIK::PointGunInDirection(float phi, float theta)
+{
+	bool result = true;
+	bool armPointedToGun = false;
+	float angle = CGeneral::LimitRadianAngle(phi - m_ped->m_fRotationCur);
+	m_flags &= (~FLAG_1);
+	m_flags |= LOOKING;
+	if (m_flags & AIMS_WITH_ARM) {
+		armPointedToGun = PointGunInDirectionUsingArm(angle, theta);
+		angle = CGeneral::LimitRadianAngle(angle - m_upperArmOrient.phi);
+	}
+	if (armPointedToGun) {
+		if (m_flags & AIMS_WITH_ARM && m_torsoOrient.phi * m_upperArmOrient.phi < 0.0f)
+			MoveLimb(m_torsoOrient, 0.0f, m_torsoOrient.theta, ms_torsoInfo);
+	} else {
+		RwMatrix *matrix = GetWorldMatrix(RwFrameGetParent(m_ped->GetNodeFrame(PED_UPPERARMR)), RwMatrixCreate());
+		float yaw, pitch;
+		ExtractYawAndPitchWorld(matrix, &yaw, &pitch);
+		RwMatrixDestroy(matrix);
+		LimbMoveStatus status = MoveLimb(m_torsoOrient, angle, theta, ms_torsoInfo);
+		if (status == ANGLES_SET_TO_MAX)
+			result = false;
+		else if (status == ANGLES_SET_EXACTLY)
+			m_flags |= FLAG_1;
+	}
+	if (TheCamera.Cams[TheCamera.ActiveCam].Using3rdPersonMouseCam() && m_flags & AIMS_WITH_ARM)
+		RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, true);
+	else
+		RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false);
+	return result;
+}
+
+bool
+CPedIK::PointGunInDirectionUsingArm(float phi, float theta)
+{
+	bool result = false;
+	RwFrame *frame = m_ped->GetNodeFrame(PED_UPPERARMR);
+	RwMatrix *matrix = GetWorldMatrix(RwFrameGetParent(frame), RwMatrixCreate());
+
+	RwV3d upVector = { matrix->right.z, matrix->up.z, matrix->at.z };
+
+	float yaw, pitch;
+	ExtractYawAndPitchWorld(matrix, &yaw, &pitch);
+	RwMatrixDestroy(matrix);
+
+	RwV3d rightVector = { 0.0f, 0.0f, 1.0f };
+	RwV3d forwardVector = { 1.0f, 0.0f, 0.0f };
+
+	float uaPhi = phi - m_torsoOrient.phi - DEGTORAD(15.0f);
+	LimbMoveStatus uaStatus = MoveLimb(m_upperArmOrient, uaPhi, CGeneral::LimitRadianAngle(theta - pitch), ms_upperArmInfo);
+	if (uaStatus == ANGLES_SET_EXACTLY) {
+		m_flags |= FLAG_1;
+		result = true;
+	}
+	if (uaStatus == ANGLES_SET_TO_MAX) {
+		float laPhi = uaPhi - m_upperArmOrient.phi;
+
+		LimbMoveStatus laStatus;
+		if (laPhi > 0.0f)
+			laStatus = MoveLimb(m_lowerArmOrient, laPhi, -DEGTORAD(45.0f), ms_lowerArmInfo);
+		else
+			laStatus = MoveLimb(m_lowerArmOrient, laPhi, 0.0f, ms_lowerArmInfo);
+
+		if (laStatus == ANGLES_SET_EXACTLY) {
+			m_flags |= FLAG_1;
+			result = true;
+		}
+		RwFrame *child = GetFirstChild(frame);
+		RwV3d pos = RwFrameGetMatrix(child)->pos;
+		RwMatrixRotate(RwFrameGetMatrix(child), &forwardVector, RADTODEG(m_lowerArmOrient.theta), rwCOMBINEPOSTCONCAT);
+		RwMatrixRotate(RwFrameGetMatrix(child), &rightVector, RADTODEG(-m_lowerArmOrient.phi), rwCOMBINEPOSTCONCAT);
+		RwFrameGetMatrix(child)->pos = pos;
+	}
+
+	RwV3d pos = RwFrameGetMatrix(frame)->pos;
+	RwMatrixRotate(RwFrameGetMatrix(frame), &rightVector, RADTODEG(m_upperArmOrient.theta), rwCOMBINEPOSTCONCAT);
+	RwMatrixRotate(RwFrameGetMatrix(frame), &upVector, RADTODEG(m_upperArmOrient.phi), rwCOMBINEPOSTCONCAT);
+	RwFrameGetMatrix(frame)->pos = pos;
+	return result;
+}
+
+bool
+CPedIK::PointGunAtPosition(CVector const& position)
+{
+	return PointGunInDirection(
+		CGeneral::GetRadianAngleBetweenPoints(position.x, position.y, m_ped->GetPosition().x, m_ped->GetPosition().y),
+		CGeneral::GetRadianAngleBetweenPoints(position.z, Distance2D(m_ped->GetPosition(), position.x, position.y),
+		m_ped->GetPosition().z,
+		0.0f));
+}
+
+bool
+CPedIK::RestoreLookAt(void)
+{
+	bool result = false;
+	RwMatrix *mat = RwFrameGetMatrix(m_ped->GetNodeFrame(PED_HEAD));
+	if (m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION) {
+		m_ped->m_pFrames[PED_HEAD]->flag &= (~AnimBlendFrameData::IGNORE_ROTATION);
+	} else {
+		float yaw, pitch;
+		ExtractYawAndPitchLocal(mat, &yaw, &pitch);
+		if (MoveLimb(m_headOrient, yaw, pitch, ms_headRestoreInfo) == ANGLES_SET_EXACTLY)
+			result = true;
+	}
+
+	CMatrix matrix(mat);
+	CVector pos = matrix.GetPosition();
+	matrix.SetRotateZ(m_headOrient.theta);
+	matrix.RotateX(m_headOrient.phi);
+	matrix.Translate(pos);
+	matrix.UpdateRW();
+
+	if (!(m_flags & LOOKING))
+		MoveLimb(m_torsoOrient, 0.0f, 0.0f, ms_torsoInfo);
+	if (!(m_flags & LOOKING))
+		RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false);
+
+	return result;
+}
+
+void
+CPedIK::ExtractYawAndPitchWorld(RwMatrixTag *mat, float *yaw, float *pitch)
+{
+	float f = clamp(DotProduct(mat->up, CVector(0.0f, 1.0f, 0.0f)), -1.0f, 1.0f);
+	*yaw = Acos(f);
+	if (mat->up.x > 0.0f) *yaw = -*yaw;
+
+	f = clamp(DotProduct(mat->right, CVector(0.0f, 0.0f, 1.0f)), -1.0f, 1.0f);
+	*pitch = Acos(f);
+	if (mat->up.z > 0.0f) *pitch = -*pitch;
+}
+
+void
+CPedIK::ExtractYawAndPitchLocal(RwMatrixTag *mat, float *yaw, float *pitch)
+{
+	float f = clamp(DotProduct(mat->at, CVector(0.0f, 0.0f, 1.0f)), -1.0f, 1.0f);
+	*yaw = Acos(f);
+	if (mat->at.y > 0.0f) *yaw = -*yaw;
+
+	f = clamp(DotProduct(mat->right, CVector(1.0f, 0.0f, 0.0f)), -1.0f, 1.0f);
+	*pitch = Acos(f);
+	if (mat->up.x > 0.0f) *pitch = -*pitch;
+}
+
 STARTPATCHES
 	InjectHook(0x4ED0F0, &CPedIK::GetComponentPosition, PATCH_JUMP);
 	InjectHook(0x4ED060, &CPedIK::GetWorldMatrix, PATCH_JUMP);
@@ -231,4 +371,10 @@ STARTPATCHES
 	InjectHook(0x4EDD70, &CPedIK::RestoreGunPosn, PATCH_JUMP);
 	InjectHook(0x4ED620, &CPedIK::LookInDirection, PATCH_JUMP);
 	InjectHook(0x4ED590, &CPedIK::LookAtPosition, PATCH_JUMP);
+	InjectHook(0x4ED9B0, &CPedIK::PointGunInDirection, PATCH_JUMP);
+	InjectHook(0x4EDB20, &CPedIK::PointGunInDirectionUsingArm, PATCH_JUMP);
+	InjectHook(0x4ED920, &CPedIK::PointGunAtPosition, PATCH_JUMP);
+	InjectHook(0x4ED810, &CPedIK::RestoreLookAt, PATCH_JUMP);
+	InjectHook(0x4ED140, &CPedIK::ExtractYawAndPitchWorld, PATCH_JUMP);
+	InjectHook(0x4ED2C0, &CPedIK::ExtractYawAndPitchLocal, PATCH_JUMP);
 ENDPATCHES
\ No newline at end of file
diff --git a/src/peds/PedIK.h b/src/peds/PedIK.h
index be063003..c23a5fcc 100644
--- a/src/peds/PedIK.h
+++ b/src/peds/PedIK.h
@@ -50,13 +50,14 @@ public:
 
 	CPedIK(CPed *ped);
 	bool PointGunInDirection(float phi, float theta);
-	bool PointGunAtPosition(CVector *position);
+	bool PointGunInDirectionUsingArm(float phi, float theta);
+	bool PointGunAtPosition(CVector const& position);
 	void GetComponentPosition(RwV3d *pos, PedNode node);
 	static RwMatrix *GetWorldMatrix(RwFrame *source, RwMatrix *destination);
 	void RotateTorso(AnimBlendFrameData* animBlend, LimbOrientation* limb, bool changeRoll);
-	void ExtractYawAndPitchLocal(RwMatrixTag*, float*, float*);
-	void ExtractYawAndPitchWorld(RwMatrixTag*, float*, float*);
-	LimbMoveStatus MoveLimb(LimbOrientation &a1, float a2, float a3, LimbMovementInfo &a4);
+	void ExtractYawAndPitchLocal(RwMatrixTag *mat, float *yaw, float *pitch);
+	void ExtractYawAndPitchWorld(RwMatrixTag *mat, float *yaw, float *pitch);
+	LimbMoveStatus MoveLimb(LimbOrientation &limb, float approxPhi, float approxTheta, LimbMovementInfo &moveInfo);
 	bool RestoreGunPosn(void);
 	bool LookInDirection(float phi, float theta);
 	bool LookAtPosition(CVector const& pos);