diff options
Diffstat (limited to 'libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h')
| -rw-r--r-- | libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h | 177 |
1 files changed, 177 insertions, 0 deletions
diff --git a/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h new file mode 100644 index 0000000..149ac91 --- /dev/null +++ b/libs/ode-0.16.1/ode/src/gimpact_contact_export_helper.h @@ -0,0 +1,177 @@ +/************************************************************************* + * * + * Open Dynamics Engine, Copyright (C) 2001-2003 Russell L. Smith. * + * All rights reserved. Email: russ@q12.org Web: www.q12.org * + * * + * This library is free software; you can redistribute it and/or * + * modify it under the terms of EITHER: * + * (1) The GNU Lesser General Public License as published by the Free * + * Software Foundation; either version 2.1 of the License, or (at * + * your option) any later version. The text of the GNU Lesser * + * General Public License is included with this library in the * + * file LICENSE.TXT. * + * (2) The BSD-style license that is included with this library in * + * the file LICENSE-BSD.TXT. * + * * + * This library is distributed in the hope that it will be useful, * + * but WITHOUT ANY WARRANTY; without even the implied warranty of * + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the files * + * LICENSE.TXT and LICENSE-BSD.TXT for more details. * + * * + *************************************************************************/ + + +#ifndef _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ +#define _ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ + + +#include "collision_kernel.h" +#include "collision_util.h" +#include "util.h" + + +#ifndef ALLOCA +#define ALLOCA(x) dALLOCA16(x) +#endif + + +struct dxGImpactContactsExportHelper +{ +public: + template<class dxGImpactContactAccessor> + static unsigned ExportMaxDepthGImpactContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) + { + unsigned result; + + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + if (contactcount > maxcontacts) + { + ExportExcesssiveContacts(srccontacts, contactcount, Flags, Contacts, Stride); + result = maxcontacts; + } + else + { + ExportFitContacts(srccontacts, contactcount, Flags, Contacts, Stride); + result = contactcount; + } + + return result; + } + +private: + template<class dxGImpactContactAccessor> + static void ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride); + template<class dxGImpactContactAccessor> + static void ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride); + template<class dxGImpactContactAccessor> + static dReal FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts); + static dReal FindContactsMarginalDepth(dReal *pdepths, unsigned contactcount, unsigned maxcontacts, dReal mindepth, dReal maxdepth); +}; + + +template<class dxGImpactContactAccessor> +/*static */ +void dxGImpactContactsExportHelper::ExportExcesssiveContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) +{ + unsigned maxcontacts = (unsigned)(Flags & NUMC_MASK); + dReal marginaldepth = FindContactsMarginalDepth(srccontacts, contactcount, maxcontacts); + + unsigned contactshead = 0, contacttail = maxcontacts; + for (unsigned i = 0; i < contactcount; i++) + { + dReal depth = srccontacts.RetrieveDepthByIndex(i); + + if (depth > marginaldepth) + { + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contactshead, Stride); + srccontacts.ExportContactGeomByIndex(pcontact, i); + + if (++contactshead == maxcontacts) + { + break; + } + } + else if (depth == marginaldepth && contactshead < contacttail) + { + --contacttail; + + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, contacttail, Stride); + srccontacts.ExportContactGeomByIndex(pcontact, i); + } + } +} + +template<class dxGImpactContactAccessor> +/*static */ +void dxGImpactContactsExportHelper::ExportFitContacts(dxGImpactContactAccessor &srccontacts, unsigned contactcount, + int Flags, dContactGeom* Contacts, int Stride) +{ + for (unsigned i = 0; i < contactcount; i++) + { + dContactGeom *pcontact = SAFECONTACT(Flags, Contacts, i, Stride); + + srccontacts.ExportContactGeomByIndex(pcontact, i); + } +} + +template<class dxGImpactContactAccessor> +/*static */ +dReal dxGImpactContactsExportHelper::FindContactsMarginalDepth(dxGImpactContactAccessor &srccontacts, unsigned contactcount, unsigned maxcontacts) +{ + dReal result; + + dReal *pdepths = (dReal *)ALLOCA(contactcount * sizeof(dReal)); + unsigned marginindex = 0; + unsigned highindex = marginindex; + + dReal firstdepth = srccontacts.RetrieveDepthByIndex(0); + dReal mindepth = firstdepth, maxdepth = firstdepth; + dIASSERT(contactcount > 1); + + for (unsigned i = 1; i < contactcount; i++) + { + dReal depth = srccontacts.RetrieveDepthByIndex(i); + + if (depth < firstdepth) + { + dReal temp = pdepths[marginindex]; pdepths[highindex++] = temp; pdepths[marginindex++] = depth; + if (depth < mindepth) { mindepth = depth; } + } + else if (depth > firstdepth) + { + pdepths[highindex++] = depth; + if (maxdepth < depth) { maxdepth = depth; } + } + } + + unsigned countabove = highindex - marginindex; + if (maxcontacts < countabove) + { + result = FindContactsMarginalDepth(pdepths + marginindex, countabove, maxcontacts, firstdepth, maxdepth); + } + else if (maxcontacts == countabove) + { + result = dNextAfter(firstdepth, dInfinity); + } + else + { + unsigned countbelow = marginindex; + if (maxcontacts <= contactcount - countbelow) + { + result = firstdepth; + } + else + { + result = FindContactsMarginalDepth(pdepths, countbelow, maxcontacts - (contactcount - countbelow), mindepth, firstdepth); + } + } + + return result; +} + + +#endif //_ODE_GIMPACT_CONTACT_EXPORT_HELPER_H_ |