BVB Source Codes

CRYENGINE Show AttachmentProxy.cpp Source code

Return Download CRYENGINE: download AttachmentProxy.cpp Source code - Download CRYENGINE Source code - Type:.cpp
  1. // Copyright 2001-2016 Crytek GmbH / Crytek Group. All rights reserved.
  2.  
  3. #include "stdafx.h"
  4. #include "AttachmentProxy.h"
  5.  
  6. #include <CryRenderer/IRenderAuxGeom.h>
  7. #include "AttachmentManager.h"
  8. #include "CharacterInstance.h"
  9.  
  10. uint32 CProxy::SetJointName(const char* szJointName)
  11. {
  12.         m_nJointID = -1;
  13.         if (!szJointName)  { assert(0); return 0; }
  14.         int nJointID = m_pAttachmentManager->m_pSkelInstance->m_pDefaultSkeleton->GetJointIDByName(szJointName);
  15.         if (nJointID < 0)
  16.         {
  17.                 // this is a severe bug if the bone name is invalid and someone tries to attach something
  18.                 g_pILog->LogError("SetJointName is called for bone \"%s\", which is not in the model \"%s\". Ignoring, but this may cause a crash because the corresponding object won't be detached after it's destroyed", szJointName, m_pAttachmentManager->m_pSkelInstance->m_pDefaultSkeleton->GetModelFilePath());
  19.                 return 0;
  20.         }
  21.         m_strJointName = szJointName;
  22.         ProjectProxy();
  23.         return 1;
  24. };
  25.  
  26. uint32 CProxy::ProjectProxy()
  27. {
  28.         m_nJointID = -1;
  29.         int32 JointId = m_pAttachmentManager->m_pSkelInstance->m_pDefaultSkeleton->GetJointIDByName(m_strJointName.c_str());
  30.         if (JointId < 0)
  31.                 return 0;
  32.         CCharInstance* pSkelInstance = m_pAttachmentManager->m_pSkelInstance;
  33.         int nJointCount = pSkelInstance->m_pDefaultSkeleton->GetJointCount();
  34.         if (JointId >= nJointCount)
  35.                 return 0;
  36.  
  37.         m_nJointID = JointId;
  38.         const CDefaultSkeleton* pDefaultSkeleton = pSkelInstance->m_pDefaultSkeleton;
  39.         QuatT jointQT = pDefaultSkeleton->GetDefaultAbsJointByID(m_nJointID);
  40.         m_ProxyRelativeDefault = jointQT.GetInverted() * m_ProxyAbsoluteDefault;
  41.         return 1;
  42. }
  43.  
  44. void CProxy::AlignProxyWithJoint()
  45. {
  46.         CCharInstance* pSkelInstance = m_pAttachmentManager->m_pSkelInstance;
  47.         int32 nJointCount = pSkelInstance->m_pDefaultSkeleton->GetJointCount();
  48.         if (m_nJointID < nJointCount)
  49.         {
  50.                 m_ProxyAbsoluteDefault = pSkelInstance->m_pDefaultSkeleton->GetDefaultAbsJointByID(m_nJointID);
  51.                 m_ProxyRelativeDefault.SetIdentity();
  52.         }
  53. }
  54.  
  55. #define HPLANEEPSILON (0.000001f)
  56. #define MAXRADEPSILON (1000.0f)
  57. #define HXEPSILON     (0.0001f)
  58.  
  59. ILINE f32 fclamp01_tpl(f32 x) { return x < 0.0f ? 0.0f : x < 1.0f ? x : 1.0f; }
  60. ILINE f64 fclamp01_tpl(f64 x) { return x < 0.0 ? 0.0 : x < 1.0 ? x : 1.0;  }
  61.  
  62. ILINE f32 fispos(f32 x)       { return x > 0 ? 1.0f : 0.0f; }
  63. ILINE f64 fispos(f64 x)       { return x > 0 ? 1.0 : 0.0; }
  64.  
  65. ILINE f32 fisneg(f32 x)       { return x < 0 ? 1.0f : 0.0f; }
  66. ILINE f64 fisneg(f64 x)       { return x < 0 ? 1.0 : 0.0; }
  67.  
  68. //returns the closest-distance of a point to the surface of a lozenge
  69. f32 CProxy::GetDistance(const Vec3& p) const
  70. {
  71.         f32 sq = 0, t;
  72.         t = fabs_tpl(p.x) - m_params.x;
  73.         if (t > 0) sq += t * t;
  74.         t = fabs_tpl(p.y) - m_params.y;
  75.         if (t > 0) sq += t * t;
  76.         t = fabs_tpl(p.z) - m_params.z;
  77.         if (t > 0) sq += t * t;
  78.         return sqrt_tpl(sq) - m_params.w;
  79. };
  80.  
  81. //returns the closest-distance of a sphere to the surface of a lozenge
  82. f32 CProxy::GetDistance(const Vec3& p, f32 r) const
  83. {
  84.         f32 sq = 0, t;
  85.         Vec4 loz = m_params;
  86.         loz.w += r;
  87.         t = fabs(p.x) - loz.x;
  88.         if (t > 0) sq += t * t;
  89.         t = fabs(p.y) - loz.y;
  90.         if (t > 0) sq += t * t;
  91.         t = fabs(p.z) - loz.z;
  92.         if (t > 0) sq += t * t;
  93.         return sqrt_tpl(sq) - loz.w;
  94. };
  95.  
  96. //checks if a capsule and a lozenge overlap and returns a "pseudo" square-distance
  97. f32 CProxy::TestOverlapping(const Vec3& p0, Vec3 dir, f32 sl, f32 sr) const
  98. {
  99.         Vec3 p1 = p0 + dir * sl, m = (p1 + p0) * 0.5f;
  100.         Vec4 loz = m_params;
  101.         loz.w += sr;
  102.         if (loz.x + loz.y + loz.z == 0)
  103.         {
  104.                 Vec3 ab = p1 - p0;
  105.                 f32 e = -p0 | ab, f = ab | ab;
  106.                 if (e <= 0) return (p0 | p0) - loz.w * loz.w;
  107.                 if (e >= f) return (p1 | p1) - loz.w * loz.w;
  108.                 return (p0 | p0) - e * e / f - loz.w * loz.w;
  109.         }
  110.         if (dir.x < 0) dir.x = -dir.x, m.x = -m.x;
  111.         if (dir.y < 0) dir.y = -dir.y, m.y = -m.y;
  112.         if (dir.z < 0) dir.z = -dir.z, m.z = -m.z;
  113.         f32 lext = sl * 0.5f;
  114.         struct
  115.         {
  116.                 f32 CheckEdge(const Vec3& p0, const Vec3& p1, const Vec4& loz)
  117.                 {
  118.                         f32 p0x = fabs(p0.x) - loz.x, p0y = fabs(p0.y) - loz.y, p0z = fabs(p0.z) - loz.z;
  119.                         f32 p1x = fabs(p1.x) - loz.x, p1y = fabs(p1.y) - loz.y, p1z = fabs(p1.z) - loz.z;
  120.                         p0x = max(0.0f, p0x), p0y = max(0.0f, p0y), p0z = max(0.0f, p0z);
  121.                         p1x = max(0.0f, p1x), p1y = max(0.0f, p1y), p1z = max(0.0f, p1z);
  122.                         return min(p0x * p0x + p0y * p0y + p0z * p0z, p1x * p1x + p1y * p1y + p1z * p1z) - loz.w * loz.w;
  123.                 }
  124.         } e;
  125.         if (dir.x == 0 && dir.y == 0)
  126.                 return fabs((loz.z - m.z) / dir.z) > lext ? e.CheckEdge(p0, p1, loz) : sqr(max(0.0f, fabsf(m.x) - loz.x)) + sqr(max(0.0f, fabsf(m.y) - loz.y)) - loz.w * loz.w;
  127.         if (dir.x == 0 && dir.z == 0)
  128.                 return fabs((loz.y - m.y) / dir.y) > lext ? e.CheckEdge(p0, p1, loz) : sqr(max(0.0f, fabsf(m.x) - loz.x)) + sqr(max(0.0f, fabsf(m.z) - loz.z)) - loz.w * loz.w;
  129.         if (dir.y == 0 && dir.z == 0)
  130.                 return fabs((loz.x - m.x) / dir.x) > lext ? e.CheckEdge(p0, p1, loz) : sqr(max(0.0f, fabsf(m.y) - loz.y)) + sqr(max(0.0f, fabsf(m.z) - loz.z)) - loz.w * loz.w;
  131.         Vec3 PmE(m.x - loz.x, m.y - loz.y, m.z - loz.z);
  132.         int p, i0 = 2, i1 = 0, i2 = 1;
  133.         if (dir.x * PmE.y <= dir.y * PmE.x && dir.x * PmE.z <= dir.z * PmE.x) i0 = 0, i1 = 1, i2 = 2;
  134.         if (dir.x * PmE.y >= dir.y * PmE.x && dir.y * PmE.z <= dir.z * PmE.y) i0 = 1, i1 = 2, i2 = 0;
  135.         if (dir[i0] && dir[i0] * (m[i1] + loz[i1]) >= dir[i1] * PmE[i0] && dir[i0] * (m[i2] + loz[i2]) >= dir[i2] * PmE[i0])
  136.                 return fabs(PmE[i0] / dir[i0]) > lext ? e.CheckEdge(p0, p1, loz) : -loz.w * loz.w;
  137.         for (uint32 i = 0; i < 2; p = i1, i1 = i2, i2 = p, i++)
  138.         {
  139.                 f32 pE1 = m[i1] + loz[i1], pE2 = m[i2] + loz[i2], bE1 = loz[i1], bE2 = loz[i2];
  140.                 f32 s = dir[i0] * dir[i0] + dir[i2] * dir[i2], x = s * pE1 - dir[i1] * (dir[i0] * PmE[i0] + dir[i2] * pE2);
  141.                 if (dir[i0] * pE1 >= dir[i1] * PmE[i0] || x >= 0)
  142.                 {
  143.                         s == 0 ? x = x <= s * bE1 * 2 ? pE1 - x * FLT_MAX : PmE[i1] : x = x <= s * bE1 * 2 ? pE1 - x / s : PmE[i1];
  144.                         f32 d = dir[i0] * PmE[i0] + dir[i1] * x + dir[i2] * pE2, t = d / (s + dir[i1] * dir[i1]);
  145.                         return fabs(t) > lext ? e.CheckEdge(p0, p1, loz) : PmE[i0] * PmE[i0] + x * x + pE2 * pE2 - d * t - loz.w * loz.w;
  146.                 }
  147.         }
  148.         f32 pE1 = m[i1] + loz[i1], pE2 = m[i2] + loz[i2];
  149.         f32 d = dir[i0] * PmE[i0] + dir[i2] * pE2 + dir[i1] * pE1, t = d / (dir | dir);
  150.         return (fabs(t) > lext) ? e.CheckEdge(p0, p1, loz) : PmE[i0] * PmE[i0] + sqr(pE2) + sqr(pE1) - d * t - loz.w * loz.w;
  151. };
  152.  
  153. //Projects a penetrating object out of a lozenge and back to its surface by using the smallest translation possible.
  154. Vec3 CProxy::ShortvecTranslationalProjection(const Vec3& ipos, f32 sr) const
  155. {
  156.         f32 fDistPointLozenge = GetDistance(ipos, sr);
  157.         if (fDistPointLozenge >= 0)
  158.                 return Vec3(ZERO);
  159.         Vec4 loz = m_params;
  160.         loz.w += sr;
  161.         if (loz.x + loz.y + loz.z == 0)
  162.         {
  163.                 if (ipos | ipos) return ipos * isqrt_tpl(ipos | ipos) * loz.w - ipos;
  164.                 return Vec3(loz.w, 0, 0);
  165.         }
  166.         struct
  167.         {
  168.                 Vec3 CheckEdge(const Vec3& ipos, f32 ca, f32 cr)
  169.                 {
  170.                         f32 t = ipos.x < 0 ? -1.0f : 1.0f;
  171.                         if (fabs(ipos.x) > ca)
  172.                         {
  173.                                 Vec3 tmp = ipos;
  174.                                 tmp.x -= ca * t;
  175.                                 if (tmp | tmp) return tmp * isqrt_tpl(tmp | tmp) * cr + Vec3(ca * t, 0, 0);
  176.                                 return Vec3(cr * t, 0, 0);
  177.                         }
  178.                         Vec3 tmp = Vec3(0, ipos.y, ipos.z);
  179.                         if (tmp | tmp) return tmp * isqrt_tpl(tmp | tmp) * cr + Vec3(ipos.x, 0, 0);
  180.                         return Vec3((cr + ca) * t, 0, 0);
  181.                 }
  182.         } e;
  183.         const Matrix33 m[2] = { Matrix33(0, 1, 0, -1, 0, 0, 0, 0, 1), Matrix33(0, 0, 1, 0, 1, 0, -1, 0, 0) };
  184.         if (loz.y + loz.z == 0)
  185.                 return e.CheckEdge(ipos, loz.x, loz.w) - ipos;
  186.         if (loz.x + loz.z == 0)
  187.                 return e.CheckEdge(Vec3(ipos.y, -ipos.x, ipos.z), loz.y, loz.w) * m[0] - ipos;
  188.         if (loz.x + loz.y == 0)
  189.                 return e.CheckEdge(Vec3(ipos.z, ipos.y, -ipos.x), loz.z, loz.w) * m[1] - ipos;
  190.         uint32 i = 3;
  191.         f32 x = FLT_MAX;
  192.         Vec3 tmp, abspos = Vec3(fabs(ipos.x), fabs(ipos.y), fabs(ipos.z));
  193.         f32 distx = loz.w + loz.x - abspos.x;
  194.         if (x > distx && abspos.y <= loz.y && abspos.z <= loz.z)
  195.                 x = distx, i = 0, tmp.x = (loz.w + loz.x) * (ipos.x < 0 ? -1 : 1), tmp.y = ipos.y, tmp.z = ipos.z;
  196.         f32 disty = loz.w + loz.y - abspos.y;
  197.         if (x > disty && abspos.x <= loz.x && abspos.z <= loz.z)
  198.                 x = disty, i = 1, tmp.x = ipos.x, tmp.y = (loz.w + loz.y) * (ipos.y < 0 ? -1 : 1), tmp.z = ipos.z;
  199.         f32 distz = loz.w + loz.z - abspos.z;
  200.         if (x > distz && abspos.x <= loz.x && abspos.y <= loz.y)
  201.                 x = distz, i = 2, tmp.x = ipos.x, tmp.y = ipos.y, tmp.z = (loz.w + loz.z) * (ipos.z < 0 ? -1.0f : 1.0f);
  202.         if (i < 3) return tmp - ipos;
  203.         f32 tx = (ipos.x < 0 ? -1.0f : 1.0f), ty = (ipos.y < 0 ? -1.0f : 1.0f), tz = (ipos.z < 0 ? -1.0f : 1.0f);
  204.         if (abspos.y >= loz.y && abspos.x >= loz.x)
  205.         {
  206.                 tmp = e.CheckEdge(Vec3(ipos.z, ipos.y - loz.y * ty, -ipos.x + loz.x * tx), loz.z, loz.w);
  207.                 return Vec3(-tmp.z + loz.x * tx, tmp.y + loz.y * ty, tmp.x) - ipos;
  208.         }
  209.         if (abspos.y >= loz.y && abspos.z >= loz.z)
  210.         {
  211.                 tmp = e.CheckEdge(Vec3(ipos.x, ipos.y - loz.y * ty, ipos.z - loz.z * tz), loz.x, loz.w), tmp.y += loz.y * ty, tmp.z += loz.z * tz;
  212.                 return tmp - ipos;
  213.         }
  214.         if (abspos.z >= loz.z && abspos.x >= loz.x)
  215.         {
  216.                 tmp = e.CheckEdge(Vec3(ipos.y, -ipos.x + loz.x * tx, ipos.z - loz.z * tz), loz.y, loz.w);
  217.                 return Vec3(-tmp.y + loz.x * tx, tmp.x, tmp.z + loz.z * tz) - ipos;
  218.         }
  219.         return Vec3(ZERO);
  220. }
  221.  
  222. //Projects a penetrating object out of a lozenge and back to its surface by using the predefined translation direction.
  223. f32 CProxy::DirectedTranslationalProjection(const Vec3& ipos, const Vec3& idir, f32 sl, f32 sr) const
  224. {
  225.         Diag33 r(idir.x < 0 ? -1.0f : 1.0f, idir.y < 0 ? -1.0f : 1.0f, idir.z < 0 ? -1.0f : 1.0f);
  226.         Vec3 rp = ipos * r, rd = idir * r, l, v;
  227.         Vec4 loz = m_params;
  228.         loz.w += sr;
  229.         f32 sl2 = sl + sl, t;
  230.         const Matrix33 m[4] = { Matrix33(0, 1, 0, -1, 0, 0, 0, 0, 1), Matrix33(1, 0, 0, 0, 1, 0, 0, 0, 1), Matrix33(1, 0, 0, 0, 0, -1, 0, 1, 0) };
  231.         struct
  232.         {
  233.                 f32 CheckEdge(const Vec3& ipos, const Vec3& idir, f32 ca, f32 cr)
  234.                 {
  235.                         Vec3 ns = ipos, ps = ipos;
  236.                         ns.x += ca, ps.x -= ca;
  237.                         f32 a = ca + ca, b = a * a, c = a * ns.x, d = a * idir.x, e = idir | idir, f = ns | idir;
  238.                         f32 g = ((ns | ns) - cr * cr) * b - c * c, h = b * e - d * d;
  239.                         if (h)
  240.                         {
  241.                                 f32 x = b * f - d * c, discr = x * x - h * g;
  242.                                 if (discr < 0) return 0;
  243.                                 f32 sq = sqrt(discr), t = (-x - sq) / h;
  244.                                 if (c + t * d > b) return CheckCorner(ps, idir, cr);
  245.                                 if (c + t * d < 0) return CheckCorner(ns, idir, cr);
  246.                                 return t * fisneg(t - (sq - x) / h);
  247.                         }
  248.                         if (g > 0) return 0;
  249.                         return CheckCorner(ps, idir, cr) * fisneg(idir.x) + CheckCorner(ns, idir, cr) * fispos(idir.x);
  250.                 }
  251.                 f32 CheckCorner(Vec3 p, Vec3 dir, f32 r)
  252.                 {
  253.                         f32 b = (p | dir), d = b * b + r * r - (p | p);
  254.                         if (d < 0)  return 0;
  255.                         return -b - sqrt_tpl(d);
  256.                 }
  257.         } e;
  258.         if (loz.x + loz.y + loz.z == 0)
  259.         {
  260.                 t = e.CheckCorner(ipos, idir, loz.w);
  261.                 return (t && t < sl2) ? sl2 - t : 0;
  262.         }
  263.         if (loz.y + loz.z == 0)
  264.         {
  265.                 t = e.CheckEdge(ipos, idir, loz.x, loz.w);
  266.                 return (t && t < sl2) ? sl2 - t : 0;
  267.         }
  268.         if (loz.x + loz.z == 0)
  269.         {
  270.                 t = e.CheckEdge(Vec3(ipos.y, -ipos.x, ipos.z), Vec3(idir.y, -idir.x, idir.z), loz.y, loz.w);
  271.                 return (t && t < sl2) ? sl2 - t : 0;
  272.         }
  273.         if (loz.x + loz.y == 0)
  274.         {
  275.                 t = e.CheckEdge(Vec3(ipos.z, ipos.y, -ipos.x), Vec3(idir.z, idir.y, -idir.x), loz.z, loz.w);
  276.                 return (t && t < sl2) ? sl2 - t : 0;
  277.         }
  278.         for (uint32 axis = 0; axis < 3; axis++)
  279.         {
  280.                 if (axis == 0)  l(loz.y, loz.x, loz.z);
  281.                 if (axis == 1)  l(loz.x, loz.y, loz.z);
  282.                 if (axis == 2)  l(loz.x, loz.z, loz.y);
  283.                 Vec3 p = rp * m[axis], d = rd * m[axis];
  284.                 if (d.y == 0)
  285.                         continue;
  286.                 f32 maxx = l.x + loz.w, maxy = l.y + loz.w, maxz = l.z + loz.w, c = (-p.y - maxy) / d.y;
  287.                 Vec3 spnt(d.x * c + p.x, -maxy, d.z * c + p.z);
  288.  
  289.                 if (fabs(spnt.z) < l.z && fabs(spnt.x) < l.x)
  290.                 {
  291.                         Vec3 spntp = spnt - p;
  292.                         f32 len = spntp.GetLength(), dot = d | spntp;
  293.                         if (dot >= 0 && sl2 > len)
  294.                                 return sl2 - len;
  295.                         if (dot < 0)
  296.                                 return sl2 + len;
  297.                 }
  298.                 if (l.z && spnt.x >= l.x && (t = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x + l.x), Vec3(d.z, d.y, -d.x), l.z, loz.w)))
  299.                 {
  300.                         v = d * t + p;
  301.                         if (fabs(v.z) <= maxz && (!l.x || v.x >= l.x) && (!l.y || v.y <= -l.y)) return (t && t < sl2) ? sl2 - t : 0;
  302.                 }
  303.                 if (l.x && spnt.z >= l.z && (t = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z - l.z), d, l.x, loz.w)))
  304.                 {
  305.                         v = d * t + p;
  306.                         if (fabs(v.x) <= maxx && (!l.z || v.z >= l.z) && (!l.y || v.y <= -l.y)) return (t && t < sl2) ? sl2 - t : 0;
  307.                 }
  308.                 if (l.z && spnt.x <= -l.x && (t = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x - l.x), Vec3(d.z, d.y, -d.x), l.z, loz.w)))
  309.                 {
  310.                         v = d * t + p;
  311.                         if (fabs(v.z) <= maxz && (!l.y || v.y <= -l.y) && (!l.x || v.x <= -l.x)) return (t && t < sl2) ? sl2 - t : 0;
  312.                 }
  313.                 if (l.x && spnt.z <= -l.z && (t = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z + l.z), d, l.x, loz.w)))
  314.                 {
  315.                         v = d * t + p;
  316.                         if (fabs(v.x) <= maxx && (!l.y || v.y <= -l.y) && (!l.z || v.z <= -l.z)) return (t && t < sl2) ? sl2 - t : 0;
  317.                 }
  318.         }
  319.         return 0;
  320. }
  321.  
  322. //Projects a penetrating object out of a lozenge and back to its surface by using the smallest rotation possible.
  323. Vec3 CProxy::ShortarcRotationalProjection(const Vec3& ipos, const Vec3& idir, f32 sl, f32 lr) const
  324. {
  325.         Vec4 loz = m_params;
  326.         loz.w += lr;
  327.         if (loz.x + loz.y + loz.z == 0)
  328.         {
  329.                 Vec3 pnt = ipos - idir * (ipos | idir), iha = (pnt % idir).GetNormalized();
  330.                 f32 pn = iha.x < 0 ? -1.0f : 1.0f, vx = pn * iha.x, vy = pn * iha.y, vz = pn * iha.z, w = 1.0f / (1.0f + vx);
  331.                 Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  332.                 Vec3 ip(ipos.x, ipos.y, ipos.z), np = ip * m;
  333.                 f32 x = ip | ip, cr2 = loz.w * loz.w, c0 = x - cr2 - cr2, cy = np.y, cz = np.z, d2, id, a, h, b, s2 = (1.0f - sqr(np.x / loz.w)) * cr2;
  334.                 if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  335.                         d2 = cy * cy + cz * cz, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, h = sqrt_tpl(max(0.0f, s2 - a * a)) * id * pn;
  336.                 else
  337.                         cy *= 0.5f, cz *= 0.5f, id = isqrt_tpl(cy * cy + cz * cz), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  338.                 return m * Vec3(np.x, cy * b - cz * h, cz * b + cy * h) - ipos;
  339.         }
  340.  
  341.         struct
  342.         {
  343.                 Vec3 CheckCapsule(const Vec3& pos, const Vec3& dir, f32 sl, f32 ca, f32 cr)
  344.                 {
  345.                         f32 rr = cr * cr, flip = 1;
  346.                         for (f32 t = -1; t < 1.5f; t += 2)
  347.                         {
  348.                                 Vec3 ip(pos.x - ca * t, pos.y, pos.z), pnt = ip - dir * (ip | dir);
  349.                                 if ((pnt | pnt) > rr) continue;
  350.                                 if (sqr(sl + cr) < (ip | ip)) continue;
  351.                                 Vec3 iha = (pnt % dir).GetNormalized();
  352.                                 f32 pn = iha.x < 0 ? -1.0f : 1.0f, vx = pn * iha.x, vy = pn * iha.y, vz = pn * iha.z, w = 1.0f / (1.0f + vx);
  353.                                 Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  354.                                 Vec3 np = ip * m;
  355.                                 f32 sine = 1.0f - sqr(np.x / cr);
  356.                                 if (sine < 0) continue;
  357.                                 f32 id, a, b, h, x = ip | ip, c0 = x - rr - rr, s2 = sine * rr, d = 0, d2 = np.y * np.y + np.z * np.z;
  358.                                 if (d2 == 0) continue;
  359.                                 if (sl * sl - sqr(x - rr) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  360.                                         id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, d = s2 - a * a, h = sqrt_tpl(fabs(d)) * id * pn;
  361.                                 else
  362.                                         np.y *= 0.5f, np.z *= 0.5f, id = isqrt_tpl(np.y * np.y + np.z * np.z), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  363.                                 if (d < 0) continue;
  364.                                 Vec3 tmp = m * Vec3(np.x, np.y * b - np.z * h, np.z * b + np.y * h);
  365.                                 if (tmp.x * t > 0)
  366.                                         return Vec3(tmp.x + ca * t, tmp.y, tmp.z);
  367.                         }
  368.                         f32 aa = pos.y * pos.y + pos.z * pos.z, a = sqrt_tpl(aa), A = 0.5f / a;
  369.                         f32 l = 1.0f / a, f = pos.y < 0 ? -l : l, fcos = f * pos.y, fsin = f * pos.z;
  370.                         f32 diry = dir.y * fcos + dir.z * fsin, dirz = dir.z * fcos - dir.y * fsin;
  371.                         if (pos.y * fcos + pos.z * fsin >= 0)
  372.                                 diry = -diry, dirz = -dirz, flip = -1;
  373.                         f32 rz = dirz < 0 ? -1.0f : 1.0f, cy = pos.y * 0.5f, cz = pos.z * 0.5f;
  374.                         f32 id = isqrt_tpl(cy * cy + cz * cz), qa = rr * id * 0.5f, b = qa * id, h = sqrt_tpl(fabs(rr - qa * qa)) * id * rz;
  375.                         f32 ty = cy * b + cz * h, tz = cz * b - cy * h, hx = ty * dir.z - tz * dir.y, hy = tz * dir.x, hz = -ty * dir.x;
  376.                         f32 tx = (pos.x * hx + pos.y * hy + pos.z * hz - hy * ty - hz * tz) / hx;
  377.                         if (sqr(Vec3(tx - pos.x, ty - pos.y, tz - pos.z)) < sl * sl)
  378.                                 return Vec3(tx, ty, tz);
  379.                         f32 ycut = (ty * fcos + tz * fsin) * flip, RR = sl * sl, rar = RR - aa - rr, C = A * rar;
  380.                         f32 minX = C > ycut ? sqrt_tpl(rar - 2 * a * ycut) : 0;
  381.                         f32 maxX = sqrt_tpl(2 * a * cr + rar) - eps - eps;
  382.                         for (uint32 limit = 0; rr - sqr(C - A * maxX * maxX) < eps && limit < 256; maxX -= eps, limit++)
  383.                                 ;
  384.                         f32 dx = fabs(dir.x), dy = diry, dz = fabs(dirz);
  385.                         f32 x = (minX + maxX) * 0.5f, y, z, iz, yiz, d1, d2, rmz, e0 = 2 * A * dy, e1 = 2 * A * dz, e2 = 4 * A * A, e3 = e2 * dz, delta = 9;
  386.                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz, d1 = (e0 - e1 * yiz) * x - dx, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0;
  387.                         x = clamp_tpl(x + d1 / d2, minX, minX + (maxX - minX) * 0.980f);
  388.                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz, d1 = (e0 - e1 * yiz) * x - dx, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0;
  389.                         x = clamp_tpl(x + d1 / d2, minX, minX + (maxX - minX) * 0.995f);
  390.                         if (x < minX + (maxX - minX) * 0.96)
  391.                         {
  392.                                 for (i = 9; fabs(delta) > eps && i; x = clamp_tpl(x - delta, minX, maxX), --i)
  393.                                 {
  394.                                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz;
  395.                                         d1 = dx - (e0 - e1 * yiz) * x, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0, delta = d1 / d2;
  396.                                 }
  397.                                 y = (C - A * x * x), z = sqrt_tpl(rr - y * y) * rz;
  398.                         }
  399.                         else
  400.                         {
  401.                                 f32 maxZ = sqrt_tpl(rr - sqr(C - A * minX * minX)), e4, zz, o;
  402.                                 for (i = 9, z = sqrt_tpl(rr - sqr(C - A * x * x)); fabs(delta) > eps && i; z = clamp_tpl(z - delta, 0.0f, maxZ), --i)
  403.                                 {
  404.                                         zz = z * z, rmz = rr - zz, iz = isqrt_tpl(rmz), o = isqrt_tpl((1.0f / iz + C) / A), e4 = 0.5f / A * o * iz;
  405.                                         d1 = z * e4 * dx - dz - z * iz * dy, d2 = (zz / rmz + 1) * (e4 * dx - iz * dy) + (dx * zz * o * o * o) / (e2 * rmz), delta = d1 / d2;
  406.                                 }
  407.                                 y = sqrt_tpl(rr - z * z), x = sqrt_tpl((y + C) / A), y = -y, z *= rz;
  408.                         }
  409.                         return Vec3(x * (dir.x < 0 ? -1.0f : 1.0f) + pos.x, (fcos * y - fsin * z) * flip, (fsin * y + fcos * z) * flip);
  410.                 }
  411.                 Vec3 CheckEdge(const Vec3& rpos, const Vec3& rdir, f32 sl, f32 lr, f32 ca, f32 cr)
  412.                 {
  413.                         rr = cr * cr;
  414.                         for (f32 t = -1; t < 1.5f; t += 2)
  415.                         {
  416.                                 Vec3 ip(rpos.x - ca * t, rpos.y, rpos.z), pnt = ip - rdir * (ip | rdir);
  417.                                 if (sqr(sl + cr) < (ip | ip)) continue;
  418.                                 f32 dot = pnt | pnt;
  419.                                 if (dot > rr) continue;
  420.                                 Vec3 iha = dot ? (pnt % rdir).GetNormalized() : rdir.GetOrthogonal().GetNormalized() * (rpos.x > 0 ? -1.0f : 1.0f);
  421.                                 f32 pn = iha.x < 0 ? -1.0f : 1.0f, vx = pn * iha.x, vy = pn * iha.y, vz = pn * iha.z, w = 1.0f / (1.0f + vx);
  422.                                 Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  423.                                 Vec3 np = ip * m;
  424.                                 f32 sine = 1.0f - sqr(np.x / cr);
  425.                                 if (sine < 0) continue;
  426.                                 f32 d2, id, a, b, h, x = ip | ip, c0 = x - rr - rr, s2 = sine * rr, d = 0;
  427.                                 if (sl * sl - sqr(x - rr) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  428.                                         d2 = np.y * np.y + np.z * np.z, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, d = s2 - a * a, h = sqrt_tpl(fabs(d)) * id * pn;
  429.                                 else
  430.                                         np.y *= 0.5f, np.z *= 0.5f, id = isqrt_tpl(np.y * np.y + np.z * np.z), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  431.                                 if (d < 0) continue;
  432.                                 Vec3 tmp = m * Vec3(np.x, np.y * b - np.z * h, np.z * b + np.y * h);
  433.                                 if (tmp.x * t > 0 && tmp.y <= 0 && tmp.z >= 0)
  434.                                         return Vec3(tmp.x + ca * t, tmp.y, tmp.z);
  435.                         }
  436.                         if (rpos.y * rpos.y + rpos.z * rpos.z < rr)
  437.                                 return rpos - rdir * sl;
  438.                         aa = rpos.y * rpos.y + rpos.z * rpos.z, a = sqrt_tpl(aa);
  439.                         l = 1.0f / a, f = rpos.y < 0 ? -l : l, fcos = f * rpos.y, fsin = f * rpos.z;
  440.                         idy = fcos * rdir.y + fsin * rdir.z, idz = fcos * rdir.z - fsin * rdir.y, rx = rdir.x < 0 ? -1.0f : 1.0f;
  441.                         cy = rpos.y * 0.5f, cz = rpos.z * 0.5f, id = isqrt_tpl(cy * cy + cz * cz);
  442.                         cid = rr * id * 0.5f, b = cid * id, h = sqrt_tpl(fabs(rr - cid * cid)) * id;
  443.                         int gvisible = 1, rvisible = 1;
  444.                         if (fsin >= 0) idz < 0 ? (gvisible = 0, rvisible = 1) : (gvisible = 1, rvisible = 0);
  445.                         Vec3 vContactPoint = rpos - rdir * sl;
  446.                         if (gvisible && CheckHalfCapsule(rpos, rdir, sl, lr, ca, cr, vContactPoint))
  447.                                 return vContactPoint;
  448.                         if (rvisible)
  449.                                 return CheckHalfCapsule(rpos, rdir, sl, lr, ca, cr);
  450.                         return vContactPoint;
  451.                 }
  452.                 int CheckHalfCapsule(const Vec3& rpos, const Vec3& rdir, const f32 sl, const f32 lr, const f32 ca, const f32 cr, Vec3& vContactPoint)
  453.                 {
  454.                         const f32 rz = idz < 0 ? -1.0f : 1.0f;
  455.                         const f32 gh = h * rz;
  456.                         // cy, cz is (rpos.y, rpos.z) * 0.5
  457.                         const f32 ty = cy * b + cz * gh;
  458.                         const f32 tz = cz * b - cy * gh;
  459.                         // cross product ( hx, hy, hz ) = ( 0, ty, tz ) x ( rdir.x, rdir.y, rdir.z )
  460.                         const f32 hx = ty * rdir.z - tz * rdir.y;
  461.                         const f32 hy = tz * rdir.x;
  462.                         const f32 hz = -ty * rdir.x;
  463.                         // solve (hx, hy, hz) * ((rpos.x, rpos.y, rpos.z) - (tx, ty, tz)) = 0 for tx
  464.                         if (fabs(hx) < HXEPSILON)
  465.                                 return 0;
  466.                         const f32 tx = (rpos.x * hx + rpos.y * hy + rpos.z * hz - hy * ty - hz * tz) / hx;
  467.                         if (sqr(Vec3(tx - rpos.x, ty - rpos.y, tz - rpos.z)) < sl * sl)
  468.                         {
  469.                                 if (fabs(tx) > ca || ty > 0 || tz < 0) return 0;
  470.                                 vContactPoint = Vec3(tx, ty, tz);
  471.                                 return 1;
  472.                         }
  473.                         f32 ycut = ty * fcos + tz * fsin, RR = sl * sl, rar = RR - aa - rr, A = 0.5f / a, C = A * rar;
  474.                         f32 minX = C > ycut ? sqrt_tpl(rar - 2 * a * ycut) : 0, rminX = rx * minX + rpos.x;
  475.                         f32 maxX = sqrt_tpl(2 * a * cr + rar) - eps - eps, rmaxX = rx * maxX + rpos.x;
  476.                         if ((rminX > ca && rmaxX > ca) || (rminX < -ca && rmaxX < -ca))
  477.                                 return 0;
  478.                         for (uint32 limit = 0; rr - sqr(C - A * maxX * maxX) < eps && limit < 256; maxX -= eps, limit++)
  479.                                 ;
  480.                         f32 dx = fabs(rdir.x), dy = fabs(idy), dz = idz * rz;
  481.                         f32 x = (minX + maxX) * 0.5f, y, z, zz, d1, d2, iz, yiz, o, e0 = 2 * A * dy, e1 = 2 * A * dz, e2, e3 = 4 * A * A * dz, dist = 9;
  482.                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz, d1 = (e0 - e1 * yiz) * x - dx, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0;
  483.                         x = clamp_tpl(x + d1 / d2, minX, minX + (maxX - minX) * 0.980f);
  484.                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz, d1 = (e0 - e1 * yiz) * x - dx, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0;
  485.                         x = clamp_tpl(x + d1 / d2, minX, minX + (maxX - minX) * 0.995f);
  486.                         if (x < minX + (maxX - minX) * 0.96)
  487.                         {
  488.                                 for (i = 9; fabs(dist) > eps && i; x = clamp_tpl(x - dist, minX, maxX), --i)
  489.                                 {
  490.                                         y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz;
  491.                                         d1 = dx - (e0 - e1 * yiz) * x, d2 = (e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0, dist = d1 / d2;
  492.                                 }
  493.                                 y = C - A * x * x, z = sqrt_tpl(rr - y * y) * rz;
  494.                         }
  495.                         else
  496.                         {
  497.                                 f32 maxZ = sqrt_tpl(rr - sqr(C - A * minX * minX));
  498.                                 for (i = 9, y = C - A * x * x, z = sqrt_tpl(rr - y * y); fabs(dist) > eps && i; z = clamp_tpl(z - dist, 0.0f, maxZ), --i)
  499.                                 {
  500.                                         zz = z * z, iz = isqrt_tpl(rr - zz), o = isqrt_tpl((1.0f / iz + C) / A), e2 = 0.5f / A * o * iz;
  501.                                         d1 = z * e2 * dx - dz - z * iz * dy, d2 = (1 + zz / (rr - zz)) * (e2 * dx - iz * dy) + (dx * zz * o * o * o) / (4 * A * A * (rr - zz)), dist = d1 / d2;
  502.                                 }
  503.                                 y = sqrt_tpl(rr - z * z), x = sqrt_tpl((y + C) / A), y = -y, z *= rz;
  504.                         }
  505.                         Vec3 gpoint(x * rx + rpos.x, fcos * y - fsin * z, fsin * y + fcos * z);
  506.                         if (fabs(gpoint.x) > ca || gpoint.y > 0 || gpoint.z < 0)
  507.                                 return 0;
  508.                         vContactPoint = gpoint;
  509.                         return 1;
  510.                 }
  511.                 Vec3 CheckHalfCapsule(const Vec3& rpos, const Vec3& rdir, const f32 sl, const f32 lr, const f32 ca, const f32 cr)
  512.                 {
  513.                         const f32 rz = idz < 0 ? 1.0f : -1.0f;
  514.                         const f32 rh = h * rz;
  515.                         const f32 ty = cy * b + cz * rh;
  516.                         const f32 tz = cz * b - cy * rh;
  517.                         const f32 hx = ty * rdir.z - tz * rdir.y;
  518.                         const f32 hy = tz * rdir.x;
  519.                         const f32 hz = -ty * rdir.x;
  520.  
  521.                         if (fabs(hx) < HXEPSILON)
  522.                                 return rpos - rdir * sl;
  523.  
  524.                         const f32 tx = (rpos.x * hx + rpos.y * hy + rpos.z * hz - hy * ty - hz * tz) / hx;
  525.                         if (sqr(Vec3(tx, ty, tz) - rpos) < sl * sl)
  526.                         {
  527.                                 if (fabs(tx) > ca || ty > 0 || tz < 0) return rpos - rdir * sl;
  528.                                 return Vec3(tx, ty, tz);
  529.                         }
  530.                         f32 ycut = ty * fcos + tz * fsin, RR = sl * sl, rar = RR - aa - rr, A = 0.5f / a, C = A * rar;
  531.                         f32 minX = C > ycut ? sqrt_tpl(rar - 2 * a * ycut) : 0, rminX = rx * minX + rpos.x;
  532.                         f32 maxX = sqrt_tpl(2 * a * cr + rar), rmaxX = rx * maxX + rpos.x;
  533.                         if ((rminX > ca && rmaxX > ca) || (rminX < -ca && rmaxX < -ca))
  534.                                 return rpos - rdir * sl;
  535.                         f32 dx = fabs(rdir.x), dy = fabs(idy), dz = idz * rz;
  536.                         f32 x = minX * 0.2f + maxX * 0.8f, y, z, clm[5] = { 0.97, 0.99, 1.0, 1.0, 1.0 }, dot = 9;
  537.                         for (i = 0; dot > 0 && i < 5; ++i)
  538.                         {
  539.                                 f32 y = C - A * x * x, iz = isqrt_tpl(rr - y * y), iz3 = iz * iz * iz * 0.25f;
  540.                                 f32 a0 = 4 * A * C, a1 = A * A * x, a2 = a0 * x - 4 * a1 * x * x, a3 = a2 * a2 * iz3, a4 = a2 * 3;
  541.                                 f32 d1 = ((a0 - 12 * a1 * x) * iz * 0.5f - a3) * dz - 2 * A * dy;
  542.                                 f32 d2 = (a4 * a3 * iz * iz * 0.5f - 12.0f * a1 * iz - a4 * a2 * iz3) * dz;
  543.                                 if (d2 == 0) break;
  544.                                 if ((x = clamp_tpl(x - d1 / d2, minX, minX + (maxX - minX) * clm[i])) >= maxX) break;
  545.                                 a0 = 2 * A * x, y = C - A * x * x, dot = dx - a0 * dy + a0 * y * dz * isqrt_tpl(rr - y * y);
  546.                         }
  547.                         maxX = x;
  548.                         if (dot > 0)
  549.                                 return rpos - rdir * sl;
  550.                         const f32 clx[10] = { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 0.995, 0.980 };
  551.                         f32 d = 9, j, iz, yiz, e0 = 2 * A * dy, e1 = 2 * A * dz * rz, e3 = 4 * A * A * dz * rz;
  552.                         for (i = 9, x = (minX + maxX) * 0.5f; fabs(d) > eps && i; x = clamp_tpl(x + d, minX, minX + (maxX - minX) * clx[i]), --i)
  553.                                 y = C - A * x * x, iz = isqrt_tpl(rr - y * y), yiz = y * iz, j = (e0 - e1 * yiz) * x - dx, d = j / ((e1 * y - (e3 + e3 * yiz * yiz) * x * x) * iz - e0);
  554.                         y = C - A * x * x, z = sqrt_tpl(rr - y * y) * rz;
  555.                         Vec3 rpoint(x * rx + rpos.x, fcos * y - fsin * z, fsin * y + fcos * z);
  556.                         if (fabs(rpoint.x) > ca || rpoint.y > 0 || rpoint.z < 0)
  557.                                 return rpos - rdir * sl;
  558.                         return rpoint;
  559.                 }
  560.                 uint32 i;
  561.                 f32    aa, a, l, f, fcos, fsin, eps, idy, idz, rx, rr, cy, cz, id, cid, b, h;
  562.         } e;
  563.         e.eps = 0.00001f;
  564.         const Matrix33 m[2] = { Matrix33(0, 1, 0, -1, 0, 0, 0, 0, 1), Matrix33(0, 0, 1, 0, 1, 0, -1, 0, 0) };
  565.         if (loz.y + loz.z == 0)
  566.                 return e.CheckCapsule(ipos, idir, sl, loz.x, loz.w) - ipos;
  567.         if (loz.x + loz.z == 0)
  568.                 return e.CheckCapsule(Vec3(ipos.y, -ipos.x, ipos.z), Vec3(idir.y, -idir.x, idir.z), sl, loz.y, loz.w) * m[0] - ipos;
  569.         if (loz.x + loz.y == 0)
  570.                 return e.CheckCapsule(Vec3(ipos.z, ipos.y, -ipos.x), Vec3(idir.z, idir.y, -idir.x), sl, loz.z, loz.w) * m[1] - ipos;
  571.         Diag33 r(idir.x < 0 ? -1.0f : 1.0f, idir.y < 0 ? -1.0f : 1.0f, idir.z < 0 ? -1.0f : 1.0f);
  572.         Vec3 p = ipos * r, d = idir * r;
  573.         f32 arrdot[12] = { -2, -2, -2, -2, -2, -2, -2, -2, -2, -2, -2, -2 };
  574.         Vec3 arrpnt[12] = { ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO, ZERO }, ip;
  575.         int s04 = 0, s26 = 0, s02 = 0, s46 = 0, s45 = 0, s67 = 0, s57 = 0, t46 = 0, t67 = 0, s37 = 0, s23 = 0, t26 = 0, xymid = 1, xzmid = 1, yzmid = 1;
  576.         f32 mx = loz.x + loz.w, my = loz.y + loz.w, mz = loz.z + loz.w;
  577.         if (-loz.x - loz.w > p.x)
  578.         {
  579.                 Vec3 linedir = Vec3(0, d.y, d.z).GetNormalizedSafe(Vec3(0, 1, 0));
  580.                 Vec3 c = d * ((p.x + loz.x + loz.w) / d.x), pol = p - c;
  581.                 f32 bb = linedir | c, off = loz.x + loz.w - lr;
  582.                 arrpnt[0] = linedir * (bb + sqrt_tpl(sl * sl + bb * bb - (c | c))) + pol, arrdot[0] = (arrpnt[0] - p).GetNormalized() | d;
  583.                 f32 rop = sqrt_tpl(sqr(p.y - arrpnt[0].y) + sqr(p.z - arrpnt[0].z));
  584.                 s04 = loz.z - p.z < rop, s26 = p.z + loz.z < rop, xzmid = !s26, s02 = loz.y - p.y < rop, s46 = p.y + loz.y < rop, xymid = !s46;
  585.         }
  586.         else if (p.x <= -loz.x && p.x >= -loz.x - loz.w)
  587.                 s04 = p.z >= loz.z && p.z <= mz, s26 = p.z <= -loz.z && p.z >= -mz, xzmid = !s26, s02 = p.y >= loz.y && p.y <= my, s46 = p.y <= -loz.y && p.y >= -my, xymid = !s46;
  588.         if (-loz.y - loz.w > p.y)
  589.         {
  590.                 Vec3 linedir = Vec3(d.x, 0, d.z).GetNormalizedSafe(Vec3(1, 0, 0));
  591.                 Vec3 c = d * ((p.y + loz.y + loz.w) / d.y), pol = p - c;
  592.                 f32 bb = linedir | c, off = loz.y + loz.w - lr;
  593.                 arrpnt[1] = linedir * (bb + sqrt_tpl(sl * sl + bb * bb - (c | c))) + pol, arrdot[1] = (arrpnt[1] - p).GetNormalized() | d;
  594.                 f32 rop = sqrt_tpl(sqr(p.x - arrpnt[1].x) + sqr(p.z - arrpnt[1].z));
  595.                 s45 = loz.z - p.z < rop, s67 = p.z + loz.z < rop, yzmid = !s67, s57 = loz.x - p.x < rop, t46 = xymid && p.x + loz.x < rop;
  596.         }
  597.         else if (p.y <= -loz.y && p.y >= -loz.y - loz.w)
  598.                 s45 = p.z >= loz.z && p.z <= mz, s67 = p.z <= -loz.z && p.z >= -mz, yzmid = !s67, s57 = p.x >= loz.x && p.x <= mx, t46 = xymid && p.x <= -loz.x && p.x >= -mx;
  599.         if (-loz.z - loz.w > p.z)
  600.         {
  601.                 Vec3 linedir = Vec3(d.x, d.y, 0).GetNormalizedSafe(Vec3(0, 1, 0));
  602.                 Vec3 c = d * ((p.z + loz.z + loz.w) / d.z), pol = p - c;
  603.                 f32 bb = linedir | c, off = loz.z + loz.w - lr;
  604.                 arrpnt[2] = linedir * (bb + sqrt_tpl(sl * sl + bb * bb - (c | c))) + pol, arrdot[2] = (arrpnt[2] - p).GetNormalized() | d;
  605.                 f32 rop = sqrt_tpl(sqr(p.x - arrpnt[2].x) + sqr(p.y - arrpnt[2].y));
  606.                 s23 = loz.y - p.y < rop, t67 = yzmid && p.y + loz.y < rop, t26 = xzmid && p.x + loz.x < rop, s37 = loz.x - p.x < rop;
  607.         }
  608.         else if (p.z <= -loz.z && p.z >= -loz.z - loz.w)
  609.                 s23 = p.y >= loz.y && p.y <= my, t67 = yzmid && p.y <= -loz.y && p.y >= -my, t26 = xzmid && p.x <= -loz.x && p.x >= -mx, s37 = p.x >= loz.x && p.x <= mx;
  610.         if (s04)
  611.                 ip = e.CheckEdge(Vec3(-p.y, p.x + loz.x, p.z - loz.z), Vec3(-d.y, d.x, d.z), sl, lr, loz.y, loz.w),
  612.                 arrpnt[5](ip.y - loz.x, -ip.x, ip.z + loz.z), arrdot[5] = (arrpnt[5] - p).GetNormalized() | d;
  613.         if (s26)
  614.                 ip = e.CheckEdge(Vec3(p.y, p.x + loz.x, -p.z - loz.z), Vec3(d.y, d.x, -d.z), sl, lr, loz.y, loz.w),
  615.                 arrpnt[4](ip.y - loz.x, ip.x, -ip.z - loz.z), arrdot[4] = (arrpnt[4] - p).GetNormalized() | d;
  616.         if (s02)
  617.                 ip = e.CheckEdge(Vec3(p.z, p.x + loz.x, p.y - loz.y), Vec3(d.z, d.x, d.y), sl, lr, loz.z, loz.w),
  618.                 arrpnt[6](ip.y - loz.x, ip.z + loz.y, ip.x), arrdot[6] = (arrpnt[6] - p).GetNormalized() | d;
  619.         if (s46)
  620.                 ip = e.CheckEdge(Vec3(-p.z, p.x + loz.x, -p.y - loz.y), Vec3(-d.z, d.x, -d.y), sl, lr, loz.z, loz.w),
  621.                 arrpnt[3](ip.y - loz.x, -ip.z - loz.y, -ip.x), arrdot[3] = (arrpnt[3] - p).GetNormalized() | d;
  622.         if (s45)
  623.                 ip = e.CheckEdge(Vec3(p.x, p.y + loz.y, p.z - loz.z), d, sl, lr, loz.x, loz.w),
  624.                 arrpnt[8](ip.x, ip.y - loz.y, ip.z + loz.z), arrdot[8] = (arrpnt[8] - p).GetNormalized() | d;
  625.         if (s67)
  626.                 ip = e.CheckEdge(Vec3(-p.x, p.y + loz.y, -p.z - loz.z), Vec3(-d.x, d.y, -d.z), sl, lr, loz.x, loz.w),
  627.                 arrpnt[7](-ip.x, ip.y - loz.y, -ip.z - loz.z), arrdot[7] = (arrpnt[7] - p).GetNormalized() | d;
  628.         if (s57)
  629.                 ip = e.CheckEdge(Vec3(-p.z, p.y + loz.y, p.x - loz.x), Vec3(-d.z, d.y, d.x), sl, lr, loz.z, loz.w),
  630.                 arrpnt[9](ip.z + loz.x, ip.y - loz.y, -ip.x), arrdot[9] = (arrpnt[9] - p).GetNormalized() | d;
  631.         if (t46)
  632.                 ip = e.CheckEdge(Vec3(p.z, p.y + loz.y, -p.x - loz.x), Vec3(d.z, d.y, -d.x), sl, lr, loz.z, loz.w),
  633.                 arrpnt[3](-ip.z - loz.x, ip.y - loz.y, ip.x), arrdot[3] = (arrpnt[3] - p).GetNormalized() | d;
  634.         if (s23)
  635.                 ip = e.CheckEdge(Vec3(-p.x, p.z + loz.z, p.y - loz.y), Vec3(-d.x, d.z, d.y), sl, lr, loz.x, loz.w),
  636.                 arrpnt[10](-ip.x, ip.z + loz.y, ip.y - loz.z), arrdot[10] = (arrpnt[10] - p).GetNormalized() | d;
  637.         if (t67)
  638.                 ip = e.CheckEdge(Vec3(p.x, p.z + loz.z, -p.y - loz.y), Vec3(d.x, d.z, -d.y), sl, lr, loz.x, loz.w),
  639.                 arrpnt[7](ip.x, -ip.z - loz.y, ip.y - loz.z), arrdot[7] = (arrpnt[7] - p).GetNormalized() | d;
  640.         if (t26)
  641.                 ip = e.CheckEdge(Vec3(-p.y, p.z + loz.z, -p.x - loz.x), Vec3(-d.y, d.z, -d.x), sl, lr, loz.y, loz.w),
  642.                 arrpnt[4](-ip.z - loz.x, -ip.x, ip.y - loz.z), arrdot[4] = (arrpnt[4] - p).GetNormalized() | d;
  643.         if (s37)
  644.                 ip = e.CheckEdge(Vec3(p.y, p.z + loz.z, p.x - loz.x), Vec3(d.y, d.z, d.x), sl, lr, loz.y, loz.w),
  645.                 arrpnt[11](ip.z + loz.x, ip.x, ip.y - loz.z), arrdot[11] = (arrpnt[11] - p).GetNormalized() | d;
  646.         int idx = 0;
  647.         for (int i = 1; i < 12; i++)
  648.                 if (arrdot[i] > arrdot[idx]) idx = i;
  649.         return arrpnt[idx] * r - ipos;
  650. }
  651.  
  652. //Projects a penetrating object out of a lozenge and back to its surface by using the predefined rotation direction.
  653. Vec3 CProxy::DirectedRotationalProjection(const Vec3& ipos, const Vec3& idir, f32 sl, f32 sr, const Vec3& iha) const
  654. {
  655.         Vec4 loz = m_params;
  656.         loz.w += sr;
  657.         if (loz.x + loz.y + loz.z == 0)
  658.         {
  659.                 f32 pn = iha.x < 0 ? -1.0f : 1.0f, vx = pn * iha.x, vy = pn * iha.y, vz = pn * iha.z, w = 1.0f / (1.0f + vx);
  660.                 Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  661.                 Vec3 ip(ipos.x, ipos.y, ipos.z), np = ip * m;
  662.                 f32 x = ip | ip, cr2 = loz.w * loz.w, c0 = x - cr2 - cr2, cy = np.y, cz = np.z, d2, id, a, h, b, s2 = (1.0f - sqr(np.x / loz.w)) * cr2;
  663.                 if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  664.                         d2 = cy * cy + cz * cz, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, h = sqrt_tpl(max(0.0f, s2 - a * a)) * id * pn;
  665.                 else
  666.                         cy *= 0.5f, cz *= 0.5f, id = isqrt_tpl(cy * cy + cz * cz), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  667.                 return m * Vec3(np.x, cy * b - cz * h, cz * b + cy * h) - ipos;
  668.         }
  669.         struct
  670.         {
  671.                 Quat CheckEdge(const Vec3& ipos, const Vec3& idir, f32 sl, const Vec3& ha, f32 ca, f32 cr)
  672.                 {
  673.                         if (ipos.y * ipos.y + ipos.z * ipos.z - sqr(cr + sl) > -0.00001f)
  674.                                 return Quat(ZERO);
  675.                         f32 pd = ipos | ha, dn = -ca * ha.x - pd, dp = ca * ha.x - pd, cr2 = cr * cr;
  676.                         if ((dn < 0 && dp < 0) | (dp > 0 && dn > 0) && min(dn * dn, dp * dp) - cr2 > 0)
  677.                                 return Quat(ZERO);
  678.                         f32 pn = ha.x < 0 ? -1.0f : 1.0f, vx = pn * ha.x, vy = pn * ha.y, vz = pn * ha.z, w = 1.0f / (1.0f + vx);
  679.                         Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  680.                         if (ipos.y * ipos.y + ipos.z * ipos.z < cr2)
  681.                         {
  682.                                 f32 t = ipos.x < 0 ? -1.0f : 1.0f;
  683.                                 Vec3 ip(ipos.x - ca * t, ipos.y, ipos.z), np = ip * m, vca(ca * t, 0, 0);
  684.                                 f32 x = ip | ip, c0 = x - cr2 - cr2, cy = np.y, cz = np.z, d2, id, a, h, b, s2 = (1.0f - sqr(np.x / cr)) * cr2;
  685.                                 if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  686.                                         d2 = cy * cy + cz * cz, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, h = sqrt_tpl(max(0.0f, s2 - a * a)) * id * pn;
  687.                                 else
  688.                                         cy *= 0.5f, cz *= 0.5f, id = isqrt_tpl(cy * cy + cz * cz), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(fabs(s2 - a * a)) * id * pn;
  689.                                 return Quat(1, m * Vec3(np.x, cy * b - cz * h, cz * b + cy * h) + vca);
  690.                         }
  691.                         if (fabs(ha.x) < f32(HPLANEEPSILON))
  692.                                 return Quat(ZERO);
  693.                         for (f32 t = -1; t < 1.5f; t += 2)
  694.                         {
  695.                                 Vec3 ip(ipos.x - ca * t, ipos.y, ipos.z);
  696.                                 if (sqr(sl + cr) < (ip | ip)) continue;
  697.                                 Vec3 np = ip * m;
  698.                                 f32 sine = 1.0f - sqr(np.x / cr);
  699.                                 if (sine < 0) continue;
  700.                                 f32 d2, id, a, b, h, x = ip | ip, c0 = x - cr2 - cr2, s2 = sine * cr2, d = 0;
  701.                                 if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  702.                                         d2 = np.y * np.y + np.z * np.z, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, d = s2 - a * a, h = sqrt_tpl(fabs(d)) * id * pn;
  703.                                 else
  704.                                         np.y *= 0.5f, np.z *= 0.5f, id = isqrt_tpl(np.y * np.y + np.z * np.z), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  705.                                 if (d < 0) continue;
  706.                                 Vec3 tmp = m * Vec3(np.x, np.y * b - np.z * h, np.z * b + np.y * h);
  707.                                 if (tmp.x * t > 0)
  708.                                         return Quat(1, tmp.x + ca * t, tmp.y, tmp.z);
  709.                         }
  710.                         f32 cy = ipos.y * 0.5f, cz = ipos.z * 0.5f, id = isqrt_tpl(cy * cy + cz * cz);
  711.                         f32 a = cr2 * id * 0.5f, b = a * id, h = sqrt_tpl(fabs(cr2 - a * a)) * id * pn, d = ipos | ha, y0 = cy * b - cz * h, z0 = cz * b + cy * h;
  712.                         Vec3 tangpnt(-(ha.y * y0 + ha.z * z0 - d) / ha.x, y0, z0), gdist = tangpnt - ipos, pdir = (idir % ha).GetNormalized();
  713.                         if ((gdist | gdist) < sl * sl)
  714.                                 return Quat(1, tangpnt);
  715.                         if (fabs(ha.x) > 0.999f && fabs(ipos.x) > ca)
  716.                                 return Quat(ZERO);
  717.                         Vec2 n1(idir.y, idir.z), n2(pdir.y, pdir.z), n0(ipos.y, ipos.z);
  718.                         f32 n11 = n1 | n1, n12 = n1 | n2, n22 = n2 | n2, n10 = n1 | n0, n20 = n2 | n0, n00 = n0 | n0, ex, ey, u, v, det;
  719.                         f32 x = gdist | idir, y = gdist | pdir, isq = isqrt_tpl(x * x + y * y) * sl;
  720.                         x *= isq, y *= isq;
  721.                         for (int i = 0; i < 10; ey = sl * sl - x * x - y * y, x += (y * ex - v * ey) * det, y += (u * ey - x * ex) * det, ++i)
  722.                                 u = n11 * x + n12 * y + n10, v = n22 * y + n12 * x + n20, det = 0.5f / (u * y - v * x), ex = cr2 - n11 * x * x - n22 * y * y - 2.0f * (n12 * x * y + n10 * x + n20 * y) - n00;
  723.                         Vec3 p = (idir * x + pdir * y).GetNormalized() * sl + ipos;
  724.                         if (sqr(max(0.0f, fabs(p.x) - ca)) - cr2 + p.y * p.y + p.z * p.z < 0.00001f)
  725.                                 return Quat(1, p);
  726.                         return Quat(ZERO);
  727.                 }
  728.                 f32 TestOverlapping(Vec3 p0, Vec3 dir, f32 sl, f32 ca, f32 cr)
  729.                 {
  730.                         Vec3 p1 = p0 + dir * sl;
  731.                         if (ca == 0)
  732.                         {
  733.                                 Vec3 ab = p1 - p0;
  734.                                 f32 e = -p0 | ab, f = ab | ab;
  735.                                 if (e <= 0) return (p0 | p0) - cr * cr;
  736.                                 if (e >= f) return (p1 | p1) - cr * cr;
  737.                                 return (p0 | p0) - e * e / f - cr * cr;
  738.                         }
  739.                         Vec3 x;
  740.                         if (p0.x > p1.x) x = p0, p0 = p1, p1 = x;
  741.                         Vec3 rdir = p1 - p0, p0ca = p0;
  742.                         p0ca.x += ca;
  743.                         f32 a = rdir | p0ca, b = ca + ca, c = b * b, d = rdir | rdir, e = p0ca.x * b, f = rdir.x * b, g = c * d - f * f;
  744.                         if (g)
  745.                         {
  746.                                 f32 s = fclamp01_tpl((e * d - f * a) / g), t = (f * s - a) / d;
  747.                                 if (t < 0.0f) t = 0, s = fclamp01_tpl(e / c);
  748.                                 if (t > 1.0f) t = 1, s = fclamp01_tpl((f + e) / c);
  749.                                 return sqr(Vec3(rdir.x * t + p0.x - b * s + ca, rdir.y * t + p0.y, rdir.z * t + p0.z)) - cr * cr;
  750.                         }
  751.                         f32 t = e / c;
  752.                         x = p0;
  753.                         if (p0.x > ca) t = fclamp01_tpl(t);
  754.                         if (p1.x < -ca) t = fclamp01_tpl((f + e) / c), x = p1;
  755.                         x.x -= b * t - ca;
  756.                         return (x | x) - cr * cr;
  757.                 }
  758.                 Vec3 CheckCapsule(const Vec3& ipos, const Vec3& idir, f32 sl, const Vec3& ha, f32 ca, f32 cr)
  759.                 {
  760.                         f32 pn = ha.x < 0 ? -1.0f : 1.0f, vx = pn * ha.x, vy = pn * ha.y, vz = pn * ha.z, w = 1.0f / (1.0f + vx), cr2 = cr * cr;
  761.                         Matrix33 m(vx, -vy, -vz, vy, w * vz * vz + vx, -w * vz * vy, vz, -w * vz * vy, w * vy * vy + vx);
  762.                         if (ipos.y * ipos.y + ipos.z * ipos.z < cr2)
  763.                         {
  764.                                 f32 t = ipos.x < 0 ? -1.0f : 1.0f;
  765.                                 Vec3 ip(ipos.x - ca * t, ipos.y, ipos.z), np = ip * m, vca(ca * t, 0, 0);
  766.                                 f32 x = ip | ip, c0 = x - cr2 - cr2, cy = np.y, cz = np.z, d2, id, a, h, b, s2 = (1.0f - sqr(np.x / cr)) * cr2;
  767.                                 if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  768.                                         d2 = cy * cy + cz * cz, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, h = sqrt_tpl(max(0.0f, s2 - a * a)) * id * pn;
  769.                                 else
  770.                                         cy *= 0.5f, cz *= 0.5f, id = isqrt_tpl(cy * cy + cz * cz), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(fabs(s2 - a * a)) * id * pn;
  771.                                 return m * Vec3(np.x, cy * b - cz * h, cz * b + cy * h) + vca;
  772.                         }
  773.                         if (fabs(ha.x) > f32(HPLANEEPSILON))
  774.                         {
  775.                                 for (f32 t = -1; t < 1.5f; t += 2)
  776.                                 {
  777.                                         Vec3 ip(ipos.x - ca * t, ipos.y, ipos.z);
  778.                                         if (sqr(sl + cr) < (ip | ip)) continue;
  779.                                         Vec3 np = ip * m;
  780.                                         f32 sine = 1.0f - sqr(np.x / cr);
  781.                                         if (sine < 0) continue;
  782.                                         f32 d2, id, a, b, h, x = ip | ip, c0 = x - cr2 - cr2, s2 = sine * cr2, d = 0;
  783.                                         if (sl * sl - sqr(x - cr2) / x - (1.0f - c0 / x * c0 / x) * x * 0.25f < 0)
  784.                                                 d2 = np.y * np.y + np.z * np.z, id = isqrt_tpl(d2), a = (s2 - sl * sl + d2) * id * 0.5f, b = a * id, d = s2 - a * a, h = sqrt_tpl(fabs(d)) * id * pn;
  785.                                         else
  786.                                                 np.y *= 0.5f, np.z *= 0.5f, id = isqrt_tpl(np.y * np.y + np.z * np.z), a = s2 * id * 0.5f, b = a * id, h = sqrt_tpl(s2 - a * a) * id * pn;
  787.                                         if (d < 0) continue;
  788.                                         Vec3 tmp = m * Vec3(np.x, np.y * b - np.z * h, np.z * b + np.y * h);
  789.                                         if (tmp.x * t > 0)
  790.                                                 return Vec3(tmp.x + ca * t, tmp.y, tmp.z);
  791.                                 }
  792.                                 f32 cy = ipos.y * 0.5f, cz = ipos.z * 0.5f, id = isqrt_tpl(cy * cy + cz * cz);
  793.                                 f32 a = cr2 * id * 0.5f, b = a * id, h = sqrt_tpl(fabs(cr2 - a * a)) * id * pn, d = ipos | ha, y0 = cy * b - cz * h, z0 = cz * b + cy * h;
  794.                                 Vec3 tangpnt(-(ha.y * y0 + ha.z * z0 - d) / ha.x, y0, z0), gdist = tangpnt - ipos, pdir = idir % ha;
  795.                                 if ((gdist | gdist) < sl * sl)
  796.                                         return tangpnt;
  797.                                 Vec2 n1(idir.y, idir.z), n2(pdir.y, pdir.z), n0(ipos.y, ipos.z);
  798.                                 f32 n11 = n1 | n1, n12 = n1 | n2, n22 = n2 | n2, n10 = n1 | n0, n20 = n2 | n0, n00 = n0 | n0, ex, ey, u, v, det;
  799.                                 f32 x = gdist | idir, y = gdist | pdir, isq = isqrt_tpl(x * x + y * y) * sl;
  800.                                 x *= isq, y *= isq;
  801.                                 for (int i = 0; i < 10; ey = sl * sl - x * x - y * y, x += (y * ex - v * ey) * det, y += (u * ey - x * ex) * det, ++i)
  802.                                         u = n11 * x + n12 * y + n10, v = n22 * y + n12 * x + n20, det = 0.5f / (u * y - v * x), ex = cr2 - n11 * x * x - n22 * y * y - 2.0f * (n12 * x * y + n10 * x + n20 * y) - n00;
  803.                                 return (idir * x + pdir * y).GetNormalized() * sl + ipos;
  804.                         }
  805.                         Vec3 g = (idir % ha - idir * MAXRADEPSILON).GetNormalized(), n, b = idir;
  806.                         for (int i = 0; ++i < 20; n = g + b, n *= isqrt_tpl(n | n), TestOverlapping(ipos, n, sl, ca, cr) > 0 ? g = n : b = n)
  807.                                 ;
  808.                         return g * sl + ipos;
  809.                 }
  810.         } e;
  811.         const Matrix33 m[4] = { Matrix33(0, 1, 0, -1, 0, 0, 0, 0, 1), Matrix33(1, 0, 0, 0, 1, 0, 0, 0, 1), Matrix33(1, 0, 0, 0, 0, -1, 0, 1, 0), Matrix33(0, 0, 1, 0, 1, 0, -1, 0, 0) };
  812.         if (loz.y + loz.z == 0)
  813.                 return e.CheckCapsule(ipos, idir, sl, iha, loz.x, loz.w) - ipos;
  814.         if (loz.x + loz.z == 0)
  815.                 return e.CheckCapsule(Vec3(ipos.y, -ipos.x, ipos.z), Vec3(idir.y, -idir.x, idir.z), sl, Vec3(iha.y, -iha.x, iha.z), loz.y, loz.w) * m[0] - ipos;
  816.         if (loz.x + loz.y == 0)
  817.                 return e.CheckCapsule(Vec3(ipos.z, ipos.y, -ipos.x), Vec3(idir.z, idir.y, -idir.x), sl, Vec3(iha.z, iha.y, -iha.x), loz.z, loz.w) * m[3] - ipos;
  818.         Quat q;
  819.         Diag33 r(idir.x < 0 ? -1.0f : 1.0f, idir.y < 0 ? -1.0f : 1.0f, idir.z < 0 ? -1.0f : 1.0f);
  820.         Vec3 rp = ipos * r, rd = idir * r, rh = iha * r * (r.x * r.y * r.z), l;
  821.         for (uint32 axis = 0; axis < 3; axis++)
  822.         {
  823.                 if (axis == 0)  l(loz.y, loz.x, loz.z);
  824.                 if (axis == 1)  l(loz.x, loz.y, loz.z);
  825.                 if (axis == 2)  l(loz.x, loz.z, loz.y);
  826.                 f32 y = rp.x * m[axis].m01 + rp.y * m[axis].m11 + rp.z * m[axis].m21;
  827.                 if (-l.y - loz.w >= y)
  828.                 {
  829.                         Matrix33 rm33 = r * m[axis];
  830.                         Vec3 p = rp * m[axis], d = rd * m[axis], h = rh * m[axis], lpnt, ldir;
  831.                         lpnt = d * (-l.y - loz.w - p.y) / d.y + p, ldir(h.z, 0, -h.x), ldir *= isqrt_tpl(ldir | ldir);
  832.                         f32 b = ldir | (p - lpnt), maxx = l.x + loz.w, maxz = l.z + loz.w;
  833.                         Vec3 spnt = ldir * (b + sqrt_tpl(sl * sl + b * b - sqr(lpnt - p))) + lpnt;
  834.                         if (fabs(spnt.z) <= l.z && fabs(spnt.x) <= l.x)
  835.                                 return rm33 * spnt - ipos;
  836.                         if (l.z && spnt.x >= l.x)
  837.                         {
  838.                                 q = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x + l.x), Vec3(d.z, d.y, -d.x), sl, Vec3(h.z, h.y, -h.x), l.z, loz.w);
  839.                                 q.v(-q.v.z + l.x, q.v.y - l.y, q.v.x);
  840.                                 if (q.w && fabs(q.v.z) <= maxz && (!l.x || q.v.x >= l.x) && (!l.y || q.v.y <= -l.y))
  841.                                         return rm33 * q.v - ipos;
  842.                         }
  843.                         if (l.x && spnt.z >= l.z)
  844.                         {
  845.                                 q = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z - l.z), d, sl, h, l.x, loz.w), q.v.y -= l.y, q.v.z += l.z;
  846.                                 if (q.w && fabs(q.v.x) <= maxx && (!l.z || q.v.z >= l.z) && (!l.y || q.v.y <= -l.y))
  847.                                         return rm33 * q.v - ipos;
  848.                         }
  849.                         if (l.z && spnt.x <= -l.x)
  850.                         {
  851.                                 q = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x - l.x), Vec3(d.z, d.y, -d.x), sl, Vec3(h.z, h.y, -h.x), l.z, loz.w);
  852.                                 q.v(-q.v.z - l.x, q.v.y - l.y, q.v.x);
  853.                                 if (q.w && fabs(q.v.z) <= maxz && (!l.y || q.v.y <= -l.y) && (!l.x || q.v.x <= -l.x))
  854.                                         return rm33 * q.v - ipos;
  855.                         }
  856.                         if (l.x && spnt.z <= -l.z)
  857.                         {
  858.                                 q = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z + l.z), d, sl, h, l.x, loz.w), q.v.y -= l.y, q.v.z -= l.z;
  859.                                 if (q.w && fabs(q.v.x) <= maxx && (!l.y || q.v.y <= -l.y) && (!l.z || q.v.z <= -l.z))
  860.                                         return rm33 * q.v - ipos;
  861.                         }
  862.                 }
  863.                 if (y <= -l.y || y >= -l.y - loz.w)
  864.                 {
  865.                         Matrix33 rm33 = r * m[axis];
  866.                         Vec3 p = rp * m[axis], d = rd * m[axis], h = rh * m[axis];
  867.                         f32 maxx = l.x + loz.w, maxz = l.z + loz.w;
  868.                         if (p.x >= l.x && p.x <= maxx)
  869.                         {
  870.                                 q = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x + l.x), Vec3(d.z, d.y, -d.x), sl, Vec3(h.z, h.y, -h.x), l.z, loz.w);
  871.                                 q.v(-q.v.z + l.x, q.v.y - l.y, q.v.x);
  872.                                 if (q.w && fabs(q.v.z) <= maxz && (!l.x || q.v.x >= l.x) && (!l.y || q.v.y <= -l.y))
  873.                                         return rm33 * q.v - ipos;
  874.                         }
  875.                         if (p.z >= l.z && p.z <= maxz)
  876.                         {
  877.                                 q = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z - l.z), d, sl, h, l.x, loz.w), q.v.y -= l.y, q.v.z += l.z;
  878.                                 if (q.w && fabs(q.v.x) <= maxx && (!l.z || q.v.z >= l.z) && (!l.y || q.v.y <= -l.y))
  879.                                         return rm33 * q.v - ipos;
  880.                         }
  881.                         if (p.x <= -l.x && p.x >= -maxx)
  882.                         {
  883.                                 q = e.CheckEdge(Vec3(p.z, p.y + l.y, -p.x - l.x), Vec3(d.z, d.y, -d.x), sl, Vec3(h.z, h.y, -h.x), l.z, loz.w);
  884.                                 q.v(-q.v.z - l.x, q.v.y - l.y, q.v.x);
  885.                                 if (q.w && fabs(q.v.z) <= maxz && (!l.y || q.v.y <= -l.y) && (!l.x || q.v.x <= -l.x))
  886.                                         return rm33 * q.v - ipos;
  887.                         }
  888.                         if (p.z <= -l.z && p.z >= -maxz)
  889.                         {
  890.                                 q = e.CheckEdge(Vec3(p.x, p.y + l.y, p.z + l.z), d, sl, h, l.x, loz.w), q.v.y -= l.y, q.v.z -= l.z;
  891.                                 if (q.w && fabs(q.v.x) <= maxx && (!l.y || q.v.y <= -l.y) && (!l.z || q.v.z <= -l.z))
  892.                                         return rm33 * q.v - ipos;
  893.                         }
  894.                 }
  895.         }
  896.         Vec3 g = (idir % iha - idir * MAXRADEPSILON).GetNormalized(), n, b = idir;
  897.         for (int i = 0; ++i < 20; n = g + b, n *= isqrt_tpl(n | n), TestOverlapping(ipos, n, sl, sr) > 0 ? g = n : b = n)
  898.                 ;
  899.         return g * sl;
  900. }
  901.  
  902. void CProxy::Draw(const QuatTS& qt, const ColorB clr, uint32 tesselation, const Vec3& vdir)
  903. {
  904.         uint32 p = 0, c = 0;
  905.         Matrix34 m34 = Matrix34(qt);
  906.         Matrix33 m33 = Matrix33(m34);
  907.         Matrix33 mx, relx;
  908.         relx.SetRotationX(gf_PI * 0.5f / f32(tesselation));
  909.         Matrix33 my, rely;
  910.         rely.SetRotationY(gf_PI * 0.5f / f32(tesselation));
  911.         Matrix33 mz, relz;
  912.         relz.SetRotationZ(gf_PI * 0.5f / f32(tesselation));
  913.         uint32 stride = sqr(tesselation + 1);
  914.         Vec3 ldir0 = vdir;
  915.         ldir0.z = 0;
  916.         (ldir0 = ldir0.normalized() * 0.5f).z = f32(sqrt3 * -0.5f);
  917.         Vec3 ldir1(0, 0, 1);
  918.         f32 cx = m_params.x, cy = m_params.y, cz = m_params.z, r = m_params.w;
  919.         struct
  920.         {
  921.                 ILINE ColorB shader(Vec3 n, const Vec3& d0, const Vec3& d1, ColorB c)
  922.                 {
  923.                         f32 a = 0.5f * n | d0, b = 0.1f * n | d1, l = min(1.0f, fabs_tpl(a) - a + fabs_tpl(b) - b + 0.05f);
  924.                         return RGBA8(uint8(l * c.r), uint8(l * c.g), uint8(l * c.b), c.a);
  925.                 }
  926.                 void Triangulate1(Vec3* p3d, Vec3* _p, ColorB* c3d, ColorB* _c, uint32 t, uint32 s, uint32& p, uint32& c, bool x = 0)
  927.                 {
  928.                         for (uint32 i = 0; i < t; i++)
  929.                         {
  930.                                 _p[p++] = p3d[i + 1], _c[c++] = c3d[i + 1], _p[p++] = p3d[i + s + 0], _c[c++] = c3d[i + s + 0];
  931.                                 _p[p++] = p3d[i + 0], _c[c++] = c3d[i + 0], _p[p++] = p3d[i + s + 0], _c[c++] = c3d[i + s + 0];
  932.                                 _p[p++] = p3d[i + 1], _c[c++] = c3d[i + 1], _p[p++] = p3d[i + s + 1], _c[c++] = c3d[i + s + 1];
  933.                         }
  934.                         if (x) return;
  935.                         for (uint32 a = 0, b = 0; a < t; a++, b++)
  936.                         {
  937.                                 for (uint32 i = 0; i < t; i++, b++)
  938.                                 {
  939.                                         _p[p++] = p3d[b + 0], _c[c++] = c3d[b + 0], _p[p++] = p3d[b + t + 1], _c[c++] = c3d[b + t + 1];
  940.                                         _p[p++] = p3d[b + 1], _c[c++] = c3d[b + 1], _p[p++] = p3d[b + t + 2], _c[c++] = c3d[b + t + 2];
  941.                                         _p[p++] = p3d[b + 1], _c[c++] = c3d[b + 1], _p[p++] = p3d[b + t + 1], _c[c++] = c3d[b + t + 1];
  942.                                         _p[p++] = p3d[b + 1 + s], _c[c++] = c3d[b + 1 + s], _p[p++] = p3d[b + s + t + 1], _c[c++] = c3d[b + s + t + 1];
  943.                                         _p[p++] = p3d[b + 0 + s], _c[c++] = c3d[b + 0 + s], _p[p++] = p3d[b + s + t + 1], _c[c++] = c3d[b + s + t + 1];
  944.                                         _p[p++] = p3d[b + 1 + s], _c[c++] = c3d[b + 1 + s], _p[p++] = p3d[b + s + t + 2], _c[c++] = c3d[b + s + t + 2];
  945.                                 }
  946.                         }
  947.                 }
  948.                 void Triangulate2(Vec3* p3d, Vec3* _p, ColorB* c3d, ColorB* _c, uint32 t, uint32 s, uint32& p, uint32& c, bool x = 0)
  949.                 {
  950.                         for (uint32 i = 0; i < t; i++)
  951.                         {
  952.                                 _p[p++] = p3d[i + 0], _c[c++] = c3d[i + 0], _p[p++] = p3d[i + s + 0], _c[c++] = c3d[i + s + 0];
  953.                                 _p[p++] = p3d[i + 1], _c[c++] = c3d[i + 1], _p[p++] = p3d[i + s + 1], _c[c++] = c3d[i + s + 1];
  954.                                 _p[p++] = p3d[i + 1], _c[c++] = c3d[i + 1], _p[p++] = p3d[i + s + 0], _c[c++] = c3d[i + s + 0];
  955.                         }
  956.                         if (x) return;
  957.                         for (uint32 a = 0, b = 0; a < t; a++, b++)
  958.                         {
  959.                                 for (uint32 i = 0; i < t; i++, b++)
  960.                                 {
  961.                                         _p[p++] = p3d[b + 1], _c[c++] = c3d[b + 1], _p[p++] = p3d[b + t + 1], _c[c++] = c3d[b + t + 1];
  962.                                         _p[p++] = p3d[b + 0], _c[c++] = c3d[b + 0], _p[p++] = p3d[b + t + 1], _c[c++] = c3d[b + t + 1];
  963.                                         _p[p++] = p3d[b + 1], _c[c++] = c3d[b + 1], _p[p++] = p3d[b + t + 2], _c[c++] = c3d[b + t + 2];
  964.                                         _p[p++] = p3d[b + 0 + s], _c[c++] = c3d[b + 0 + s], _p[p++] = p3d[b + s + t + 1], _c[c++] = c3d[b + s + t + 1];
  965.                                         _p[p++] = p3d[b + 1 + s], _c[c++] = c3d[b + 1 + s], _p[p++] = p3d[b + s + t + 2], _c[c++] = c3d[b + s + t + 2];
  966.                                         _p[p++] = p3d[b + 1 + s], _c[c++] = c3d[b + 1 + s], _p[p++] = p3d[b + s + t + 1], _c[c++] = c3d[b + s + t + 1];
  967.                                 }
  968.                         }
  969.                 }
  970.         } draw;
  971.         Vec3 p3d[0x4000], _p[0x4000];
  972.         ColorB c3d[0x4000], _c[0x4000];
  973.         my.SetIdentity();
  974.         for (uint32 a = 0, b = 0; a < tesselation + 1; a++, my *= rely)
  975.         {
  976.                 mx.SetIdentity();
  977.                 for (uint32 i = 0; i < tesselation + 1; i++, b++, mx *= relx)
  978.                 {
  979.                         Vec3 v = mx * Vec3(my.m02, my.m12, my.m22);
  980.                         p3d[b] = m34 * Vec3(-cx - v.x * r, -cy + v.y * r, cz + v.z * r);
  981.                         p3d[b + stride] = m34 * Vec3(+cx + v.x * r, -cy + v.y * r, cz + v.z * r);
  982.                         c3d[b] = draw.shader(m33 * Vec3(-v.x, v.y, v.z), ldir0, ldir1, clr);
  983.                         c3d[b + stride] = draw.shader(m33 * Vec3(v.x, v.y, v.z), ldir0, ldir1, clr);
  984.                 }
  985.         }
  986.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c);
  987.         my.SetIdentity();
  988.         for (uint32 a = 0, b = 0; a < tesselation + 1; a++, my *= rely)
  989.         {
  990.                 mx.SetIdentity();
  991.                 for (uint32 i = 0; i < tesselation + 1; i++, b++, mx *= relx)
  992.                 {
  993.                         Vec3 v = mx * Vec3(my.m02, my.m12, my.m22);
  994.                         p3d[b] = m34 * Vec3(-cx - v.x * r, -cy + v.y * r, -cz - v.z * r);
  995.                         p3d[b + stride] = m34 * Vec3(+cx + v.x * r, -cy + v.y * r, -cz - v.z * r);
  996.                         c3d[b] = draw.shader(m33 * Vec3(-v.x, v.y, -v.z), ldir0, ldir1, clr);
  997.                         c3d[b + stride] = draw.shader(m33 * Vec3(v.x, v.y, -v.z), ldir0, ldir1, clr);
  998.                 }
  999.         }
  1000.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c);
  1001.         my.SetIdentity();
  1002.         for (uint32 a = 0, b = 0; a < tesselation + 1; a++, my *= rely)
  1003.         {
  1004.                 mx.SetIdentity();
  1005.                 for (uint32 i = 0; i < tesselation + 1; i++, b++, mx *= relx)
  1006.                 {
  1007.                         Vec3 v = mx * Vec3(my.m02, my.m12, my.m22);
  1008.                         p3d[b] = m34 * Vec3(-cx - v.x * r, +cy - v.y * r, cz + v.z * r);
  1009.                         p3d[b + stride] = m34 * Vec3(+cx + v.x * r, +cy - v.y * r, cz + v.z * r);
  1010.                         c3d[b] = draw.shader(m33 * Vec3(-v.x, -v.y, v.z), ldir0, ldir1, clr);
  1011.                         c3d[b + stride] = draw.shader(m33 * Vec3(v.x, -v.y, v.z), ldir0, ldir1, clr);
  1012.                 }
  1013.         }
  1014.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c);
  1015.         my.SetIdentity();
  1016.         for (uint32 a = 0, b = 0; a < tesselation + 1; a++, my *= rely)
  1017.         {
  1018.                 mx.SetIdentity();
  1019.                 for (uint32 i = 0; i < tesselation + 1; i++, b++, mx *= relx)
  1020.                 {
  1021.                         Vec3 v = mx * Vec3(my.m02, my.m12, my.m22);
  1022.                         p3d[b] = m34 * Vec3(-cx - v.x * r, +cy - v.y * r, -cz - v.z * r);
  1023.                         p3d[b + stride] = m34 * Vec3(+cx + v.x * r, +cy - v.y * r, -cz - v.z * r);
  1024.                         c3d[b] = draw.shader(m33 * Vec3(-v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1025.                         c3d[b + stride] = draw.shader(m33 * Vec3(v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1026.                 }
  1027.         }
  1028.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c);
  1029.         my.SetIdentity();
  1030.         for (uint32 i = 0, b = 0; i < tesselation + 1; i++, b++, my *= rely)
  1031.         {
  1032.                 Vec3 v = my.GetColumn2();
  1033.                 p3d[b] = m34 * Vec3(+cx + v.x * r, -cy + v.y * r, cz + v.z * r);
  1034.                 p3d[b + stride] = m34 * Vec3(+cx + v.x * r, cy - v.y * r, cz + v.z * r);
  1035.                 c3d[b] = draw.shader(m33 * Vec3(v.x, v.y, v.z), ldir0, ldir1, clr);
  1036.                 c3d[b + stride] = draw.shader(m33 * Vec3(v.x, -v.y, v.z), ldir0, ldir1, clr);
  1037.         }
  1038.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1039.         my.SetIdentity();
  1040.         for (uint32 i = 0, b = 0; i < tesselation + 1; i++, b++, my *= rely)
  1041.         {
  1042.                 Vec3 v = my.GetColumn2();
  1043.                 p3d[b] = m34 * Vec3(+cx + v.x * r, -cy + v.y * r, -cz - v.z * r);
  1044.                 p3d[b + stride] = m34 * Vec3(+cx + v.x * r, cy - v.y * r, -cz - v.z * r);
  1045.                 c3d[b] = draw.shader(m33 * Vec3(v.x, v.y, -v.z), ldir0, ldir1, clr);
  1046.                 c3d[b + stride] = draw.shader(m33 * Vec3(v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1047.         }
  1048.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1049.         my.SetIdentity();
  1050.         for (uint32 i = 0, b = 0; i < tesselation + 1; i++, b++, my *= rely)
  1051.         {
  1052.                 Vec3 v = my.GetColumn2();
  1053.                 p3d[b] = m34 * Vec3(-cx - v.x * r, -cy + v.y * r, cz + v.z * r);
  1054.                 p3d[b + stride] = m34 * Vec3(-cx - v.x * r, cy - v.y * r, cz + v.z * r);
  1055.                 c3d[b] = draw.shader(m33 * Vec3(-v.x, v.y, v.z), ldir0, ldir1, clr);
  1056.                 c3d[b + stride] = draw.shader(m33 * Vec3(-v.x, -v.y, v.z), ldir0, ldir1, clr);
  1057.         }
  1058.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1059.         my.SetIdentity();
  1060.         for (uint32 i = 0, b = 0; i < tesselation + 1; i++, b++, my *= rely)
  1061.         {
  1062.                 Vec3 v = my.GetColumn2();
  1063.                 p3d[b] = m34 * Vec3(-cx - v.x * r, -cy + v.y * r, -cz - v.z * r);
  1064.                 p3d[b + stride] = m34 * Vec3(-cx - v.x * r, cy - v.y * r, -cz - v.z * r);
  1065.                 c3d[b] = draw.shader(m33 * Vec3(-v.x, v.y, -v.z), ldir0, ldir1, clr);
  1066.                 c3d[b + stride] = draw.shader(m33 * Vec3(-v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1067.         }
  1068.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1069.         mz.SetIdentity();
  1070.         for (uint32 i = 0, b = 0; i < tesselation + 1; i++, b++, mz *= relz)
  1071.         {
  1072.                 Vec3 v = mz.GetColumn1();
  1073.                 p3d[b] = m34 * Vec3(-cx + v.x * r, cy + v.y * r, -cz - v.z * r);
  1074.                 p3d[b + stride] = m34 * Vec3(-cx + v.x * r, cy + v.y * r, cz + v.z * r);
  1075.                 c3d[b] = draw.shader(m33 * Vec3(v.x, v.y, -v.z), ldir0, ldir1, clr);
  1076.                 c3d[b + stride] = draw.shader(m33 * Vec3(v.x, v.y, v.z), ldir0, ldir1, clr);
  1077.         }
  1078.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1079.         mz.SetIdentity();
  1080.         for (uint32 i = 0, c = 0; i < tesselation + 1; i++, c++, mz *= relz)
  1081.         {
  1082.                 Vec3 v = mz.GetColumn1();
  1083.                 p3d[c] = m34 * Vec3(cx - v.x * r, cy + v.y * r, -cz - v.z * r);
  1084.                 p3d[c + stride] = m34 * Vec3(cx - v.x * r, cy + v.y * r, cz + v.z * r);
  1085.                 c3d[c] = draw.shader(m33 * Vec3(-v.x, v.y, -v.z), ldir0, ldir1, clr);
  1086.                 c3d[c + stride] = draw.shader(m33 * Vec3(-v.x, v.y, v.z), ldir0, ldir1, clr);
  1087.         }
  1088.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1089.         mz.SetIdentity();
  1090.         for (uint32 i = 0, c = 0; i < tesselation + 1; i++, c++, mz *= relz)
  1091.         {
  1092.                 Vec3 v = mz * Vec3(0, 1, 0);
  1093.                 p3d[c] = m34 * Vec3(-cx + v.x * r, -cy - v.y * r, -cz - v.z * r);
  1094.                 p3d[c + stride] = m34 * Vec3(-cx + v.x * r, -cy - v.y * r, cz + v.z * r);
  1095.                 c3d[c] = draw.shader(m33 * Vec3(v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1096.                 c3d[c + stride] = draw.shader(m33 * Vec3(v.x, -v.y, v.z), ldir0, ldir1, clr);
  1097.         }
  1098.         draw.Triangulate2(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1099.         mz.SetIdentity();
  1100.         for (uint32 i = 0, c = 0; i < tesselation + 1; i++, c++, mz *= relz)
  1101.         {
  1102.                 Vec3 v = mz * Vec3(0, 1, 0);
  1103.                 p3d[c] = m34 * Vec3(cx - v.x * r, -cy - v.y * r, -cz - v.z * r);
  1104.                 p3d[c + stride] = m34 * Vec3(cx - v.x * r, -cy - v.y * r, cz + v.z * r);
  1105.                 c3d[c] = draw.shader(m33 * Vec3(-v.x, -v.y, -v.z), ldir0, ldir1, clr);
  1106.                 c3d[c + stride] = draw.shader(m33 * Vec3(-v.x, -v.y, v.z), ldir0, ldir1, clr);
  1107.         }
  1108.         draw.Triangulate1(p3d, _p, c3d, _c, tesselation, stride, p, c, 1);
  1109.         p3d[0] = m34 * Vec3(-cx - r, -cy, -cz);
  1110.         p3d[0 + stride] = m34 * Vec3(-cx - r, +cy, -cz);
  1111.         c3d[0] = draw.shader(m33 * Vec3(-1, 0, 0), ldir0, ldir1, clr);
  1112.         c3d[0 + stride] = draw.shader(m33 * Vec3(-1, 0, 0), ldir0, ldir1, clr);
  1113.         p3d[1] = m34 * Vec3(-cx - r, -cy, +cz);
  1114.         p3d[1 + stride] = m34 * Vec3(-cx - r, +cy, +cz);
  1115.         c3d[1] = draw.shader(m33 * Vec3(-1, 0, 0), ldir0, ldir1, clr);
  1116.         c3d[1 + stride] = draw.shader(m33 * Vec3(-1, 0, 0), ldir0, ldir1, clr);
  1117.         draw.Triangulate1(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1118.         p3d[0] = m34 * Vec3(+cx + r, -cy, -cz);
  1119.         p3d[0 + stride] = m34 * Vec3(+cx + r, +cy, -cz);
  1120.         c3d[0] = draw.shader(m33 * Vec3(1, 0, 0), ldir0, ldir1, clr);
  1121.         c3d[0 + stride] = draw.shader(m33 * Vec3(1, 0, 0), ldir0, ldir1, clr);
  1122.         p3d[1] = m34 * Vec3(+cx + r, -cy, +cz);
  1123.         p3d[1 + stride] = m34 * Vec3(+cx + r, +cy, +cz);
  1124.         c3d[1] = draw.shader(m33 * Vec3(1, 0, 0), ldir0, ldir1, clr);
  1125.         c3d[1 + stride] = draw.shader(m33 * Vec3(1, 0, 0), ldir0, ldir1, clr);
  1126.         draw.Triangulate2(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1127.         p3d[0] = m34 * Vec3(-cx, +cy + r, -cz);
  1128.         p3d[0 + stride] = m34 * Vec3(cx, +cy + r, -cz);
  1129.         c3d[0] = draw.shader(m33 * Vec3(0, 1, 0), ldir0, ldir1, clr);
  1130.         c3d[0 + stride] = draw.shader(m33 * Vec3(0, 1, 0), ldir0, ldir1, clr);
  1131.         p3d[1] = m34 * Vec3(-cx, +cy + r, +cz);
  1132.         p3d[1 + stride] = m34 * Vec3(cx, +cy + r, +cz);
  1133.         c3d[1] = draw.shader(m33 * Vec3(0, 1, 0), ldir0, ldir1, clr);
  1134.         c3d[1 + stride] = draw.shader(m33 * Vec3(0, 1, 0), ldir0, ldir1, clr);
  1135.         draw.Triangulate1(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1136.         p3d[0] = m34 * Vec3(-cx, -cy - r, -cz);
  1137.         p3d[0 + stride] = m34 * Vec3(cx, -cy - r, -cz);
  1138.         c3d[0] = draw.shader(m33 * Vec3(0, -1, 0), ldir0, ldir1, clr);
  1139.         c3d[0 + stride] = draw.shader(m33 * Vec3(0, -1, 0), ldir0, ldir1, clr);
  1140.         p3d[1] = m34 * Vec3(-cx, -cy - r, +cz);
  1141.         p3d[1 + stride] = m34 * Vec3(cx, -cy - r, +cz);
  1142.         c3d[1] = draw.shader(m33 * Vec3(0, -1, 0), ldir0, ldir1, clr);
  1143.         c3d[1 + stride] = draw.shader(m33 * Vec3(0, -1, 0), ldir0, ldir1, clr);
  1144.         draw.Triangulate2(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1145.         p3d[0] = m34 * Vec3(-cx, -cy, cz + r);
  1146.         p3d[0 + stride] = m34 * Vec3(cx, -cy, cz + r);
  1147.         c3d[0] = draw.shader(m33 * Vec3(0, 0, 1), ldir0, ldir1, clr);
  1148.         c3d[0 + stride] = draw.shader(m33 * Vec3(0, 0, 1), ldir0, ldir1, clr);
  1149.         p3d[1] = m34 * Vec3(-cx, +cy, cz + r);
  1150.         p3d[1 + stride] = m34 * Vec3(cx, +cy, cz + r);
  1151.         c3d[1] = draw.shader(m33 * Vec3(0, 0, 1), ldir0, ldir1, clr);
  1152.         c3d[1 + stride] = draw.shader(m33 * Vec3(0, 0, 1), ldir0, ldir1, clr);
  1153.         draw.Triangulate2(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1154.         p3d[0] = m34 * Vec3(-cx, -cy, -cz - r);
  1155.         p3d[0 + stride] = m34 * Vec3(cx, -cy, -cz - r);
  1156.         c3d[0] = draw.shader(m33 * Vec3(0, 0, -1), ldir0, ldir1, clr);
  1157.         c3d[0 + stride] = draw.shader(m33 * Vec3(0, 0, -1), ldir0, ldir1, clr);
  1158.         p3d[1] = m34 * Vec3(-cx, +cy, -cz - r);
  1159.         p3d[1 + stride] = m34 * Vec3(cx, +cy, -cz - r);
  1160.         c3d[1] = draw.shader(m33 * Vec3(0, 0, -1), ldir0, ldir1, clr);
  1161.         c3d[1 + stride] = draw.shader(m33 * Vec3(0, 0, -1), ldir0, ldir1, clr);
  1162.         draw.Triangulate1(p3d, _p, c3d, _c, 1, stride, p, c, 1);
  1163.         SAuxGeomRenderFlags renderFlags(e_Def3DPublicRenderflags);
  1164.         g_pAuxGeom->SetRenderFlags(renderFlags);
  1165.         g_pAuxGeom->DrawTriangles(&_p[0], p, &_c[0]);
  1166. }
  1167.  
downloadAttachmentProxy.cpp Source code - Download CRYENGINE Source code
Related Source Codes/Software:
postal - 2017-06-11
reactide - Reactide is the first dedicated IDE for React web ... 2017-06-11
rkt - rkt is a pod-native container engine for Linux. It... 2017-06-11
uWebSockets - Tiny WebSockets https://for... 2017-06-11
realworld - TodoMVC for the RealWorld - Exemplary fullstack Me... 2017-06-11
CRYENGINE - CRYENGINE is a powerful real-time game development... 2017-06-11
goreplay - GoReplay is an open-source tool for capturing and ... 2017-06-10
pyenv - Simple Python version management 2017-06-10
redux-saga - An alternative side effect model for Redux apps ... 2017-06-10
angular-starter - 2017-06-10

 Back to top