Code/Sandbox/EditorQt/UnitTest_PlayerControl.cpp

   1 // Copyright 2001-2018 Crytek GmbH / Crytek Group. All rights reserved.
   2
   3 #include "stdafx.h"
   4
   5 #include "CharacterEditor\ModelViewportCE.h"
   6 #include "CharacterEditor\CharPanel_Animation.h"
   7 #include <CryAnimation/ICryAnimation.h>
   8
   9 #include <Cry3DEngine/I3DEngine.h>
  10 #include <CryPhysics/IPhysics.h>
  11 #include <CrySystem/ITimer.h>
  12 #include <CryInput/IInput.h>
  13 #include <CryRenderer/IRenderAuxGeom.h>
  14
  15 extern uint32 g_ypos;
  16
  17 //------------------------------------------------------------------------------
  18 //---              simple player-control test-application                    ---
  19 //------------------------------------------------------------------------------
  20 void CModelViewportCE::PlayerControl_UnitTest(ICharacterInstance* pInstance, const SRendParams& rRP, const SRenderingPassInfo& passInfo)
  21 {
  22         IRenderAuxGeom* pAuxGeom = m_renderer->GetIRenderAuxGeom();
  23         pAuxGeom->SetRenderFlags(e_Def3DPublicRenderflags);
  24         f32 FrameTime = GetIEditor()->GetSystem()->GetITimer()->GetFrameTime();
  25
  26         ISkeletonAnim* pISkeletonAnim = pInstance->GetISkeletonAnim();
  27         ISkeletonPose* pISkeletonPose = pInstance->GetISkeletonPose();
  28
  29         AABB aabb;
  30         Vec3 wpos;
  31         wpos = Vec3(2, 2, 0);
  32         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  33         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x00, 0xff, 0x1f, 0xff), eBBD_Extremes_Color_Encoded);
  34         wpos = Vec3(4, 2, 0);
  35         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  36         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x00, 0x0f, 0x4f, 0xff), eBBD_Extremes_Color_Encoded);
  37         wpos = Vec3(5.5f, 2, 0);
  38         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  39         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x00, 0x0f, 0x7f, 0xff), eBBD_Extremes_Color_Encoded);
  40
  41         wpos = Vec3(0, 5, 0);
  42         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  43         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x7f, 0x7f, 0x7f, 0xff), eBBD_Extremes_Color_Encoded);
  44         wpos = Vec3(0, 8, 0);
  45         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  46         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x7f, 0x7f, 0x7f, 0xff), eBBD_Extremes_Color_Encoded);
  47         wpos = Vec3(0, 11, 0);
  48         aabb = AABB(Vec3(-0.2f, -0.2f, -0.00f) + wpos, Vec3(+0.2f, +0.2f, +2.0f) + wpos);
  49         pAuxGeom->DrawAABB(aabb, Matrix34(IDENTITY), 1, RGBA8(0x7f, 0x7f, 0x7f, 0xff), eBBD_Extremes_Color_Encoded);
  50
  51         uint32 ypos = 2;
  52         float color1[4] = { 1, 1, 1, 1 };
  53
  54         //set the required callbacks
  55         int PostProcessCallback_PlayControl(ICharacterInstance * pInstance, void* pPlayer);
  56         pISkeletonPose->SetPostProcessCallback(PostProcessCallback_PlayControl, this);
  57         pISkeletonAnim->SetEventCallback(0, this);
  58
  59         PlayerControlIO();
  60         PlayerControlHuman(m_absLookDirectionXY, 0, 0);
  61         PlayerControlCamera();
  62
  63         m_relCameraRotZ = 0;
  64         m_relCameraRotX = 0;
  65
  66         //update the skeleton pose
  67         const CCamera& camera = GetCamera();
  68         float fDistance = (camera.GetPosition() - m_PhysicalLocation.t).GetLength();
  69         float fZoomFactor = 0.001f + 0.999f * (RAD2DEG(camera.GetFov()) / 60.f);
  70
  71         SAnimationProcessParams params;
  72         params.locationAnimation = m_PhysicalLocation;
  73         params.bOnRender = 0;
  74         params.zoomAdjustedDistanceFromCamera = fDistance * fZoomFactor;
  75         ExternalPostProcessing(pInstance);
  76         pInstance->StartAnimationProcessing(params);
  77         pInstance->FinishAnimationComputations();
  78
  79         //render character
  80         m_AnimatedCharacterMat = Matrix34(m_PhysicalLocation);
  81         SRendParams rp = rRP;
  82         rp.pMatrix = &m_AnimatedCharacterMat;
  83         rp.pPrevMatrix = &m_AnimatedCharacterMat;
  84         rp.fDistance = (m_PhysicalLocation.t - m_absCameraPos).GetLength();
  85         pInstance->Render(rp, QuatTS(IDENTITY), passInfo);
  86
  87         //just debugging
  88         Vec3 absAxisX = m_PhysicalLocation.GetColumn0();
  89         Vec3 absAxisY = m_PhysicalLocation.GetColumn1();
  90         Vec3 absAxisZ = m_PhysicalLocation.GetColumn2();
  91         Vec3 absRootPos = m_PhysicalLocation.t + pISkeletonPose->GetAbsJointByID(0).t;
  92         pAuxGeom->DrawLine(absRootPos, RGBA8(0xff, 0x00, 0x00, 0x00), absRootPos + m_PhysicalLocation.GetColumn0() * 2.0f, RGBA8(0x00, 0x00, 0x00, 0x00));
  93         pAuxGeom->DrawLine(absRootPos, RGBA8(0x00, 0xff, 0x00, 0x00), absRootPos + m_PhysicalLocation.GetColumn1() * 2.0f, RGBA8(0x00, 0x00, 0x00, 0x00));
  94         pAuxGeom->DrawLine(absRootPos, RGBA8(0x00, 0x00, 0xff, 0x00), absRootPos + m_PhysicalLocation.GetColumn2() * 2.0f, RGBA8(0x00, 0x00, 0x00, 0x00));
  95
  96         //---------------------------------------
  97         //---   draw the path of the past
  98         //---------------------------------------
  99         uint32 numEntries = m_arrAnimatedCharacterPath.size();
 100         for (int32 i = (numEntries - 2); i > -1; i--)
 101                 m_arrAnimatedCharacterPath[i + 1] = m_arrAnimatedCharacterPath[i];
 102         m_arrAnimatedCharacterPath[0] = m_PhysicalLocation.t;
 103         for (uint32 i = 0; i < numEntries; i++)
 104         {
 105                 aabb.min = Vec3(-0.01f, -0.01f, -0.01f) + m_arrAnimatedCharacterPath[i];
 106                 aabb.max = Vec3(+0.01f, +0.01f, +0.01f) + m_arrAnimatedCharacterPath[i];
 107                 pAuxGeom->DrawAABB(aabb, 1, RGBA8(0x00, 0x00, 0xff, 0x00), eBBD_Extremes_Color_Encoded);
 108         }
 109
 110         if (mv_showGrid)
 111                 DrawHeightField();
 112
 113         GetIEditor()->GetSystem()->GetIConsole()->GetCVar("ca_DrawSkeleton")->Set(mv_showSkeleton);
 114 }
 115
 116 //--------------------------------------------------------------------------
 117
 118 void CModelViewportCE::PlayerControlIO()
 119 {
 120
 121         IInput* pIInput = GetISystem()->GetIInput(); // Cache IInput pointer.
 122         pIInput->Update(true);
 123
 124         //-------------------------------------------------------------------
 125         //-------            Keyboard and Game-Pad handling         ---------
 126         //-------------------------------------------------------------------
 127         m_key_SPACE = m_key_W = m_key_A = m_key_D = m_key_S = 0;
 128         if (CheckVirtualKey(VK_UP) || CheckVirtualKey('W')) m_key_W = 1;
 129         if (CheckVirtualKey(VK_DOWN) || CheckVirtualKey('S')) m_key_S = 1;
 130         if (CheckVirtualKey(VK_LEFT) || CheckVirtualKey('A')) m_key_A = 1;
 131         if (CheckVirtualKey(VK_RIGHT) || CheckVirtualKey('D')) m_key_D = 1;
 132         if (m_key_W) m_key_S = 0; //forward has priority
 133         if (m_key_A) m_key_D = 0; //left has priority
 134
 135         m_keyrcr_W <<= 1;
 136         m_keyrcr_W |= m_key_W;
 137         m_keyrcr_S <<= 1;
 138         m_keyrcr_S |= m_key_S;
 139         m_keyrcr_A <<= 1;
 140         m_keyrcr_A |= m_key_A;
 141         m_keyrcr_D <<= 1;
 142         m_keyrcr_D |= m_key_D;
 143
 144         if (CheckVirtualKey(VK_RETURN)) { m_key_SPACE = 1; }
 145         m_keyrcr_SPACE <<= 1;
 146         m_keyrcr_SPACE |= m_key_SPACE;
 147
 148         if ((m_keyrcr_A & 3) == 2 || (m_keyrcr_D & 3) == 2)
 149                 m_LTHUMB.x = 0.0f;
 150         if ((m_keyrcr_W & 3) == 2 || (m_keyrcr_S & 3) == 2)
 151                 m_LTHUMB.y = 0.0f;
 152
 153         m_LTHUMB.x += -f32(m_key_A);
 154         m_LTHUMB.x += f32(m_key_D);
 155         m_LTHUMB.y += f32(m_key_W);
 156         m_LTHUMB.y += -f32(m_key_S);
 157         m_LTHUMB.x = clamp_tpl(m_LTHUMB.x, -1.0f, 1.0f);
 158         m_LTHUMB.y = clamp_tpl(m_LTHUMB.y, -1.0f, 1.0f);
 159
 160 }
 161
 162 //---------------------------------------------------------------------------
 163
 164 void CModelViewportCE::PlayerControlHuman(const Vec2& vViewDir2D, f32 fGroundRadian, f32 fGroundRadianMoveDir)
 165 {
 166         float color1[4] = { 1, 1, 1, 1 };
 167         IRenderAuxGeom* pAuxGeom = m_renderer->GetIRenderAuxGeom();
 168         f32 FrameTime = GetISystem()->GetITimer()->GetFrameTime();
 169
 170         ISkeletonAnim* pISkeletonAnim = GetCharacterBase()->GetISkeletonAnim();
 171         ISkeletonPose* pISkeletonPose = GetCharacterBase()->GetISkeletonPose();
 172
 173         pISkeletonAnim->SetAnimationDrivenMotion(1);
 174         pISkeletonAnim->SetLayerPlaybackScale(0, 1); //no scaling
 175
 176         //------------------------------------------------------------------------------
 177         //------------------------------------------------------------------------------
 178         //------------------------------------------------------------------------------
 179
 180         if (m_LTHUMB.GetLength() > 1.0f)
 181                 m_LTHUMB.Normalize();
 182
 183         uint32 num = sizeof(m_arrLTHUMB) / sizeof(Vec2);
 184         for (int32 i = (num - 2); i > -1; i--)
 185                 m_arrLTHUMB[i + 1] = m_arrLTHUMB[i];
 186         m_arrLTHUMB[0] = m_LTHUMB;
 187
 188         uint32 index = 0;  //min(uint32(0.10f/m_AverageFrameTime),num-1);
 189
 190         f32 fDesiredSpeed = m_arrLTHUMB[index].GetLength() * 5.0f;
 191         if (fDesiredSpeed > 0.1f)
 192         {
 193                 if (fDesiredSpeed > 3.0f)
 194                         fDesiredSpeed = 5.0f;
 195                 else
 196                         fDesiredSpeed = 1.3f;
 197         }
 198
 199         f32 fSlopeSlowDown = 1.0f - fabsf(fGroundRadian);
 200         m_renderer->Draw2dLabel(12, g_ypos, 1.2f, color1, false, "fSlopeSlowDown: %f", fSlopeSlowDown);
 201         g_ypos += 15;
 202
 203         static f32 fDesiredSpeedSmooth = 0;
 204         static f32 fDesiredSpeedRate = 0;
 205         SmoothCD(fDesiredSpeedSmooth, fDesiredSpeedRate, m_AverageFrameTime, fDesiredSpeed, 0.001f);
 206
 207         //      uint32 SnapTurn=0;
 208         f32 rad = Ang3::CreateRadZ(m_arrLTHUMB[index], m_arrLTHUMB[0]);
 209         /*if ( fabsf(rad)>(gf_PI*0.5f) && m_arrLTHUMB[0].GetLength()>0.85f && m_arrLTHUMB[index].GetLength()>0.80f )
 210            {
 211            //           SnapTurn=1;
 212            //fDesiredSpeed=0.0f;
 213            for (int32 i=1; i<num; i++)
 214             m_arrLTHUMB[i]=m_arrLTHUMB[0];
 215            Vec2 dir = m_arrLTHUMB[0].GetNormalized();
 216            m_vWorldDesiredBodyDirection.x       =       dir.x*vViewDir2D.y+dir.y*vViewDir2D.x;
 217            m_vWorldDesiredBodyDirection.y       =       dir.y*vViewDir2D.y-dir.x*vViewDir2D.x;
 218            }*/
 219
 220         //use the GamePad radian to set the m_vWorldDesiredBodyDirection
 221         m_renderer->Draw2dLabel(12, g_ypos, 1.2f, color1, false, "vViewDir2D: %f %f    RAD:%f ", vViewDir2D.x, vViewDir2D.y, fabsf(rad));
 222         g_ypos += 15;
 223         m_renderer->Draw2dLabel(12, g_ypos, 1.2f, color1, false, "index: %d", index);
 224         g_ypos += 15;
 225
 226         if (fDesiredSpeed > 0.1f)
 227         {
 228                 Vec2 dir = m_arrLTHUMB[index].GetNormalized();
 229                 m_vWorldDesiredBodyDirection.x = dir.x * vViewDir2D.y + dir.y * vViewDir2D.x;
 230                 m_vWorldDesiredBodyDirection.y = dir.y * vViewDir2D.y - dir.x * vViewDir2D.x;
 231         }
 232         m_vWorldDesiredMoveDirection = m_vWorldDesiredBodyDirection;  //in this case we always travel in the body direction.
 233
 234         uint32 LookIK = m_pCharPanel_Animation->GetLookIK();
 235         IAnimationPoseBlenderDir* pIPoseBlenderLook = pISkeletonPose->GetIPoseBlenderLook();
 236         if (pIPoseBlenderLook)
 237         {
 238                 pIPoseBlenderLook->SetState(LookIK);
 239                 pIPoseBlenderLook->SetFadeoutAngle(DEG2RAD(120));
 240                 pIPoseBlenderLook->SetTarget(m_PhysicalLocation.t + m_vWorldDesiredBodyDirection * 20);
 241         }
 242
 243         uint32 AimIK = m_pCharPanel_Animation->GetAimIK();
 244         Vec3 vAimPos = Vec3(12.0f, +10.0f, 1.0f);
 245         IAnimationPoseBlenderDir* pIPoseBlenderAim = pISkeletonPose->GetIPoseBlenderAim();
 246         if (pIPoseBlenderAim)
 247         {
 248                 pIPoseBlenderAim->SetState(AimIK);
 249                 pIPoseBlenderAim->SetTarget(vAimPos);
 250                 pIPoseBlenderAim->SetFadeOutSpeed(0.4f);
 251         }
 252         pAuxGeom->SetRenderFlags(e_Def3DPublicRenderflags);
 253
 254         static uint32 keyrcr_R = 0;
 255         keyrcr_R = (keyrcr_R << 1) | uint32(m_RT > 0.8f);
 256         if ((keyrcr_R & 3) == 1)
 257                 pISkeletonPose->ApplyRecoilAnimation(0.2f, 0.10f, 1);
 258
 259         static Ang3 angle = Ang3(ZERO);
 260         angle.x += 0.1f;
 261         angle.y += 0.01f;
 262         angle.z += 0.001f;
 263         AABB sAABB = AABB(Vec3(-0.1f, -0.1f, -0.1f), Vec3(+0.1f, +0.1f, +0.1f));
 264         OBB obb = OBB::CreateOBBfromAABB(Matrix33::CreateRotationXYZ(angle), sAABB);
 265         pAuxGeom->DrawOBB(obb, vAimPos, 1, RGBA8(0xff, 0xff, 0xff, 0xff), eBBD_Extremes_Color_Encoded);
 266
 267         static f32 fGroundRadianSmooth = 0;
 268         static f32 fGroundRadianRate = 0;
 269         SmoothCD(fGroundRadianSmooth, fGroundRadianRate, m_AverageFrameTime, fGroundRadian, 0.10f);
 270
 271         static f32 fGroundRadianMoveDirSmooth = 0;
 272         static f32 fGroundRadianMoveDirRate = 0;
 273         SmoothCD(fGroundRadianMoveDirSmooth, fGroundRadianMoveDirRate, m_AverageFrameTime, fGroundRadianMoveDir, 0.10f);
 274
 275         {
 276                 Vec2 vLTHUMB = m_arrLTHUMB[0] * 5.0f;
 277                 if (vLTHUMB.GetLength() > 0.001f)
 278                 {
 279                         Vec2 dir = vLTHUMB.GetNormalized();
 280                         m_vWorldDesiredBodyDirection2.x = dir.x * vViewDir2D.y + dir.y * vViewDir2D.x;
 281                         m_vWorldDesiredBodyDirection2.y = dir.y * vViewDir2D.y - dir.x * vViewDir2D.x;
 282                 }
 283
 284                 //just for debugging
 285                 f32 fRadBody = -atan2f(m_vWorldDesiredBodyDirection2.x, m_vWorldDesiredBodyDirection2.y);
 286                 Matrix33 MatBody33 = Matrix33::CreateRotationZ(fRadBody);
 287                 const f32 size = 0.009f;
 288                 const f32 length = 2.2f;
 289                 AABB yaabb = AABB(Vec3(-size, 0.0f, -size), Vec3(size, length, size));
 290                 OBB obb = OBB::CreateOBBfromAABB(MatBody33, yaabb);
 291                 pAuxGeom->DrawOBB(obb, m_PhysicalLocation.t, 1, RGBA8(0x00, 0xff, 0xff, 0x00), eBBD_Extremes_Color_Encoded);
 292                 pAuxGeom->DrawCone(m_PhysicalLocation.t + MatBody33.GetColumn1() * length, MatBody33.GetColumn1(), 0.03f, 0.15f, RGBA8(0x00, 0xff, 0xff, 0x00));
 293         }
 294
 295 }
 296
 297 //------------------------------------------------------------------------
 298 //---  camera control
 299 //------------------------------------------------------------------------
 300 void CModelViewportCE::PlayerControlCamera()
 301 {
 302
 303         m_relCameraRotZ = m_RTHUMB.x * m_AverageFrameTime;
 304         m_relCameraRotX = m_RTHUMB.y * m_AverageFrameTime;
 305
 306         //update and clamp rotation-speed
 307         f32 YawRad = -m_relCameraRotZ * 2; //yaw-speed
 308         m_absLookDirectionXY = Matrix33::CreateRotationZ(YawRad) * m_absLookDirectionXY;
 309         m_absLookDirectionXY.Normalize();
 310
 311         m_absCameraHigh -= m_relCameraRotX;
 312         if (m_absCameraHigh > 3)
 313                 m_absCameraHigh = 3;
 314         if (m_absCameraHigh < 0)
 315                 m_absCameraHigh = 0;
 316
 317         SmoothCD(m_LookAt, m_LookAtRate, m_AverageFrameTime, Vec3(m_PhysicalLocation.t.x, m_PhysicalLocation.t.y, 0.7f), 0.02f);
 318         //      Vec3 dir = (m_absLookDirectionXY+vWorldCurrentBodyDirection).GetNormalizedSafe(Vec3(0,1,0));
 319         //      m_absCameraPos          =       -dir*2 + m_LookAt+Vec3(0,0,m_absCameraHigh);
 320         m_absCameraPos = -m_absLookDirectionXY * 2 + m_LookAt + Vec3(0, 0, m_absCameraHigh);
 321
 322         Matrix33 orientation = Matrix33::CreateRotationVDir((m_LookAt - m_absCameraPos).GetNormalized(), 0);
 323         if (m_pCharPanel_Animation->GetFixedCamera())
 324         {
 325                 m_absCameraPos = Vec3(0, 3, 1);
 326                 orientation = Matrix33::CreateRotationVDir((m_LookAt - m_absCameraPos).GetNormalized());
 327         }
 328         SetViewTM(Matrix34(orientation, m_absCameraPos));
 329 }
 330
 331 //---------------------------------------------------------------------------
 332 //---------------------------------------------------------------------------
 333 //---------------------------------------------------------------------------
 334
 335 int PostProcessCallback_PlayControl(ICharacterInstance* pInstance, void* pPlayer)
 336 {
 337         //process bones specific stuff (IK, torso rotation, etc)
 338         ((CModelViewportCE*)pPlayer)->PostProcessCallback_PlayControl(pInstance);
 339         return 1;
 340 }
 341 void CModelViewportCE::PostProcessCallback_PlayControl(ICharacterInstance* pInstance)
 342 {
 343         IRenderAuxGeom* pAuxGeom = m_renderer->GetIRenderAuxGeom();
 344         ISkeletonAnim* pISkeletonAnim = pInstance->GetISkeletonAnim();
 345         //float color1[4] = {1,1,1,1};
 346         //m_renderer->Draw2dLabel(12,g_ypos,1.2f,color1,false,"PlayerControl PostprocessCallback" );
 347         //g_ypos+=10;
 348         UseFootIKNew(pInstance);
 349 }
 350
 351 //-------------------------------------------------------------------------------------------
 352 //-------------------------------------------------------------------------------------------
 353 //-------------------------------------------------------------------------------------------
 354 uint32 CModelViewportCE::UseFootIKNew(ICharacterInstance* pInstance)
 355 {
 356         float color1[4] = { 1, 1, 1, 1 };
 357         IRenderAuxGeom* pAuxGeom = m_renderer->GetIRenderAuxGeom();
 358         ISkeletonAnim* pISkeletonAnim = pInstance->GetISkeletonAnim();
 359         ISkeletonPose* pISkeletonPose = pInstance->GetISkeletonPose();
 360         const IDefaultSkeleton& rIDefaultSkeleton = pInstance->GetIDefaultSkeleton();
 361
 362         f32 fSmoothTime = 0.07f;
 363
 364         static Ang3 angle = Ang3(ZERO);
 365         angle.x += 0.01f;
 366         angle.y += 0.001f;
 367         angle.z += 0.0001f;
 368         AABB sAABB = AABB(Vec3(-0.02f, -0.02f, -0.02f), Vec3(+0.02f, +0.02f, +0.02f));
 369         OBB obb = OBB::CreateOBBfromAABB(Matrix33::CreateRotationXYZ(angle), sAABB);
 370
 371         Matrix33 m33;
 372         AABB wAABB;
 373
 374         BBox bbox;
 375         m_arrBBoxes.resize(0);
 376
 377         m33 = Matrix33::CreateRotationXYZ(Ang3(-0.5f, 0, 0));
 378         wAABB = AABB(Vec3(-10.00f, -10.00f, -0.10f), Vec3(+10.00f, +10.00f, +0.20f));
 379         bbox.obb = OBB::CreateOBBfromAABB(m33, wAABB);
 380         bbox.pos = Vec3(0, -5.0f, 0);
 381         bbox.col = RGBA8(0xff, 0x00, 0x00, 0xff);
 382         m_arrBBoxes.push_back(bbox);
 383
 384         m33 = Matrix33::CreateRotationXYZ(Ang3(0, 0, 0));
 385         wAABB = AABB(Vec3(-20.00f, -20.00f, -0.50f), Vec3(+20.00f, +20.00f, -0.01f));
 386         bbox.obb = OBB::CreateOBBfromAABB(m33, wAABB);
 387         bbox.pos = Vec3(0, -5.0f, 0);
 388         bbox.col = RGBA8(0x1f, 0x1f, 0x3f, 0xff);
 389         m_arrBBoxes.push_back(bbox);
 390
 391         m33 = Matrix33::CreateRotationXYZ(Ang3(0, 0, 0));
 392         wAABB = AABB(Vec3(-2.00f, -2.00f, -0.20f), Vec3(+2.00f, +2.00f, +0.20f));
 393         bbox.obb = OBB::CreateOBBfromAABB(m33, wAABB);
 394         bbox.pos = Vec3(5, 4, 0);
 395         bbox.col = RGBA8(0x3f, 0x1f, 0x1f, 0xff);
 396         m_arrBBoxes.push_back(bbox);
 397
 398         m33 = Matrix33::CreateRotationXYZ(Ang3(0, 0, 0));
 399         wAABB = AABB(Vec3(-1.00f, -1.00f, -0.20f), Vec3(+1.00f, +1.00f, +0.30f));
 400         bbox.obb = OBB::CreateOBBfromAABB(m33, wAABB);
 401         bbox.pos = Vec3(5, 4, 0);
 402         bbox.col = RGBA8(0x3f, 0x1f, 0x1f, 0xff);
 403         m_arrBBoxes.push_back(bbox);
 404
 405         m33 = Matrix33::CreateRotationXYZ(Ang3(0, 0, 0));
 406         wAABB = AABB(Vec3(-1.00f, -1.00f, -0.20f), Vec3(+1.00f, +1.00f, +0.20f));
 407         bbox.obb = OBB::CreateOBBfromAABB(m33, wAABB);
 408         bbox.pos = Vec3(-5, 7, 0.1f);
 409         bbox.col = RGBA8(0x3f, 0x1f, 0x1f, 0xff);
 410         m_arrBBoxes.push_back(bbox);
 411
 412         Vec3 vCharWorldPos = m_PhysicalLocation.t;
 413         IVec CWP = CheckFootIntersection(vCharWorldPos, vCharWorldPos);
 414
 415         f32 fMinHigh = m_PhysicalLocation.t.z;
 416         if (fMinHigh < CWP.heel.z)
 417                 fMinHigh = CWP.heel.z;
 418         Vec3 vGroundNormalRoot = CWP.nheel * m_PhysicalLocation.q;
 419
 420         f32 fGroundAngle = acos_tpl(vGroundNormalRoot.z);
 421         f32 fGroundHeight = fMinHigh - (fGroundAngle * 0.3f);
 422         Vec3 gnormal = Vec3(0, vGroundNormalRoot.y, vGroundNormalRoot.z);
 423         f32 cosine = Vec3(0, 0, 1) | gnormal;
 424         Vec3 sine = Vec3(0, 0, 1) % gnormal;
 425         f32 fGroundAngleMoveDir = RAD2DEG(atan2(sgn(sine.x) * sine.GetLength(), cosine));
 426
 427         //      m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"GroundAngle: rad:%f  degree:%f  fGroundAngleMoveDir:%f", fGroundAngle, RAD2DEG(fGroundAngle), fGroundAngleMoveDir );
 428         //      g_ypos+=14;
 429
 430         //take always the lowest leg for the body position
 431         static f32 fGroundHeightSmooth = 0.0f;
 432         static f32 fGroundHeightRate = 0.0f;
 433         SmoothCD(fGroundHeightSmooth, fGroundHeightRate, m_AverageFrameTime, fGroundHeight, fSmoothTime);
 434
 435         //      m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"fGroundHeightSmooth: %f", fGroundHeightSmooth );
 436         //      g_ypos+=14;
 437         //      m_PhysicalLocation.t.z = fGroundHeightSmooth; //stay on ground
 438
 439         int32 LHeelIdx = rIDefaultSkeleton.GetJointIDByName("Bip01 L Heel");
 440         if (LHeelIdx < 0) return 0;
 441         int32 LToe0Idx = rIDefaultSkeleton.GetJointIDByName("Bip01 L Toe0");
 442         if (LToe0Idx < 0) return 0;
 443         uint32 FootplantL = 0;
 444         Vec3 Final_LHeel = m_PhysicalLocation * pISkeletonPose->GetAbsJointByID(LHeelIdx).t;
 445         Vec3 Final_LToe0 = m_PhysicalLocation * pISkeletonPose->GetAbsJointByID(LToe0Idx).t;
 446
 447         int32 RHeelIdx = rIDefaultSkeleton.GetJointIDByName("Bip01 R Heel");
 448         if (RHeelIdx < 0) return 0;
 449         int32 RToe0Idx = rIDefaultSkeleton.GetJointIDByName("Bip01 R Toe0");
 450         if (RToe0Idx < 0) return 0;
 451         uint32 FootplantR = 0;
 452         Vec3 Final_RHeel = m_PhysicalLocation * pISkeletonPose->GetAbsJointByID(RHeelIdx).t;
 453         Vec3 Final_RToe0 = m_PhysicalLocation * pISkeletonPose->GetAbsJointByID(RToe0Idx).t;
 454
 455         //-------------------------------------------------------------------------
 456
 457         IVec L4 = CheckFootIntersection(Final_LHeel, Final_LToe0);
 458         pAuxGeom->DrawOBB(obb, L4.toe, 1, RGBA8(0xff, 0xff, 0xff, 0xff), eBBD_Extremes_Color_Encoded);
 459         pAuxGeom->DrawOBB(obb, L4.heel, 1, RGBA8(0x1f, 0xff, 0xff, 0xff), eBBD_Extremes_Color_Encoded);
 460
 461         IVec R4 = CheckFootIntersection(Final_RHeel, Final_RToe0);
 462         pAuxGeom->DrawOBB(obb, R4.toe, 1, RGBA8(0xff, 0xff, 0xff, 0xff), eBBD_Extremes_Color_Encoded);
 463         pAuxGeom->DrawOBB(obb, R4.heel, 1, RGBA8(0x1f, 0xff, 0xff, 0xff), eBBD_Extremes_Color_Encoded);
 464
 465         static Plane LSmoothGroundPlane(Vec3(0, 0, 1), 0);
 466         static Plane LSmoothGroundPlaneRate(Vec3(0, 0, 1), 0);
 467         {
 468                 Plane LGroundPlane(Vec3(0, 0, 1), 0);
 469                 if ((L4.heel.z - Final_LHeel.z) > -0.03f)
 470                         LGroundPlane = Plane::CreatePlane(L4.nheel * m_PhysicalLocation.q, m_PhysicalLocation.GetInverted() * L4.heel);
 471                 //m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"LGroundPlane: %f %f %f  d:%f", LGroundPlane.n.x,LGroundPlane.n.y,LGroundPlane.n.z,LGroundPlane.d );
 472                 //g_ypos+=14;
 473
 474                 SmoothCD(LSmoothGroundPlane, LSmoothGroundPlaneRate, m_AverageFrameTime, LGroundPlane, fSmoothTime);
 475                 //      m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"LSmoothGroundPlane: %f %f %f  d:%f", LSmoothGroundPlane.n.x,LSmoothGroundPlane.n.y,LSmoothGroundPlane.n.z,LSmoothGroundPlane.d );
 476                 //      g_ypos+=14;
 477         }
 478
 479         static Plane RSmoothGroundPlane(Vec3(0, 0, 1), 0);
 480         static Plane RSmoothGroundPlaneRate(Vec3(0, 0, 1), 0);
 481         {
 482                 Plane RGroundPlane(Vec3(0, 0, 1), 0);
 483                 if ((R4.heel.z - Final_RToe0.z) > -0.03f)
 484                         RGroundPlane = Plane::CreatePlane(R4.nheel * m_PhysicalLocation.q, m_PhysicalLocation.GetInverted() * R4.heel);
 485                 //m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"RGroundPlane: %f %f %f  d:%f", RGroundPlane.n.x,RGroundPlane.n.y,RGroundPlane.n.z,RGroundPlane.d );
 486                 //g_ypos+=14;
 487
 488                 SmoothCD(RSmoothGroundPlane, RSmoothGroundPlaneRate, m_AverageFrameTime, RGroundPlane, fSmoothTime);
 489                 //      m_renderer->Draw2dLabel(12,g_ypos,1.5f,color1,false,"RSmoothGroundPlane: %f %f %f  d:%f", RSmoothGroundPlane.n.x,RSmoothGroundPlane.n.y,RSmoothGroundPlane.n.z,RSmoothGroundPlane.d );
 490                 //      g_ypos+=14;
 491         }
 492
 493         return 1;
 494 }
 495
 496 IVec CModelViewportCE::CheckFootIntersection(const Vec3& Final_Heel, const Vec3& Final_Toe0)
 497 {
 498         IRenderAuxGeom* pAuxGeom = m_renderer->GetIRenderAuxGeom();
 499
 500         Vec3 out;
 501         uint32 ival = 0;
 502         f32 HeightHeel = -100.0f;
 503         f32 HeightToe0 = -100.0f;
 504         Vec3 normalHeel = Vec3(0, 0, 1);
 505         Vec3 normalToe0 = Vec3(0, 0, 1);
 506
 507         Ray RayHeel = Ray(Final_Heel + Vec3(0.0f, 0.0f, 5.5f), Vec3(0, 0, -1.0f));
 508         Ray RayToe0 = Ray(Final_Toe0 + Vec3(0.0f, 0.0f, 5.5f), Vec3(0, 0, -1.0f));
 509         //pAuxGeom->DrawLine(RayToeN.origin,RGBA8(0xff,0xff,0xff,0x00),RayToeN.origin+RayToeN.direction*10.0f,RGBA8(0x00,0x0,0xff,0x00));
 510         //pAuxGeom->DrawLine(RayHeel.origin,RGBA8(0xff,0xff,0xff,0x00),RayHeel.origin+RayHeel.direction*10.0f,RGBA8(0xff,0x0,0x00,0x00));
 511
 512         uint32 numBoxes = m_arrBBoxes.size();
 513
 514         for (uint32 i = 0; i < numBoxes; i++)
 515         {
 516                 OBB obb = m_arrBBoxes[i].obb;
 517                 Vec3 pos = m_arrBBoxes[i].pos;
 518                 ColorB col = m_arrBBoxes[i].col;
 519                 pAuxGeom->DrawOBB(obb, pos, 1, col, eBBD_Extremes_Color_Encoded);
 520
 521                 ival = Intersect::Ray_OBB(RayHeel, pos, obb, out);
 522                 if (ival == 0x01)
 523                         if (HeightHeel < out.z)
 524                         {
 525                                 HeightHeel = out.z;
 526                                 normalHeel = obb.m33.GetColumn2(); // * m_PhysicalLocation.q;
 527                         }
 528                 ;
 529
 530                 ival = Intersect::Ray_OBB(RayToe0, pos, obb, out);
 531                 if (ival == 0x01)
 532                         if (HeightToe0 < out.z)
 533                         {
 534                                 HeightToe0 = out.z;
 535                                 normalToe0 = obb.m33.GetColumn2(); // * m_PhysicalLocation.q;
 536                         }
 537                 ;
 538         }
 539
 540         IVec retval;
 541         retval.nheel = normalHeel.GetNormalized();
 542         retval.ntoe = normalToe0.GetNormalized();
 543         retval.heel = Vec3(Final_Heel.x, Final_Heel.y, HeightHeel);
 544         retval.toe = Vec3(Final_Toe0.x, Final_Toe0.y, HeightToe0);
 545         return retval;
 546 }