diff --git a/src/peds/Ped.cpp b/src/peds/Ped.cpp index 8bfa6654..db2692ec 100644 --- a/src/peds/Ped.cpp +++ b/src/peds/Ped.cpp @@ -2522,40 +2522,23 @@ CPed::CalculateNewVelocity(void) if ((!idleAssoc || idleAssoc->blendAmount < 0.5f) && !fightAssoc) { #endif LimbOrientation newUpperLegs; - newUpperLegs.phi = localWalkAngle; + newUpperLegs.yaw = localWalkAngle; - if (newUpperLegs.phi < -DEGTORAD(100.0f)) { - newUpperLegs.phi += PI; - } else if (newUpperLegs.phi > DEGTORAD(100.0f)) { - newUpperLegs.phi -= PI; + if (newUpperLegs.yaw < -DEGTORAD(100.0f)) { + newUpperLegs.yaw += PI; + } else if (newUpperLegs.yaw > DEGTORAD(100.0f)) { + newUpperLegs.yaw -= PI; } - if (newUpperLegs.phi > -DEGTORAD(50.0f) && newUpperLegs.phi < DEGTORAD(50.0f)) { -#ifdef PED_SKIN - if(IsClumpSkinned(GetClump())){ -/* - // this looks shit - newUpperLegs.theta = 0.0f; - RwV3d axis = { -1.0f, 0.0f, 0.0f }; - RtQuatRotate(&m_pFrames[PED_UPPERLEGL]->hanimFrame->q, &axis, RADTODEG(newUpperLegs.phi), rwCOMBINEPRECONCAT); - RtQuatRotate(&m_pFrames[PED_UPPERLEGR]->hanimFrame->q, &axis, RADTODEG(newUpperLegs.phi), rwCOMBINEPRECONCAT); -*/ - newUpperLegs.theta = 0.1f; - RwV3d Xaxis = { 1.0f, 0.0f, 0.0f }; - RwV3d Zaxis = { 0.0f, 0.0f, 1.0f }; - RtQuatRotate(&m_pFrames[PED_UPPERLEGL]->hanimFrame->q, &Zaxis, RADTODEG(newUpperLegs.theta), rwCOMBINEPOSTCONCAT); - RtQuatRotate(&m_pFrames[PED_UPPERLEGL]->hanimFrame->q, &Xaxis, RADTODEG(newUpperLegs.phi), rwCOMBINEPOSTCONCAT); - RtQuatRotate(&m_pFrames[PED_UPPERLEGR]->hanimFrame->q, &Zaxis, RADTODEG(newUpperLegs.theta), rwCOMBINEPOSTCONCAT); - RtQuatRotate(&m_pFrames[PED_UPPERLEGR]->hanimFrame->q, &Xaxis, RADTODEG(newUpperLegs.phi), rwCOMBINEPOSTCONCAT); - - bDontAcceptIKLookAts = true; - }else -#endif - { - newUpperLegs.theta = 0.0f; - m_pedIK.RotateTorso(m_pFrames[PED_UPPERLEGL], &newUpperLegs, false); - m_pedIK.RotateTorso(m_pFrames[PED_UPPERLEGR], &newUpperLegs, false); - } + if (newUpperLegs.yaw > -DEGTORAD(50.0f) && newUpperLegs.yaw < DEGTORAD(50.0f)) { + newUpperLegs.pitch = 0.1f; + RwV3d Xaxis = { 1.0f, 0.0f, 0.0f }; + RwV3d Zaxis = { 0.0f, 0.0f, 1.0f }; + RtQuatRotate(&m_pFrames[PED_UPPERLEGL]->hanimFrame->q, &Zaxis, RADTODEG(newUpperLegs.pitch), rwCOMBINEPOSTCONCAT); + RtQuatRotate(&m_pFrames[PED_UPPERLEGL]->hanimFrame->q, &Xaxis, RADTODEG(newUpperLegs.yaw), rwCOMBINEPOSTCONCAT); + RtQuatRotate(&m_pFrames[PED_UPPERLEGR]->hanimFrame->q, &Zaxis, RADTODEG(newUpperLegs.pitch), rwCOMBINEPOSTCONCAT); + RtQuatRotate(&m_pFrames[PED_UPPERLEGR]->hanimFrame->q, &Xaxis, RADTODEG(newUpperLegs.yaw), rwCOMBINEPOSTCONCAT); + bDontAcceptIKLookAts = true; } } } diff --git a/src/peds/Ped.h b/src/peds/Ped.h index ccffc3cb..573b74c1 100644 --- a/src/peds/Ped.h +++ b/src/peds/Ped.h @@ -833,39 +833,19 @@ public: // Using this to abstract nodes of skinned and non-skinned meshes CVector GetNodePosition(int32 node) { -#ifdef PED_SKIN - if(IsClumpSkinned(GetClump())){ - RwV3d pos = { 0.0f, 0.0f, 0.0f }; - RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump()); - int32 idx = RpHAnimIDGetIndex(hier, m_pFrames[node]->nodeID); - RwMatrix *mats = RpHAnimHierarchyGetMatrixArray(hier); - // this is just stupid - //RwV3dTransformPoints(&pos, &pos, 1, &mats[idx]); - pos = mats[idx].pos; - return pos; - }else -#endif - { - RwMatrix mat; - CPedIK::GetWorldMatrix(m_pFrames[node]->frame, &mat); - return mat.pos; - } + RwV3d pos = { 0.0f, 0.0f, 0.0f }; + RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump()); + int32 idx = RpHAnimIDGetIndex(hier, m_pFrames[node]->nodeID); + RwMatrix *mats = RpHAnimHierarchyGetMatrixArray(hier); + pos = mats[idx].pos; + return pos; } void TransformToNode(CVector &pos, int32 node) { -#ifdef PED_SKIN - if(IsClumpSkinned(GetClump())){ - RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump()); - int32 idx = RpHAnimIDGetIndex(hier, m_pFrames[node]->nodeID); - RwMatrix *mats = RpHAnimHierarchyGetMatrixArray(hier); - RwV3dTransformPoints((RwV3d*)&pos, (RwV3d*)&pos, 1, &mats[idx]); - }else -#endif - { - RwFrame *frame; - for (frame = m_pFrames[node]->frame; frame; frame = RwFrameGetParent(frame)) - RwV3dTransformPoints((RwV3d*)&pos, (RwV3d*)&pos, 1, RwFrameGetMatrix(frame)); - } + RpHAnimHierarchy *hier = GetAnimHierarchyFromSkinClump(GetClump()); + int32 idx = RpHAnimIDGetIndex(hier, m_pFrames[node]->nodeID); + RwMatrix *mats = RpHAnimHierarchyGetMatrixArray(hier); + RwV3dTransformPoints((RwV3d*)&pos, (RwV3d*)&pos, 1, &mats[idx]); } // set by 0482:set_threat_reaction_range_multiplier opcode diff --git a/src/peds/PedIK.cpp b/src/peds/PedIK.cpp index 9dae5ff1..8b3835b3 100644 --- a/src/peds/PedIK.cpp +++ b/src/peds/PedIK.cpp @@ -7,11 +7,13 @@ #include "General.h" #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) }; +//--MIAMI: file almost done (only some special weapon cases left) + +LimbMovementInfo CPedIK::ms_torsoInfo = { DEGTORAD(50.0f), DEGTORAD(-50.0f), DEGTORAD(8.0f), DEGTORAD(45.0f), DEGTORAD(-45.0f), DEGTORAD(5.0f) }; +LimbMovementInfo CPedIK::ms_headInfo = { DEGTORAD(90.0f), DEGTORAD(-90.0f), DEGTORAD(15.0f), DEGTORAD(45.0f), DEGTORAD(-45.0f), DEGTORAD(8.0f) }; LimbMovementInfo CPedIK::ms_headRestoreInfo = { DEGTORAD(90.0f), DEGTORAD(-90.0f), DEGTORAD(10.0f), DEGTORAD(45.0f), DEGTORAD(-45.0f), DEGTORAD(5.0f) }; -LimbMovementInfo CPedIK::ms_upperArmInfo = { DEGTORAD(20.0f), DEGTORAD(-100.0f), DEGTORAD(20.0f), DEGTORAD(70.0f), DEGTORAD(-70.0f), DEGTORAD(10.0f) }; -LimbMovementInfo CPedIK::ms_lowerArmInfo = { DEGTORAD(80.0f), DEGTORAD(0.0f), DEGTORAD(20.0f), DEGTORAD(90.0f), DEGTORAD(-90.0f), DEGTORAD(5.0f) }; +LimbMovementInfo CPedIK::ms_upperArmInfo = { DEGTORAD(5.0f), DEGTORAD(-120.0f), DEGTORAD(20.0f), DEGTORAD(70.0f), DEGTORAD(-70.0f), DEGTORAD(20.0f) }; +LimbMovementInfo CPedIK::ms_lowerArmInfo = { DEGTORAD(60.0f), DEGTORAD(0.0f), DEGTORAD(15.0f), DEGTORAD(90.0f), DEGTORAD(-90.0f), DEGTORAD(10.0f) }; const RwV3d XaxisIK = { 1.0f, 0.0f, 0.0f}; const RwV3d YaxisIK = { 0.0f, 1.0f, 0.0f}; @@ -21,17 +23,16 @@ CPedIK::CPedIK(CPed *ped) { m_ped = ped; m_flags = 0; - m_headOrient.phi = 0.0f; - m_headOrient.theta = 0.0f; - m_torsoOrient.phi = 0.0f; - m_torsoOrient.theta = 0.0f; - m_upperArmOrient.phi = 0.0f; - m_upperArmOrient.theta = 0.0f; - m_lowerArmOrient.phi = 0.0f; - m_lowerArmOrient.theta = 0.0f; + m_headOrient.yaw = 0.0f; + m_headOrient.pitch = 0.0f; + m_torsoOrient.yaw = 0.0f; + m_torsoOrient.pitch = 0.0f; + m_upperArmOrient.yaw = 0.0f; + m_upperArmOrient.pitch = 0.0f; + m_lowerArmOrient.yaw = 0.0f; + m_lowerArmOrient.pitch = 0.0f; } -#ifdef PED_SKIN inline RwMatrix* GetBoneMatrix(CPed *ped, int32 bone) { @@ -45,174 +46,60 @@ GetComponentMatrix(CPed *ped, int32 node) { return GetBoneMatrix(ped, ped->m_pFrames[node]->nodeID); } -#endif void CPedIK::RotateTorso(AnimBlendFrameData *node, LimbOrientation *limb, bool changeRoll) { -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - RtQuat *q = &node->hanimFrame->q; -#ifndef FIX_BUGS - // this is what the game does (also VC), but it does not look great - RtQuatRotate(q, &XaxisIK, RADTODEG(limb->phi), rwCOMBINEPRECONCAT); - RtQuatRotate(q, &ZaxisIK, RADTODEG(limb->theta), rwCOMBINEPRECONCAT); // pitch -#else - // copied the code from the non-skinned case - // this seems to work ok - - // We can't get the parent matrix of an hanim frame but - // this function is always called with PED_MID, so we know the parent frame. - // Trouble is that PED_MID is "Smid" on PS2/PC but BONE_torso on mobile/xbox... - // so this doesn't exactly do what we'd like anyway - RwMatrix* mat = GetComponentMatrix(m_ped, PED_MID); - - RwV3d vec1, vec2; - vec1.x = mat->right.z; - vec1.y = mat->up.z; - vec1.z = mat->at.z; - float c = Cos(m_ped->m_fRotationCur); - float s = Sin(m_ped->m_fRotationCur); - vec2.x = -(c*mat->right.x + s*mat->right.y); - vec2.y = -(c*mat->up.x + s*mat->up.y); - vec2.z = -(c*mat->at.x + s*mat->at.y); - - // Not sure what exactly to do here - RtQuatRotate(q, &vec1, RADTODEG(limb->phi), rwCOMBINEPRECONCAT); - RtQuatRotate(q, &vec2, RADTODEG(limb->theta), rwCOMBINEPRECONCAT); -#endif - m_ped->bDontAcceptIKLookAts = true; - }else -#endif - { - RwFrame *f = node->frame; - RwMatrix *mat = GetWorldMatrix(RwFrameGetParent(f), RwMatrixCreate()); - - RwV3d upVector = { mat->right.z, mat->up.z, mat->at.z }; - RwV3d rightVector; - RwV3d pos = RwFrameGetMatrix(f)->pos; - - // rotation == 0 -> looking in y direction - // left? vector - float c = Cos(m_ped->m_fRotationCur); - float s = Sin(m_ped->m_fRotationCur); - rightVector.x = -(c*mat->right.x + s*mat->right.y); - rightVector.y = -(c*mat->up.x + s*mat->up.y); - rightVector.z = -(c*mat->at.x + s*mat->at.y); - - if(changeRoll){ - // Used when aiming only involves over the legs.(canAimWithArm) - // Automatically changes roll(forward rotation) axis of the parts above upper legs while moving, based on position of upper legs. - // Not noticeable in normal conditions... - - RwV3d forwardVector; - CVector inversedForward = CrossProduct(CVector(0.0f, 0.0f, 1.0f), mat->up); - inversedForward.Normalise(); - float dotProduct = DotProduct(mat->at, inversedForward); - if(dotProduct > 1.0f) dotProduct = 1.0f; - if(dotProduct < -1.0f) dotProduct = -1.0f; - float alpha = Acos(dotProduct); - - if(mat->at.z < 0.0f) - alpha = -alpha; - - forwardVector.x = s * mat->right.x - c * mat->right.y; - forwardVector.y = s * mat->up.x - c * mat->up.y; - forwardVector.z = s * mat->at.x - c * mat->at.y; - - float curYaw, curPitch; - ExtractYawAndPitchWorld(mat, &curYaw, &curPitch); - RwMatrixRotate(RwFrameGetMatrix(f), &rightVector, RADTODEG(limb->theta), rwCOMBINEPOSTCONCAT); - RwMatrixRotate(RwFrameGetMatrix(f), &upVector, RADTODEG(limb->phi - (curYaw - m_ped->m_fRotationCur)), rwCOMBINEPOSTCONCAT); - RwMatrixRotate(RwFrameGetMatrix(f), &forwardVector, RADTODEG(alpha), rwCOMBINEPOSTCONCAT); - }else{ - // pitch - RwMatrixRotate(RwFrameGetMatrix(f), &rightVector, RADTODEG(limb->theta), rwCOMBINEPOSTCONCAT); - // yaw - RwMatrixRotate(RwFrameGetMatrix(f), &upVector, RADTODEG(limb->phi), rwCOMBINEPOSTCONCAT); - } - RwFrameGetMatrix(f)->pos = pos; - RwMatrixDestroy(mat); - } + RtQuat *q = &node->hanimFrame->q; + RtQuatRotate(q, &XaxisIK, RADTODEG(limb->yaw), rwCOMBINEREPLACE); + RtQuatRotate(q, &ZaxisIK, RADTODEG(limb->pitch), rwCOMBINEPRECONCAT); + m_ped->bDontAcceptIKLookAts = true; } void CPedIK::GetComponentPosition(RwV3d *pos, uint32 node) { - RwFrame *f; - RwMatrix *mat; - -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - pos->x = 0.0f; - pos->y = 0.0f; - pos->z = 0.0f; - mat = GetComponentMatrix(m_ped, node); - // could just copy the position out of the matrix... - RwV3dTransformPoints(pos, pos, 1, mat); - }else -#endif - { - f = m_ped->m_pFrames[node]->frame; - mat = RwFrameGetMatrix(f); - *pos = mat->pos; - - for (f = RwFrameGetParent(f); f; f = RwFrameGetParent(f)) - RwV3dTransformPoints(pos, pos, 1, RwFrameGetMatrix(f)); - } -} - -RwMatrix* -CPedIK::GetWorldMatrix(RwFrame *source, RwMatrix *destination) -{ - RwFrame *i; - - *destination = *RwFrameGetMatrix(source); - - for (i = RwFrameGetParent(source); i; i = RwFrameGetParent(i)) - RwMatrixTransform(destination, RwFrameGetMatrix(i), rwCOMBINEPOSTCONCAT); - - return destination; + *pos = GetComponentMatrix(m_ped, node)->pos; } LimbMoveStatus -CPedIK::MoveLimb(LimbOrientation &limb, float approxPhi, float approxTheta, LimbMovementInfo &moveInfo) +CPedIK::MoveLimb(LimbOrientation &limb, float targetYaw, float targetPitch, LimbMovementInfo &moveInfo) { LimbMoveStatus result = ONE_ANGLE_COULDNT_BE_SET_EXACTLY; - // phi + // yaw - if (limb.phi > approxPhi) { - limb.phi -= moveInfo.yawD; - } else if (limb.phi < approxPhi) { - limb.phi += moveInfo.yawD; - } - - if (Abs(limb.phi - approxPhi) < moveInfo.yawD) { - limb.phi = approxPhi; + if(Abs(limb.yaw-targetYaw) < moveInfo.yawD){ + limb.yaw = targetYaw; result = ANGLES_SET_EXACTLY; + }else{ + if (limb.yaw > targetYaw) { + limb.yaw -= moveInfo.yawD; + } else if (limb.yaw < targetYaw) { + limb.yaw += moveInfo.yawD; + } } - if (limb.phi > moveInfo.maxYaw || limb.phi < moveInfo.minYaw) { - limb.phi = clamp(limb.phi, moveInfo.minYaw, moveInfo.maxYaw); + if (limb.yaw > moveInfo.maxYaw || limb.yaw < moveInfo.minYaw) { + limb.yaw = clamp(limb.yaw, moveInfo.minYaw, moveInfo.maxYaw); result = ANGLES_SET_TO_MAX; } - // theta + // pitch - if (limb.theta > approxTheta) { - limb.theta -= moveInfo.pitchD; - } else if (limb.theta < approxTheta) { - limb.theta += moveInfo.pitchD; + if (Abs(limb.pitch - targetPitch) < moveInfo.pitchD){ + limb.pitch = targetPitch; + }else{ + if (limb.pitch > targetPitch) { + limb.pitch -= moveInfo.pitchD; + } else if (limb.pitch < targetPitch) { + limb.pitch += moveInfo.pitchD; + } + result = ONE_ANGLE_COULDNT_BE_SET_EXACTLY; } - if (Abs(limb.theta - approxTheta) < moveInfo.pitchD) - limb.theta = approxTheta; - else - result = ONE_ANGLE_COULDNT_BE_SET_EXACTLY; - - if (limb.theta > moveInfo.maxPitch || limb.theta < moveInfo.minPitch) { - limb.theta = clamp(limb.theta, moveInfo.minPitch, moveInfo.maxPitch); + if (limb.pitch > moveInfo.maxPitch || limb.pitch < moveInfo.minPitch) { + limb.pitch = clamp(limb.pitch, moveInfo.minPitch, moveInfo.maxPitch); result = ANGLES_SET_TO_MAX; } return result; @@ -226,259 +113,189 @@ CPedIK::RestoreGunPosn(void) return limbStatus == ANGLES_SET_EXACTLY; } -#ifdef PED_SKIN -void -CPedIK::RotateHead(void) -{ - RtQuat *q = &m_ped->m_pFrames[PED_HEAD]->hanimFrame->q; - RtQuatRotate(q, &XaxisIK, RADTODEG(m_headOrient.phi), rwCOMBINEREPLACE); - RtQuatRotate(q, &ZaxisIK, RADTODEG(m_headOrient.theta), rwCOMBINEPOSTCONCAT); - m_ped->bDontAcceptIKLookAts = true; -} -#endif - bool -CPedIK::LookInDirection(float phi, float theta) +CPedIK::LookInDirection(float targetYaw, float targetPitch) { bool success = true; float yaw, pitch; -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - if (!(m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION)) { - m_ped->m_pFrames[PED_HEAD]->flag |= AnimBlendFrameData::IGNORE_ROTATION; - ExtractYawAndPitchLocalSkinned(m_ped->m_pFrames[PED_HEAD], &m_headOrient.phi, &m_headOrient.theta); - } - - // parent of head is torso - RwMatrix worldMat = *GetComponentMatrix(m_ped, PED_NECK); - ExtractYawAndPitchWorld(&worldMat, &yaw, &pitch); - - LimbMoveStatus headStatus = MoveLimb(m_headOrient, CGeneral::LimitRadianAngle(phi - yaw), - CGeneral::LimitRadianAngle(DEGTORAD(10.0f)), ms_headInfo); - if (headStatus == ANGLES_SET_TO_MAX) - success = false; - - if (headStatus != ANGLES_SET_EXACTLY){ - if (!(m_flags & LOOKAROUND_HEAD_ONLY)){ - if (MoveLimb(m_torsoOrient, CGeneral::LimitRadianAngle(phi), theta, ms_torsoInfo)) - success = true; - }else{ - RotateHead(); - return success; - } - } - - if (!(m_flags & LOOKAROUND_HEAD_ONLY)) - RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false); - RotateHead(); - }else -#endif - { - RwFrame *frame = m_ped->m_pFrames[PED_HEAD]->frame; - RwMatrix *frameMat = RwFrameGetMatrix(frame); - - if (!(m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION)) { - m_ped->m_pFrames[PED_HEAD]->flag |= AnimBlendFrameData::IGNORE_ROTATION; - ExtractYawAndPitchLocal(frameMat, &m_headOrient.phi, &m_headOrient.theta); - } - - RwMatrix *worldMat = RwMatrixCreate(); - worldMat = GetWorldMatrix(RwFrameGetParent(frame), worldMat); - - ExtractYawAndPitchWorld(worldMat, &yaw, &pitch); - RwMatrixDestroy(worldMat); - - yaw += m_torsoOrient.phi; - float neededPhiTurn = CGeneral::LimitRadianAngle(phi - yaw); - pitch *= Cos(neededPhiTurn); - - float neededThetaTurn = CGeneral::LimitRadianAngle(theta - pitch); - LimbMoveStatus headStatus = MoveLimb(m_headOrient, neededPhiTurn, neededThetaTurn, ms_headInfo); - if (headStatus == ANGLES_SET_TO_MAX) - success = false; - - if (headStatus != ANGLES_SET_EXACTLY && !(m_flags & LOOKAROUND_HEAD_ONLY)) { - float remainingTurn = CGeneral::LimitRadianAngle(phi - m_ped->m_fRotationCur); - if (MoveLimb(m_torsoOrient, remainingTurn, theta, ms_torsoInfo)) - success = true; - } - CMatrix nextFrame = CMatrix(frameMat); - CVector framePos = nextFrame.GetPosition(); - - nextFrame.SetRotateZ(m_headOrient.theta); - nextFrame.RotateX(m_headOrient.phi); - nextFrame.GetPosition() += framePos; - nextFrame.UpdateRW(); - - if (!(m_flags & LOOKAROUND_HEAD_ONLY)) - RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false); - + if (!(m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION)) { + m_ped->m_pFrames[PED_HEAD]->flag |= AnimBlendFrameData::IGNORE_ROTATION; + RwMatrix *m = GetComponentMatrix(m_ped, PED_NECK); + m_headOrient.yaw = Atan2(-m->at.y, -m->at.x); + m_headOrient.yaw -= m_ped->m_fRotationCur; + m_headOrient.yaw = CGeneral::LimitRadianAngle(m_headOrient.yaw); + float up = clamp(m->up.z, -1.0f, 1.0f); + m_headOrient.pitch = Atan2(-up, Sqrt(1.0f - SQR(-up))); } + + // parent of head is neck + RwMatrix *m = GetComponentMatrix(m_ped, PED_NECK); + yaw = CGeneral::LimitRadianAngle(Atan2(-m->at.y, -m->at.x)); + float up = clamp(m->up.z, -1.0f, 1.0f); + pitch = Atan2(-up, Sqrt(1.0f - SQR(-up))); + float headYaw = CGeneral::LimitRadianAngle(targetYaw - (yaw + m_torsoOrient.yaw)); + float headPitch = CGeneral::LimitRadianAngle(targetPitch - pitch) * Cos(Min(Abs(headYaw), HALFPI)); + + LimbMoveStatus headStatus = MoveLimb(m_headOrient, headYaw, headPitch, ms_headInfo); + if (headStatus == ANGLES_SET_TO_MAX) + success = false; + + if (headStatus != ANGLES_SET_EXACTLY && !(m_flags & LOOKAROUND_HEAD_ONLY)) + if (MoveLimb(m_torsoOrient, CGeneral::LimitRadianAngle(targetYaw-m_ped->m_fRotationCur), targetPitch, ms_torsoInfo)) + success = true; + + // This was RotateHead + RtQuat *q = &m_ped->m_pFrames[PED_HEAD]->hanimFrame->q; + RtQuatRotate(q, &ZaxisIK, RADTODEG(m_headOrient.pitch), rwCOMBINEREPLACE); + RtQuatRotate(q, &XaxisIK, RADTODEG(m_headOrient.yaw), rwCOMBINEPRECONCAT); + m_ped->bDontAcceptIKLookAts = true; + + if (!(m_flags & LOOKAROUND_HEAD_ONLY)) + RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false); return success; } bool CPedIK::LookAtPosition(CVector const &pos) { - float phiToFace = CGeneral::GetRadianAngleBetweenPoints( + RwV3d *pedpos = &GetComponentMatrix(m_ped, PED_MID)->pos; + float yawToFace = CGeneral::GetRadianAngleBetweenPoints( pos.x, pos.y, - m_ped->GetPosition().x, m_ped->GetPosition().y); + pedpos->x, pedpos->y); - float thetaToFace = CGeneral::GetRadianAngleBetweenPoints( + float pitchToFace = CGeneral::GetRadianAngleBetweenPoints( + // BUG? not using pedpos here pos.z, (m_ped->GetPosition() - pos).Magnitude2D(), - m_ped->GetPosition().z, 0.0f); + pedpos->z, 0.0f); - return LookInDirection(phiToFace, thetaToFace); + return LookInDirection(yawToFace, pitchToFace); } bool -CPedIK::PointGunInDirection(float phi, float theta) +CPedIK::PointGunInDirection(float targetYaw, float targetPitch) { bool result = true; bool armPointedToGun = false; - float angle = CGeneral::LimitRadianAngle(phi - m_ped->m_fRotationCur); - m_flags &= (~GUN_POINTED_SUCCESSFULLY); + targetYaw = CGeneral::LimitRadianAngle(targetYaw - m_ped->GetForward().Heading()); + m_flags &= ~GUN_POINTED_SUCCESSFULLY; m_flags |= LOOKAROUND_HEAD_ONLY; if (m_flags & AIMS_WITH_ARM) { - armPointedToGun = PointGunInDirectionUsingArm(angle, theta); - angle = CGeneral::LimitRadianAngle(angle - m_upperArmOrient.phi); + armPointedToGun = PointGunInDirectionUsingArm(targetYaw, targetPitch); + targetYaw = CGeneral::LimitRadianAngle(targetYaw - (m_upperArmOrient.yaw + m_lowerArmOrient.yaw)); } 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); + if (m_flags & AIMS_WITH_ARM && m_torsoOrient.yaw * m_upperArmOrient.yaw < 0.0f) + MoveLimb(m_torsoOrient, 0.0f, m_torsoOrient.pitch, ms_torsoInfo); } else { // Unused code RwMatrix *matrix; float yaw, pitch; -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - matrix = RwMatrixCreate(); - *matrix = *GetComponentMatrix(m_ped, PED_UPPERARMR); - ExtractYawAndPitchWorld(matrix, &yaw, &pitch); - RwMatrixDestroy(matrix); - }else -#endif - { - matrix = GetWorldMatrix(RwFrameGetParent(m_ped->m_pFrames[PED_UPPERARMR]->frame), RwMatrixCreate()); - ExtractYawAndPitchWorld(matrix, &yaw, &pitch); - RwMatrixDestroy(matrix); - } - // + matrix = RwMatrixCreate(); + *matrix = *GetComponentMatrix(m_ped, PED_CLAVICLER); + ExtractYawAndPitchWorld(matrix, &yaw, &pitch); + RwMatrixDestroy(matrix); - LimbMoveStatus status = MoveLimb(m_torsoOrient, angle, theta, ms_torsoInfo); + if(m_flags & AIMS_WITH_ARM){ + if(targetPitch > 0.0f) + targetPitch = Max(targetPitch - Abs(targetYaw), 0.0f); + else + targetPitch = Min(targetPitch + Abs(targetYaw), 0.0f); + } + LimbMoveStatus status = MoveLimb(m_torsoOrient, targetYaw, targetPitch, ms_torsoInfo); if (status == ANGLES_SET_TO_MAX) result = false; else if (status == ANGLES_SET_EXACTLY) m_flags |= GUN_POINTED_SUCCESSFULLY; } - 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); + RwMatrix *m = GetBoneMatrix(m_ped, BONE_spine); // BUG: game uses index 2 directly, which happens to be identical to BONE_spine + RwV3d axis = { 0.0f, 0.0f, 0.0f }; + float axisangle = -CGeneral::LimitRadianAngle(Atan2(-m->at.y, -m->at.x) - m_ped->m_fRotationCur); + axis.y = -Sin(axisangle); + axis.z = Cos(axisangle); + + // this was RotateTorso + RtQuat *q = &m_ped->m_pFrames[PED_MID]->hanimFrame->q; + RtQuatRotate(q, &axis, RADTODEG(m_torsoOrient.pitch), rwCOMBINEPOSTCONCAT); + RtQuatRotate(q, &XaxisIK, RADTODEG(m_torsoOrient.yaw), rwCOMBINEPOSTCONCAT); + m_ped->bDontAcceptIKLookAts = true; + return result; } bool -CPedIK::PointGunInDirectionUsingArm(float phi, float theta) +CPedIK::PointGunInDirectionUsingArm(float targetYaw, float targetPitch) { bool result = false; - - RwV3d upVector; // only for non-skinned RwMatrix *matrix; float yaw, pitch; -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - matrix = RwMatrixCreate(); - *matrix = *GetComponentMatrix(m_ped, PED_UPPERARMR); - ExtractYawAndPitchWorld(matrix, &yaw, &pitch); - RwMatrixDestroy(matrix); - }else -#endif - { - RwFrame *frame = m_ped->m_pFrames[PED_UPPERARMR]->frame; - matrix = GetWorldMatrix(RwFrameGetParent(frame), RwMatrixCreate()); - // with PED_SKIN this is actually done below (with a memory leak) - upVector.x = matrix->right.z; - upVector.y = matrix->up.z; - upVector.z = matrix->at.z; + float uaRoll = 45.0f; + float handRoll = 30.0f; - ExtractYawAndPitchWorld(matrix, &yaw, &pitch); - RwMatrixDestroy(matrix); - } + matrix = GetComponentMatrix(m_ped, PED_CLAVICLER); + yaw = CGeneral::LimitRadianAngle(Atan2(matrix->right.y, matrix->right.x) - m_ped->m_fRotationCur); + pitch = Atan2(matrix->up.z, Sqrt(1.0f - SQR(matrix->up.z))); - RwV3d rightVector = { 0.0f, 0.0f, 1.0f }; - RwV3d forwardVector = { 1.0f, 0.0f, 0.0f }; - - float uaPhi, uaTheta; -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - uaPhi = phi; - uaTheta = theta + DEGTORAD(10.0f); - }else -#endif - { - uaPhi = phi - m_torsoOrient.phi - DEGTORAD(15.0f); - uaTheta = CGeneral::LimitRadianAngle(theta - pitch); - } - LimbMoveStatus uaStatus = MoveLimb(m_upperArmOrient, uaPhi, uaTheta, ms_upperArmInfo); + float uaYaw, uaPitch; + uaYaw = CGeneral::LimitRadianAngle(targetYaw - yaw - DEGTORAD(15.0f)); + uaPitch = CGeneral::LimitRadianAngle(targetPitch - pitch + DEGTORAD(10.0f)); + LimbMoveStatus uaStatus = MoveLimb(m_upperArmOrient, uaYaw, uaPitch, ms_upperArmInfo); if (uaStatus == ANGLES_SET_EXACTLY) { m_flags |= GUN_POINTED_SUCCESSFULLY; result = true; } -#ifdef PED_SKIN - // this code is completely missing on xbox & android, but we can keep it with the check - // TODO? implement it for skinned geometry? - if(!IsClumpSkinned(m_ped->GetClump())) -#endif if (uaStatus == ANGLES_SET_TO_MAX) { - float laPhi = uaPhi - m_upperArmOrient.phi; + float laYaw = uaYaw - m_upperArmOrient.yaw; 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 (laYaw > 0.0f){ + float rollReduce = laYaw/DEGTORAD(30.0f); + uaRoll *= 1.0f - Min(rollReduce, 1.0f); + handRoll *= 1.0f - Min(rollReduce, 1.0f); + + laYaw *= 1.9f; + laStatus = MoveLimb(m_lowerArmOrient, laYaw, 0.0f, ms_lowerArmInfo); + + // some unused statics here + float uaPitchAmount = 1.0f - (m_lowerArmOrient.yaw + m_upperArmOrient.yaw) * 0.34f; + float f1 = ms_upperArmInfo.maxPitch * Max(uaPitchAmount, 0.0f); + float f2 = 0.2f*m_lowerArmOrient.yaw + m_upperArmOrient.pitch; + m_upperArmOrient.pitch = Min(f1, f2); + }else + laStatus = MoveLimb(m_lowerArmOrient, laYaw, 0.0f, ms_lowerArmInfo); if (laStatus == ANGLES_SET_EXACTLY) { m_flags |= GUN_POINTED_SUCCESSFULLY; result = true; } - RwFrame *child = GetFirstChild(m_ped->m_pFrames[PED_UPPERARMR]->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; + + // game does this stupidly by going through the clump extension... + RtQuat *q = &m_ped->m_pFrames[PED_FOREARMR]->hanimFrame->q; + RtQuatRotate(q, &ZaxisIK, -RADTODEG(m_lowerArmOrient.yaw), rwCOMBINEREPLACE); + RtQuatRotate(q, &XaxisIK, -RADTODEG(m_lowerArmOrient.pitch), rwCOMBINEPOSTCONCAT); + m_ped->bDontAcceptIKLookAts = true; } -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - RtQuat *q = &m_ped->m_pFrames[PED_UPPERARMR]->hanimFrame->q; - RtQuatRotate(q, &XaxisIK, RADTODEG(m_upperArmOrient.phi), rwCOMBINEPOSTCONCAT); - RtQuatRotate(q, &ZaxisIK, RADTODEG(m_upperArmOrient.theta), rwCOMBINEPOSTCONCAT); - m_ped->bDontAcceptIKLookAts = true; - }else -#endif - { - RwFrame *frame = m_ped->m_pFrames[PED_UPPERARMR]->frame; - // with PED_SKIN we're also getting upVector here - 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; - } + RtQuat *q = &m_ped->m_pFrames[PED_UPPERARMR]->hanimFrame->q; + RtQuatRotate(q, &XaxisIK, uaRoll, rwCOMBINEREPLACE); + RtQuatRotate(q, &YaxisIK, -RADTODEG(m_upperArmOrient.pitch), rwCOMBINEPOSTCONCAT); + RtQuatRotate(q, &ZaxisIK, -RADTODEG(m_upperArmOrient.yaw+HALFPI), rwCOMBINEPOSTCONCAT); + m_ped->bDontAcceptIKLookAts = true; + + q = &m_ped->m_pFrames[PED_HANDR]->hanimFrame->q; + RtQuatRotate(q, &XaxisIK, handRoll, rwCOMBINEPRECONCAT); + return result; } bool CPedIK::PointGunAtPosition(CVector const& position) { + // TODO(MIAMI): special cases for some weapons 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)); + CGeneral::GetRadianAngleBetweenPoints(position.z, Distance2D(m_ped->GetPosition(), position.x, position.y), m_ped->GetPosition().z, 0.0f)); } bool @@ -487,40 +304,24 @@ CPedIK::RestoreLookAt(void) bool result = false; float yaw, pitch; -#ifdef PED_SKIN - if(IsClumpSkinned(m_ped->GetClump())){ - if (m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION) { - m_ped->m_pFrames[PED_HEAD]->flag &= (~AnimBlendFrameData::IGNORE_ROTATION); - } else { - ExtractYawAndPitchLocalSkinned(m_ped->m_pFrames[PED_HEAD], &yaw, &pitch); - if (MoveLimb(m_headOrient, yaw, pitch, ms_headRestoreInfo) == ANGLES_SET_EXACTLY) - result = true; - } - RotateHead(); - }else -#endif - { - RwMatrix *mat = RwFrameGetMatrix(m_ped->m_pFrames[PED_HEAD]->frame); - if (m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION) { - m_ped->m_pFrames[PED_HEAD]->flag &= (~AnimBlendFrameData::IGNORE_ROTATION); - } else { - ExtractYawAndPitchLocal(mat, &yaw, &pitch); - if (MoveLimb(m_headOrient, yaw, pitch, ms_headRestoreInfo) == ANGLES_SET_EXACTLY) - result = true; - } + if (m_ped->m_pFrames[PED_HEAD]->flag & AnimBlendFrameData::IGNORE_ROTATION) { + m_ped->m_pFrames[PED_HEAD]->flag &= (~AnimBlendFrameData::IGNORE_ROTATION); + } else { + ExtractYawAndPitchLocalSkinned(m_ped->m_pFrames[PED_HEAD], &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 & LOOKAROUND_HEAD_ONLY)){ + // This was RotateHead + RtQuat *q = &m_ped->m_pFrames[PED_HEAD]->hanimFrame->q; + RtQuatRotate(q, &XaxisIK, RADTODEG(m_headOrient.yaw), rwCOMBINEREPLACE); + RtQuatRotate(q, &ZaxisIK, RADTODEG(m_headOrient.pitch), rwCOMBINEPRECONCAT); + m_ped->bDontAcceptIKLookAts = true; + + if (!(m_flags & LOOKAROUND_HEAD_ONLY)) MoveLimb(m_torsoOrient, 0.0f, 0.0f, ms_torsoInfo); - if (!(m_flags & LOOKAROUND_HEAD_ONLY)) - RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false); - } + if (!(m_flags & LOOKAROUND_HEAD_ONLY)) + RotateTorso(m_ped->m_pFrames[PED_MID], &m_torsoOrient, false); return result; } @@ -548,7 +349,6 @@ CPedIK::ExtractYawAndPitchLocal(RwMatrix *mat, float *yaw, float *pitch) if (mat->up.x > 0.0f) *pitch = -*pitch; } -#ifdef PED_SKIN void CPedIK::ExtractYawAndPitchLocalSkinned(AnimBlendFrameData *node, float *yaw, float *pitch) { @@ -557,4 +357,3 @@ CPedIK::ExtractYawAndPitchLocalSkinned(AnimBlendFrameData *node, float *yaw, flo ExtractYawAndPitchLocal(mat, yaw, pitch); RwMatrixDestroy(mat); } -#endif diff --git a/src/peds/PedIK.h b/src/peds/PedIK.h index a1cb5d13..b5f895cd 100644 --- a/src/peds/PedIK.h +++ b/src/peds/PedIK.h @@ -4,8 +4,8 @@ struct LimbOrientation { - float phi; - float theta; + float yaw; + float pitch; }; struct LimbMovementInfo { @@ -48,19 +48,17 @@ public: static LimbMovementInfo ms_lowerArmInfo; CPedIK(CPed *ped); - bool PointGunInDirection(float phi, float theta); - bool PointGunInDirectionUsingArm(float phi, float theta); + bool PointGunInDirection(float targetYaw, float targetPitch); + bool PointGunInDirectionUsingArm(float targetYaw, float targetPitch); bool PointGunAtPosition(CVector const& position); void GetComponentPosition(RwV3d *pos, uint32 node); - static RwMatrix *GetWorldMatrix(RwFrame *source, RwMatrix *destination); void RotateTorso(AnimBlendFrameData* animBlend, LimbOrientation* limb, bool changeRoll); void ExtractYawAndPitchLocal(RwMatrix *mat, float *yaw, float *pitch); void ExtractYawAndPitchLocalSkinned(AnimBlendFrameData *node, float *yaw, float *pitch); void ExtractYawAndPitchWorld(RwMatrix *mat, float *yaw, float *pitch); - LimbMoveStatus MoveLimb(LimbOrientation &limb, float approxPhi, float approxTheta, LimbMovementInfo &moveInfo); + LimbMoveStatus MoveLimb(LimbOrientation &limb, float targetYaw, float targetPitch, LimbMovementInfo &moveInfo); bool RestoreGunPosn(void); - void RotateHead(void); - bool LookInDirection(float phi, float theta); + bool LookInDirection(float targetYaw, float targetPitch); bool LookAtPosition(CVector const& pos); bool RestoreLookAt(void); };