public eStatus Evaluate(tShape shapearg, btVector3 guess) { U iterations = 0; float sqdist = 0; float alpha = 0; //btVector3 *lastw=stackalloc btVector3[4]; //btVector3[] lastw = new btVector3[4]; StackPtr<btVector3> lastw = StackPtr<btVector3>.Allocate(4); try { U clastw = 0; /* Initialize solver */ m_free[0] = m_store[0]; m_free[1] = m_store[1]; m_free[2] = m_store[2]; m_free[3] = m_store[3]; m_nfree = 4; m_current = 0; m_status = eStatus.Valid; m_shape = shapearg; m_distance = 0; /* Initialize simplex */ m_simplices[0].rank = 0; m_ray = guess; float sqrl = m_ray.Length2; appendvertice(m_simplices[0], sqrl > 0 ? -m_ray : new btVector3(1, 0, 0)); m_simplices[0].p[0] = 1; m_ray = m_simplices[0].c[0].w; sqdist = sqrl; lastw[0] = lastw[1] = lastw[2] = lastw[3] = m_ray; /* Loop */ do { U next = 1 - m_current; sSimplex cs = m_simplices[m_current]; sSimplex ns = m_simplices[next]; /* Check zero */ float rl = m_ray.Length; if (rl < GJK_MIN_DISTANCE) {/* Touching or inside */ m_status = eStatus.Inside; break; } /* Append new vertice in -'v' direction */ appendvertice(cs, -m_ray); btVector3 w = cs.c[cs.rank - 1].w; bool found = false; for (U i = 0; i < 4; ++i) { if ((w - lastw[i]).Length2 < GJK_DUPLICATED_EPS) { found = true; break; } } if (found) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } else {/* Update lastw */ lastw[clastw = (clastw + 1) & 3] = w; } /* Check for termination */ float omega = btVector3.dot(m_ray, w) / rl; alpha = (float)Math.Max(omega, alpha); if (((rl - alpha) - (GJK_ACCURARY * rl)) <= 0) {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } /* Reduce simplex */ //float* weights = stackalloc float[4]; StackPtr<float> weights = StackPtr<float>.Allocate(4); try { U mask = 0; switch (cs.rank) { case 2: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, weights, ref mask); break; case 3: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, weights, ref mask); break; case 4: sqdist = projectorigin(cs.c[0].w, cs.c[1].w, cs.c[2].w, cs.c[3].w, weights, ref mask); break; } if (sqdist >= 0) {/* Valid */ ns.rank = 0; m_ray = new btVector3(0, 0, 0); m_current = next; for (U i = 0, ni = cs.rank; i < ni; ++i) { if ((mask & (1 << (int)i)) != 0) { ns.c[ns.rank] = cs.c[i]; ns.p[ns.rank++] = weights[i]; #region m_ray += cs.c[i].w * weights[i]; { btVector3 temp; btVector3.Multiply(ref cs.c[i].w, weights[i], out temp); m_ray.Add(ref temp); } #endregion } else { m_free[m_nfree++] = cs.c[i]; } } if (mask == 15) m_status = eStatus.Inside; } else {/* Return old simplex */ removevertice(m_simplices[m_current]); break; } m_status = ((++iterations) < GJK_MAX_ITERATIONS) ? m_status : eStatus.Failed; } finally { weights.Dispose(); } } while (m_status == eStatus.Valid); m_simplex = m_simplices[m_current]; switch (m_status) { case eStatus.Valid: m_distance = m_ray.Length; break; case eStatus.Inside: m_distance = 0; break; default: { break; } } return (m_status); } finally { lastw.Dispose(); } }
static void Initialize(ConvexShape shape0, btTransform wtrs0, ConvexShape shape1, btTransform wtrs1, ref GjkEpaSolver2.sResults results, ref tShape shape, bool withmargins) { /* Results */ results.witnesses0 = results.witnesses1 = new btVector3(0, 0, 0); results.status = GjkEpaSolver2.sResults.eStatus.Separated; /* Shape */ shape.m_shapes0 = shape0; shape.m_shapes1 = shape1; //shape.m_toshape1 = wtrs1.Basis.transposeTimes(wtrs0.Basis); wtrs1.Basis.transposeTimes(ref wtrs0.Basis, out shape.m_toshape1); shape.m_toshape0 = wtrs0.inverseTimes(wtrs1); shape.EnableMargin(withmargins); }