ode/src/gimpact_contact_export_helper.cpp

   1 /*************************************************************************
   2  *                                                                       *
   3  * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith.       *
   4  * All rights reserved.  Email: russ@q12.org   Web: www.q12.org          *
   5  *                                                                       *
   6  * This library is free software; you can redistribute it and/or         *
   7  * modify it under the terms of EITHER:                                  *
   8  *   (1) The GNU Lesser General Public License as published by the Free  *
   9  *       Software Foundation; either version 2.1 of the License, or (at  *
  10  *       your option) any later version. The text of the GNU Lesser      *
  11  *       General Public License is included with this library in the     *
  12  *       file LICENSE.TXT.                                               *
  13  *   (2) The BSD-style license that is included with this library in     *
  14  *       the file LICENSE-BSD.TXT.                                       *
  15  *                                                                       *
  16  * This library is distributed in the hope that it will be useful,       *
  17  * but WITHOUT ANY WARRANTY; without even the implied warranty of        *
  18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files    *
  19  * LICENSE.TXT and LICENSE-BSD.TXT for more details.                     *
  20  *                                                                       *
  21  *************************************************************************/
  22
  23 #include <ode/collision.h>
  24 #include "config.h"
  25
  26
  27 #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
  28
  29 #include "gimpact_contact_export_helper.h"
  30 #include "error.h"
  31
  32
  33 /*static */
  34 dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth)
  35 {
  36     dReal result;
  37
  38     while (true)
  39     {
  40         dReal firstdepth = REAL(0.5) * (mindepth + maxdepth);
  41         dReal lowdepth = maxdepth, highdepth = mindepth;
  42
  43         unsigned marginindex = 0;
  44         unsigned highindex = marginindex;
  45         dIASSERT(contactcount != 0);
  46
  47         for (unsigned i = 0; i < contactcount; i++)
  48         {
  49             dReal depth = pdepths[i];
  50
  51             if (depth < firstdepth)
  52             {
  53                 dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth;
  54                 if (highdepth < depth) { highdepth = depth; }
  55             }
  56             else if (depth > firstdepth)
  57             {
  58                 pdepths[highindex++] = depth;
  59                 if (depth < lowdepth) { lowdepth = depth; }
  60             }
  61         }
  62
  63         unsigned countabove = highindex - marginindex;
  64         if (maxcontacts < countabove)
  65         {
  66             contactcount = countabove;
  67             pdepths += marginindex;
  68             mindepth = lowdepth;
  69         }
  70         else if (maxcontacts == countabove)
  71         {
  72             result = dNextAfter(firstdepth, dInfinity);
  73             break;
  74         }
  75         else
  76         {
  77             unsigned countbelow = marginindex;
  78             if (maxcontacts <= contactcount - countbelow)
  79             {
  80                 result = firstdepth;
  81                 break;
  82             }
  83
  84             maxcontacts -= contactcount - countbelow;
  85             contactcount = countbelow;
  86             maxdepth = highdepth;
  87         }
  88     }
  89
  90     return result;
  91 }
  92
  93
  94 #endif // #if dTRIMESH_ENABLED && dTRIMESH_GIMPACT
  95