Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

moved files around

Browse Source
This commit is contained in:
ejcoumans
2006-05-25 19:18:29 +00:00
commit e061ec1ebf
1024 changed files with 349445 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

227
Extras/ExtraSolid35/CombinedSimplexSolver.cpp Normal file
View File

@@ -0,0 +1,227 @@
/*
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#include "CombinedSimplexSolver.h"
#include <stdio.h>
//switch off asserts
//#define MY_ASSERT assert
#define MY_ASSERT
bool useVoronoi = true;
CombinedSimplexSolver::CombinedSimplexSolver()
:m_useVoronoiSolver(useVoronoi)
{
}
void CombinedSimplexSolver::reset()
{
m_voronoiSolver.reset();
m_johnsonSolver.reset();
}
void CombinedSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
{
printf("addVertex (%f %f %f)\n",w[0],w[1],w[2]);
m_voronoiSolver.addVertex(w,p,q);
m_johnsonSolver.addVertex(w,p,q);
int i;
i=0;
SimdPoint3 vp1,vp2;
SimdPoint3 jp1,jp2;
/*
bool isClosest0 = m_voronoiSolver.closest(vp1);
bool isClosest1 = m_johnsonSolver.closest(vp1);
m_voronoiSolver.compute_points(vp1, vp2);
m_johnsonSolver.compute_points(jp1,jp2);
i=0;
*/
}
bool CombinedSimplexSolver::closest(SimdVector3& v)
{
bool result0 = 0;
bool result1 = 0;
SimdVector3 v0,v1;
result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1);
if (result0 != result1)
{
result0 = m_voronoiSolver.closest(v0);
result1 = m_johnsonSolver.closest(v1);
int i;
i=0;
}
if (m_useVoronoiSolver)
{
v = v0;
return result0;
}
v = v1;
return result1;
}
SimdScalar CombinedSimplexSolver::maxVertex()
{
SimdScalar maxv0 = m_voronoiSolver.maxVertex();
SimdScalar maxv1 = m_johnsonSolver.maxVertex();
MY_ASSERT(maxv0 = maxv1);
if (m_useVoronoiSolver)
return maxv0;
return maxv1;
}
bool CombinedSimplexSolver::fullSimplex() const
{
bool fullSimplex0 = m_voronoiSolver.fullSimplex();
bool fullSimplex1 = m_johnsonSolver.fullSimplex();
MY_ASSERT(fullSimplex0 == fullSimplex1);
if (m_useVoronoiSolver)
return fullSimplex0;
return fullSimplex1;
}
int CombinedSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
{
int simplex0 = m_voronoiSolver.getSimplex(pBuf, qBuf, yBuf);
int simplex1 = m_johnsonSolver.getSimplex(pBuf, qBuf, yBuf);
// MY_ASSERT(simplex0 == simplex1);
if (m_useVoronoiSolver)
{
return m_voronoiSolver.getSimplex(pBuf, qBuf, yBuf);
}
return simplex1;
}
void CombinedSimplexSolver::debugPrint()
{
SimdPoint3 pBuf0[4];
SimdPoint3 qBuf0[4];
SimdPoint3 yBuf0[4];
SimdPoint3 pBuf1[4];
SimdPoint3 qBuf1[4];
SimdPoint3 yBuf1[4];
int verts0,verts1;
verts0 = m_voronoiSolver.getSimplex(&pBuf0[0], &qBuf0[0], &yBuf0[0]);
verts1 = m_johnsonSolver.getSimplex(&pBuf1[0], &qBuf1[0], &yBuf1[0]);
printf("numverts0 = %d, numverts1 = %d\n",verts0,verts1);
for (int i=0;i<verts0;i++)
{
printf("vert0 pBuf %d = %f , %f , %f\n",i,pBuf0[i].x(),pBuf0[i].y(),pBuf0[i].z());
printf("vert0 qBuf %d = %f , %f , %f\n",i,qBuf0[i].x(),qBuf0[i].y(),qBuf0[i].z());
printf("vert0 yBuf %d = %f , %f , %f\n",i,yBuf0[i].x(),yBuf0[i].y(),yBuf0[i].z());
}
for (int i=0;i<verts1;i++)
{
printf("vert1 pBuf %d = %f , %f , %f\n",i,pBuf1[i].x(),pBuf1[i].y(),pBuf1[i].z());
printf("vert1 qBuf %d = %f , %f , %f\n",i,qBuf1[i].x(),qBuf1[i].y(),qBuf1[i].z());
printf("vert1 yBuf %d = %f , %f , %f\n",i,yBuf1[i].x(),yBuf1[i].y(),yBuf1[i].z());
}
}
bool CombinedSimplexSolver::inSimplex(const SimdVector3& w)
{
bool insimplex0 = m_voronoiSolver.inSimplex(w);
bool insimplex1 = m_johnsonSolver.inSimplex(w);
if (insimplex0 != insimplex1)
{
debugPrint();
}
if (m_useVoronoiSolver)
return insimplex0;
return insimplex1;
}
void CombinedSimplexSolver::backup_closest(SimdVector3& v)
{
SimdVector3 v0,v1;
m_voronoiSolver.backup_closest(v0);
m_johnsonSolver.backup_closest(v1);
if (m_useVoronoiSolver)
{
v = v0;
} else
{
v = v1;
}
}
bool CombinedSimplexSolver::emptySimplex() const
{
bool empty0 = m_voronoiSolver.emptySimplex();
bool empty1 = m_johnsonSolver.emptySimplex();
MY_ASSERT(empty0 == empty1);
if (m_useVoronoiSolver)
return empty0;
return empty1;
}
void CombinedSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
{
SimdPoint3 tmpP1,tmpP2;
SimdPoint3 tmpJP1,tmpJP2;
m_voronoiSolver.compute_points(tmpP1,tmpP2);
m_johnsonSolver.compute_points(tmpJP1,tmpJP2);
if (m_useVoronoiSolver)
{
p1 = tmpP1;
p2 = tmpP2;
}
else
{
p1 = tmpJP1;
p2 = tmpJP2;
}
}
int CombinedSimplexSolver::numVertices() const
{
int numverts0 = m_voronoiSolver.numVertices();
int numverts1 = m_johnsonSolver.numVertices();
MY_ASSERT(numverts0==numverts1);
if (numverts0 != numverts1)
{
printf("--------- (numverts0 != numverts1) -----------------------\n");
}
printf("numverts0 = %d, numverts1 %d \n",numverts0,numverts1);
if (m_useVoronoiSolver)
return numverts0;
return numverts1;
}

62
Extras/ExtraSolid35/CombinedSimplexSolver.h Normal file
View File

@@ -0,0 +1,62 @@
/*
* Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
*
* Permission to use, copy, modify, distribute and sell this software
* and its documentation for any purpose is hereby granted without fee,
* provided that the above copyright notice appear in all copies.
* Erwin Coumans makes no representations about the suitability
* of this software for any purpose.
* It is provided "as is" without express or implied warranty.
*/
#ifndef COMBINED_SIMPLEX_SOLVER
#define COMBINED_SIMPLEX_SOLVER
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
#include "NarrowPhaseCollision/VoronoiSimplexSolver.h"
#include "Solid3JohnsonSimplexSolver.h"
/// CombinedSimplexSolver runs both Solid and Voronoi Simplex Solver for comparison
class CombinedSimplexSolver: public SimplexSolverInterface
{
VoronoiSimplexSolver m_voronoiSolver;
// VoronoiSimplexSolver m_johnsonSolver;
Solid3JohnsonSimplexSolver m_johnsonSolver;
bool m_useVoronoiSolver;
void debugPrint();
public:
CombinedSimplexSolver();
virtual ~CombinedSimplexSolver() {};
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual bool closest(SimdVector3& v);
virtual SimdScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual bool emptySimplex() const;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2);
virtual int numVertices() const;
};
#endif //COMBINED_SIMPLEX_SOLVER

137
Extras/ExtraSolid35/ExtraSolid35_vc7.vcproj Normal file
View File

@@ -0,0 +1,137 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.10"
Name="ExtraSolid35"
ProjectGUID="{23F98301-2F09-4064-86CC-AB00D097BFB6}"
Keyword="Win32Proj">
<Platforms>
<Platform
Name="Win32"/>
</Platforms>
<Configurations>
<Configuration
Name="Debug|Win32"
OutputDirectory="Debug"
IntermediateDirectory="Debug"
ConfigurationType="4"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
AdditionalIncludeDirectories="..\..\Bullet;..\..\LinearMath"
PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
MinimalRebuild="TRUE"
BasicRuntimeChecks="3"
RuntimeLibrary="3"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="4"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/ExtraSolid35.lib"/>
<Tool
Name="VCMIDLTool"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Release|Win32"
OutputDirectory="Release"
IntermediateDirectory="Release"
ConfigurationType="4"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
AdditionalIncludeDirectories="..\..\Bullet;..\..\LinearMath"
PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
RuntimeLibrary="2"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="3"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/ExtraSolid35.lib"/>
<Tool
Name="VCMIDLTool"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Source Files"
Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}">
<File
RelativePath=".\CombinedSimplexSolver.cpp">
</File>
<File
RelativePath=".\Solid3EpaPenetrationDepth.cpp">
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.cpp">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="h;hpp;hxx;hm;inl;inc;xsd"
UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}">
<File
RelativePath=".\CombinedSimplexSolver.h">
</File>
<File
RelativePath=".\Solid3EpaPenetrationDepth.h">
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.h">
</File>
</Filter>
<Filter
Name="Resource Files"
Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx"
UniqueIdentifier="{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}">
</Filter>
<File
RelativePath=".\ReadMe.txt">
</File>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

195
Extras/ExtraSolid35/ExtraSolid35_vc8.vcproj Normal file
View File

@@ -0,0 +1,195 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="8.00"
Name="ExtraSolid35"
ProjectGUID="{23F98301-2F09-4064-86CC-AB00D097BFB6}"
Keyword="Win32Proj"
>
<Platforms>
<Platform
Name="Win32"
/>
</Platforms>
<ToolFiles>
</ToolFiles>
<Configurations>
<Configuration
Name="Debug|Win32"
OutputDirectory="Debug"
IntermediateDirectory="Debug"
ConfigurationType="4"
CharacterSet="2"
>
<Tool
Name="VCPreBuildEventTool"
/>
<Tool
Name="VCCustomBuildTool"
/>
<Tool
Name="VCXMLDataGeneratorTool"
/>
<Tool
Name="VCWebServiceProxyGeneratorTool"
/>
<Tool
Name="VCMIDLTool"
/>
<Tool
Name="VCCLCompilerTool"
Optimization="0"
AdditionalIncludeDirectories="..\..\Bullet;..\..\LinearMath"
PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
MinimalRebuild="true"
BasicRuntimeChecks="3"
RuntimeLibrary="1"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="true"
DebugInformationFormat="4"
/>
<Tool
Name="VCManagedResourceCompilerTool"
/>
<Tool
Name="VCResourceCompilerTool"
/>
<Tool
Name="VCPreLinkEventTool"
/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/ExtraSolid35.lib"
/>
<Tool
Name="VCALinkTool"
/>
<Tool
Name="VCXDCMakeTool"
/>
<Tool
Name="VCBscMakeTool"
/>
<Tool
Name="VCFxCopTool"
/>
<Tool
Name="VCPostBuildEventTool"
/>
</Configuration>
<Configuration
Name="Release|Win32"
OutputDirectory="Release"
IntermediateDirectory="Release"
ConfigurationType="4"
CharacterSet="2"
>
<Tool
Name="VCPreBuildEventTool"
/>
<Tool
Name="VCCustomBuildTool"
/>
<Tool
Name="VCXMLDataGeneratorTool"
/>
<Tool
Name="VCWebServiceProxyGeneratorTool"
/>
<Tool
Name="VCMIDLTool"
/>
<Tool
Name="VCCLCompilerTool"
AdditionalIncludeDirectories="..\..\Bullet;..\..\LinearMath"
PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
RuntimeLibrary="0"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="true"
DebugInformationFormat="3"
/>
<Tool
Name="VCManagedResourceCompilerTool"
/>
<Tool
Name="VCResourceCompilerTool"
/>
<Tool
Name="VCPreLinkEventTool"
/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/ExtraSolid35.lib"
/>
<Tool
Name="VCALinkTool"
/>
<Tool
Name="VCXDCMakeTool"
/>
<Tool
Name="VCBscMakeTool"
/>
<Tool
Name="VCFxCopTool"
/>
<Tool
Name="VCPostBuildEventTool"
/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Source Files"
Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}"
>
<File
RelativePath=".\CombinedSimplexSolver.cpp"
>
</File>
<File
RelativePath=".\Solid3EpaPenetrationDepth.cpp"
>
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.cpp"
>
</File>
</Filter>
<Filter
Name="Header Files"
Filter="h;hpp;hxx;hm;inl;inc;xsd"
UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}"
>
<File
RelativePath=".\CombinedSimplexSolver.h"
>
</File>
<File
RelativePath=".\Solid3EpaPenetrationDepth.h"
>
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.h"
>
</File>
</Filter>
<Filter
Name="Resource Files"
Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx"
UniqueIdentifier="{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}"
>
</Filter>
<File
RelativePath=".\ReadMe.txt"
>
</File>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

110
Extras/ExtraSolid35/LICENSE_QPL.txt Normal file
View File

@@ -0,0 +1,110 @@
The SOLID library is Copyright (C) 2001-2003 Dtecta.
You may use, distribute and copy the SOLID library under the terms of
the Q Public License, which is displayed below.
-------------------------------------------------------------------------
THE Q PUBLIC LICENSE
version 1.0
Copyright (C) 1999-2000 Trolltech AS, Norway.
Everyone is permitted to copy and
distribute this license document.
The intent of this license is to establish freedom to share and change the
software regulated by this license under the open source model.
This license applies to any software containing a notice placed by the
copyright holder saying that it may be distributed under the terms of
the Q Public License version 1.0. Such software is herein referred to as
the Software. This license covers modification and distribution of the
Software, use of third-party application programs based on the Software,
and development of free software which uses the Software.
Granted Rights
1. You are granted the non-exclusive rights set forth in this license
provided you agree to and comply with any and all conditions in this
license. Whole or partial distribution of the Software, or software
items that link with the Software, in any form signifies acceptance of
this license.
2. You may copy and distribute the Software in unmodified form provided
that the entire package, including - but not restricted to - copyright,
trademark notices and disclaimers, as released by the initial developer
of the Software, is distributed.
3. You may make modifications to the Software and distribute your
modifications, in a form that is separate from the Software, such as
patches. The following restrictions apply to modifications:
a. Modifications must not alter or remove any copyright notices in
the Software.
b. When modifications to the Software are released under this
license, a non-exclusive royalty-free right is granted to the
initial developer of the Software to distribute your modification
in future versions of the Software provided such versions remain
available under these terms in addition to any other license(s) of
the initial developer.
4. You may distribute machine-executable forms of the Software or
machine-executable forms of modified versions of the Software, provided
that you meet these restrictions:
a. You must include this license document in the distribution.
b. You must ensure that all recipients of the machine-executable forms
are also able to receive the complete machine-readable source code
to the distributed Software, including all modifications, without
any charge beyond the costs of data transfer, and place prominent
notices in the distribution explaining this.
c. You must ensure that all modifications included in the
machine-executable forms are available under the terms of this
license.
5. You may use the original or modified versions of the Software to
compile, link and run application programs legally developed by you
or by others.
6. You may develop application programs, reusable components and other
software items that link with the original or modified versions of the
Software. These items, when distributed, are subject to the following
requirements:
a. You must ensure that all recipients of machine-executable forms of
these items are also able to receive and use the complete
machine-readable source code to the items without any charge
beyond the costs of data transfer.
b. You must explicitly license all recipients of your items to use
and re-distribute original and modified versions of the items in
both machine-executable and source code forms. The recipients must
be able to do so without any charges whatsoever, and they must be
able to re-distribute to anyone they choose.
c. If the items are not available to the general public, and the
initial developer of the Software requests a copy of the items,
then you must supply one.
Limitations of Liability
In no event shall the initial developers or copyright holders be liable
for any damages whatsoever, including - but not restricted to - lost
revenue or profits or other direct, indirect, special, incidental or
consequential damages, even if they have been advised of the possibility
of such damages, except to the extent invariable law, if any, provides
otherwise.
No Warranty
The Software and this license document are provided AS IS with NO WARRANTY
OF ANY KIND, INCLUDING THE WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE.
Choice of Law
This license is governed by the Laws of Norway. Disputes shall be settled
by Oslo City Court.

21
Extras/ExtraSolid35/ReadMe.txt Normal file
View File

@@ -0,0 +1,21 @@
========================================================================
STATIC LIBRARY : Solid35 Project Overview
========================================================================
AppWizard has created this Solid35 library project for you.
No source files were created as part of your project.
Solid35.vcproj
This is the main project file for VC++ projects generated using an Application Wizard.
It contains information about the version of Visual C++ that generated the file, and
information about the platforms, configurations, and project features selected with the
Application Wizard.
/////////////////////////////////////////////////////////////////////////////
Other notes:
AppWizard uses "TODO:" comments to indicate parts of the source code you
should add to or customize.
/////////////////////////////////////////////////////////////////////////////

129
Extras/ExtraSolid35/Solid35.vcproj Normal file
View File

@@ -0,0 +1,129 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.10"
Name="OptionalSolid35"
ProjectGUID="{698BA0FD-10D3-4F4C-8686-1303D6273D46}"
Keyword="Win32Proj">
<Platforms>
<Platform
Name="Win32"/>
</Platforms>
<Configurations>
<Configuration
Name="Debug|Win32"
OutputDirectory="Debug"
IntermediateDirectory="Debug"
ConfigurationType="4"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
PreprocessorDefinitions="WIN32;_DEBUG;_LIB"
MinimalRebuild="TRUE"
BasicRuntimeChecks="3"
RuntimeLibrary="5"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="4"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/Solid35.lib"/>
<Tool
Name="VCMIDLTool"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Release|Win32"
OutputDirectory="Release"
IntermediateDirectory="Release"
ConfigurationType="4"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
PreprocessorDefinitions="WIN32;NDEBUG;_LIB"
RuntimeLibrary="4"
UsePrecompiledHeader="0"
WarningLevel="3"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="3"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/Solid35.lib"/>
<Tool
Name="VCMIDLTool"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Source Files"
Filter="cpp;c;cxx;def;odl;idl;hpj;bat;asm;asmx"
UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}">
<File
RelativePath=".\Solid3EpaPenetrationDepth.cpp">
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.cpp">
</File>
</Filter>
<Filter
Name="Header Files"
Filter="h;hpp;hxx;hm;inl;inc;xsd"
UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}">
<File
RelativePath=".\Solid3EpaPenetrationDepth.h">
</File>
<File
RelativePath=".\Solid3JohnsonSimplexSolver.h">
</File>
</Filter>
<Filter
Name="Resource Files"
Filter="rc;ico;cur;bmp;dlg;rc2;rct;bin;rgs;gif;jpg;jpeg;jpe;resx"
UniqueIdentifier="{67DA6AB6-F800-4c08-8B7A-83BB121AAD01}">
</Filter>
<File
RelativePath=".\ReadMe.txt">
</File>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

587
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.cpp Normal file
View File

@@ -0,0 +1,587 @@
/*
* SOLID - Software Library for Interference Detection
*
* Copyright (C) 2001-2003 Dtecta. All rights reserved.
*
* This library may be distributed under the terms of the Q Public License
* (QPL) as defined by Trolltech AS of Norway and appearing in the file
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
* This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Commercial use or any other use of this library not covered by either
* the QPL or the GPL requires an additional license from Dtecta.
* Please contact info@dtecta.com for enquiries about the terms of commercial
* use of this library.
*/
#include "Solid3EpaPenetrationDepth.h"
#include <algorithm>
#include <vector>
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
#include "CollisionShapes/ConvexShape.h"
#include "GEN_MinMax.h"
#define ASSERT_MESSAGE
class ReplaceMeAccuracy {
public:
static SimdScalar rel_error2; // squared relative error in the computed distance
static SimdScalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2
static SimdScalar tol_error; // error tolerance if the distance is almost zero
static void setAccuracy(SimdScalar rel_error)
{
rel_error2 = rel_error * rel_error;
depth_tolerance = SimdScalar(1.0f) + SimdScalar(2.0f) * rel_error;
}
static void setTolerance(SimdScalar epsilon)
{
tol_error = epsilon;
}
};
static const SimdScalar rel_error = SimdScalar(1.0e-3);
SimdScalar ReplaceMeAccuracy::rel_error2 = rel_error * rel_error;
SimdScalar ReplaceMeAccuracy::depth_tolerance = SimdScalar(1.0) + SimdScalar(2.0) * rel_error;
SimdScalar ReplaceMeAccuracy::tol_error = SIMD_EPSILON;
class ReplaceMeFacet;
class ReplaceMeEdge {
public:
ReplaceMeEdge() {}
ReplaceMeEdge(ReplaceMeFacet *facet, int index) :
m_facet(facet),
m_index(index) {}
ReplaceMeFacet *getFacet() const { return m_facet; }
int getIndex() const { return m_index; }
int getSource() const;
int getTarget() const;
private:
ReplaceMeFacet *m_facet;
int m_index;
};
typedef std::vector<ReplaceMeEdge> ReplaceMeEdgeBuffer;
class ReplaceMeFacet {
public:
ReplaceMeFacet() {}
ReplaceMeFacet(int i0, int i1, int i2)
: m_obsolete(false)
{
m_indices[0] = i0;
m_indices[1] = i1;
m_indices[2] = i2;
}
inline int operator[](int i) const { return m_indices[i]; }
bool link(int edge0, ReplaceMeFacet *facet, int edge1);
bool isObsolete() const { return m_obsolete; }
bool computeClosest(const SimdVector3 *verts);
const SimdVector3& getClosest() const { return m_closest; }
bool isClosestInternal() const
{
return m_lambda1 >= SimdScalar(0.0) &&
m_lambda2 >= SimdScalar(0.0) &&
m_lambda1 + m_lambda2 <= m_det;
}
SimdScalar getDist2() const { return m_dist2; }
SimdPoint3 getClosestPoint(const SimdPoint3 *points) const
{
const SimdPoint3& p0 = points[m_indices[0]];
return p0 + (m_lambda1 * (points[m_indices[1]] - p0) +
m_lambda2 * (points[m_indices[2]] - p0)) / m_det;
}
void silhouette(const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer)
{
edgeBuffer.clear();
m_obsolete = true;
m_adjFacets[0]->silhouette(m_adjEdges[0], w, edgeBuffer);
m_adjFacets[1]->silhouette(m_adjEdges[1], w, edgeBuffer);
m_adjFacets[2]->silhouette(m_adjEdges[2], w, edgeBuffer);
}
private:
void silhouette(int index, const SimdVector3& w, ReplaceMeEdgeBuffer& edgeBuffer);
int m_indices[3];
bool m_obsolete;
ReplaceMeFacet *m_adjFacets[3];
int m_adjEdges[3];
SimdScalar m_det;
SimdScalar m_lambda1;
SimdScalar m_lambda2;
SimdVector3 m_closest;
SimdScalar m_dist2;
};
inline int incMod3(int i) { return ++i % 3; }
bool ReplaceMeFacet::link(int edge0, ReplaceMeFacet *facet, int edge1)
{
m_adjFacets[edge0] = facet;
m_adjEdges[edge0] = edge1;
facet->m_adjFacets[edge1] = this;
facet->m_adjEdges[edge1] = edge0;
bool b = m_indices[edge0] == facet->m_indices[incMod3(edge1)] &&
m_indices[incMod3(edge0)] == facet->m_indices[edge1];
return b;
}
bool ReplaceMeFacet::computeClosest(const SimdVector3 *verts)
{
const SimdVector3& p0 = verts[m_indices[0]];
SimdVector3 v1 = verts[m_indices[1]] - p0;
SimdVector3 v2 = verts[m_indices[2]] - p0;
SimdScalar v1dv1 = v1.length2();
SimdScalar v1dv2 = v1.dot(v2);
SimdScalar v2dv2 = v2.length2();
SimdScalar p0dv1 = p0.dot(v1);
SimdScalar p0dv2 = p0.dot(v2);
m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative
//printf("m_det = %f\n",m_det);
//ASSERT(m_det >= 0.f);
if (m_det >= (SIMD_EPSILON*SIMD_EPSILON)) {
m_lambda1 = p0dv2 * v1dv2 - p0dv1 * v2dv2;
m_lambda2 = p0dv1 * v1dv2 - p0dv2 * v1dv1;
m_closest = p0 + (m_lambda1 * v1 + m_lambda2 * v2) / m_det;
m_dist2 = m_closest.length2();
return true;
}
return false;
}
void ReplaceMeFacet::silhouette(int index, const SimdVector3& w,
ReplaceMeEdgeBuffer& edgeBuffer)
{
if (!m_obsolete) {
if (m_closest.dot(w) < m_dist2) {
edgeBuffer.push_back(ReplaceMeEdge(this, index));
}
else {
m_obsolete = true; // Facet is visible
int next = incMod3(index);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
next = incMod3(next);
m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer);
}
}
}
inline int ReplaceMeEdge::getSource() const
{
return (*m_facet)[m_index];
}
inline int ReplaceMeEdge::getTarget() const
{
return (*m_facet)[incMod3(m_index)];
}
//#define DEBUG
const int MaxSupportPoints = 100;//1000;
const int MaxFacets = 200;//b2000;
static SimdPoint3 pBuf[MaxSupportPoints];
static SimdPoint3 qBuf[MaxSupportPoints];
static SimdVector3 yBuf[MaxSupportPoints];
static ReplaceMeFacet facetBuf[MaxFacets];
static int freeFacet = 0;
static ReplaceMeFacet *facetHeap[MaxFacets];
static int num_facets;
class ReplaceMeFacetComp {
public:
bool operator()(const ReplaceMeFacet *face1, const ReplaceMeFacet *face2)
{
return face1->getDist2() > face2->getDist2();
}
};
ReplaceMeFacetComp myFacetComp;
inline ReplaceMeFacet *addFacet(int i0, int i1, int i2,
SimdScalar lower2, SimdScalar upper2)
{
assert(i0 != i1 && i0 != i2 && i1 != i2);
if (freeFacet < MaxFacets)
{
ReplaceMeFacet *facet = new(&facetBuf[freeFacet++]) ReplaceMeFacet(i0, i1, i2);
#ifdef DEBUG
std::cout << "Facet " << i0 << ' ' << i1 << ' ' << i2;
#endif
if (facet->computeClosest(yBuf))
{
if (facet->isClosestInternal() &&
lower2 <= facet->getDist2() && facet->getDist2() <= upper2)
{
facetHeap[num_facets++] = facet;
ASSERT_MESSAGE(num_facets<MaxFacets,"Error in facet/pendepth");
std::push_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
#ifdef DEBUG
std::cout << " accepted" << std::endl;
#endif
}
else
{
#ifdef DEBUG
std::cout << " rejected, ";
if (!facet->isClosestInternal())
{
std::cout << "closest point not internal";
}
else if (lower2 > facet->getDist2())
{
std::cout << "facet is closer than orignal facet";
}
else
{
std::cout << "facet is further than upper bound";
}
std::cout << std::endl;
#endif
}
return facet;
}
}
return 0;
}
inline bool originInTetrahedron(const SimdVector3& p1, const SimdVector3& p2,
const SimdVector3& p3, const SimdVector3& p4)
{
SimdVector3 normal1 = (p2 - p1).cross(p3 - p1);
SimdVector3 normal2 = (p3 - p2).cross(p4 - p2);
SimdVector3 normal3 = (p4 - p3).cross(p1 - p3);
SimdVector3 normal4 = (p1 - p4).cross(p2 - p4);
return
(normal1.dot(p1) > SimdScalar(0.0)) != (normal1.dot(p4) > SimdScalar(0.0)) &&
(normal2.dot(p2) > SimdScalar(0.0)) != (normal2.dot(p1) > SimdScalar(0.0)) &&
(normal3.dot(p3) > SimdScalar(0.0)) != (normal3.dot(p2) > SimdScalar(0.0)) &&
(normal4.dot(p4) > SimdScalar(0.0)) != (normal4.dot(p3) > SimdScalar(0.0));
}
bool Solid3EpaPenetrationDepth::CalcPenDepth( SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb)
{
int num_verts = simplexSolver.getSimplex(pBuf, qBuf, yBuf);
switch (num_verts)
{
case 1:
// Touching contact. Yes, we have a collision,
// but no penetration.
return false;
case 2:
{
// We have a line segment inside the Minkowski sum containing the
// origin. Blow it up by adding three additional support points.
SimdVector3 dir = (yBuf[1] - yBuf[0]).normalized();
int axis = dir.furthestAxis();
static SimdScalar sin_60 = 0.8660254037f;//84438646763723170752941.22474487f;//13915890490986420373529;//
SimdQuaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, SimdScalar(0.5));
SimdMatrix3x3 rot_mat(rot);
SimdVector3 aux1 = dir.cross(SimdVector3(axis == 0, axis == 1, axis == 2));
SimdVector3 aux2 = rot_mat * aux1;
SimdVector3 aux3 = rot_mat * aux2;
pBuf[2] = transformA(convexA->LocalGetSupportingVertex(aux1*transformA.getBasis()));
qBuf[2] = transformB(convexB->LocalGetSupportingVertex((-aux1)*transformB.getBasis()));
yBuf[2] = pBuf[2] - qBuf[2];
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(aux2*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-aux2)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex(aux3*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex((-aux3)*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[1] = pBuf[4];
qBuf[1] = qBuf[4];
yBuf[1] = yBuf[4];
}
else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4]))
{
pBuf[0] = pBuf[4];
qBuf[0] = qBuf[4];
yBuf[0] = yBuf[4];
}
else
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
case 3:
{
// We have a triangle inside the Minkowski sum containing
// the origin. First blow it up.
SimdVector3 v1 = yBuf[1] - yBuf[0];
SimdVector3 v2 = yBuf[2] - yBuf[0];
SimdVector3 vv = v1.cross(v2);
pBuf[3] = transformA(convexA->LocalGetSupportingVertex(vv*transformA.getBasis()));
qBuf[3] = transformB(convexB->LocalGetSupportingVertex((-vv)*transformB.getBasis()));
yBuf[3] = pBuf[3] - qBuf[3];
pBuf[4] = transformA(convexA->LocalGetSupportingVertex((-vv)*transformA.getBasis()));
qBuf[4] = transformB(convexB->LocalGetSupportingVertex(vv*transformB.getBasis()));
yBuf[4] = pBuf[4] - qBuf[4];
if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4]))
{
pBuf[3] = pBuf[4];
qBuf[3] = qBuf[4];
yBuf[3] = yBuf[4];
}
else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// Origin not in initial polytope
return false;
}
num_verts = 4;
break;
}
}
// We have a tetrahedron inside the Minkowski sum containing
// the origin (if GJK did it's job right ;-)
if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3]))
{
// assert(false);
return false;
}
num_facets = 0;
freeFacet = 0;
ReplaceMeFacet *f0 = addFacet(0, 1, 2, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f1 = addFacet(0, 3, 1, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f2 = addFacet(0, 2, 3, SimdScalar(0.0), SIMD_INFINITY);
ReplaceMeFacet *f3 = addFacet(1, 3, 2, SimdScalar(0.0), SIMD_INFINITY);
if (!f0 || f0->getDist2() == SimdScalar(0.0) ||
!f1 || f1->getDist2() == SimdScalar(0.0) ||
!f2 || f2->getDist2() == SimdScalar(0.0) ||
!f3 || f3->getDist2() == SimdScalar(0.0))
{
return false;
}
f0->link(0, f1, 2);
f0->link(1, f3, 2);
f0->link(2, f2, 0);
f1->link(0, f2, 2);
f1->link(1, f3, 0);
f2->link(1, f3, 1);
if (num_facets == 0)
{
return false;
}
// at least one facet on the heap.
ReplaceMeEdgeBuffer edgeBuffer(20);
ReplaceMeFacet *facet = 0;
SimdScalar upper_bound2 = SIMD_INFINITY;
do {
facet = facetHeap[0];
std::pop_heap(&facetHeap[0], &facetHeap[num_facets], myFacetComp);
--num_facets;
if (!facet->isObsolete())
{
assert(facet->getDist2() > SimdScalar(0.0));
if (num_verts == MaxSupportPoints)
{
#ifdef DEBUG
std::cout << "Ouch, no convergence!!!" << std::endl;
#endif
ASSERT_MESSAGE(false,"Error: pendepth calc failed");
break;
}
pBuf[num_verts] = transformA(convexA->LocalGetSupportingVertex((facet->getClosest())*transformA.getBasis()));
qBuf[num_verts] = transformB(convexB->LocalGetSupportingVertex((-facet->getClosest())*transformB.getBasis()));
yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts];
int index = num_verts++;
SimdScalar far_dist2 = yBuf[index].dot(facet->getClosest());
// Make sure the support mapping is OK.
//assert(far_dist2 > SimdScalar(0.0));
//
// this is to avoid problems with implicit-sphere-touching contact
//
if (far_dist2 < SimdScalar(0.0))
{
return false;
}
GEN_set_min(upper_bound2, (far_dist2 * far_dist2) / facet->getDist2());
if (upper_bound2 <= ReplaceMeAccuracy::depth_tolerance * facet->getDist2()
#define CHECK_NEW_SUPPORT
#ifdef CHECK_NEW_SUPPORT
|| yBuf[index] == yBuf[(*facet)[0]]
|| yBuf[index] == yBuf[(*facet)[1]]
|| yBuf[index] == yBuf[(*facet)[2]]
#endif
)
{
break;
}
// Compute the silhouette cast by the new vertex
// Note that the new vertex is on the positive side
// of the current facet, so the current facet is will
// not be in the convex hull. Start local search
// from this facet.
facet->silhouette(yBuf[index], edgeBuffer);
if (edgeBuffer.empty())
{
return false;
}
ReplaceMeEdgeBuffer::const_iterator it = edgeBuffer.begin();
ReplaceMeFacet *firstFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!firstFacet)
{
break;
}
firstFacet->link(0, (*it).getFacet(), (*it).getIndex());
ReplaceMeFacet *lastFacet = firstFacet;
++it;
for (; it != edgeBuffer.end(); ++it)
{
ReplaceMeFacet *newFacet =
addFacet((*it).getTarget(), (*it).getSource(),
index, facet->getDist2(), upper_bound2);
if (!newFacet)
{
break;
}
if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex()))
{
break;
}
if (!newFacet->link(2, lastFacet, 1))
{
break;
}
lastFacet = newFacet;
}
if (it != edgeBuffer.end())
{
break;
}
firstFacet->link(2, lastFacet, 1);
}
}
while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2);
#ifdef DEBUG
std::cout << "#facets left = " << num_facets << std::endl;
#endif
v = facet->getClosest();
pa = facet->getClosestPoint(pBuf);
pb = facet->getClosestPoint(qBuf);
return true;
}

42
Extras/ExtraSolid35/Solid3EpaPenetrationDepth.h Normal file
View File

@@ -0,0 +1,42 @@
/*
* SOLID - Software Library for Interference Detection
*
* Copyright (C) 2001-2003 Dtecta. All rights reserved.
*
* This library may be distributed under the terms of the Q Public License
* (QPL) as defined by Trolltech AS of Norway and appearing in the file
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
* This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Commercial use or any other use of this library not covered by either
* the QPL or the GPL requires an additional license from Dtecta.
* Please contact info@dtecta.com for enquiries about the terms of commercial
* use of this library.
*/
#ifndef SOLID3_EPA_PENETRATION_DEPTH_H
#define SOLID3_EPA_PENETRATION_DEPTH_H
#include "NarrowPhaseCollision/ConvexPenetrationDepthSolver.h"
/// Solid3EpaPenetrationDepth contains the 'Expanding Polytope Algorithm' from Solid 3.5
class Solid3EpaPenetrationDepth : public ConvexPenetrationDepthSolver
{
public:
virtual bool CalcPenDepth(SimplexSolverInterface& simplexSolver,
ConvexShape* convexA,ConvexShape* convexB,
const SimdTransform& transformA,const SimdTransform& transformB,
SimdVector3& v, SimdPoint3& pa, SimdPoint3& pb);
};
#endif //SOLID3_EPA_PENETRATION_DEPTH_H

450
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.cpp Normal file
View File

@@ -0,0 +1,450 @@
/*
* SOLID - Software Library for Interference Detection
*
* Copyright (C) 2001-2003 Dtecta. All rights reserved.
*
* This library may be distributed under the terms of the Q Public License
* (QPL) as defined by Trolltech AS of Norway and appearing in the file
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
* This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Commercial use or any other use of this library not covered by either
* the QPL or the GPL requires an additional license from Dtecta.
* Please contact info@dtecta.com for enquiries about the terms of commercial
* use of this library.
*/
#include "Solid3JohnsonSimplexSolver.h"
#include "GEN_MinMax.h"
//#define USE_BACKUP_PROCEDURE
//#define FAST_CLOSEST
Solid3JohnsonSimplexSolver::Solid3JohnsonSimplexSolver()
:
m_bits1(0x0),
m_all_bits(0x0)
{
}
Solid3JohnsonSimplexSolver::~Solid3JohnsonSimplexSolver()
{
}
void Solid3JohnsonSimplexSolver::reset()
{
m_bits1 = 0x0;
m_all_bits = 0x0;
}
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w)
{
assert(!fullSimplex());
m_last = 0;
m_last_bit = 0x1;
while (contains(m_bits1, m_last_bit))
{
++m_last;
m_last_bit <<= 1;
}
m_y[m_last] = w;
m_ylen2[m_last] = w.length2();
m_all_bits = m_bits1 | m_last_bit;
update_cache();
compute_det();
}
void Solid3JohnsonSimplexSolver::addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q)
{
addVertex(w);
m_p[m_last] = p;
m_q[m_last] = q;
}
bool Solid3JohnsonSimplexSolver::emptySimplex() const
{
return m_bits1 == 0x0;
}
bool Solid3JohnsonSimplexSolver::fullSimplex() const
{
return m_bits1 == 0xf;
}
SimdScalar Solid3JohnsonSimplexSolver::maxVertex()
{
return m_maxlen2;
}
bool Solid3JohnsonSimplexSolver::closest(SimdVector3& v)
{
#ifdef FAST_CLOSEST
T_Bits s;
for (s = m_bits1; s != 0x0; --s)
{
if (subseteq(s, m_bits1) && valid(s | m_last_bit))
{
//update bits !
m_bits1 = s | m_last_bit;
compute_vector(m_bits1, v);
return true;
}
}
if (valid(m_last_bit))
{
//update bits !
m_bits1 = m_last_bit;
m_maxlen2 = m_ylen2[m_last];
v = m_y[m_last];
return true;
}
#else
T_Bits s;
for (s = m_all_bits; s != 0x0; --s)
{
if (subseteq(s, m_all_bits) && valid(s))
{
m_bits1 = s;
compute_vector(m_bits1, v);
return true;
}
}
#endif
// Original GJK calls the backup procedure at this point.
#ifdef USE_BACKUP_PROCEDURE
backup_closest(SimdVector3& v);
#endif
return false;
}
int Solid3JohnsonSimplexSolver::getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const
{
int num_verts = 0;
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_bits1, bit))
{
pBuf[num_verts] = m_p[i];
qBuf[num_verts] = m_q[i];
yBuf[num_verts] = m_y[i];
#ifdef DEBUG
std::cout << "Point " << i << " = " << m_y[i] << std::endl;
#endif
++num_verts;
}
}
return num_verts;
}
bool Solid3JohnsonSimplexSolver::inSimplex(const SimdVector3& w)
{
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_all_bits, bit) && w == m_y[i])
{
return true;
}
}
return false;
}
void Solid3JohnsonSimplexSolver::backup_closest(SimdVector3& v)
{
SimdScalar min_dist2 = SIMD_INFINITY;
T_Bits s;
for (s = m_all_bits; s != 0x0; --s)
{
if (subseteq(s, m_all_bits) && proper(s))
{
SimdVector3 u;
compute_vector(s, u);
SimdScalar dist2 = u.length2();
if (dist2 < min_dist2)
{
min_dist2 = dist2;
//update bits !
m_bits1 = s;
v = u;
}
}
}
}
void Solid3JohnsonSimplexSolver::compute_points(SimdPoint3& p1, SimdPoint3& p2)
{
SimdScalar sum = SimdScalar(0.0);
p1.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
p2.setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_bits1, bit))
{
sum += m_det[m_bits1][i];
p1 += m_p[i] * m_det[m_bits1][i];
p2 += m_q[i] * m_det[m_bits1][i];
}
}
assert(sum > SimdScalar(0.0));
SimdScalar s = SimdScalar(1.0) / sum;
p1 *= s;
p2 *= s;
}
int Solid3JohnsonSimplexSolver::numVertices() const
{
int numverts = 0;
int i,bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_bits1, bit))
{
numverts++;
}
}
return numverts;
}
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//////////////////////////////////////////////////////////////
//internal
inline void Solid3JohnsonSimplexSolver::update_cache()
{
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_bits1, bit))
{
m_edge[i][m_last] = m_y[i] - m_y[m_last];
m_edge[m_last][i] = -m_edge[i][m_last];
#ifdef JOHNSON_ROBUST
m_norm[i][m_last] = m_norm[m_last][i] = m_edge[i][m_last].length2();
#endif
}
}
}
bool Solid3JohnsonSimplexSolver::valid(T_Bits s)
{
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(m_all_bits, bit))
{
if (contains(s, bit))
{
if (m_det[s][i] <= SimdScalar(0.0))
{
return false;
}
}
else if (m_det[s | bit][i] > SimdScalar(0.0))
{
return false;
}
}
}
return true;
}
bool Solid3JohnsonSimplexSolver::proper(T_Bits s)
{
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(s, bit) && m_det[s][i] <= SimdScalar(0.0))
{
return false;
}
}
return true;
}
void Solid3JohnsonSimplexSolver::compute_vector(T_Bits s, SimdVector3& v)
{
m_maxlen2 = SimdScalar(0.0);
SimdScalar sum = SimdScalar(0.0);
v .setValue(SimdScalar(0.0), SimdScalar(0.0), SimdScalar(0.0));
int i;
T_Bits bit;
for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1)
{
if (contains(s, bit))
{
sum += m_det[s][i];
GEN_set_max(m_maxlen2, m_ylen2[i]);
v += m_y[i] * m_det[s][i];
}
}
assert(sum > SimdScalar(0.0));
v /= sum;
}
#ifdef JOHNSON_ROBUST
inline void Solid3JohnsonSimplexSolver::compute_det()
{
m_det[m_last_bit][m_last] = 1;
int i;
T_Bits si;
for (i = 0, si = 0x1; i < 4; ++i, si <<= 1)
{
if (contains(m_bits1, si))
{
T_Bits s2 = si | m_last_bit;
m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]);
m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]);
int j;
T_Bits sj;
for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1)
{
if (contains(m_bits1, sj))
{
int k;
T_Bits s3 = sj | s2;
k = m_norm[i][j] < m_norm[m_last][j] ? i : m_last;
m_det[s3][j] = m_det[s2][i] * m_edge[k][j].dot(m_y[i]) +
m_det[s2][m_last] * m_edge[k][j].dot(m_y[m_last]);
k = m_norm[j][i] < m_norm[m_last][i] ? j : m_last;
m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[k][i].dot(m_y[j]) +
m_det[sj|m_last_bit][m_last] * m_edge[k][i].dot(m_y[m_last]);
k = m_norm[i][m_last] < m_norm[j][m_last] ? i : j;
m_det[s3][m_last] = m_det[sj|si][j] * m_edge[k][m_last].dot(m_y[j]) +
m_det[sj|si][i] * m_edge[k][m_last].dot(m_y[i]);
}
}
}
}
if (m_all_bits == 0xf)
{
int k;
k = m_norm[1][0] < m_norm[2][0] ? (m_norm[1][0] < m_norm[3][0] ? 1 : 3) : (m_norm[2][0] < m_norm[3][0] ? 2 : 3);
m_det[0xf][0] = m_det[0xe][1] * m_edge[k][0].dot(m_y[1]) +
m_det[0xe][2] * m_edge[k][0].dot(m_y[2]) +
m_det[0xe][3] * m_edge[k][0].dot(m_y[3]);
k = m_norm[0][1] < m_norm[2][1] ? (m_norm[0][1] < m_norm[3][1] ? 0 : 3) : (m_norm[2][1] < m_norm[3][1] ? 2 : 3);
m_det[0xf][1] = m_det[0xd][0] * m_edge[k][1].dot(m_y[0]) +
m_det[0xd][2] * m_edge[k][1].dot(m_y[2]) +
m_det[0xd][3] * m_edge[k][1].dot(m_y[3]);
k = m_norm[0][2] < m_norm[1][2] ? (m_norm[0][2] < m_norm[3][2] ? 0 : 3) : (m_norm[1][2] < m_norm[3][2] ? 1 : 3);
m_det[0xf][2] = m_det[0xb][0] * m_edge[k][2].dot(m_y[0]) +
m_det[0xb][1] * m_edge[k][2].dot(m_y[1]) +
m_det[0xb][3] * m_edge[k][2].dot(m_y[3]);
k = m_norm[0][3] < m_norm[1][3] ? (m_norm[0][3] < m_norm[2][3] ? 0 : 2) : (m_norm[1][3] < m_norm[2][3] ? 1 : 2);
m_det[0xf][3] = m_det[0x7][0] * m_edge[k][3].dot(m_y[0]) +
m_det[0x7][1] * m_edge[k][3].dot(m_y[1]) +
m_det[0x7][2] * m_edge[k][3].dot(m_y[2]);
}
}
#else //JOHNSON_ROBUST
inline void Solid3JohnsonSimplexSolver::compute_det()
{
m_det[m_last_bit][m_last] = 1;
int i;
T_Bits si;
for (i = 0, si = 0x1; i < 4; ++i, si <<= 1)
{
if (contains(m_bits1, si))
{
T_Bits s2 = si | m_last_bit;
m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]);
m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]);
int j;
T_Bits sj;
for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1)
{
if (contains(m_bits1, sj))
{
T_Bits s3 = sj | s2;
m_det[s3][j] = m_det[s2][i] * m_edge[i][j].dot(m_y[i]) +
m_det[s2][m_last] * m_edge[i][j].dot(m_y[m_last]);
m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[j][i].dot(m_y[j]) +
m_det[sj|m_last_bit][m_last] * m_edge[j][i].dot(m_y[m_last]);
m_det[s3][m_last] = m_det[sj|si][j] * m_edge[j][m_last].dot(m_y[j]) +
m_det[sj|si][i] * m_edge[j][m_last].dot(m_y[i]);
}
}
}
}
if (m_all_bits == 0xf)
{
m_det[0xf][0] = m_det[0xe][1] * m_edge[1][0].dot(m_y[1]) +
m_det[0xe][2] * m_edge[1][0].dot(m_y[2]) +
m_det[0xe][3] * m_edge[1][0].dot(m_y[3]);
m_det[0xf][1] = m_det[0xd][0] * m_edge[0][1].dot(m_y[0]) +
m_det[0xd][2] * m_edge[0][1].dot(m_y[2]) +
m_det[0xd][3] * m_edge[0][1].dot(m_y[3]);
m_det[0xf][2] = m_det[0xb][0] * m_edge[0][2].dot(m_y[0]) +
m_det[0xb][1] * m_edge[0][2].dot(m_y[1]) +
m_det[0xb][3] * m_edge[0][2].dot(m_y[3]);
m_det[0xf][3] = m_det[0x7][0] * m_edge[0][3].dot(m_y[0]) +
m_det[0x7][1] * m_edge[0][3].dot(m_y[1]) +
m_det[0x7][2] * m_edge[0][3].dot(m_y[2]);
}
}
#endif //JOHNSON_ROBUST

105
Extras/ExtraSolid35/Solid3JohnsonSimplexSolver.h Normal file
View File

@@ -0,0 +1,105 @@
/*
* SOLID - Software Library for Interference Detection
*
* Copyright (C) 2001-2003 Dtecta. All rights reserved.
*
* This library may be distributed under the terms of the Q Public License
* (QPL) as defined by Trolltech AS of Norway and appearing in the file
* LICENSE.QPL included in the packaging of this file.
*
* This library may be distributed and/or modified under the terms of the
* GNU General Public License (GPL) version 2 as published by the Free Software
* Foundation and appearing in the file LICENSE.GPL included in the
* packaging of this file.
*
* This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*
* Commercial use or any other use of this library not covered by either
* the QPL or the GPL requires an additional license from Dtecta.
* Please contact info@dtecta.com for enquiries about the terms of commercial
* use of this library.
*/
#ifndef SOLID3JOHNSON_SIMPLEX_SOLVER_H
#define SOLID3JOHNSON_SIMPLEX_SOLVER_H
#include "NarrowPhaseCollision/SimplexSolverInterface.h"
//#define JOHNSON_ROBUST
/// Solid3JohnsonSimplexSolver contains Johnson subdistance algorithm from Solid 3.5 library
class Solid3JohnsonSimplexSolver : public SimplexSolverInterface
{
private:
typedef unsigned int T_Bits;
inline static bool subseteq(T_Bits a, T_Bits b) { return (a & b) == a; }
inline static bool contains(T_Bits a, T_Bits b) { return (a & b) != 0x0; }
void update_cache();
void compute_det();
bool valid(T_Bits s);
bool proper(T_Bits s);
void compute_vector(T_Bits s, SimdVector3& v);
SimdScalar m_det[16][4]; // cached sub-determinants
SimdVector3 m_edge[4][4];
#ifdef JOHNSON_ROBUST
SimdScalar m_norm[4][4];
#endif
SimdPoint3 m_p[4]; // support points of object A in local coordinates
SimdPoint3 m_q[4]; // support points of object B in local coordinates
SimdVector3 m_y[4]; // support points of A - B in world coordinates
SimdScalar m_ylen2[4]; // Squared lengths support points y
SimdScalar m_maxlen2; // Maximum squared length to a vertex of the current
// simplex
T_Bits m_bits1; // identifies current simplex
T_Bits m_last; // identifies last found support point
T_Bits m_last_bit; // m_last_bit == 0x1 << last
T_Bits m_all_bits; // m_all_bits == m_bits | m_last_bit
private:
void addVertex(const SimdVector3& w);
public:
Solid3JohnsonSimplexSolver();
virtual ~Solid3JohnsonSimplexSolver();
virtual void reset();
virtual void addVertex(const SimdVector3& w, const SimdPoint3& p, const SimdPoint3& q);
virtual bool closest(SimdVector3& v);
virtual SimdScalar maxVertex();
virtual bool fullSimplex() const;
virtual int getSimplex(SimdPoint3 *pBuf, SimdPoint3 *qBuf, SimdVector3 *yBuf) const;
virtual bool inSimplex(const SimdVector3& w);
virtual void backup_closest(SimdVector3& v) ;
virtual bool emptySimplex() const ;
virtual void compute_points(SimdPoint3& p1, SimdPoint3& p2) ;
virtual int numVertices() const ;
};
#endif //SOLID3JOHNSON_SIMPLEX_SOLVER_H