RomkaZVO/FC1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

123

Browse Source
This commit is contained in:
romkazvo
2023-08-07 19:29:24 +08:00
commit 34d6c5d489
4832 changed files with 1389451 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

57
CryPhysics/CryPhysics.cpp Normal file
View File

@@ -0,0 +1,57 @@
// CryPhysics.cpp : Defines the entry point for the DLL application.
//
#include "stdafx.h"
//#include <float.h>
#ifndef GAMECUBE
#ifndef _XBOX
//#if !defined(LINUX)
_ACCESS_POOL;
//#endif//LINUX
#endif //_XBOX
#include <CrtDebugStats.h>
#endif
#include "IPhysics.h"
#include "geoman.h"
#include "bvtree.h"
#include "geometry.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "physicalworld.h"
float g_costab[SINCOSTABSZ],g_sintab[SINCOSTABSZ];
//////////////////////////////////////////////////////////////////////////
// Pointer to Global ISystem.
static ISystem* gISystem = 0;
ISystem* GetISystem()
{
return gISystem;
}
//////////////////////////////////////////////////////////////////////////
#ifdef _WIN32
BOOL APIENTRY DllMain(HANDLE hModule, DWORD ul_reason_for_call, LPVOID lpReserved)
{
return TRUE;
}
#endif
#ifndef _XBOX
CRYPHYSICS_API IPhysicalWorld *CreatePhysicalWorld(ISystem *pSystem)
#else
IPhysicalWorld *CreatePhysicalWorld(ISystem *pSystem)
#endif
{
gISystem = pSystem;
g_bHasSSE = (pSystem->GetCPUFlags() & CPUF_SSE)!=0;
for(int i=0; i<SINCOSTABSZ; i++) {
g_costab[i] = cosf(i*(pi*0.5f/SINCOSTABSZ));
g_sintab[i] = sinf(i*(pi*0.5f/SINCOSTABSZ));
}
//_controlfp(_EM_ZERODIVIDE,_MCW_EM);
return new CPhysicalWorld(pSystem->GetILog());
}

22
CryPhysics/CryPhysics.h Normal file
View File

@@ -0,0 +1,22 @@
#ifndef cryphysics_h
#define cryphysics_h
#pragma once
#ifndef _XBOX
#ifdef PHYSICS_EXPORTS
#define CRYPHYSICS_API __declspec(dllexport)
#else
#define CRYPHYSICS_API __declspec(dllimport)
#define vector_class Vec3d
#endif
#else
#define CRYPHYSICS_API
#endif
#include "utils.h"
#include "primitives.h"
#include "physinterface.h"
extern "C" CRYPHYSICS_API IPhysicalWorld *CreatePhysicalWorld();
#endif

634
CryPhysics/CryPhysics.vcproj Normal file
View File

@@ -0,0 +1,634 @@
<?xml version="1.0" encoding="Windows-1252"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.10"
Name="CryPhysics"
ProjectGUID="{9F51A48A-EE94-4A94-AABA-C0C18B7B8774}"
SccProjectName="Perforce Project"
SccAuxPath=""
SccLocalPath="."
SccProvider="MSSCCI:Perforce SCM">
<Platforms>
<Platform
Name="Win32"/>
</Platforms>
<Configurations>
<Configuration
Name="Profile|Win32"
OutputDirectory="D:\Games\FC\Bin32"
IntermediateDirectory="Profile"
ConfigurationType="2"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2"
WholeProgramOptimization="TRUE">
<Tool
Name="VCCLCompilerTool"
Optimization="3"
GlobalOptimizations="TRUE"
InlineFunctionExpansion="2"
EnableIntrinsicFunctions="TRUE"
FavorSizeOrSpeed="1"
OmitFramePointers="TRUE"
EnableFiberSafeOptimizations="FALSE"
OptimizeForProcessor="3"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="NDEBUG;WIN32;_WINDOWS;_USRDLL;PHYSICS_EXPORTS;PIII_SSE;ENTITY_PROFILER_ENABLED"
StringPooling="TRUE"
RuntimeLibrary="2"
BufferSecurityCheck="FALSE"
EnableFunctionLevelLinking="FALSE"
EnableEnhancedInstructionSet="0"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile=".\Profile/CryPhysics.pch"
AssemblerListingLocation=".\Profile/"
ObjectFile=".\Profile/"
ProgramDataBaseFileName=".\Profile/"
BrowseInformation="1"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="3"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalOptions="/MACHINE:I386"
AdditionalDependencies="odbc32.lib odbccp32.lib"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
BaseAddress="0x35000000"/>
<Tool
Name="VCMIDLTool"
PreprocessorDefinitions="NDEBUG"
MkTypLibCompatible="TRUE"
SuppressStartupBanner="TRUE"
TargetEnvironment="1"
TypeLibraryName="C:\MasterCD/CryPhysics.tlb"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="NDEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Debug|Win32"
OutputDirectory="D:\Games\FC\Bin32"
IntermediateDirectory=".\Debug"
ConfigurationType="2"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
OptimizeForProcessor="0"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_DEBUG;WIN32;_WINDOWS;_USRDLL;PHYSICS_EXPORTS;PIII_SSE;ENTITY_PROFILER_ENABLED"
MinimalRebuild="TRUE"
BasicRuntimeChecks="0"
RuntimeLibrary="3"
BufferSecurityCheck="TRUE"
EnableFunctionLevelLinking="TRUE"
ForceConformanceInForLoopScope="TRUE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile=".\Debug/CryPhysics.pch"
AssemblerListingLocation=".\Debug/"
ObjectFile=".\Debug/"
ProgramDataBaseFileName=".\Debug/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="4"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalOptions="/MACHINE:I386"
AdditionalDependencies="odbc32.lib odbccp32.lib"
LinkIncremental="2"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
ImportLibrary="$(IntDir)/$(TargetName).lib"/>
<Tool
Name="VCMIDLTool"
PreprocessorDefinitions="_DEBUG"
MkTypLibCompatible="TRUE"
SuppressStartupBanner="TRUE"
TargetEnvironment="1"
TypeLibraryName=".\Debug/CryPhysics.tlb"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="_DEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Release|Win32"
OutputDirectory="D:\Games\FC\Bin32"
IntermediateDirectory="Release"
ConfigurationType="2"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2"
WholeProgramOptimization="TRUE">
<Tool
Name="VCCLCompilerTool"
Optimization="3"
GlobalOptimizations="TRUE"
InlineFunctionExpansion="2"
EnableIntrinsicFunctions="TRUE"
FavorSizeOrSpeed="1"
OmitFramePointers="TRUE"
EnableFiberSafeOptimizations="FALSE"
OptimizeForProcessor="3"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_RELEASE;NDEBUG;WIN32;_WINDOWS;_USRDLL;PHYSICS_EXPORTS;PIII_SSE"
StringPooling="TRUE"
RuntimeLibrary="2"
BufferSecurityCheck="FALSE"
EnableFunctionLevelLinking="FALSE"
EnableEnhancedInstructionSet="0"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile=".\Release/CryPhysics.pch"
AssemblerListingLocation=".\Release/"
ObjectFile=".\Release/"
ProgramDataBaseFileName=".\Release/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalOptions="/MACHINE:I386"
AdditionalDependencies="odbc32.lib odbccp32.lib"
OutputFile=".\Release/CryPhysics.dll"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
ProgramDatabaseFile=".\Release/CryPhysics.pdb"
ImportLibrary="$(IntDir)/$(TargetName).lib"/>
<Tool
Name="VCMIDLTool"
PreprocessorDefinitions="NDEBUG"
MkTypLibCompatible="TRUE"
SuppressStartupBanner="TRUE"
TargetEnvironment="1"
TypeLibraryName=".\Release/CryPhysics.tlb"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="NDEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Debug64|Win32"
OutputDirectory="D:\Games\FC\Bin32"
IntermediateDirectory="Debug64"
ConfigurationType="2"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_DEBUG;WIN64;_WINDOWS;_USRDLL;PHYSICS_EXPORTS"
BasicRuntimeChecks="0"
RuntimeLibrary="1"
BufferSecurityCheck="FALSE"
ForceConformanceInForLoopScope="TRUE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile="$(IntDir)/$(ProjectName).pch"
AssemblerListingLocation="$(IntDir)/"
ObjectFile="$(IntDir)/"
ProgramDataBaseFileName="$(IntDir)/$(ProjectName).pdb"
WarningLevel="3"
SuppressStartupBanner="TRUE"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="3"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalOptions="/MACHINE:AMD64"
AdditionalDependencies="../CryCommon/fSinCos64.lib"
OutputFile="$(OutDir)/$(ProjectName).dll"
LinkIncremental="2"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
ProgramDatabaseFile="$(OutDir)/$(ProjectName).pdb"
LargeAddressAware="2"
ImportLibrary="$(OutDir)/$(ProjectName).lib"/>
<Tool
Name="VCMIDLTool"
PreprocessorDefinitions="_DEBUG"
MkTypLibCompatible="TRUE"
SuppressStartupBanner="TRUE"
TargetEnvironment="1"
TypeLibraryName=".\Debug/CryPhysics.tlb"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="_DEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
<Configuration
Name="Release64|Win32"
OutputDirectory="D:\Games\FC\Bin32"
IntermediateDirectory="Release64"
ConfigurationType="2"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2"
WholeProgramOptimization="TRUE">
<Tool
Name="VCCLCompilerTool"
AdditionalOptions="/fp:fast /GL"
Optimization="2"
GlobalOptimizations="TRUE"
InlineFunctionExpansion="2"
EnableIntrinsicFunctions="TRUE"
FavorSizeOrSpeed="1"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_RELEASE;NDEBUG;WIN64;WIN32;_AMD64_;_WINDOWS;_USRDLL;PHYSICS_EXPORTS"
BasicRuntimeChecks="0"
RuntimeLibrary="2"
BufferSecurityCheck="FALSE"
ForceConformanceInForLoopScope="TRUE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile="$(IntDir)/$(ProjectName).pch"
AssemblerListingLocation="$(IntDir)/"
ObjectFile="$(IntDir)/"
ProgramDataBaseFileName="$(IntDir)/$(ProjectName).pdb"
WarningLevel="3"
SuppressStartupBanner="TRUE"
Detect64BitPortabilityProblems="TRUE"
DebugInformationFormat="3"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalDependencies="../CryCommon/fSinCos64.lib"
OutputFile="$(OutDir)/$(ProjectName).dll"
LinkIncremental="1"
GenerateDebugInformation="TRUE"
ProgramDatabaseFile="$(OutDir)/$(ProjectName).pdb"
LargeAddressAware="2"
ImportLibrary="$(OutDir)/$(ProjectName).lib"/>
<Tool
Name="VCMIDLTool"
PreprocessorDefinitions="_DEBUG"
MkTypLibCompatible="TRUE"
SuppressStartupBanner="TRUE"
TargetEnvironment="1"
TypeLibraryName=".\Debug/CryPhysics.tlb"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="VCResourceCompilerTool"
PreprocessorDefinitions="_DEBUG"
Culture="1033"/>
<Tool
Name="VCWebServiceProxyGeneratorTool"/>
<Tool
Name="VCXMLDataGeneratorTool"/>
<Tool
Name="VCWebDeploymentTool"/>
<Tool
Name="VCManagedWrapperGeneratorTool"/>
<Tool
Name="VCAuxiliaryManagedWrapperGeneratorTool"/>
</Configuration>
</Configurations>
<References>
</References>
<Files>
<Filter
Name="Dynamics"
Filter="">
<File
RelativePath="articulatedentity.cpp">
</File>
<File
RelativePath="articulatedentity.h">
</File>
<File
RelativePath=".\livingentity.cpp">
</File>
<File
RelativePath=".\livingentity.h">
</File>
<File
RelativePath=".\particleentity.cpp">
</File>
<File
RelativePath=".\particleentity.h">
</File>
<File
RelativePath=".\physicalentity.cpp">
</File>
<File
RelativePath=".\physicalentity.h">
</File>
<File
RelativePath="physicalplaceholder.cpp">
</File>
<File
RelativePath="physicalplaceholder.h">
</File>
<File
RelativePath=".\physicalworld.cpp">
</File>
<File
RelativePath="physicalworld.h">
</File>
<File
RelativePath=".\rigidbody.cpp">
</File>
<File
RelativePath=".\rigidbody.h">
</File>
<File
RelativePath=".\rigidentity.cpp">
</File>
<File
RelativePath=".\rigidentity.h">
</File>
<File
RelativePath="ropeentity.cpp">
</File>
<File
RelativePath="ropeentity.h">
</File>
<File
RelativePath="softentity.cpp">
</File>
<File
RelativePath="softentity.h">
</File>
<File
RelativePath="wheeledvehicleentity.cpp">
</File>
<File
RelativePath="wheeledvehicleentity.h">
</File>
</Filter>
<Filter
Name="Utils"
Filter="">
<File
RelativePath="boolean2d.cpp">
</File>
<File
RelativePath=".\matrixnm.cpp">
</File>
<File
RelativePath=".\matrixnm.h">
</File>
<File
RelativePath="polynomial.h">
</File>
<File
RelativePath=".\qhull.cpp">
</File>
<File
RelativePath="quotient.h">
</File>
<File
RelativePath=".\utils.cpp">
</File>
<File
RelativePath=".\utils.h">
</File>
<File
RelativePath="vectorn.h">
</File>
</Filter>
<Filter
Name="Collisions"
Filter="">
<File
RelativePath="aabbtree.cpp">
</File>
<File
RelativePath="aabbtree.h">
</File>
<File
RelativePath="boxgeom.cpp">
</File>
<File
RelativePath="boxgeom.h">
</File>
<File
RelativePath="bvtree.h">
</File>
<File
RelativePath="cylindergeom.cpp">
</File>
<File
RelativePath="cylindergeom.h">
</File>
<File
RelativePath="geoman.cpp">
</File>
<File
RelativePath="geoman.h">
</File>
<File
RelativePath="geometries.h">
</File>
<File
RelativePath="geometry.cpp">
</File>
<File
RelativePath="geometry.h">
</File>
<File
RelativePath="heightfieldbv.cpp">
</File>
<File
RelativePath="heightfieldbv.h">
</File>
<File
RelativePath="heightfieldgeom.cpp">
</File>
<File
RelativePath="heightfieldgeom.h">
</File>
<File
RelativePath="intersectionchecks.cpp">
</File>
<File
RelativePath="intersectionchecks.h">
</File>
<File
RelativePath="linunprojectionchecks.cpp">
</File>
<File
RelativePath=".\obbtree.cpp">
</File>
<File
RelativePath=".\obbtree.h">
</File>
<File
RelativePath="overlapchecks.cpp">
</File>
<File
RelativePath="overlapchecks.h">
</File>
<File
RelativePath="raybv.cpp">
</File>
<File
RelativePath="raybv.h">
</File>
<File
RelativePath="raygeom.cpp">
</File>
<File
RelativePath="raygeom.h">
</File>
<File
RelativePath="rotunprojectionchecks.cpp">
</File>
<File
RelativePath="singleboxtree.cpp">
</File>
<File
RelativePath="singleboxtree.h">
</File>
<File
RelativePath="spheregeom.cpp">
</File>
<File
RelativePath="spheregeom.h">
</File>
<File
RelativePath="trimesh.cpp">
</File>
<File
RelativePath="trimesh.h">
</File>
<File
RelativePath="unprojectionchecks.h">
</File>
</Filter>
<File
RelativePath=".\CryPhysics.cpp">
</File>
<File
RelativePath="IPhysics.h">
</File>
<File
RelativePath=".\StdAfx.cpp">
<FileConfiguration
Name="Profile|Win32">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Debug|Win32">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Release|Win32">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Debug64|Win32">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Release64|Win32">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
</File>
<File
RelativePath=".\StdAfx.h">
</File>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

10
CryPhysics/CryPhysics.vcproj.vspscc Normal file
View File

@@ -0,0 +1,10 @@
""
{
"FILE_VERSION" = "9237"
"ENLISTMENT_CHOICE" = "NEVER"
"PROJECT_FILE_RELATIVE_PATH" = ""
"NUMBER_OF_EXCLUDED_FILES" = "0"
"ORIGINAL_PROJECT_FILE_PATH" = "file:F:\\Crytek\\CryPhysics\\CryPhysics.vcproj"
"NUMBER_OF_NESTED_PROJECTS" = "0"
"SOURCE_CONTROL_SETTINGS_PROVIDER" = "PROJECT"
}

396
CryPhysics/CryPhysics_XBox.vcproj Normal file
View File

@@ -0,0 +1,396 @@
<?xml version="1.0" encoding = "windows-1251"?>
<VisualStudioProject
ProjectType="Visual C++"
Version="7.00"
Name="CryPhysics_XBox"
SccProjectName="&quot;$/Game01/CryPhysics&quot;, NEPAAAAA"
SccAuxPath=""
SccLocalPath="."
SccProvider="MSSCCI:Microsoft Visual SourceSafe">
<Platforms>
<Platform
Name="Xbox"/>
</Platforms>
<Configurations>
<Configuration
Name="Profile|Xbox"
OutputDirectory="Profile"
IntermediateDirectory="Profile"
ConfigurationType="1"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="3"
GlobalOptimizations="TRUE"
InlineFunctionExpansion="2"
EnableIntrinsicFunctions="TRUE"
FavorSizeOrSpeed="1"
OmitFramePointers="TRUE"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="NDEBUG;_XBOX;_LIB;PHYSICS_EXPORTS"
StringPooling="TRUE"
BufferSecurityCheck="FALSE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile=".\Profile/CryPhysics.pch"
AssemblerListingLocation=".\Profile/"
ObjectFile=".\Profile/"
ProgramDataBaseFileName=".\Profile/"
BrowseInformation="1"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="3"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLinkerTool"
AdditionalOptions="/MACHINE:I386"
AdditionalDependencies="odbc32.lib odbccp32.lib"
OutputFile="C:\MasterCD/CryPhysics.dll"
LinkIncremental="1"
SuppressStartupBanner="TRUE"
GenerateDebugInformation="TRUE"
GenerateMapFile="TRUE"
MapFileName=".\Profile/CryPhysics.map"
ImportLibrary="C:\MasterCD/CryPhysics.lib"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
<Tool
Name="XboxDeploymentTool"/>
<Tool
Name="XboxImageTool"/>
</Configuration>
<Configuration
Name="Debug|Xbox"
OutputDirectory="Debug_XBox"
IntermediateDirectory="Debug_XBox"
ConfigurationType="4"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2">
<Tool
Name="VCCLCompilerTool"
Optimization="0"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_DEBUG;_XBOX;_LIB;PHYSICS_EXPORTS"
BasicRuntimeChecks="3"
RuntimeLibrary="5"
BufferSecurityCheck="TRUE"
EnableFunctionLevelLinking="TRUE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile="$(IntDir)/CryPhysics.pch"
AssemblerListingLocation=".\Debug_XBox/"
ObjectFile=".\Debug_XBox/"
ProgramDataBaseFileName=".\Debug_XBox/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="3"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/CryPhysics.lib"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
</Configuration>
<Configuration
Name="Release|Xbox"
OutputDirectory="Release_XBox"
IntermediateDirectory="Release_XBox"
ConfigurationType="4"
UseOfMFC="0"
ATLMinimizesCRunTimeLibraryUsage="FALSE"
CharacterSet="2"
WholeProgramOptimization="TRUE">
<Tool
Name="VCCLCompilerTool"
Optimization="3"
GlobalOptimizations="TRUE"
InlineFunctionExpansion="2"
EnableIntrinsicFunctions="TRUE"
FavorSizeOrSpeed="1"
OmitFramePointers="TRUE"
AdditionalIncludeDirectories="..\CryCommon"
PreprocessorDefinitions="_RELEASE;NDEBUG;_XBOX;_LIB;PHYSICS_EXPORTS"
StringPooling="TRUE"
RuntimeLibrary="4"
BufferSecurityCheck="FALSE"
EnableFunctionLevelLinking="TRUE"
UsePrecompiledHeader="3"
PrecompiledHeaderThrough="stdafx.h"
PrecompiledHeaderFile="$(IntDir)/CryPhysics.pch"
AssemblerListingLocation="$(IntDir)/"
ObjectFile="$(IntDir)/"
ProgramDataBaseFileName="$(IntDir)/"
WarningLevel="3"
SuppressStartupBanner="TRUE"
DebugInformationFormat="2"
CompileAs="0"/>
<Tool
Name="VCCustomBuildTool"/>
<Tool
Name="VCLibrarianTool"
OutputFile="$(OutDir)/CryPhysics.lib"/>
<Tool
Name="VCPostBuildEventTool"/>
<Tool
Name="VCPreBuildEventTool"/>
<Tool
Name="VCPreLinkEventTool"/>
</Configuration>
</Configurations>
<Files>
<Filter
Name="Dynamics"
Filter="">
<File
RelativePath="articulatedentity.cpp">
</File>
<File
RelativePath="articulatedentity.h">
</File>
<File
RelativePath=".\livingentity.cpp">
</File>
<File
RelativePath=".\livingentity.h">
</File>
<File
RelativePath=".\particleentity.cpp">
</File>
<File
RelativePath=".\particleentity.h">
</File>
<File
RelativePath=".\physicalentity.cpp">
</File>
<File
RelativePath=".\physicalentity.h">
</File>
<File
RelativePath=".\physicalworld.cpp">
</File>
<File
RelativePath="physicalworld.h">
</File>
<File
RelativePath=".\rigidbody.cpp">
</File>
<File
RelativePath=".\rigidbody.h">
</File>
<File
RelativePath=".\rigidentity.cpp">
</File>
<File
RelativePath=".\rigidentity.h">
</File>
<File
RelativePath="ropeentity.cpp">
</File>
<File
RelativePath="ropeentity.h">
</File>
<File
RelativePath="wheeledvehicleentity.cpp">
</File>
<File
RelativePath="wheeledvehicleentity.h">
</File>
</Filter>
<Filter
Name="Utils"
Filter="">
<File
RelativePath="boolean2d.cpp">
</File>
<File
RelativePath=".\matrixnm.cpp">
</File>
<File
RelativePath=".\matrixnm.h">
</File>
<File
RelativePath="polynomial.h">
</File>
<File
RelativePath=".\qhull.cpp">
</File>
<File
RelativePath=".\quotient.h">
</File>
<File
RelativePath=".\utils.cpp">
</File>
<File
RelativePath=".\utils.h">
</File>
<File
RelativePath="vectorn.h">
</File>
</Filter>
<Filter
Name="Collisions"
Filter="">
<File
RelativePath="boxgeom.cpp">
</File>
<File
RelativePath="bvtree.h">
</File>
<File
RelativePath="cylindergeom.cpp">
</File>
<File
RelativePath="cylindergeom.h">
</File>
<File
RelativePath="geoman.cpp">
</File>
<File
RelativePath="geoman.h">
</File>
<File
RelativePath="geometries.h">
</File>
<File
RelativePath="geometry.cpp">
</File>
<File
RelativePath="geometry.h">
</File>
<File
RelativePath="heightfieldbv.cpp">
</File>
<File
RelativePath="heightfieldbv.h">
</File>
<File
RelativePath="heightfieldgeom.cpp">
</File>
<File
RelativePath="heightfieldgeom.h">
</File>
<File
RelativePath="intersectionchecks.cpp">
</File>
<File
RelativePath="intersectionchecks.h">
</File>
<File
RelativePath="linunprojectionchecks.cpp">
</File>
<File
RelativePath=".\obbtree.cpp">
</File>
<File
RelativePath=".\obbtree.h">
</File>
<File
RelativePath="overlapchecks.cpp">
</File>
<File
RelativePath="overlapchecks.h">
</File>
<File
RelativePath="primitives.h">
</File>
<File
RelativePath="raybv.cpp">
</File>
<File
RelativePath="raybv.h">
</File>
<File
RelativePath="raygeom.cpp">
</File>
<File
RelativePath="raygeom.h">
</File>
<File
RelativePath="rotunprojectionchecks.cpp">
</File>
<File
RelativePath="singleboxtree.cpp">
</File>
<File
RelativePath="singleboxtree.h">
</File>
<File
RelativePath="spheregeom.cpp">
</File>
<File
RelativePath="spheregeom.h">
</File>
<File
RelativePath="trimesh.cpp">
</File>
<File
RelativePath="trimesh.h">
</File>
<File
RelativePath="unprojectionchecks.h">
</File>
</Filter>
<File
RelativePath=".\CryPhysics.cpp">
</File>
<File
RelativePath="IPhysics.h">
</File>
<File
RelativePath=".\StdAfx.cpp">
<FileConfiguration
Name="Profile|Xbox">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Debug|Xbox">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
<FileConfiguration
Name="Release|Xbox">
<Tool
Name="VCCLCompilerTool"
UsePrecompiledHeader="1"/>
</FileConfiguration>
</File>
<File
RelativePath=".\StdAfx.h">
</File>
<File
RelativePath="aabbtree.cpp">
</File>
<File
RelativePath="aabbtree.h">
</File>
<File
RelativePath="physicalplaceholder.cpp">
</File>
<File
RelativePath="physicalplaceholder.h">
</File>
<File
RelativePath=".\physinterface.h">
</File>
</Files>
<Globals>
</Globals>
</VisualStudioProject>

10
CryPhysics/CryPhysics_XBox.vcproj.vspscc Normal file
View File

@@ -0,0 +1,10 @@
""
{
"FILE_VERSION" = "9237"
"ENLISTMENT_CHOICE" = "NEVER"
"PROJECT_FILE_RELATIVE_PATH" = ""
"NUMBER_OF_EXCLUDED_FILES" = "0"
"ORIGINAL_PROJECT_FILE_PATH" = "file:F:\\Crytek\\CryPhysics\\CryPhysics_XBox.vcproj"
"NUMBER_OF_NESTED_PROJECTS" = "0"
"SOURCE_CONTROL_SETTINGS_PROVIDER" = "PROJECT"
}

30
CryPhysics/IPhysics.h Normal file
View File

@@ -0,0 +1,30 @@
#ifndef cryphysics_h
#define cryphysics_h
#pragma once
#ifdef WIN32
#ifdef PHYSICS_EXPORTS
#define CRYPHYSICS_API __declspec(dllexport)
#else
#define CRYPHYSICS_API __declspec(dllimport)
#endif
#else
#define CRYPHYSICS_API
#endif
#define vector_class Vec3_tpl
#ifndef GAMECUBE
#include <CrySizer.h>
#endif
//#include "utils.h"
#include "Cry_Math.h"
#include "primitives.h"
#include "physinterface.h"
extern "C" CRYPHYSICS_API IPhysicalWorld *CreatePhysicalWorld(struct ISystem *pLog);
#endif

5
CryPhysics/MSSCCPRJ.SCC Normal file
View File

@@ -0,0 +1,5 @@
SCC = This is a source code control file
[CryPhysics.vcproj]
SCC_Aux_Path = "P4SCC#perforce:1666##marcoc_code##PC018"
SCC_Project_Name = Perforce Project

8
CryPhysics/StdAfx.cpp Normal file
View File

@@ -0,0 +1,8 @@
// stdafx.cpp : source file that includes just the standard includes
// CryPhysics.pch will be the pre-compiled header
// stdafx.obj will contain the pre-compiled type information
#include "stdafx.h"
// TODO: reference any additional headers you need in STDAFX.H
// and not in this file

119
CryPhysics/StdAfx.h Normal file
View File

@@ -0,0 +1,119 @@
// stdafx.h : include file for standard system include files,
// or project specific include files that are used frequently, but
// are changed infrequently
//
#if !defined(AFX_STDAFX_H__4AA14050_1B79_4A11_9D24_4E209BF87E2C__INCLUDED_)
#define AFX_STDAFX_H__4AA14050_1B79_4A11_9D24_4E209BF87E2C__INCLUDED_
#if _MSC_VER > 1000
#pragma once
#endif // _MSC_VER > 1000
//////////////////////////////////////////////////////////////////////////
// THIS MUST BE AT THE VERY BEGINING OF STDAFX.H FILE.
// Disable STL threading support, (makes STL faster)
//////////////////////////////////////////////////////////////////////////
#define _NOTHREADS
#define _STLP_NO_THREADS
//////////////////////////////////////////////////////////////////////////
#pragma warning (disable : 4554 4305 4244)
#include "platform.h"
#include "Cry_Math.h"
#include "Cry_XOptimise.h"
#ifdef GAMECUBE
//#include <math.h>
#include <stdlib.h>
#include <stdio.h>
#include <ctype.h>
//#include <string.h>
//#include <time.h>
#include <dolphin.h>
#include <stdarg.h>
#include "GCDefines.h"
#endif
#ifndef _XBOX
#ifdef WIN32
#define WIN32_LEAN_AND_MEAN // Exclude rarely-used stuff from Windows headers
#include <windows.h>
#endif
#else
#include <xtl.h>
#endif
#include <stdlib.h>
#include <float.h>
#ifndef NO_CRY_STREAM
#include "Stream.h"
#else
class CStream {
public:
bool WriteBits(BYTE *pBits, DWORD nSize) { return true; }
bool ReadBits(BYTE *pBits, DWORD nSize) { return true; }
bool Write(bool b) { return true; }
bool Write(char c) { return true; }
bool Write(unsigned char uc) { return true; }
bool Write(float f) { return true; }
bool Write(unsigned short us) { return true; }
bool Write(short s) { return true; }
bool Write(int i) { return true; }
bool Write(unsigned int ui) { return true; }
bool Write(const vectorf &v) { return true; }
bool Read(bool &b) { return true; }
bool Read(char &c) { return true; }
bool Read(unsigned char &uc) { return true; }
bool Read(unsigned short &us) { return true; }
bool Read(short &s) { return true; }
bool Read(int &i) { return true; }
bool Read(unsigned int &ui) { return true; }
bool Read(float &f) { return true; }
bool Read(vectorf &v) { return true; }
bool WriteNumberInBits(int n,size_t nSize) { return true; }
bool WriteNumberInBits(unsigned int n,size_t nSize) { return true; }
bool ReadNumberInBits(int &n,size_t nSize) { return true; }
bool ReadNumberInBits(unsigned int &n,size_t nSize) { return true; }
bool Seek(size_t dwPos = 0) { return true; }
size_t GetReadPos() { return 0; }
unsigned char *GetPtr() const { return 0; };
size_t GetSize() const { return 0; }
bool SetSize(size_t indwBitSize) { return true; }
};
#endif
#ifdef WIN64
#undef min
#undef max
#endif
#include <string>
#include <map>
#include "ILog.h"
#include "CrySizer.h"
// TODO: reference additional headers your program requires here
#include "primitives.h"
#include "utils.h"
#include "physinterface.h"
#ifdef _DEBUG
#ifdef WIN32
#include <crtdbg.h>
#define DEBUG_CLIENTBLOCK new( _NORMAL_BLOCK, __FILE__, __LINE__)
#define new DEBUG_CLIENTBLOCK
#endif //WIN32
#endif
//{{AFX_INSERT_LOCATION}}
// Microsoft Visual C++ will insert additional declarations immediately before the previous line.
#endif // !defined(AFX_STDAFX_H__4AA14050_1B79_4A11_9D24_4E209BF87E2C__INCLUDED_)

516
CryPhysics/aabbtree.cpp Normal file
View File

@@ -0,0 +1,516 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "aabbtree.h"
#include "trimesh.h"
struct BBoxExt : BBox {
box aboxStatic;
};
BBoxExt g_BBoxExtBuf[64];
int g_BBoxExtBufPos;
void CAABBTree::SetParams(int nMinTrisPerNode, int nMaxTrisPerNode, float skipdim, const matrix3x3f &Basis)
{
m_nMinTrisPerNode = nMinTrisPerNode; m_nMaxTrisPerNode = nMaxTrisPerNode;
m_maxSkipDim = skipdim;
m_Basis = Basis;
m_bOriented = m_Basis.IsIdentity()^1;
}
float CAABBTree::Build(CGeometry *pMesh)
{
m_pMesh = (CTriMesh*)pMesh;
m_pNodes = new AABBnode[m_nNodesAlloc=32];
m_pTri2Node = new int[m_pMesh->m_nTris];
m_nMaxTrisInNode = 0;
m_nNodes = 2;
m_pNodes[0].ntris = m_pNodes[1].ntris = 0;
vectorf ptmin,ptmax,pt;
ptmin = ptmax = m_Basis*m_pMesh->m_pVertices[m_pMesh->m_pIndices[0]];
int i;
for(i=1;i<m_pMesh->m_nTris*3;i++) {
pt = m_Basis*m_pMesh->m_pVertices[m_pMesh->m_pIndices[i]];
ptmin.x = min(ptmin.x,pt.x); ptmax.x = max(ptmax.x,pt.x);
ptmin.y = min(ptmin.y,pt.y); ptmax.y = max(ptmax.y,pt.y);
ptmin.z = min(ptmin.z,pt.z); ptmax.z = max(ptmax.z,pt.z);
}
m_size = (ptmax-ptmin)*0.5f;
m_center = ((ptmax+ptmin)*0.5f)*m_Basis;
m_maxSkipDim *= max(max(m_size.x,m_size.y),m_size.z);
float mindim = m_maxSkipDim*0.001f;
m_size.Set(max_safe(m_size.x,mindim), max_safe(m_size.y,mindim), max_safe(m_size.z,mindim));
float volume = BuildNode(0, 0,m_pMesh->m_nTris, m_Basis*m_center,m_size, 0);
if (m_nNodesAlloc>m_nNodes) {
AABBnode *pNodes = m_pNodes;
memcpy(m_pNodes = new AABBnode[m_nNodesAlloc=m_nNodes], pNodes, sizeof(AABBnode)*m_nNodes);
delete[] pNodes;
}
m_nBitsLog = m_nNodes<=256 ? 3 : 4;
int *pNewTri2Node = new int[(m_pMesh->m_nTris-1>>5-m_nBitsLog)+1];
memset(pNewTri2Node,0,sizeof(int)*((m_pMesh->m_nTris-1>>5-m_nBitsLog)+1));
for(i=0;i<m_pMesh->m_nTris;i++)
pNewTri2Node[i>>5-m_nBitsLog] |= m_pTri2Node[i] << ((i&(1<<5-m_nBitsLog)-1)<<m_nBitsLog);
delete[] m_pTri2Node;
m_pTri2Node = pNewTri2Node;
return volume*8+isneg(63-m_nMaxTrisInNode)*1E10f;
}
float CAABBTree::BuildNode(int iNode, int iTriStart,int nTris, vectorf center,vectorf size, int nDepth)
{
int i,j;
vectorf ptmin(MAX),ptmax(MIN),pt,rsize;
float mindim = max(max(size.x,size.y),size.z)*0.001f;
for(i=iTriStart*3;i<(iTriStart+nTris)*3;i++) {
pt = m_Basis*m_pMesh->m_pVertices[m_pMesh->m_pIndices[i]];
ptmin.x = min_safe(ptmin.x,pt.x); ptmax.x = max_safe(ptmax.x,pt.x);
ptmin.y = min_safe(ptmin.y,pt.y); ptmax.y = max_safe(ptmax.y,pt.y);
ptmin.z = min_safe(ptmin.z,pt.z); ptmax.z = max_safe(ptmax.z,pt.z);
}
ptmin += size-center; ptmax += size-center;
if (size.x>mindim) {
rsize.x = (0.5f*128)/size.x;
m_pNodes[iNode].minx = float2int(min(max(ptmin.x*rsize.x-0.5f,0.0f),127.0f));
m_pNodes[iNode].maxx = float2int(min(max(ptmax.x*rsize.x-0.5f,0.0f),127.0f));
m_pNodes[iNode].minx = min((int)m_pNodes[iNode].minx,(int)m_pNodes[iNode].maxx);
m_pNodes[iNode].maxx = max((int)m_pNodes[iNode].minx,(int)m_pNodes[iNode].maxx);
ptmin.x = m_pNodes[iNode].minx*size.x*(2.0f/128);
ptmax.x = (m_pNodes[iNode].maxx+1)*size.x*(2.0f/128);
center.x += (ptmax.x+ptmin.x)*0.5f-size.x;
size.x = (ptmax.x-ptmin.x)*0.5f;
} else {
m_pNodes[iNode].minx = 0; m_pNodes[iNode].maxx = 127;
}
if (size.y>mindim) {
rsize.y = (0.5f*128)/size.y;
m_pNodes[iNode].miny = float2int(min(max(ptmin.y*rsize.y-0.5f,0.0f),127.0f));
m_pNodes[iNode].maxy = float2int(min(max(ptmax.y*rsize.y-0.5f,0.0f),127.0f));
m_pNodes[iNode].miny = min((int)m_pNodes[iNode].miny,(int)m_pNodes[iNode].maxy);
m_pNodes[iNode].maxy = max((int)m_pNodes[iNode].miny,(int)m_pNodes[iNode].maxy);
ptmin.y = m_pNodes[iNode].miny*size.y*(2.0f/128);
ptmax.y = (m_pNodes[iNode].maxy+1)*size.y*(2.0f/128);
center.y += (ptmax.y+ptmin.y)*0.5f-size.y;
size.y = (ptmax.y-ptmin.y)*0.5f;
} else {
m_pNodes[iNode].miny = 0; m_pNodes[iNode].maxy = 127;
}
if (size.z>mindim) {
rsize.z = (0.5f*128)/size.z;
m_pNodes[iNode].minz = float2int(min(max(ptmin.z*rsize.z-0.5f,0.0f),127.0f));
m_pNodes[iNode].maxz = float2int(min(max(ptmax.z*rsize.z-0.5f,0.0f),127.0f));
m_pNodes[iNode].minz = min((int)m_pNodes[iNode].minz,(int)m_pNodes[iNode].maxz);
m_pNodes[iNode].maxz = max((int)m_pNodes[iNode].minz,(int)m_pNodes[iNode].maxz);
ptmin.z = m_pNodes[iNode].minz*size.z*(2.0f/128);
ptmax.z = (m_pNodes[iNode].maxz+1)*size.z*(2.0f/128);
center.z += (ptmax.z+ptmin.z)*0.5f-size.z;
size.z = (ptmax.z-ptmin.z)*0.5f;
} else {
m_pNodes[iNode].minz = 0; m_pNodes[iNode].maxz = 127;
}
//center += (ptmax+ptmin)*0.5f-size; size = (ptmax-ptmin)*0.5f;
if (nTris<=m_nMaxTrisPerNode) {
m_pNodes[iNode].ichild = iTriStart;
m_pNodes[iNode].ntris = nTris;
m_nMaxTrisInNode = max(m_nMaxTrisInNode, nTris);
m_pNodes[iNode].bSingleColl = isneg(max(max(size.x,size.y),size.z)-m_maxSkipDim);
for(i=iTriStart;i<iTriStart+nTris;i++)
m_pTri2Node[i] = iNode;
return size.volume();
}
#if defined(WIN64) || defined(LINUX64)
volatile // compiler bug workaround?
#endif
int iAxis;
int numtris[3],nTrisAx[3],iPart,iMode[3],idx;
float x0,x1,x2,cx,xlim[2],bounds[3][2],diff[3],axdiff[3];
vectorf axis,c;
for(iAxis=0;iAxis<3;iAxis++) {
if (size[iAxis]<mindim) {
axdiff[iAxis] = -1E10f; nTrisAx[iAxis] = 0;
continue;
}
axis = m_Basis.GetRow(iAxis); cx = center[iAxis];
bounds[0][0]=bounds[1][0]=bounds[2][0] = -size[iAxis];
bounds[0][1]=bounds[1][1]=bounds[2][1] = size[iAxis];
numtris[0]=numtris[1]=numtris[2] = 0;
for(i=iTriStart;i<iTriStart+nTris;i++) {
c = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3]]+m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]+
m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]];
x0 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+0]]*axis-cx;
x1 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]*axis-cx;
x2 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]]*axis-cx;
xlim[0] = min(min(x0,x1),x2); xlim[1] = max(max(x0,x1),x2);
/*for(j=0,c.zero(),xlim[0]=size[iAxis],xlim[1]=-size[iAxis]; j<3; j++) {
c += m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+j]];
x = axis*m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+j]]-cx;
xlim[0] = min(xlim[0],x); xlim[1] = max(xlim[1],x);
}*/
iPart = isneg(xlim[1])^1; // mode0: group all triangles that are entirely below center
bounds[0][iPart] = minmax(bounds[0][iPart],xlim[iPart^1],iPart^1); numtris[0]+=iPart;
iPart = isnonneg(xlim[0]); // mode1: group all triangles that are entirely above center
bounds[1][iPart] = minmax(bounds[1][iPart],xlim[iPart^1],iPart^1); numtris[1]+=iPart;
iPart = isnonneg(((c*axis)*(1.0f/3)-cx)); // mode2: sort triangles basing on centroids only
bounds[2][iPart] = minmax(bounds[2][iPart],xlim[iPart^1],iPart^1); numtris[2]+=iPart;
}
for(i=0;i<3;i++) diff[i] = bounds[i][1]-bounds[i][0]-size[iAxis]*((isneg(numtris[i]-m_nMinTrisPerNode)|isneg(nTris-numtris[i]-m_nMinTrisPerNode))*8);
iMode[iAxis] = idxmax3(diff); nTrisAx[iAxis] = numtris[iMode[iAxis]];
axdiff[iAxis] = diff[iMode[iAxis]]*size[dec_mod3[iAxis]]*size[inc_mod3[iAxis]];
}
iAxis = idxmax3(axdiff);
axis = m_Basis.GetRow(iAxis); cx = center[iAxis];
for(i=j=iTriStart;i<iTriStart+nTris;i++) {
x0 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+0]]*axis-cx;
x1 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]*axis-cx;
x2 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]]*axis-cx;
#if 0//def WIN64
if ((unsigned)iAxis >= 3)
OutputDebugString ("iAxis>=3!");
else
if ((unsigned)iMode[iAxis] >= 3)
OutputDebugString("iMode[iAxis]>=3!");
#endif
switch(iMode[iAxis]) {
case 0: iPart = isneg(max(max(x0,x1),x2))^1; break;
case 1: iPart = isnonneg(min(min(x0,x1),x2)); break;
case 2: iPart = isnonneg(x0+x1+x2);
}
if (iPart==0) {
// swap triangles
idx=m_pMesh->m_pIndices[i*3+0]; m_pMesh->m_pIndices[i*3+0]=m_pMesh->m_pIndices[j*3+0]; m_pMesh->m_pIndices[j*3+0]=idx;
idx=m_pMesh->m_pIndices[i*3+1]; m_pMesh->m_pIndices[i*3+1]=m_pMesh->m_pIndices[j*3+1]; m_pMesh->m_pIndices[j*3+1]=idx;
idx=m_pMesh->m_pIndices[i*3+2]; m_pMesh->m_pIndices[i*3+2]=m_pMesh->m_pIndices[j*3+2]; m_pMesh->m_pIndices[j*3+2]=idx;
if (m_pMesh->m_pIds) {
idx=m_pMesh->m_pIds[i]; m_pMesh->m_pIds[i]=m_pMesh->m_pIds[j]; m_pMesh->m_pIds[j]=idx;
}
j++;
}
}
j -= iTriStart;
if (j<m_nMinTrisPerNode || j>nTris-m_nMinTrisPerNode) {
m_pNodes[iNode].ichild = iTriStart;
m_pNodes[iNode].ntris = nTris;
m_nMaxTrisInNode = max(m_nMaxTrisInNode, nTris);
m_pNodes[iNode].bSingleColl = isneg(max(max(size.x,size.y),size.z)-m_maxSkipDim);
for(i=iTriStart;i<iTriStart+nTris;i++)
m_pTri2Node[i] = iNode;
return size.volume();
}
// proceed with the children
if (m_nNodes+2 > m_nNodesAlloc) {
AABBnode *pNodes = m_pNodes;
memcpy(m_pNodes = new AABBnode[m_nNodesAlloc+=32], pNodes, m_nNodes*sizeof(AABBnode));
delete[] pNodes;
}
m_pNodes[iNode].ichild = m_nNodes;
m_pNodes[m_nNodes].ntris = m_pNodes[m_nNodes+1].ntris = 0;
iNode = m_nNodes; m_nNodes += 2;
float volume = 0;
volume += BuildNode(iNode+1, iTriStart+j,nTris-j, center,size, nDepth+1);
volume += BuildNode(iNode, iTriStart,j, center,size, nDepth+1);
return volume;
}
void CAABBTree::SetGeomConvex()
{
for(int i=0;i<m_nNodes;i++)
m_pNodes[i].bSingleColl = 1;
}
void CAABBTree::GetBBox(box *pbox)
{
pbox->Basis = m_Basis;
pbox->bOriented = m_bOriented;
pbox->center = m_center;
pbox->size = m_size;
}
void CAABBTree::GetNodeBV(BV *&pBV,int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
((BBox*)pBV)->iNode = 0;
((BBox*)pBV)->abox.Basis = m_Basis;
((BBox*)pBV)->abox.bOriented = m_bOriented;
((BBox*)pBV)->abox.center = m_center;
((BBox*)pBV)->abox.size = m_size;
}
void CAABBTree::GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode)
{
pBV = g_BBoxExtBuf; g_BBoxExtBufPos = 0;
pBV->type = box::type;
((BBoxExt*)pBV)->iNode = 0;
box &boxstatic = ((BBoxExt*)pBV)->aboxStatic;
boxstatic.Basis = m_Basis;
boxstatic.bOriented = m_bOriented;
boxstatic.center = m_center;
boxstatic.size = m_size;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &((BBoxExt*)pBV)->abox);
}
void CAABBTree::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = 0;
((BBox*)pBV)->abox.Basis = m_Basis*Rw.T();
((BBox*)pBV)->abox.bOriented = 1;
((BBox*)pBV)->abox.center = Rw*m_center*scalew + offsw;
((BBox*)pBV)->abox.size = m_size*scalew;
}
float CAABBTree::SplitPriority(const BV* pBV)
{
BBox *pbox = (BBox*)pBV;
return pbox->abox.size.volume()*(m_pNodes[pbox->iNode].ntris-1>>31 & 1);
}
void CAABBTree::GetNodeChildrenBVs(const matrix3x3f &Rw,const vectorf &offsw,float scalew,
const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2)
{
BBox *bbox_parent=(BBox*)pBV_parent, *&bbox_child1=(BBox*&)pBV_child1, *&bbox_child2=(BBox*&)pBV_child2;
bbox_child1 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
bbox_child1->abox.Basis = bbox_parent->abox.Basis; bbox_child2->abox.Basis = bbox_parent->abox.Basis;
bbox_child1->abox.bOriented = bbox_child2->abox.bOriented = 1;
int ichild = bbox_child1->iNode;
const vectorf size=bbox_parent->abox.size, center=bbox_parent->abox.center;
vectorf ptmax,ptmin;
ptmin.x = m_pNodes[ichild].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild].maxz+1)*size.z*(2.0/128);
bbox_child1->abox.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->abox.Basis;
bbox_child1->abox.size = (ptmax-ptmin)*0.5f;
ptmin.x = m_pNodes[ichild+1].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild+1].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild+1].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild+1].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild+1].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild+1].maxz+1)*size.z*(2.0/128);
bbox_child2->abox.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->abox.Basis;
bbox_child2->abox.size = (ptmax-ptmin)*0.5f;
}
void CAABBTree::GetNodeChildrenBVs(const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2)
{
BBox *bbox_parent=(BBox*)pBV_parent, *&bbox_child1=(BBox*&)pBV_child1, *&bbox_child2=(BBox*&)pBV_child2;
bbox_child1 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
if (bbox_child1->abox.bOriented = bbox_child2->abox.bOriented = bbox_parent->abox.bOriented) {
bbox_child1->abox.Basis = bbox_parent->abox.Basis;
bbox_child2->abox.Basis = bbox_parent->abox.Basis;
}
int ichild = bbox_child1->iNode;
const vectorf size=bbox_parent->abox.size, center=bbox_parent->abox.center;
vectorf ptmax,ptmin;
ptmin.x = m_pNodes[ichild].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild].maxz+1)*size.z*(2.0/128);
if (bbox_parent->abox.bOriented)
bbox_child1->abox.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->abox.Basis;
else
bbox_child1->abox.center = center+((ptmax+ptmin)*0.5f-size);
bbox_child1->abox.size = (ptmax-ptmin)*0.5f;
ptmin.x = m_pNodes[ichild+1].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild+1].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild+1].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild+1].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild+1].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild+1].maxz+1)*size.z*(2.0/128);
if (bbox_parent->abox.bOriented)
bbox_child2->abox.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->abox.Basis;
else
bbox_child2->abox.center = center+((ptmax+ptmin)*0.5f-size);
bbox_child2->abox.size = (ptmax-ptmin)*0.5f;
}
void CAABBTree::GetNodeChildrenBVs(const BV *pBV_parent, const vectorf &sweepdir,float sweepstep, BV *&pBV_child1,BV *&pBV_child2)
{
BBoxExt *bbox_parent=(BBoxExt*)pBV_parent, *&bbox_child1=(BBoxExt*&)pBV_child1, *&bbox_child2=(BBoxExt*&)pBV_child2;
bbox_child1 = g_BBoxExtBuf + g_BBoxExtBufPos++;
bbox_child2 = g_BBoxExtBuf + g_BBoxExtBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
if (bbox_child1->aboxStatic.bOriented = bbox_child2->aboxStatic.bOriented = bbox_parent->aboxStatic.bOriented) {
bbox_child1->aboxStatic.Basis = bbox_parent->aboxStatic.Basis;
bbox_child2->aboxStatic.Basis = bbox_parent->aboxStatic.Basis;
}
int ichild = bbox_child1->iNode;
const vectorf size=bbox_parent->aboxStatic.size, center=bbox_parent->aboxStatic.center;
vectorf ptmax,ptmin;
ptmin.x = m_pNodes[ichild].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild].maxz+1)*size.z*(2.0/128);
if (bbox_parent->aboxStatic.bOriented)
bbox_child1->aboxStatic.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->aboxStatic.Basis;
else
bbox_child1->aboxStatic.center = center+((ptmax+ptmin)*0.5f-size);
bbox_child1->aboxStatic.size = (ptmax-ptmin)*0.5f;
ExtrudeBox(&bbox_child1->aboxStatic, sweepdir,sweepstep, &bbox_child1->abox);
bbox_child1->abox.bOriented = 1+bbox_child1->iNode;
ptmin.x = m_pNodes[ichild+1].minx*size.x*(2.0/128);
ptmax.x = (m_pNodes[ichild+1].maxx+1)*size.x*(2.0/128);
ptmin.y = m_pNodes[ichild+1].miny*size.y*(2.0/128);
ptmax.y = (m_pNodes[ichild+1].maxy+1)*size.y*(2.0/128);
ptmin.z = m_pNodes[ichild+1].minz*size.z*(2.0/128);
ptmax.z = (m_pNodes[ichild+1].maxz+1)*size.z*(2.0/128);
if (bbox_parent->abox.bOriented)
bbox_child2->aboxStatic.center = center+((ptmax+ptmin)*0.5f-size)*bbox_parent->aboxStatic.Basis;
else
bbox_child2->aboxStatic.center = center+((ptmax+ptmin)*0.5f-size);
bbox_child2->aboxStatic.size = (ptmax-ptmin)*0.5f;
ExtrudeBox(&bbox_child2->aboxStatic, sweepdir,sweepstep, &bbox_child2->abox);
bbox_child2->abox.bOriented = 1+bbox_child2->iNode;
}
void CAABBTree::ReleaseLastBVs()
{
g_BBoxBufPos-=2;
}
void CAABBTree::ReleaseLastSweptBVs()
{
g_BBoxExtBufPos-=2;
}
int CAABBTree::GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp)
{
return m_pMesh->GetPrimitiveList(m_pNodes[iNode].ichild,m_pNodes[iNode].ntris, pBVCollider->type,*pBVCollider,bColliderLocal, pGTest,pGTestOp,
pGTest->primbuf,pGTest->idbuf);
}
void CAABBTree::MarkUsedTriangle(int itri, geometry_under_test *pGTest)
{
if (!pGTest->pUsedNodesMap) return;
int iNode = m_pTri2Node[itri>>5-m_nBitsLog]>>((itri&(1<<5-m_nBitsLog)-1)<<m_nBitsLog) & (1<<(1<<m_nBitsLog))-1;
if (m_pNodes[iNode].bSingleColl) {
pGTest->pUsedNodesMap[iNode>>5] |= 1<<(iNode&31);
pGTest->pUsedNodesIdx[pGTest->nUsedNodes = min(pGTest->nUsedNodes+1,pGTest->nMaxUsedNodes-1)] = iNode;
pGTest->bCurNodeUsed = 1;
}
}
int CAABBTree::PrepareForIntersectionTest(geometry_under_test *pGTest)
{
if (m_maxSkipDim<=0) {
pGTest->pUsedNodesMap = 0;
pGTest->pUsedNodesIdx = 0;
pGTest->nMaxUsedNodes = 0;
} else {
int mapsz = (m_nNodes-1>>5) + 1;
if (mapsz<=(int)(sizeof(g_UsedNodesMap)/sizeof(g_UsedNodesMap[0]))-g_UsedNodesMapPos) {
pGTest->pUsedNodesMap = g_UsedNodesMap+g_UsedNodesMapPos; g_UsedNodesMapPos += mapsz;
} else
pGTest->pUsedNodesMap = new int[mapsz];
pGTest->pUsedNodesIdx = g_UsedNodesIdx+g_UsedNodesIdxPos;
pGTest->nMaxUsedNodes = min(32,sizeof(g_UsedNodesIdx)/sizeof(g_UsedNodesIdx[0])-g_UsedNodesIdxPos);
g_UsedNodesIdxPos += pGTest->nMaxUsedNodes;
}
pGTest->nUsedNodes = -1;
return 1;
}
void CAABBTree::CleanupAfterIntersectionTest(geometry_under_test *pGTest)
{
if (!pGTest->pUsedNodesMap)
return;
if ((unsigned int)(pGTest->pUsedNodesMap-g_UsedNodesMap) > (unsigned int)sizeof(g_UsedNodesMap)/sizeof(g_UsedNodesMap[0])) {
delete[] pGTest->pUsedNodesMap; return;
}
if (pGTest->nUsedNodes < pGTest->nMaxUsedNodes-1) {
for(int i=0;i<=pGTest->nUsedNodes;i++)
pGTest->pUsedNodesMap[pGTest->pUsedNodesIdx[i]>>5] &= ~(1<<(pGTest->pUsedNodesIdx[i]&31));
} else
memset(pGTest->pUsedNodesMap, 0, ((m_nNodes-1>>5)+1)*4);
}
void CAABBTree::GetMemoryStatistics(ICrySizer *pSizer)
{
SIZER_COMPONENT_NAME(pSizer, "AABB trees");
pSizer->AddObject(this, sizeof(CAABBTree));
pSizer->AddObject(m_pNodes, m_nNodesAlloc*sizeof(m_pNodes[0]));
if (m_pTri2Node)
pSizer->AddObject(m_pTri2Node, ((m_pMesh->m_nTris-1>>5-m_nBitsLog)+1)*sizeof(int));
}
void CAABBTree::Save(CMemStream &stm)
{
stm.Write(m_nNodes);
stm.Write(m_pNodes, m_nNodes*sizeof(m_pNodes[0]));
stm.Write(m_center);
stm.Write(m_size);
stm.Write(m_Basis);
stm.Write(m_nMaxTrisInNode);
stm.Write(m_nMinTrisPerNode);
stm.Write(m_nMaxTrisPerNode);
stm.Write(m_maxSkipDim);
}
void CAABBTree::Load(CMemStream &stm, CGeometry *pGeom)
{
m_pMesh = (CTriMesh*)pGeom;
stm.Read(m_nNodes);
m_pNodes = new AABBnode[m_nNodesAlloc=m_nNodes];
stm.Read(m_pNodes, m_nNodes*sizeof(m_pNodes[0]));
m_pTri2Node = new int[(m_pMesh->m_nTris-1>>5-m_nBitsLog)+1];
memset(m_pTri2Node,0,sizeof(int)*((m_pMesh->m_nTris-1>>5-m_nBitsLog)+1));
for(int i=0;i<m_nNodes;i++) for(int j=0;j<(int)m_pNodes[i].ntris;j++)
m_pTri2Node[m_pNodes[i].ichild+j>>5-m_nBitsLog] |= i << ((m_pNodes[i].ichild+j&(1<<5-m_nBitsLog)-1)<<m_nBitsLog);
stm.Read(m_center);
stm.Read(m_size);
stm.Read(m_Basis);
stm.Read(m_nMaxTrisInNode);
stm.Read(m_nMinTrisPerNode);
stm.Read(m_nMaxTrisPerNode);
stm.Read(m_maxSkipDim);
}

69
CryPhysics/aabbtree.h Normal file
View File

@@ -0,0 +1,69 @@
#ifndef aabbtree_h
#define aabbtree_h
struct AABBnode {
unsigned int minx : 7;
unsigned int maxx : 7;
unsigned int miny : 7;
unsigned int maxy : 7;
unsigned int minz : 7;
unsigned int maxz : 7;
unsigned int ichild : 15;
unsigned int ntris : 6;
unsigned int bSingleColl : 1;
};
class CTriMesh;
class CAABBTree : public CBVTree {
public:
CAABBTree() {}
virtual ~CAABBTree() {
if (m_pNodes) delete[] m_pNodes; m_pNodes=0;
if (m_pTri2Node) delete[] m_pTri2Node; m_pTri2Node=0;
}
virtual int GetType() { return BVT_AABB; }
float Build(CGeometry *pMesh);
virtual void SetGeomConvex();
void SetParams(int nMinTrisPerNode,int nMaxTrisPerNode, float skipdim, const matrix3x3f &Basis);
float BuildNode(int iNode, int iTriStart,int nTris, vectorf center,vectorf size, int nDepth);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest);
virtual void CleanupAfterIntersectionTest(geometry_under_test *pGTest);
virtual void GetBBox(box *pbox);
virtual int MaxPrimsInNode() { return m_nMaxTrisInNode; }
virtual void GetNodeBV(BV *&pBV, int iNode=0);
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual float SplitPriority(const BV* pBV);
virtual void GetNodeChildrenBVs(const matrix3x3f &Rw,const vectorf &offsw,float scalew, const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2);
virtual void GetNodeChildrenBVs(const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2);
virtual void GetNodeChildrenBVs(const BV *pBV_parent, const vectorf &sweepdir,float sweepstep, BV *&pBV_child1,BV *&pBV_child2);
virtual void ReleaseLastBVs();
virtual void ReleaseLastSweptBVs();
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp);
virtual int GetNodeContentsIdx(int iNode, int &iStartPrim) { iStartPrim = m_pNodes[iNode].ichild; return m_pNodes[iNode].ntris; }
virtual void MarkUsedTriangle(int itri, geometry_under_test *pGTest);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm, CGeometry *pGeom);
CTriMesh *m_pMesh;
AABBnode *m_pNodes;
int m_nNodes,m_nNodesAlloc;
vectorf m_center;
vectorf m_size;
matrix3x3f m_Basis;
int m_bOriented;
int *m_pTri2Node,m_nBitsLog;
int m_nMaxTrisPerNode,m_nMinTrisPerNode;
int m_nMaxTrisInNode;
float m_maxSkipDim;
};
#endif

2341
CryPhysics/articulatedentity.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

218
CryPhysics/articulatedentity.h Normal file
View File

@@ -0,0 +1,218 @@
//////////////////////////////////////////////////////////////////////
//
// Articulated Entity header
//
// File: articulatedentity.h
// Description : CArticulatedEntity class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef articulatedentity_h
#define articulatedentity_h
#pragma once
struct featherstone_data {
vectori qidx2axidx,axidx2qidx;
vectorf Ya_vec[2];
vectorf dv_vec[2];
vectorf s_vec[3][2];
vectorf Q;
float qinv[9];
float qinv_down[9];
float s[18];
DEFINE_ALIGNED_DATA( float, Ia[6][6], 16 );
float Ia_s[18];
DEFINE_ALIGNED_DATA( float, Ia_s_qinv_sT[6][6], 16 );
DEFINE_ALIGNED_DATA( float, s_qinv_sT[6][6], 16 );
DEFINE_ALIGNED_DATA( float, s_qinv_sT_Ia[6][6], 16 );
float qinv_sT[3][6];
float qinv_sT_Ia[3][6];
DEFINE_ALIGNED_DATA( float, Iinv[6][6], 16 );
};
struct ae_joint {
ae_joint() {
nChildren=nChildrenTree=0; iParent=-2; idbody=-1;
q.zero(); qext.zero(); dq.zero(); dqext.zero(); ddq.zero();
MARK_UNUSED dq_req.x,dq_req.y,dq_req.z; dq_limit.zero();
bounciness.zero(); ks.zero(); kd.zero();
qdashpot.zero(); kdashpot.zero();
limits[0].Set(-1E10,-1E10,-1E10);
limits[1].Set(1E10,1E10,1E10);
flags=all_angles_locked;
quat0.SetIdentity();
Pext.zero(); Lext.zero();
Pimpact.zero(); Limpact.zero();
//fs.Q.zero(); fs.Ya_vec[0].zero(); fs.Ya_vec[1].zero();
//matrix3x3f(fs.qinv).identity();
nActiveAngles = nPotentialAngles = 0;
pivot[0].zero(); pivot[1].zero();
iLevel = 0;
iContacts = 0;
iStartPart = nParts = 0;
dv_body.zero(); dw_body.zero();
selfCollMask = 0;
fs = 0; fsbuf = 0;
bQuat0Changed = 0;
}
~ae_joint() {
if (fsbuf) delete fsbuf;
}
vectorf q;
vectorf qext;
vectorf dq;
vectorf dqext;
vectorf dq_req;
vectorf dq_limit;
vectorf ddq;
quaternionf quat;
vectorf prev_q,prev_dq;
vectorf prev_pos,prev_v,prev_w;
quaternionf prev_qrot;
vectorf q0;
unsigned int flags;
quaternionf quat0;
vectorf limits[2];
vectorf bounciness;
vectorf ks,kd;
vectorf qdashpot,kdashpot;
vectorf pivot[2];
vectorf hingePivot[2];
int iStartPart,nParts;
int iParent;
int nChildren,nChildrenTree;
int iLevel;
masktype selfCollMask;
masktype iContacts;
int bAwake;
int bQuat0Changed;
int bHasExtContacts;
int idbody;
RigidBody body;
vectorf dv_body,dw_body;
vectorf Pext,Lext;
vectorf Pimpact,Limpact;
int nActiveAngles,nPotentialAngles;
vectorf rotaxes[3];
matrix3x3f I;
featherstone_data *fs;
void *fsbuf;
};
struct ae_part_info {
quaternionf q0;
vectorf pos0;
int iJoint;
int idbody;
};
class CArticulatedEntity : public CRigidEntity, public IRigidBodyOwner {
public:
CArticulatedEntity(CPhysicalWorld *pworld);
virtual ~CArticulatedEntity();
virtual pe_type GetType() { return PE_ARTICULATED; }
virtual void AlertNeighbourhoodND();
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual int SetParams(pe_params *_params);
virtual int GetParams(pe_params *_params);
virtual int GetStatus(pe_status *_status);
virtual int Action(pe_action*);
virtual RigidBody *GetRigidBody(int ipart=-1);
virtual void GetContactMatrix(const vectorf &pt, int ipart, matrix3x3f &K);
virtual void AddImpulseAtContact(entity_contact *pcontact, int iop, const vectorf &dP) {};
virtual vectorf GetVelocityAtContact(entity_contact *pcontact, int iop) { return vectorf(zero); };
virtual int OnRegisterContact(entity_contact *pcontact, int iop);
virtual void OnSolverEvent(int iEvent) {};
virtual void GetMemoryStatistics(ICrySizer *pSizer);
enum snapver { SNAPSHOT_VERSION = 6 };
virtual int GetStateSnapshot(CStream &stm, float time_back=0,int flags=0);
virtual int SetStateFromSnapshot(CStream &stm, int flags);
virtual float GetMaxTimeStep(float time_interval);
virtual int Step(float time_interval);
virtual void StepBack(float time_interval);
virtual int RegisterContacts(float time_interval,int nMaxPlaneContacts);
virtual int Update(float time_interval, float damping);
virtual float CalcEnergy(float time_interval);
virtual float GetDamping(float time_interval);
virtual int GetPotentialColliders(CPhysicalEntity **&pentlist);
virtual int CheckSelfCollision(int ipart0,int ipart1);
virtual int IsAwake(int ipart=-1);
virtual void RecomputeMassDistribution(int ipart=-1,int bMassChanged=1);
void SyncWithHost(int bRecalcJoints,float time_interval);
void SyncBodyWithJoint(int idx, int flags=3);
void SyncJointWithBody(int idx, int flags=1);
void UpdateJointRotationAxes(int idx);
void CheckForGimbalLock(int idx);
int GetUnprojAxis(int idx, vectorf &axis);
int StepJoint(int idx, float time_interval,masktype &contact_mask,int &bBounced, int bFlying);
void JointListUpdated();
int CalcBodyZa(int idx, float time_interval, vectornf &Za_change);
int CalcBodyIa(int idx, matrixf& Ia_change);
void CalcBodiesIinv(int bLockLimits);
int CollectPendingImpulses(int idx,int &bNotZero);
void PropagateImpulses(const vectorf &dv,int bLockLimits=0);
void CalcVelocityChanges(float time_interval, const vectorf &dv,const vectorf &dw);
void GetJointTorqueResponseMatrix(int idx, matrix3x3f &K);
int IsChildOf(int idx, int iParent) { return isnonneg(iParent) & isneg(iParent-idx) & isneg(idx-iParent-m_joints[iParent].nChildrenTree-1); }
entity_contact *CreateConstraintContact(int idx);
ae_part_info *m_infos;
ae_joint *m_joints;
int m_nJoints, m_nJointsAlloc;
vectorf m_posPivot, m_offsPivot;
vectorf m_acc,m_wacc;
matrix3x3f m_M0inv;
vectorf m_Ya_vec[2];
float m_simTime,m_simTimeAux;
float m_scaleBounceResponse;
int m_bGrounded;
int m_bInheritVel;
CPhysicalEntity *m_pHost;
vectorf m_posHostPivot;
int m_bCheckCollisions;
int m_bCollisionResp;
int m_bExertImpulse;
int m_iSimType,m_iSimTypeLyingMode;
int m_iSimTypeCur;
int m_iSimTypeOverride;
int m_bIaReady;
int m_bPartPosForced;
int m_bFastLimbs;
int m_bExpandHinges;
float m_maxPenetrationCur;
int m_bUsingUnproj;
vectorf m_prev_pos,m_prev_vel;
int m_bUpdateBodies;
int m_nDynContacts,m_bInGroup;
int m_nCollLyingMode;
vectorf m_gravityLyingMode;
float m_dampingLyingMode;
float m_EminLyingMode;
int m_nContacts;
CPhysicalEntity **m_pCollEntList;
int m_nCollEnts;
};
#endif

268
CryPhysics/boolean2d.cpp Normal file
View File

@@ -0,0 +1,268 @@
#include "stdafx.h"
#include "utils.h"
vector2df g_BoolPtBuf[4096];
int g_BoolIdBuf[4096];
int g_BoolGrid[4096];
unsigned int g_BoolHash[8192];
struct inters2d {
vector2df pt;
int iedge[2];
};
inters2d g_BoolInters[256];
inline int line_seg_inters(const vector2df *seg0, const vector2df *seg1, vector2df *ptres)
{
float denom,t0,t1,sg;
vector2df dp0,dp1,ds;
dp0 = seg0[1]-seg0[0]; dp1 = seg1[1]-seg1[0]; ds = seg1[0]-seg0[0];
denom = dp0^dp1;
if (sqr(denom) > 1E-6f*dp0.len2()*dp1.len2()) { // stable intersection
t0 = ds^dp1; t1 = ds^dp0;
sg = sgnnz(denom); denom*=sg; t0*=sg; t1*=sg;
if (isneg(fabs_tpl(t0*2-denom)-denom) & isneg(fabs_tpl(t1*2-denom)-denom)) {
ptres[0] = seg0[0] + dp0*(t0/denom); return 1;
} else return 0;
}
if (sqr(ds^dp0) < 1E-6f*ds.len2()*dp0.len2()) { // segments are [almost] parallel and touching
float t[2][2]; const vector2df *ppt[2][2]; int idir;
t[0][0]=0; t[0][1]=dp0.len2(); ppt[0][0]=seg0; ppt[0][1]=seg0+1;
t0=(seg1[0]-seg0[0])*dp0; t1=(seg1[1]-seg0[0])*dp0;
idir = isneg(t1-t0); t[1][idir]=t0; t[1][idir^1]=t0; ppt[1][idir]=seg1; ppt[1][idir^1]=seg1+1;
if (max(t[0][0],t[1][0])>min(t[0][1],t[1][1]))
return 0;
ptres[0] = *ppt[0][isneg(t[0][0]-t[1][0])];
ptres[1] = *ppt[1][isneg(t[1][1]-t[0][1])];
return 2;
}
return 0;
}
inline vector2di& get_cell(const vector2df &pt, const vector2df &rstep, vector2di &ipt)
{
ipt.set(float2int(pt.x*rstep.x-0.5f),float2int(pt.y*rstep.y-0.5f));
return ipt;
}
inline void get_rect(const vector2di &ipt0,const vector2di &ipt1,vector2di *irect,const vector2di &isz)
{
irect[0].x = max(0,min(ipt0.x,ipt1.x)); irect[0].y = min(isz.y,max(0,min(ipt0.y,ipt1.y)));
irect[1].x = min(isz.x-1,max(ipt0.x,ipt1.x)); irect[1].y = min(isz.y,max(ipt0.y,ipt1.y));
}
inline int check_if_inside(int iobj, vector2di &ipt,const vector2di &isz,const vector2df *ptsrc, const vector2df &pt)
{
int bInside,i,bStop=0;
vector2df pt0,pt1,dp;
quotientf ycur;
if ((unsigned int)ipt.x>=(unsigned int)isz.x || ipt.y<0)
return 0;
for(bInside=0; ipt.y<=isz.y && !bStop; ipt.y++)
for(i=g_BoolGrid[ipt.y*isz.x+ipt.x]; i<g_BoolGrid[ipt.y*isz.x+ipt.x+1]; i++) if (g_BoolHash[i]>>31^iobj) {
pt0 = ptsrc[g_BoolHash[i]&0x7FFFFFFF]; pt1 = ptsrc[(g_BoolHash[i]&0x7FFFFFFF)+1]; dp = pt1-pt0;
ycur.set((dp^pt0)+pt.x*dp.y, dp.x).fixsign();
if (isneg(fabs_tpl((pt0.x+pt1.x)-pt.x*2)-fabs_tpl(pt0.x-pt1.x)) & isneg(pt.y-ycur))
{ bInside -= sgn(dp.x); bStop=1; }
}
return isneg(-bInside);
}
int boolean2d(booltype type, vector2df *ptbuf1,int npt1, vector2df *ptbuf2,int npt2,int bClosed, vector2df *&ptres,int *&pidres)
{
vector2df *ptsrc[2] = { ptbuf1,ptbuf2 };
int npt[2] = { npt1,npt2 };
vector2df dp,dp1,ptmin[2],ptmax[2],sz,ptbox[2],ptint[2],pttest,rstep;
int iobj,i,j,idx,nsz,n,npttmp,nptres,ninters=0,istart,ix,iy,bInside,bPrevInside,idx_next,idx_prev,inext,inext1,iobj1;
vector2di ipt0,ipt1,irect[2],isz;
float t,ratioxy,ratioyx;
static int __bforce1=0,__bforce2=0;
//for(i=0;i<max(npt1,npt2);i++) g_BoolIdBuf[i]=0;
pidres = g_BoolIdBuf;
if (npt1<3 || __bforce2)
{ ptres = ptbuf2; return npt2&-(bClosed|__bforce2); }
if (npt2<2+bClosed || __bforce1)
{ ptres = ptbuf1; return npt1&-(bClosed|__bforce1); }
for(iobj=0;iobj<2;iobj++) {
ptmin[iobj] = ptmax[iobj] = ptsrc[iobj][0];
for(i=1;i<npt[iobj];i++) {
ptmin[iobj].x = min(ptmin[iobj].x,ptsrc[iobj][i].x); ptmin[iobj].y = min(ptmin[iobj].y,ptsrc[iobj][i].y);
ptmax[iobj].x = max(ptmax[iobj].x,ptsrc[iobj][i].x); ptmax[iobj].y = max(ptmax[iobj].y,ptsrc[iobj][i].y);
}
}
ptbox[0].x = max(ptmin[0].x,ptmin[1].x); ptbox[0].y = max(ptmin[0].y,ptmin[1].y);
ptbox[1].x = min(ptmax[0].x,ptmax[1].x); ptbox[1].y = min(ptmax[0].y,ptmax[1].y);
sz = ptbox[1]-ptbox[0];
sz.x += fabs_tpl(sz.y)*1E-5f; sz.y += fabs_tpl(sz.x)*1E-5f;
if (sz.x<fabs_tpl(sz.y)*5E-6f || sz.y<fabs_tpl(sz.x)*5E-6f)
return 0;
ptbox[0] -= sz*0.01f; ptbox[1] += sz*0.01f; sz = ptbox[1]-ptbox[0];
sz.x += fabs_tpl(sz.y)*1E-5f; sz.y += fabs_tpl(sz.x)*1E-5f;
i = (int)(sizeof(g_BoolGrid)/sizeof(g_BoolGrid[0])-1);
npttmp = min(npt[0]+npt[1]<<1, i);
ratioyx = max(min(sz.y/sz.x,(float)npttmp),1.0f);
ratioxy = max(min(sz.x/sz.y,(float)npttmp),1.0f);
isz.y = max(1,float2int(cry_sqrtf(npttmp*4*ratioyx+1)*0.5f-1.0f));
isz.x = max(1,float2int(isz.y*ratioxy-0.5f));
if (isz.x*(isz.y+1)>i) {
isz.y = min(isz.y,i); isz.x = i/(isz.y+1);
}
rstep.set(isz.x/sz.x, isz.y/sz.y);
nsz = isz.x*(isz.y+1);
if (nsz>=sizeof(g_BoolGrid)/sizeof(g_BoolGrid[0])-1)
return 0;
npt[1] -= bClosed^1;
for(i=0;i<=nsz;i++) g_BoolGrid[i] = 0;
for(iobj=0;iobj<2;iobj++) { // calculate number of elements in each cell
for(get_cell(ptsrc[iobj][i=0]-ptbox[0],rstep,ipt0); i<npt[iobj]; i++) {
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
g_BoolGrid[iy*isz.x+ix]++;
ipt0 = ipt1;
}
}
for(i=1;i<=nsz;i++) g_BoolGrid[i]+=g_BoolGrid[i-1];
if (g_BoolGrid[nsz-1]>(int)(sizeof(g_BoolHash)/sizeof(g_BoolHash[0])))
return 0;
for(iobj=0;iobj<2;iobj++) { // put each line segment into the corresponding hash cell(s)
for(get_cell(ptsrc[iobj][i=0]-ptbox[0],rstep,ipt0); i<npt[iobj]; i++) {
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
g_BoolHash[--g_BoolGrid[iy*isz.x+ix]] = iobj<<31 | i;
ipt0 = ipt1;
}
}
// select iobj which is likely to have shorter edges
if (bClosed)
iobj = isneg((ptmax[1].x-ptmin[1].x+ptmax[1].y-ptmin[1].y)*npt[0] -
(ptmax[0].x-ptmin[0].x+ptmax[0].y-ptmin[0].y)*npt[1]);
else iobj = 1;
// build list of intersections by traversing iobj
if (!bClosed) {
g_BoolInters[0].pt = ptsrc[1][0];
g_BoolInters[0].iedge[0] = -1; g_BoolInters[0].iedge[1] = 0;
ninters = 1;
}
for(get_cell(ptsrc[iobj][0]-ptbox[0],rstep,ipt0),i=0; i<npt[iobj]; i++) {
get_cell(ptsrc[iobj][i+1]-ptbox[0],rstep,ipt1); get_rect(ipt0,ipt1,irect,isz);
istart = ninters;
for(ix=irect[0].x;ix<=irect[1].x;ix++) for(iy=irect[0].y;iy<=irect[1].y;iy++)
for(j=g_BoolGrid[iy*isz.x+ix]; j<g_BoolGrid[iy*isz.x+ix+1]; j++) if (g_BoolHash[j]>>31^iobj) {
for(n=line_seg_inters(ptsrc[iobj]+i, ptsrc[iobj^1]+(g_BoolHash[j]&0x7FFFFFFF), ptint)-1; n>=0; n--) {
dp = ptsrc[iobj][i+1]-ptsrc[iobj][i]; t = (ptint[n]-ptsrc[iobj][i])*dp;
for(idx=istart; idx<ninters && fabs_tpl((g_BoolInters[idx].pt-ptsrc[iobj][i])*dp-t)>t*1E-7f; idx++);
if (idx<ninters)
continue; // ignore possible intersections with the same line found in different cell
if (ninters==sizeof(g_BoolInters)/sizeof(g_BoolInters[0])-1)
return 0;
for(idx=ninters-1; idx>=istart && (g_BoolInters[idx].pt-ptsrc[iobj][i])*dp>t; idx--)
g_BoolInters[idx+1] = g_BoolInters[idx];
g_BoolInters[idx+1].pt = ptint[n];
g_BoolInters[idx+1].iedge[iobj] = i;
g_BoolInters[idx+1].iedge[iobj^1] = g_BoolHash[j]&0x7FFFFFFF;
ninters++;
}
}
ipt0 = ipt1;
}
if (!bClosed) {
if (ninters==sizeof(g_BoolInters)/sizeof(g_BoolInters[0])-1)
return 0;
g_BoolInters[ninters].pt = ptsrc[1][npt[1]];
g_BoolInters[ninters].iedge[0] = -1; g_BoolInters[ninters].iedge[1] = npt[1];
g_BoolInters[ninters+1] = g_BoolInters[ninters]; ninters++;
} else
g_BoolInters[ninters] = g_BoolInters[0];
nptres = 0; ptres = g_BoolPtBuf;
// if there were no intersections, return the object that is inside the other
if (ninters-(bClosed^1)*2==0) {
get_cell(ptsrc[iobj][0]-ptbox[0],rstep,ipt0);
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],ptsrc[iobj][0]);
npt[1] += bClosed^1;
if (bClosed) // assume that objects cannot have empty intersection area, thus if iobj is not inside iobj^1, ibj1^1 should be inside iobj
iobj ^= bInside^1;
ptres = ptsrc[iobj];
for(nptres=0;nptres<npt[iobj];nptres++)
g_BoolIdBuf[nptres] = nptres+1<<iobj*16;
return nptres & -(bClosed|bInside);
}
npt[1] += bClosed^1; // compensate for 1 subtracted earlier
// build boolean intersection by at each intersection point selecting the stripe that is more 'inward' than the other one
for(idx=bPrevInside=0; idx<ninters; idx++) {
idx_next = idx+1; //& ~(ninters-2-idx>>31);
idx_prev = idx-1; idx_prev = idx_prev&~(idx_prev>>31) | ninters-1&idx_prev>>31;
i = g_BoolInters[idx].iedge[iobj]; inext = i+1 & i+1-npt[iobj]>>31;
dp = ptsrc[iobj][inext]-ptsrc[iobj][i];
inext1 = min(inext+1,npt[iobj]-1) & (i+1-npt[iobj]>>31 | ~-bClosed);
j = g_BoolInters[idx].iedge[iobj^1];
if (j>=0) {
dp1 = ptsrc[iobj^1][j+1 & j+1-npt[iobj^1]>>31]-ptsrc[iobj^1][j];
bInside = isneg(dp^dp1);
} else {
pttest = g_BoolInters[idx].pt;
get_cell(pttest-ptbox[0],rstep,ipt0);
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],pttest);
}
/*
// build boolean intersection by selecting fragments (stripes between 2 intersection points) of the object that is inside another object
// cannot just switch "inside" state at each intersection, since it's less stable in case of degenerate contacts
// find some representative point for this fragment
if (g_BoolInters[idx_next].iedge[iobj]==i && (g_BoolInters[idx_next].pt-ptsrc[iobj][i])*dp>(g_BoolInters[idx].pt-ptsrc[iobj][i])*dp)
pttest = (g_BoolInters[idx].pt+g_BoolInters[idx_next].pt)*0.5f; // next intersection lies on the same edge
else if ((g_BoolInters[idx].pt-ptsrc[iobj][i])*dp < dp.len2()*0.95f*0.95f)
pttest = (g_BoolInters[idx].pt+ptsrc[iobj][inext1])*0.5f; // this intersection is not too close to the edge end
else if (g_BoolInters[idx_next].iedge[iobj]==inext)
pttest = (ptsrc[iobj][inext]+g_BoolInters[idx_next].pt)*0.5f; // next intersection lies on the next edge
else
pttest = (ptsrc[iobj][inext]+ptsrc[iobj][inext1])*0.5f;
// check if a ray upwards from pttest first hits right-to-left edge of iobj^1, in this case we are inside
get_cell(pttest-ptbox[0],rstep,ipt0);
bInside = check_if_inside(iobj,ipt0,isz,ptsrc[iobj^1],pttest);*/
iobj1 = iobj^bInside^1;
if (bInside | bPrevInside | bClosed) {
g_BoolPtBuf[nptres] = g_BoolInters[idx].pt;
g_BoolIdBuf[nptres++] = g_BoolInters[idx].iedge[1]+1<<16 | g_BoolInters[idx].iedge[0]+1;
if (nptres>=(int)(sizeof(g_BoolPtBuf)/sizeof(g_BoolPtBuf[0])))
return nptres;
}
if (bInside | bClosed) {
i = g_BoolInters[idx].iedge[iobj1];
inext = i+1 & ~(npt[iobj1]-2-i>>31);
dp = ptsrc[iobj1][inext]-ptsrc[iobj1][i];
bool bForceFirstStep = i==g_BoolInters[idx_next].iedge[iobj1] &&
(g_BoolInters[idx].pt-ptsrc[iobj1][i])*dp > (g_BoolInters[idx_next].pt-ptsrc[iobj1][i])*dp;
for(; bForceFirstStep ||
i!=g_BoolInters[idx_next].iedge[iobj1] && (iobj1==iobj || i!=g_BoolInters[idx_prev].iedge[iobj1]);
i=i+1&~(npt[iobj1]-2-i>>31))
{
g_BoolPtBuf[nptres] = ptsrc[iobj1][i+1];
g_BoolIdBuf[nptres++] = i+2<<iobj1*16;
if (nptres>=(int)(sizeof(g_BoolPtBuf)/sizeof(g_BoolPtBuf[0])))
return nptres;
bForceFirstStep = false;
}
}
bPrevInside = bInside;
}
return nptres;
}

485
CryPhysics/boxgeom.cpp Normal file
View File

@@ -0,0 +1,485 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "overlapchecks.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "singleboxtree.h"
#include "geometry.h"
#include "trimesh.h"
#include "boxgeom.h"
vector2df g_BoxCont[8];
int g_BoxVtxId[8],g_BoxEdgeId[8];
CBoxGeom* CBoxGeom::CreateBox(box *pbox)
{
m_box.center = pbox->center;
m_box.size = pbox->size;
m_box.Basis = pbox->Basis;
m_box.bOriented = m_box.Basis.IsIdentity()^1;
m_Tree.SetBox(&m_box);
m_Tree.Build(this);
m_minVtxDist = (m_box.size.x+m_box.size.y+m_box.size.z)*1E-4f;
return this;
}
int CBoxGeom::CalcPhysicalProperties(phys_geometry *pgeom)
{
pgeom->pGeom = this;
pgeom->origin = m_box.center;
if (!m_box.bOriented)
pgeom->q.SetIdentity();
else
pgeom->q = quaternionf(m_box.Basis.T());
pgeom->V = m_box.size.volume()*8;
pgeom->Ibody.Set(sqr(m_box.size.y)+sqr(m_box.size.z), sqr(m_box.size.x)+sqr(m_box.size.z),
sqr(m_box.size.x)+sqr(m_box.size.y)) *= pgeom->V*(1.0/3);
return 1;
}
int CBoxGeom::PointInsideStatus(const vectorf &pt)
{
vectorf ptloc = pt-m_box.center;
if (m_box.bOriented)
ptloc = m_box.Basis*ptloc;
return isneg(fabsf(ptloc.x)-m_box.size.x) & isneg(fabsf(ptloc.y)-m_box.size.y) & isneg(fabsf(ptloc.z)-m_box.size.z);
}
int CBoxGeom::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
static short g_BoxIdBuf[3];
static surface_desc g_BoxSurfaceBuf[3];
static edge_desc g_BoxEdgeBuf[3];
pGTest->pGeometry = this;
pGTest->pBVtree = &m_Tree;
m_Tree.PrepareForIntersectionTest(pGTest);
pGTest->primbuf = pGTest->primbuf1 = g_BoxBuf+g_BoxBufPos++;
pGTest->szprimbuf = 1;
pGTest->typeprim = box::type;
pGTest->szprim = sizeof(box);
pGTest->idbuf = g_BoxIdBuf;
pGTest->surfaces = g_BoxSurfaceBuf;
pGTest->edges = g_BoxEdgeBuf;
pGTest->minAreaEdge = 0;
return 1;
}
void CBoxGeom::PrepareBox(box *pbox, geometry_under_test *pGTest)
{
pbox->center = pGTest->R*m_box.center*pGTest->scale + pGTest->offset;
if (!m_box.bOriented) pbox->Basis = pGTest->R.T();
else pbox->Basis = m_box.Basis*pGTest->R.T();
pbox->bOriented = 1;
pbox->size = m_box.size*pGTest->scale;
}
int CBoxGeom::PreparePrimitive(geom_world_data *pgwd, primitive *&pprim)
{
box *pbox = g_BoxBuf+g_BoxBufPos;
g_BoxBufPos = g_BoxBufPos+1 & sizeof(g_BoxBuf)/sizeof(g_BoxBuf[0])-1;
pbox->center = pgwd->R*m_box.center*pgwd->scale + pgwd->offset;
if (!m_box.bOriented) pbox->Basis = pgwd->R.T();
else pbox->Basis = m_box.Basis*pgwd->R.T();
pbox->bOriented = 1;
pbox->size = m_box.size*pgwd->scale;
pprim = pbox;
return box::type;
}
int CBoxGeom::GetFeature(int iPrim,int iFeature, vectorf *pt)
{
int i,sg,ix,iy,iz;
switch (iFeature & 0x60) {
case 0: // vertex
pt->x=m_box.size.x*((iFeature<<1&2)-1); pt->y=m_box.size.y*((iFeature&2)-1);
pt->z=m_box.size.z*((iFeature>>1&2)-1); pt[0] = pt[0]*m_box.Basis+m_box.center; return 1;
case 0x20: // edge
iz = iFeature>>2&3; ix = inc_mod3[iz]; iy = dec_mod3[iz];
pt[0][ix]=pt[1][ix]=m_box.size[ix]*((iFeature<<1&2)-1); pt[0][iy]=pt[1][iy]=m_box.size[iy]*((iFeature&2)-1);
pt[0][iz]=-m_box.size[iz]; pt[1][iz]=m_box.size[iz];
pt[0] = pt[0]*m_box.Basis+m_box.center; pt[1] = pt[1]*m_box.Basis+m_box.center; return 2;
case 0x40: // face
iz = iFeature>>1&3; ix = inc_mod3[iz]; iy = dec_mod3[iz]; sg = (iFeature<<1&2)-1;
for(i=0;i<4;i++) {
pt[i^(sg&3)][ix] = m_box.size[ix]*(1-((i^i<<1)&2));
pt[i^(sg&3)][iy] = m_box.size[iy]*(1-(i&2));
pt[i^(sg&3)][iz] = m_box.size[iz]*sg;
pt[i^(sg&3)] = pt[i^(sg&3)]*m_box.Basis+m_box.center;
}
return 4;
}
return 0;
}
int CBoxGeom::GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId)
{
PrepareBox((box*)pRes, pGTest);
box *pbox = (box*)pRes;
float szmax=max(max(pbox->size.x,pbox->size.y),pbox->size.z), szmin=min(min(pbox->size.x,pbox->size.y),pbox->size.z);
if (pGTestOp->ptOutsidePivot.x<1E10 && szmin*8.0f<szmax) {
int iz=idxmin3((float*)&pbox->size), ix=inc_mod3[iz], iy=dec_mod3[iz];
vectorf ptloc = pbox->Basis*(pGTestOp->ptOutsidePivot-pbox->center);
if (ptloc[ix]<pbox->size[ix] && ptloc[iy]<pbox->size[iy]) {
pbox->center -= pbox->Basis.GetRow(iz)*(sgnnz(ptloc[iz])*szmax);
pbox->size[iz] += szmax;
}
}
pGTest->bTransformUpdated = 0; *pResId = -1;
return 1;
}
int CBoxGeom::GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest)
{
if (pGTest->bTransformUpdated) {
pprim = pGTest->primbuf1;
PrepareBox((box*)pprim, pGTest);
}
box *pbox = (box*)pprim;
pGTest->idbuf[0] = -1;
int iFeature = pcontact->iFeature[iop],ix,iy,iz;
vectorf ptloc = pbox->Basis*(pcontact->pt-pbox->center);
if ((iFeature&0xE0)==0x40) { // check if the point on the face is close to some edge
iz=iFeature>>1 & 3; ix=inc_mod3[iz]; iy=dec_mod3[iz];
if (fabsf(ptloc[ix]-pbox->size[ix]) < pbox->size[ix]*0.001f)
iFeature = 0x20 | ix<<2 | (iFeature&1<<1) | sgnnz(ptloc[iy])+1>>1;
else if (fabsf(ptloc[iy]-pbox->size[iy]) < pbox->size[iy]*0.001f)
iFeature = 0x20 | iy<<2 | sgnnz(ptloc[ix])+1 | iFeature&1;
}
if ((iFeature&0xE0)==0x20) { // check if the point on the edge is close to one of its ends
iz = iFeature>>2 & 3; ix=inc_mod3[iz]; iy=dec_mod3[iz];
if (fabsf(ptloc[iz]-pbox->size[iz])<pbox->size[iz]*0.001f)
iFeature = (iFeature&1)<<ix | (iFeature>>1&1)<<iy | (sgnnz(ptloc[iz])+1>>1)<<iz;
}
if ((iFeature&0xE0)==0x40) {
iz = iFeature>>1 & 3;
pGTest->surfaces[0].n = pbox->Basis.GetRow(iz)*((iFeature<<1&2)-1);
pGTest->surfaces[0].idx = 0;
pGTest->surfaces[0].iFeature = iFeature;
pGTest->nSurfaces = 1;
pGTest->nEdges = 0;
} else if ((iFeature&0xE0)==0x20) {
iz = iFeature>>2 & 3; ix=inc_mod3[iz]; iy=dec_mod3[iz];
pGTest->surfaces[0].idx = pGTest->surfaces[1].idx = 0;
pGTest->edges[0].n[0] = -(pGTest->surfaces[0].n = pbox->Basis.GetRow(ix)*((iFeature<<1&2)-1));
pGTest->edges[0].n[1] = -(pGTest->surfaces[1].n = pbox->Basis.GetRow(iy)*((iFeature&2)-1));
pGTest->surfaces[0].iFeature = 0x40 | ix<<1 | iFeature&1;
pGTest->surfaces[1].iFeature = 0x40 | iy<<1 | iFeature>>1&1;
pGTest->nSurfaces = 2;
pGTest->edges[0].dir = pbox->Basis.GetRow(iz);
pGTest->edges[0].idx = 0;
pGTest->edges[0].iFeature = iFeature;
pGTest->nEdges = 1;
} else {
for(iz=0;iz<3;iz++) {
pGTest->surfaces[iz].idx = pGTest->edges[iz].idx = 0;
pGTest->edges[iz].dir = pGTest->surfaces[iz].n = pbox->Basis.GetRow(iz)*(((iFeature>>iz)<<1&2)-1);
pGTest->surfaces[iz].iFeature = 0x40 | iz<<1 | iFeature>>iz&1;
ix = inc_mod3[iz]; iy = dec_mod3[iz];
pGTest->edges[iz].n[0] = pbox->Basis.GetRow(ix)*(1-((iFeature>>ix)<<1&2));
pGTest->edges[iz].n[1] = pbox->Basis.GetRow(iy)*(1-((iFeature>>iy)<<1&2));
pGTest->edges[iz].iFeature = 0x20 | iz<<2 | (iFeature>>iy&1)<<1 | iFeature>>ix&1;
}
pGTest->nSurfaces = pGTest->nEdges = 3;
}
return 1;
}
int CBoxGeom::PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
int i,sg;
vectorf size,center,pt;
matrix3x3f R;
if (!m_box.bOriented) R = pGTest->R;
else R = pGTest->R*m_box.Basis.T();
Vec3_tpl<int> ic;
size = m_box.size*pGTest->scale;
ic.z = iFeature>>1&3; ic.x = inc_mod3[ic.z]; ic.y = dec_mod3[ic.z]; sg = (iFeature<<1&2)-1;
center = pGTest->R*m_box.center*pGTest->scale + pGTest->offset + R.GetColumn(ic.z)*(size[ic.z]*sg);
if (psurface->n.len2()==0) {
psurface->n = R.GetColumn(ic.z)*sg;
psurface->axes[0] = R.GetColumn(ic.x);
psurface->axes[1] = R.GetColumn(ic.y)*sg;
psurface->origin = center; sg &= 3;
} else
sg = -isneg(psurface->n*R.GetColumn(ic.z))&3;
//sg = sgnnz(psurface->n*R.GetColumn(ic.z))>>31 & 3;
center -= psurface->origin;
for(i=0;i<4;i++) {
pt[ic.x] = size[ic.x]*(1-((i^i<<1)&2));
pt[ic.y] = size[ic.y]*(1-(i&2));
pt[ic.z] = 0;
pt = center+R*pt;
g_BoxCont[i^sg].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_BoxEdgeId[(i^sg)-(sg&1) & 3] = 0x20 | ic[i&1]<<2 | (iFeature&1)<<(i&1) | (((i^i>>1)&1)^1)<<((i&1)^1);
g_BoxVtxId[i^sg] = (iFeature&1)<<ic.z | ((i^i>>1)&1)<<ic.x | (i>>1&1)<<ic.y;
}
g_BoxCont[4] = g_BoxCont[0];
ptbuf = g_BoxCont; pVtxIdBuf = g_BoxVtxId; pEdgeIdBuf = g_BoxEdgeId;
return 4;
}
int CBoxGeom::PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
int ix,iy,iz;
vectorf pt,offs;
matrix3x3f R;
if (!m_box.bOriented) R = pGTest->R;
else R = pGTest->R*m_box.Basis.T();
iz=iFeature>>2&3; ix=inc_mod3[iz]; iy=dec_mod3[iz];
offs[ix] = m_box.size[ix]*((iFeature<<1&2)-1);
offs[iy] = m_box.size[iy]*((iFeature&2)-1);
offs[iz] = -m_box.size[iz];
pt = (pGTest->R*m_box.center+R*offs)*pGTest->scale + pGTest->offset - psurface->origin;
g_BoxCont[0].set(pt*psurface->axes[0],pt*psurface->axes[1]);
offs[ix]=offs[iy] = 0; offs[iz] = m_box.size[iz]*2;
pt += R*offs*pGTest->scale;
g_BoxCont[1].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_BoxEdgeId[0] = g_BoxEdgeId[1] = iFeature;
g_BoxVtxId[0] = (iFeature&1)<<ix | (iFeature>>1&1)<<iy;
g_BoxVtxId[1] = g_BoxVtxId[0] | 1<<iz;
g_BoxCont[2] = g_BoxCont[0];
ptbuf = g_BoxCont; pVtxIdBuf = g_BoxVtxId; pEdgeIdBuf = g_BoxEdgeId;
return 2;
}
int CBoxGeom::FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters)
{
int i,iz,nFacesi,nFaces[2],bLine;
vectori sg[2],idir[2],sgi,idiri;
vectorf ptloc[2],size,diri,dir[2],ptdst[2]={ ptdst0,ptdst1 };
ptres[0].Set(1E10f,1E10f,1E10f);
ptres[1] = ptdst0;
ptloc[0] = (ptdst0-pgwd->offset)*pgwd->R-m_box.center*pgwd->scale;
if (m_box.bOriented)
ptloc[0] = m_box.Basis*ptloc[0];
size = m_box.size*pgwd->scale;
iPrim = 0;
if (bLine = isneg(size.x*1E-5f-(ptdst0-ptdst1).len2())) {
int ix,iy,iedge,ivtx;
vectorf ptbox,n;
float n2,dir2,t0,t1;
ptloc[1] = (ptdst1-pgwd->offset)*pgwd->R-m_box.center*pgwd->scale;
if (m_box.bOriented)
ptloc[1] = m_box.Basis*ptloc[1];
diri = ptloc[1]-ptloc[0]; dir2 = diri.len2();
for(iz=0;iz<3;iz++) {
n = cross_with_ort(diri,iz); n2 = n.len2();
ptbox[iz] = 0; ix = inc_mod3[iz]; iy = dec_mod3[iz];
for(iedge=0;iedge<4;iedge++) {
ptbox[ix] = size[ix]*((iedge&1)*2-1); ptbox[iy] = size[iy]*((iedge&2)-1);
t0 = cross_with_ort(ptbox-ptloc[0],iz)*n;
t1 = (ptbox-ptloc[0]^diri)*n;
if (inrange(t0, (float)0,n2) & isneg(fabs_tpl(t1)-n2*size[iz])) {
iFeature = 0x20|iz<<2|iedge; n2 = 1.0f/n2;
ptbox[iz] = t1*n2;
ptres[0] = pgwd->R*(ptbox*m_box.Basis+m_box.center*pgwd->scale)+pgwd->offset;
ptres[1] = pgwd->R*((ptloc[0]+diri*(t0*n2))*m_box.Basis+m_box.center*pgwd->scale)+pgwd->offset;
return 1;
}
}
}
for(ivtx=0;ivtx<8;ivtx++) {
ptbox.x = size.x*((ivtx&1)*2-1); ptbox.y = size.y*((ivtx&2)-1); ptbox.z = size.z*((ivtx>>1&2)-1);;
if (inrange(t0=(ptbox-ptloc[0])*diri,(float)0,dir2)) {
iFeature = ivtx;
ptres[0] = pgwd->R*(ptbox*m_box.Basis+m_box.center*pgwd->scale)+pgwd->offset;
ptres[1] = pgwd->R*((ptloc[0]+diri*(t0/dir2))*m_box.Basis+m_box.center*pgwd->scale)+pgwd->offset;
return 1;
}
}
}
dir[1].zero(); nFaces[1]=0;
for(i=0;i<=bLine;i++) {
for(iz=nFacesi=0;iz<3;iz++) {
sgi[nFacesi] = sgnnz(ptloc[i][iz]);
diri[iz] = max(0.0f,fabsf(ptloc[i][iz])-size[iz])*-sgi[nFacesi];
idiri[nFacesi] = iz; nFacesi += isneg(1E-4f-fabsf(diri[iz]));
}
nFaces[i]=nFacesi; sg[i]=sgi;
dir[i]=diri; idir[i]=idiri;
}
if (nFaces[0]+nFaces[1]==0)
return -1;
i = isneg(dir[1].len2()-dir[0].len2()) & isneg(-nFaces[1]);
if (nFaces[i]==1)
iFeature = 0x40 | idir[i][0]<<1 | sg[i][0]+1>>1;
else if (nFaces[i]==2) {
iz = 3-idir[i][0]-idir[i][1];
iFeature = 0x20 | iz<<2 | (sg[i][0]+1>>1)<<(iz&1) | sg[i][1]+1>>(iz&1);
} else
iFeature = sg[i][2]+1<<1 | sg[i][1]+1 | sg[i][0]+1>>1;
ptloc[i] += dir[i];
if (m_box.bOriented)
ptloc[i] = ptloc[i]*m_box.Basis;
ptres[0] = pgwd->R*(ptloc[i]+m_box.center*pgwd->scale)+pgwd->offset;
ptres[1] = ptdst[i];
return 1;
}
void CBoxGeom::BuildTriMesh(CTriMesh &mesh)
{
static vectorf normals[12],vtx[8];
static int idx[36] = { 0,4,6, 6,2,0, 1,3,7, 7,5,1, 0,1,5, 5,4,0, 2,6,7, 7,3,2, 0,2,3, 3,1,0, 4,5,7, 7,6,4 };
mesh.m_nTris = 12;
mesh.m_nVertices = 8;
mesh.m_pNormals = normals;
mesh.m_pVertices = vtx;
mesh.m_pIndices = idx;
mesh.m_flags = 4;
int i; for(i=0;i<8;i++) {
vtx[i].Set(m_box.size.x*((i<<1&2)-1),m_box.size.y*((i&2)-1),m_box.size.z*((i>>1&2)-1));
if (m_box.bOriented) vtx[i] = vtx[i]*m_box.Basis;
vtx[i] += m_box.center;
}
for(i=0;i<6;i++) {
normals[i*2].Set(0,0,0); normals[i*2][i>>1] = ((i&1)<<1)-1;
if (m_box.bOriented) normals[i*2] = normals[i*2]*m_box.Basis;
normals[i*2+1] = normals[i*2];
}
}
int CBoxGeom::UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact)
{
sphere sph;
sph.center = center;
sph.r = rsep;
if (!box_sphere_overlap_check(&m_box,&sph))
return 0;
unprojection_mode umode;
umode.dir.Set(0,0,0);
box_sphere_lin_unprojection(&umode,&m_box,-1,&sph,-1,pcontact,0);
pcontact->pt -= umode.dir*pcontact->t;
return 1;
}
void CBoxGeom::CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L)
{
CTriMesh mesh; BuildTriMesh(mesh);
mesh.CalcVolumetricPressure(gwd,epicenter,k,rmin,centerOfMass,P,L);
}
float CBoxGeom::CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter)
{
CTriMesh mesh; BuildTriMesh(mesh);
return mesh.CalculateBuoyancy(pplane,pgwd,massCenter);
}
void CBoxGeom::CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres)
{
int i,j,ix,iy,iz;
vectorf pt[4],n, size=m_box.size*pgwd->scale, offset=pgwd->R*m_box.center*pgwd->scale+pgwd->offset;
matrix3x3f R;
if (!m_box.bOriented) R = pgwd->R;
else R = pgwd->R*m_box.Basis.T();
dPres.Set(0,0,0); dLres.zero();
for(i=0;i<6;i++) {
iz=i>>1; ix=inc_mod3[iz]; iy=dec_mod3[iz];
n.Set(0,0,0); n[iz] = ((i<<1&2)-1);
for(j=0;j<4;j++) {
pt[j][ix] = size[ix]*(1-((j^j<<1)&2));
pt[j][iy] = size[iy]*((j&2^i<<1&2)-1);
pt[j][iz] = size[iz]*n[iz];
pt[j] = R*pt[j]+offset;
}
CalcMediumResistance(pt,4,R*n, *pplane, pgwd->v,pgwd->w,pgwd->centerOfMass, dPres,dLres);
}
}
int CBoxGeom::DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale)
{
int i,j,ix,iy,iz;
vectorf pt[4],n, size=m_box.size*pgwd->scale, offset=pgwd->R*m_box.center*pgwd->scale+pgwd->offset;;
matrix3x3f R;
if (!m_box.bOriented) R = pgwd->R;
else R = pgwd->R*m_box.Basis.T();
for(i=0;i<6;i++) {
iz=i>>1; ix=inc_mod3[iz]; iy=dec_mod3[iz];
n.Set(0,0,0); n[iz] = (((i&1)<<1)-1);
for(j=0;j<4;j++) {
pt[j][ix] = size[ix]*(1-((j^j<<1)&2));
pt[j][iy] = size[iy]*((j&2^i<<1&2)-1);
pt[j][iz] = size[iz]*n[iz];
pt[j] = R*pt[j]+offset;
}
RasterizePolygonIntoCubemap(pt,4, iPass, pGrid,nRes, rmin,rmax,zscale);
}
return 1;
}
void CBoxGeom::DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *pgwd, int iLevel)
{
int i,j;
vectorf pt[8], size=m_box.size*pgwd->scale, offset=pgwd->R*m_box.center*pgwd->scale+pgwd->offset;
matrix3x3f R;
if (!m_box.bOriented) R = pgwd->R;
else R = pgwd->R*m_box.Basis.T();
for(i=0;i<8;i++)
pt[i] = R*vectorf(size.x*((i<<1&2)-1),size.y*((i&2)-1),size.z*((i>>1&2)-1))+offset;
for(i=0;i<8;i++) for(j=0;j<3;j++) if (i&1<<j)
DrawLineFunc(pt[i],pt[i^1<<j]);
}
void CBoxGeom::GetMemoryStatistics(ICrySizer *pSizer)
{
pSizer->AddObject(this, sizeof(CBoxGeom));
}
void CBoxGeom::Save(CMemStream &stm)
{
stm.Write(m_box);
m_Tree.Save(stm);
}
void CBoxGeom::Load(CMemStream &stm)
{
stm.Read(m_box);
m_Tree.Load(stm,this);
}

54
CryPhysics/boxgeom.h Normal file
View File

@@ -0,0 +1,54 @@
#ifndef boxgeom_h
#define boxgeom_h
#pragma once
class CTriMesh;
class CBoxGeom : public CPrimitive {
public:
CBoxGeom() { m_iCollPriority = 3; m_minVtxDist = 0; }
CBoxGeom* CreateBox(box *pcyl);
void PrepareBox(box *pbox, geometry_under_test *pGTest);
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim);
virtual int GetType() { return GEOM_BOX; }
virtual void GetBBox(box *pbox) { m_Tree.GetBBox(pbox); }
virtual int CalcPhysicalProperties(phys_geometry *pgeom);
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters);
virtual int PointInsideStatus(const vectorf &pt);
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L);
virtual float CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter);
virtual void CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres);
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale);
virtual CBVTree *GetBVTree() { return &m_Tree; }
virtual void DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel);
virtual int GetPrimitive(int iPrim, primitive *pprim) { *(box*)pprim = m_box; return sizeof(box); }
virtual int GetFeature(int iPrim,int iFeature, vectorf *pt);
virtual int UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false);
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId);
virtual int GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest);
virtual int PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
virtual int PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm);
void BuildTriMesh(CTriMesh &mesh);
box m_box;
CSingleBoxTree m_Tree;
};
#endif

142
CryPhysics/bvtree.h Normal file
View File

@@ -0,0 +1,142 @@
#ifndef bvtree_h
#define bvtree_h
extern indexed_triangle g_IdxTriBuf[256];
extern int g_IdxTriBufPos;
extern cylinder g_CylBuf[2];
extern int g_CylBufPos;
extern sphere g_SphBuf[2];
extern int g_SphBufPos;
extern box g_BoxBuf[2];
extern int g_BoxBufPos;
////////////////////////// bounding volumes ////////////////////////
struct BV {
int type;
int iNode;
operator primitive*() { return (primitive*)((char*)this+sizeof(type)+sizeof(iNode)); }
};
struct BBox : BV {
box abox;
};
extern BBox g_BBoxBuf[128];
extern int g_BBoxBufPos;
struct BVheightfield : BV {
heightfield hf;
};
struct BVray : BV {
ray aray;
};
inline void ResetGlobalPrimsBuffers()
{
g_BBoxBufPos = 0;
g_IdxTriBufPos = 0;
g_CylBufPos = 0;
g_BoxBufPos = 0;
g_SphBufPos = 0;
}
class CGeometry;
class CBVTree;
struct surface_desc {
vectorf n;
int idx;
int iFeature;
};
struct edge_desc {
vectorf dir;
vectorf n[2];
int idx;
int iFeature;
};
struct geometry_under_test {
CGeometry *pGeometry;
CBVTree *pBVtree;
int *pUsedNodesMap;
int *pUsedNodesIdx;
int nUsedNodes;
int nMaxUsedNodes;
int bStopIntersection;
int bCurNodeUsed;
matrix3x3f R,R_rel;
vectorf offset,offset_rel;
float scale,rscale, scale_rel,rscale_rel;
int bTransformUpdated;
vectorf v;
vectorf w,centerOfMass;
vectorf centerOfRotation;
intersection_params *pParams;
vectorf axisContactNormal;
vectorf sweepdir,sweepdir_loc;
float sweepstep,sweepstep_loc;
vectorf ptOutsidePivot;
int typeprim;
primitive *primbuf; // used to get node contents
primitive *primbuf1;// used to get unprojection candidates
int szprimbuf,szprimbuf1;
int *iFeature_buf; // feature that led to this primitive
short *idbuf; // ids of unprojection candidates
int szprim;
surface_desc *surfaces; // the last potentially surfaces
edge_desc *edges; // the last potentially contacting edges
int nSurfaces,nEdges;
float minAreaEdge;
geom_contact *contacts;
int *pnContacts;
int nMaxContacts;
};
enum BVtreetypes { BVT_OBB=0, BVT_AABB=1, BVT_SINGLEBOX=2, BVT_RAY=3, BVT_HEIGHTFIELD=4 };
class CBVTree {
public:
virtual ~CBVTree() {}
virtual int GetType() = 0;
virtual void GetBBox(box *pbox) {}
virtual int MaxPrimsInNode() { return 1; }
virtual float Build(CGeometry *pGeom) = 0;
virtual void SetGeomConvex() {}
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest) {
pGTest->pUsedNodesMap = 0;
pGTest->pUsedNodesIdx = 0;
pGTest->nMaxUsedNodes = 0;
pGTest->nUsedNodes = -1;
return 1;
}
virtual void CleanupAfterIntersectionTest(geometry_under_test *pGTest) {}
virtual void GetNodeBV(BV *&pBV, int iNode=0) = 0;
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) = 0;
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0) = 0;
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) = 0;
virtual float SplitPriority(const BV* pBV) { return 0.0f; }
virtual void GetNodeChildrenBVs(const matrix3x3f &Rw,const vectorf &offsw,float scalew, const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2) {}
virtual void GetNodeChildrenBVs(const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2) {}
virtual void GetNodeChildrenBVs(const BV *pBV_parent, const vectorf &sweepdir,float sweepstep, BV *&pBV_child1,BV *&pBV_child2) {}
virtual void ReleaseLastBVs() {}
virtual void ReleaseLastSweptBVs() {}
virtual void GetMemoryStatistics(ICrySizer *pSizer) {}
virtual void Save(CMemStream &stm) {}
virtual void Load(CMemStream &stm, CGeometry *pGeom) {}
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp) = 0;
virtual int GetNodeContentsIdx(int iNode, int &iStartPrim) { iStartPrim=0; return 1; }
virtual void MarkUsedTriangle(int itri, geometry_under_test *pGTest) {}
};
#endif

617
CryPhysics/cylindergeom.cpp Normal file
View File

@@ -0,0 +1,617 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "singleboxtree.h"
#include "geometry.h"
#include "cylindergeom.h"
#include "trimesh.h"
#include "boxgeom.h"
vector2df g_CylCont[64];
int g_CylContId[64];
CCylinderGeom* CCylinderGeom::CreateCylinder(cylinder *pcyl)
{
m_cyl.center = pcyl->center;
m_cyl.axis = pcyl->axis;
m_cyl.hh = pcyl->hh;
m_cyl.r = pcyl->r;
box bbox;
bbox.Basis.SetRow(2,m_cyl.axis);
//bbox.Basis.SetRow(0,m_cyl.axis.orthogonal().normalized());
bbox.Basis.SetRow(0,GetOrthogonal(m_cyl.axis).normalized());
bbox.Basis.SetRow(1,m_cyl.axis ^ bbox.Basis.GetRow(0));
bbox.bOriented = 1;
bbox.center = m_cyl.center;
bbox.size.z = m_cyl.hh;
bbox.size.x = bbox.size.y = m_cyl.r;
m_Tree.SetBox(&bbox);
m_Tree.Build(this);
m_minVtxDist = (m_cyl.r+m_cyl.hh)*1E-4f;
return this;
}
int CCylinderGeom::CalcPhysicalProperties(phys_geometry *pgeom)
{
pgeom->pGeom = this;
pgeom->origin = m_cyl.center;
//pgeom->q = quaternionf(vectorf(0,0,1),m_cyl.axis);
pgeom->q.SetRotationV0V1(vectorf(0,0,1),m_cyl.axis);
pgeom->V = sqr(m_cyl.r)*m_cyl.hh*(pi*2);
float x2=pi*m_cyl.hh*sqr(sqr(m_cyl.r))*(1.0/2), z2=pi*sqr(m_cyl.r)*cube(m_cyl.hh)*(2.0/3);
pgeom->Ibody.Set(x2+z2,x2+z2,x2*2);
return 1;
}
int CCylinderGeom::PointInsideStatus(const vectorf &pt)
{
vectorf ptr; float h;
ptr = pt-m_cyl.center; h = m_cyl.axis*ptr; ptr -= m_cyl.axis*h;
return isneg(ptr.len2()-sqr(m_cyl.r)) & isneg(fabs_tpl(h)-m_cyl.hh);
}
int CCylinderGeom::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
static short g_CylIdBuf[1];
static surface_desc g_CylSurfaceBuf[1];
static edge_desc g_CylEdgeBuf[1];
pGTest->pGeometry = this;
pGTest->pBVtree = &m_Tree;
m_Tree.PrepareForIntersectionTest(pGTest);
pGTest->primbuf = pGTest->primbuf1 = g_CylBuf+g_CylBufPos++;
pGTest->szprimbuf = 1;
pGTest->typeprim = cylinder::type;
pGTest->szprim = sizeof(cylinder);
pGTest->idbuf = g_CylIdBuf;
pGTest->surfaces = g_CylSurfaceBuf;
pGTest->edges = g_CylEdgeBuf;
pGTest->minAreaEdge = pi*m_cyl.r*2.0f/m_nTesselation;
return 1;
}
void CCylinderGeom::PrepareCylinder(cylinder *pcyl, geometry_under_test *pGTest)
{
pcyl->center = pGTest->R*m_cyl.center*pGTest->scale + pGTest->offset;
pcyl->axis = pGTest->R*m_cyl.axis;
pcyl->hh = m_cyl.hh*pGTest->scale;
pcyl->r = m_cyl.r*pGTest->scale;
}
int CCylinderGeom::PreparePrimitive(geom_world_data *pgwd, primitive *&pprim)
{
cylinder *pcyl = g_CylBuf+g_CylBufPos;
g_CylBufPos = g_CylBufPos+1 & sizeof(g_CylBuf)/sizeof(g_CylBuf[0])-1;
pcyl->center = pgwd->R*m_cyl.center*pgwd->scale + pgwd->offset;
pcyl->axis = pgwd->R*m_cyl.axis;
pcyl->hh = m_cyl.hh*pgwd->scale;
pcyl->r = m_cyl.r*pgwd->scale;
pprim = pcyl;
return cylinder::type;
}
int CCylinderGeom::GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId)
{
PrepareCylinder((cylinder*)pRes, pGTest);
pGTest->bTransformUpdated = 0; *pResId = -1;
return 1;
}
int CCylinderGeom::GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest)
{
if (pGTest->bTransformUpdated) {
pprim = pGTest->primbuf1;
PrepareCylinder((cylinder*)pprim, pGTest);
}
cylinder *pcyl = (cylinder*)pprim;
pGTest->idbuf[0] = -1;
int iFeature = pcontact->iFeature[iop],iCap;
float r2,hh;
r2 = (pcontact->pt-pcyl->center^pcyl->axis).len2();
hh = (pcontact->pt-pcyl->center)*pcyl->axis;
if ((iFeature&0xE0)!=0x20 && fabs_tpl(r2-sqr(pcyl->r))<sqr(m_minVtxDist) && fabs_tpl(fabs_tpl(hh)-pcyl->hh)<m_minVtxDist)
iFeature = 0x20 | isnonneg(hh);
if ((iFeature&0xE0)==0x20 || iFeature>0x40) {
iCap = (iFeature&3)-(iFeature>>6);
pGTest->surfaces[0].n = pcyl->axis*((iCap<<1)-1);
pGTest->surfaces[0].idx = 0;
pGTest->surfaces[0].iFeature = 0x40 | iCap+1;
pGTest->nSurfaces = 1;
} else
pGTest->nSurfaces = 0;
if ((iFeature&0xE0)==0x20 || iFeature==0x40) {
pGTest->edges[0].dir = pcyl->axis;
pGTest->edges[0].n[0] = pcyl->center-pcontact->pt^pcyl->axis;
pGTest->edges[0].n[1] = -pGTest->edges[0].n[0];
pGTest->edges[0].idx = 0;
pGTest->edges[0].iFeature = iFeature;
pGTest->nEdges = 1;
} else
pGTest->nEdges = 0;
return 1;
}
int CCylinderGeom::PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
int i,icap;
float ang,step;
vectorf axes[3],pt,center;
axes[2] = pGTest->R*m_cyl.axis;
//axes[0] = axes[2].orthogonal().normalized();
axes[0] = GetOrthogonal(axes[2]).normalized();
axes[1] = axes[2]^axes[0];
icap = (((iFeature&3)-(iFeature>>6)<<1)-1);
center = pGTest->R*m_cyl.center*pGTest->scale + axes[2]*(m_cyl.hh*pGTest->scale*icap) + pGTest->offset;
if (psurface->n.len2()==0) {
psurface->n = axes[2]*icap;
psurface->axes[0] = axes[0]; psurface->axes[1] = axes[1]*icap;
psurface->origin = center;
}
center -= psurface->origin;
step = (pi*2/m_nTesselation)*sgnnz(axes[2]*psurface->n);
axes[0] *= (pGTest->scale*m_cyl.r);
axes[1] *= (pGTest->scale*m_cyl.r);
for(i=0,ang=0;i<m_nTesselation;i++,ang+=step) {
pt = center + axes[0]*cos_tpl(ang) + axes[1]*sin_tpl(ang);
g_CylCont[i].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_CylContId[i] = 0x20 | icap+1>>1;
}
g_CylCont[i] = g_CylCont[0]; g_CylContId[2] = g_CylContId[0];
ptbuf = g_CylCont;
pVtxIdBuf = pEdgeIdBuf = g_CylContId;
return i;
}
int CCylinderGeom::PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf)
{
vectorf axis,center,pt;
axis = pGTest->R*m_cyl.axis;
center = pGTest->R*m_cyl.center*pGTest->scale + pGTest->offset;
pt = center + axis*(m_cyl.hh*pGTest->scale) - psurface->origin;
g_CylCont[0].set(pt*psurface->axes[0],pt*psurface->axes[1]);
pt = center - axis*(m_cyl.hh*pGTest->scale) - psurface->origin;
g_CylCont[1].set(pt*psurface->axes[0],pt*psurface->axes[1]);
g_CylCont[2] = g_CylCont[0];
g_CylContId[0] = g_CylContId[1] = g_CylContId[2] = 0x40;
ptbuf = g_CylCont;
pVtxIdBuf = pEdgeIdBuf = g_CylContId;
return 2;
}
int CCylinderGeom::FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters)
{
vectorr axis,center,pt,l,ptdst[]={ptdst0,ptdst1},ptresi[2];
real r,hh,r2;
int i,icap,bLine;
axis = pgwd->R*m_cyl.axis;
center = pgwd->R*m_cyl.center*pgwd->scale + pgwd->offset;
r = m_cyl.r*pgwd->scale; r2 = r*r; hh = m_cyl.hh*pgwd->scale;
pt = ptdst0-center; ptres[1] = ptdst0;
if (bLine = isneg(r2*1E-6f-(l=ptdst1-ptdst0).len2())) {
vectorr n,la,pa;
real n2,l2,t0,t1,kt,pl,pal,roots[4];
polynomial_tpl<real,4> pn;
n = l^axis; n2 = n.len2();
if (isneg(n2*r2-sqr(pt*n)) & inrange(t0=(-pt^axis)*n, (real)0,n2) & inrange(t1=(-pt^l)*n, (real)0,n2*hh)) {
ptres[1] = ptdst0+l*(t0/n2); // line-cylinder side distance
ptres[0] = center+axis*(axis*(ptres[1]-center));
ptres[0] = center+(ptres[1]-ptres[0]).normalized()*r;
return 1;
}
icap = sgnnz(t1);
la = l-axis*(l*axis); l2 = l.len2(); kt = l2-sqr(axis*l);
pt = ptdst0-(center+axis*(hh*icap));
pa = pt-axis*(pt*axis); pl = pt*l; pal = pa*l;
pn = psqr(P1(l2)+pt*l)*(P2(la.len2())+P1((pa*la)*2)+pa.len2()) - psqr(P1(kt*r)+(pa*l)*r);
if (pn.nroots(0,1)) for(i=pn.findroots(0,1,roots)-1;i>=0;i--)
if ((pl+l2*roots[i])*(pal+kt*roots[i])>0) { // skips phantom roots
pa = pt+l*roots[i];
if (isneg(r2-(pa^axis).len2()) & isneg((pa*axis)*-icap)) { // check if this is the global minimum
center += axis*(hh*icap);
ptres[1] = center + pa;
ptres[0] = center + (pa-axis*(axis*pa)).normalized()*r;
return 1;
}
}
}
ptresi[1].zero();
for(i=0;i<=bLine;i++) {
pt = ptdst[i]-center;
if (fabsf(pt*axis)<hh) { // the closest point lies on cylinder side
pt -= axis*(axis*pt);
ptresi[i] = ptdst[i]-pt+pt.normalized()*r;
continue;
}
if ((pt^axis).len2()<r*r) { // .. cylinder cap
ptresi[i] = ptdst[i]+axis*(sgn(pt*axis)*hh-pt*axis);
continue;
}
// cylinder cap edge
ptresi[i] = center+axis*(sgn(axis*pt)*hh) + (pt-axis*(axis*pt)).normalized()*r;
}
i = bLine & isneg((ptresi[1]-ptdst[1]).len2()-(ptresi[0]-ptdst[0]).len2());
ptres[0] = ptresi[i];
ptres[1] = ptdst[i];
return 1;
}
int CCylinderGeom::UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact)
{
vectorf dc = center-m_cyl.center;
float axdist2,capdist,r2=sqr(m_cyl.r);
axdist2 = max((dc^m_cyl.axis).len2(),r2);
capdist = max(0.0f,fabsf(dc*m_cyl.axis)-m_cyl.hh);
if (sqr_signed(axdist2+r2+sqr(capdist)-sqr(rsep)) > axdist2*r2*4)
return 0;
unprojection_mode umode;
sphere sph;
sph.center = center;
sph.r = r;
umode.dir.Set(0,0,0);
cylinder_sphere_lin_unprojection(&umode,&m_cyl,-1,&sph,-1,pcontact,0);
pcontact->pt -= umode.dir*pcontact->t;
return 1;
}
void CCylinderGeom::CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L)
{
vectorf c,dc;
float blendf,area,r;
c = gwd->R*m_cyl.center*gwd->scale + gwd->offset;
dc = c-epicenter; r = dc.len();
if (r>m_cyl.r*0.01f)
dc/=r;
else dc.Set(0,0,1);
blendf = (gwd->R*m_cyl.axis^dc).len();
area = (pi*sqr(m_cyl.r)*(1.0f-blendf) + m_cyl.hh*m_cyl.r*4*blendf)*sqr(gwd->scale);
P += dc*(area*k/sqr(max(r,rmin)));
}
// debug testing code
/*float x,x0,dx=r*0.001f,Stest,Stesta,CxStest,CxStesta,Vtest,CxVtest,CzVtest,CxVtesta,CzVtesta;
for(x0=a,Vtest=CxVtest=CzVtest=0; x0<b; x0+=dx) {
for(x=x0,Stest=CxStest=0; x<b; x+=dx) {
Stest += sqrt_tpl(r2-x*x)*2*dx;
CxStest += x*sqrt_tpl(r2-x*x)*2*dx;
}
Stesta = b*sqrt_tpl(r2-b*b)+r2*asin_tpl(b*rinv)-x0*sqrt_tpl(r2-x0*x0)-r2*asin_tpl(x0*rinv);
CxStesta = (-2.0f/3)*(cube(sqrt_tpl(r2-b*b))-cube(sqrt_tpl(r2-x0*x0)));
Vtest += Stest*dx*k;
CxVtest += CxStest*dx*k;
CzVtest += ((x0-a)*k+z0)*Stest*dx*k;
}*/
/*CTriMesh mesh;
vectorf pt3d[48],normals[92];
int idx[92*3];float x,y;
axes.x=m_cyl.axis.orthogonal().normalized(); axes.y=m_cyl.axis^axes.x;
for(i=0,x=r,y=0; i<24; i++) {
pt3d[i] = m_cyl.center+axes.x*x+axes.y*y-m_cyl.axis*m_cyl.hh;
pt3d[i+24] = pt3d[i]+m_cyl.axis*(2*m_cyl.hh);
k=x; x=x*cos_tpl(pi/12)-y*sin_tpl(pi/12); y=y*cos_tpl(pi/12)+k*sin_tpl(pi/12);
}
for(i=0;i<22;i++) { idx[i*3]=0; idx[i*3+1]=i+2; idx[i*3+2]=i+1; normals[i]=-m_cyl.axis; }
for(;i<44;i++) { idx[i*3]=24; idx[i*3+1]=i+3; idx[i*3+2]=i+4; normals[i]=m_cyl.axis; }
for(;i<92;i++) {
idx[i*3]=i-44>>1; idx[i*3+1]=idx[i*3]+1; idx[i*3+2]=idx[i*3]+24; normals[i]=(pt3d[idx[i*3+1]]-pt3d[idx[i*3]]^pt3d[idx[i*3+2]]-pt3d[idx[i*3]]).normalized(); i++;
idx[i*3]=(i-44>>1)+1; idx[i*3+1]=idx[i*3]+24; idx[i*3+2]=idx[i*3]+23; normals[i]=(pt3d[idx[i*3+1]]-pt3d[idx[i*3]]^pt3d[idx[i*3+2]]-pt3d[idx[i*3]]).normalized();
}
float V1; vectorf c1;
mesh.m_pVertices.data = pt3d;
mesh.m_pIndices = idx;
mesh.m_pNormals = normals;
mesh.m_nVertices=48; mesh.m_nTris=92;
V1 = mesh.CalculateBuoyancy(pplane,pgwd,c1);
if (fabs_tpl(V1-Vaccum)>V1*0.05f || (c1-massCenter).len2()>r2*sqr(0.05f))
i=0;
mesh.m_pNormals=0; mesh.m_pIndices=0; mesh.m_pVertices.data=0;*/
/*vectorf axisx,axisy,pt3d[16];
axisx = vectorf(1,0,0).rotated(rotax,n.z,-sina);
axisy = vectorf(0,1,0).rotated(rotax,n.z,-sina);
pt[0].set(r,0); itype[0]=1; pt3d[0] = center+axisx*pt[0].x+axisy*pt[0].y;
for(i=1;i<8;i++) {
itype[i]=1; pt[i].x=(pt[i-1].x-pt[i-1].y)*(sqrt2/2); pt[i].y=(pt[i-1].x+pt[i-1].y)*(sqrt2/2);
pt3d[i] = center+axisx*pt[i].x+axisy*pt[i].y;
} npt=8; pt[8]=pt[0];
vectorf Ptest(zero),Ltest(zero);
CalcMediumResistance(pt3d,8,n,*pplane,pgwd->v,pgwd->w,pgwd->centerOfMass,Ptest,Ltest);*/
/*Ptest += dP.rotated(rotax,n.z,-sina);
Ltest += dL.rotated(rotax,n.z,-sina);
if (Ptest.len()+Ltest.len()>0.001)
i=0;*/
float CCylinderGeom::CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter)
{
Vec3_tpl<vectorf> axes;
float r=m_cyl.r*pgwd->scale, h=m_cyl.hh*pgwd->scale*2, r2=r*r, rinv=1.0f/r,
denom,k,a,b,z0,z1,ya,yb,anglea,angleb,V[4],Vaccum;
vectorf n = pplane->n, center = pgwd->R*m_cyl.center+pgwd->offset, com[3],com_accum;
int i,nPieces=0;
axes.z = pgwd->R*m_cyl.axis; axes.z *= sgnnz(axes.z*n);
center -= axes.z*(h*0.5f); axes.y = n^axes.z;
massCenter = center+axes.z*(h*0.5f);
if (axes.y.len2()<0.0001f) {
// water plane is perpendicular to cylinder axis
z0 = max(0.0f,min(h,(pplane->origin-center)*pplane->n));
massCenter = center+axes.z*(z0*0.5f);
return pi*r2*z0;
}
axes.y.normalize(); axes.x = axes.y^axes.z;
denom = 1.0f/(axes.x*n);
a = ((pplane->origin-center)*n)*denom;
if (a>=r)
return 0;
else if (a<-r) {
if (sqr(axes.z*n)<0.0001f)
return pi*r2*h;
a = -r; z0 = ((pplane->origin-(center-axes.x*r))*n)/ (axes.z*n);
V[nPieces] = pi*r2*z0; com[nPieces++] = axes.z*(z0*0.5f*V[nPieces]);
} else z0 = 0;
b = ((pplane->origin-(center+axes.z*h))*n)*denom;
if (b<-r)
return pi*r2*h;
else if (b>r) {
b = r; z1 = ((pplane->origin-(center+axes.x*r))*n)/ (axes.z*n);
} else {
z1 = h; yb = sqrt_tpl(r2-b*b);
V[nPieces] = (r2*(pi*0.5f-asin_tpl(b*rinv))-b*yb)*(h-z0);
com[nPieces++] = axes.x*((2.0f/3)*cube(yb)*(h-z0)) + axes.z*((h+z0)*0.5f*V[nPieces]);
}
if (b>a+r*0.01f) {
denom = 1.0f/(b-a); k = (z1-z0)*denom;
ya = sqrt_tpl(r2-a*a); anglea = asin_tpl(a*rinv);
yb = sqrt_tpl(r2-b*b); angleb = asin_tpl(b*rinv);
V[nPieces] = k*((b*yb+r2*angleb)*(b-a)+(1.0f/3)*(cube(yb)-cube(ya))-r2*(b*angleb-a*anglea+yb-ya));
com[nPieces] = axes.z*(k*k*0.5f*((b*yb+r2*angleb)*(b*b-a*a)+0.5f*(b*cube(yb)-a*cube(ya))-r2*(0.75f*(b*yb-a*ya)-
0.25f*r2*(angleb-anglea)+b*b*angleb-a*a*anglea)) + V[nPieces]*(z0-a*k));
com[nPieces++] += axes.x*((2.0f/3)*k*(0.25f*(b*cube(yb)-a*cube(ya))+(3.0f/8)*r2*(b*yb-a*ya+r2*(angleb-anglea))-
cube(yb)*(b-a)));
}
for(i=0,Vaccum=0,com_accum.Set(0,0,0);i<nPieces;i++) {
Vaccum += V[i]; com_accum += com[i];//*V[i];
}
if (Vaccum>0)
massCenter = center+com_accum/Vaccum;
else Vaccum=0;
return Vaccum;
}
int crop_polyarc2d_with_line(const vector2df *pt,const int *itype,int npt, float r, vector2df *ptout,int *itypeout,
const vector2df &p0, const vector2df &l)
{
if (npt==0) return 0;
vector2df d0,c,ptt;
quotientf at[2],a[2],t[3];
float ka,kb,kc,kd;
int state,i,nout=0,i0,i1,j;
ka=l.len2(); kb=p0*l; kc=p0.len2()-r*r;
if ((kd=kb*kb-ka*kc)>=0) {
t[0].y=t[1].y=ka; kd=sqrt_tpl(kd);
t[0].x=-kb-kd; ptt = p0*t[0].y+l*t[0].x; at[0] = fake_atan2(ptt.y,ptt.x);
t[1].x=-kb+kd; ptt = p0*t[1].y+l*t[1].x; at[1] = fake_atan2(ptt.y,ptt.x);
}
state = isnonneg(l ^ pt[0]-p0);
for(i=0; i<npt; i++) {
i0=i1 = 0;
if (itype[i]) {
d0=pt[i+1]-pt[i]; c=pt[i]-p0;
t[2].y=d0^l; t[2].x=l^c;
if (inrange(t[2].x, 0.0f,t[2].y)) {
i0=2;i1=3; t[2].x=d0^c;
}
} else if (kd>=0) {
a[0] = fake_atan2(pt[i].y,pt[i].x);
a[1] = fake_atan2(pt[i+1].y,pt[i+1].x);
i0 = isneg(isneg(a[0]-at[0])+isneg(at[0]-a[1])+isneg(a[1]-a[0])-2);
i1 = isneg(1-isneg(a[0]-at[1])-isneg(at[1]-a[1])-isneg(a[1]-a[0]))+1;
}
if (state) {
ptout[nout]=pt[i]; itypeout[nout++]=itype[i];
}
for(j=i0; j<i1; j++,state^=1) {
ptout[nout]=p0+l*t[j].x/t[j].y;
itypeout[nout++] = itype[i]|state;
}
}
return nout;
}
void CCylinderGeom::CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres)
{
vectorf n,rotax,v,w,center,dir,org,pt0,dP(zero),dL(zero),ptside[4];
vector2df ptbuf0[16],ptbuf1[16],*pt=ptbuf0,*pt1=ptbuf1,*ptt;
float sina,r,rr,r2,hh,x0,y0,x1,y1,dx,dy,Fx,Fxy,Fxx,Fxxy,Fxyy,Fxxx,alpha,square;
int i,icap,npt,sgx,itypebuf0[16],itypebuf1[16],*itype=itypebuf0,*itype1=itypebuf1,*itypet;
dPres.zero(); dLres.zero();
r2 = sqr(r=m_cyl.r*pgwd->scale);
hh = m_cyl.hh*pgwd->scale;
n = pgwd->R*-m_cyl.axis;
rotax = n^vectorf(0,0,1);
sina = rotax.len();
if (sina>0.001f)
rotax/=sina;
else
rotax.Set(1,0,0);
center = pgwd->R*m_cyl.center*pgwd->scale+pgwd->offset+n*hh;
org = center + pplane->n*(pplane->n*(pplane->origin-center));
rr = 1/r;
x1 = 0.965925826f; y1 = 0.258819045f; // 15 degrees sin/cos
ptside[0] = vectorf(x0=r,y0=0,0).rotated(rotax,n.z,-sina)+center;
for(i=0; i<12; i++) {
ptside[1] = ptside[0];
dx = x0; x0 = (x0*sqrt3-y0)*0.5f; y0 = (y0*sqrt3+dx)*0.5f;
ptside[0] = vectorf(x0,y0,0).rotated(rotax,n.z,-sina)+center;
ptside[2] = ptside[1]-n*(hh*2); ptside[3] = ptside[0]-n*(hh*2);
CalcMediumResistance(ptside,4, vectorf(x1,y1,0).rotated(rotax,n.z,-sina),
*pplane,pgwd->v,pgwd->w,pgwd->centerOfMass,dPres,dLres);
dx = x1; x1 = (x1*sqrt3-y1)*0.5f; y1 = (y1*sqrt3+dx)*0.5f;
}
for(icap=-1; icap<=1; icap+=2,n.flip(),center+=n*(hh*2),rotax.flip()) {
v = (pgwd->v+(pgwd->w^center-pgwd->centerOfMass)).rotated(rotax,n.z,sina);
w = pgwd->w.rotated(rotax,n.z,sina);
pt[0].set(0,r); pt[1].set(0,-r); pt[2]=pt[0];
itype[0]=itype[1] = 0; npt = 2;
dir = pplane->n*(pplane->n*n)-n;
if (dir.len2() > 0.001f)
npt = crop_polyarc2d_with_line(pt,itype,npt, r, pt1,itype1,
vector2df((org+dir*(((center-org)*n)/(dir*n))-center).rotated(rotax,n.z,sina)),
-vector2df(cross_with_ort(pplane->n.rotated(rotax,n.z,sina),2)));
else {
ptt=pt;pt=pt1;pt1=ptt; itypet=itype;itype=itype1;itype1=itypet;
if (center*pplane->n > org*pplane->n)
npt = 0;
} pt1[npt] = pt1[0];
dir = pgwd->w^n;
if (dir.len2() > w.len2()*0.001f)
npt = crop_polyarc2d_with_line(pt1,itype1,npt, r, pt,itype,
vector2df((dir*((pgwd->v*n-center*dir)/dir.len2())).rotated(rotax,n.z,sina)), vector2df(w));
else {
ptt=pt;pt=pt1;pt1=ptt; itypet=itype;itype=itype1;itype1=itypet;
if (center*(pgwd->w^n) > pgwd->v*n)
npt = 0;
} pt[npt] = pt[0];
for(i=0,square=0,dP.zero(),dL.zero(); i<npt; i++) {
x0=pt[i].x; y0=pt[i].y; x1=pt[i+1].x; y1=pt[i+1].y;
if (itype[i]) {
dx = x1-x0; dy = y1-y0;
Fx = (x0*y1-x1*y0)*0.5f;
Fxy = dy*(x0*y0 + (dx*y0+dy*x0)*0.5f + dx*dy*(1.0f/3));
Fxx = dy*(x0*x0 + dx*x0 + dx*dx*(1.0f/3));
Fxxy = dy*(dx*dx*dy*0.25f + (dx*dx*y0+dx*dy*x0*2)*(1.0f/3) + (x0*y0*dx*2+x0*x0*dy)*0.5f + x0*x0*y0);
Fxyy = dy*(dy*dy*dx*0.25f + (dy*dy*x0+dy*dx*y0*2)*(1.0f/3) + (y0*x0*dy*2+y0*y0*dx)*0.5f + y0*y0*x0);
Fxxx = dy*(dx*dx*dx*0.25f + dx*dx*x0 + dx*x0*x0*1.5f + x0*x0*x0);
} else {
alpha = asin_tpl(max(-0.9999f,min(0.9999f,y1*rr)))-asin_tpl(max(-0.9999f,min(0.9999f,y0*rr)));
sgx = sgnnz(y1-y0);
Fx = r2*alpha*0.5f*sgx;
Fxy = (cube(x1)-cube(x0))*(-1.0f/3);
Fxx = r2*(y1-y0)-(cube(y1)-cube(y0))*(1.0f/3);
Fxxy = r2*(sqr(y1)-sqr(y0))*0.5f-(cube(y1)-cube(y0))*(1.0f/3);
Fxyy = (y1*cube(x1)-y0*cube(x0))*(-1.0f/4) + r2*(1.0f/8)*(x1*y1-x0*y0+r2*alpha*sgx);
Fxxx = 0.25f*(y1*cube(x1)-y0*cube(x0)+1.5f*r2*(y1*x1-y0*x0+r2*alpha*sgx));
}
square += Fx;
dP.z += w.x*Fxy-w.y*0.5f*Fxx;
dL.x += v.z*Fxy;
dL.y -= v.z*0.5f*Fxx;
dL.x += w.x*Fxyy-w.y*0.5f*Fxxy;
dL.y -= w.x*0.5*Fxxy-w.y*(1.0f/3)*Fxxx;
}
dP.z += v.z*square;
dPres -= dP.rotated(rotax,n.z,-sina);
dLres -= dL.rotated(rotax,n.z,-sina);
}
}
int CCylinderGeom::DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale)
{
CBoxGeom geombox;
geombox.m_box = m_Tree.m_Box;
return geombox.DrawToOcclusionCubemap(pgwd,iStartPrim,nPrims,iPass,pGrid,nRes,rmin,rmax,zscale);
}
void CCylinderGeom::DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel)
{
if (iLevel==0) {
int i; float step,ang;
vectorf axes[3],center,pt0,pt1;
step = (pi*2/m_nTesselation);
axes[2] = gwd->R*m_cyl.axis;
// axes[0] = axes[2].orthogonal().normalized();
axes[0] = GetOrthogonal(axes[2]).normalized();
axes[1] = axes[2]^axes[0];
axes[0] *= m_cyl.r*gwd->scale; axes[1] *= m_cyl.r*gwd->scale;
axes[2] *= m_cyl.hh*gwd->scale;
center = gwd->R*m_cyl.center*gwd->scale + gwd->offset;
pt0 = center+axes[0];
for(i=0,ang=step;i<m_nTesselation;i++,ang+=step,pt0=pt1) {
pt1 = center + axes[0]*cos_tpl(ang) + axes[1]*sin_tpl(ang);
DrawLineFunc(pt0-axes[2], pt1-axes[2]);
DrawLineFunc(pt0+axes[2], pt1+axes[2]);
DrawLineFunc(pt1-axes[2], pt1+axes[2]);
}
} else {
BV *pbbox;
ResetGlobalPrimsBuffers();
m_Tree.GetNodeBV(gwd->R,gwd->offset,gwd->scale, pbbox);
DrawBBox(DrawLineFunc,gwd, &m_Tree,(BBox*)pbbox,iLevel-1);
}
}
void CCylinderGeom::GetMemoryStatistics(ICrySizer *pSizer)
{
pSizer->AddObject(this, sizeof(CCylinderGeom));
}
void CCylinderGeom::Save(CMemStream &stm)
{
stm.Write(m_cyl);
stm.Write(m_nTesselation);
m_Tree.Save(stm);
}
void CCylinderGeom::Load(CMemStream &stm)
{
stm.Read(m_cyl);
stm.Read(m_nTesselation);
m_Tree.Load(stm,this);
}

50
CryPhysics/cylindergeom.h Normal file
View File

@@ -0,0 +1,50 @@
#ifndef cylindergeom_h
#define cylindergeom_h
#pragma once
class CCylinderGeom : public CPrimitive {
public:
CCylinderGeom() { m_iCollPriority = 3; m_nTesselation = 18; m_minVtxDist = 0; }
CCylinderGeom* CreateCylinder(cylinder *pcyl);
void PrepareCylinder(cylinder *pcyl, geometry_under_test *pGTest);
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim);
virtual int GetType() { return GEOM_CYLINDER; }
virtual void GetBBox(box *pbox) { m_Tree.GetBBox(pbox); }
virtual int CalcPhysicalProperties(phys_geometry *pgeom);
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters);
virtual int PointInsideStatus(const vectorf &pt);
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L);
virtual float CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter);
virtual void CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres);
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale);
virtual CBVTree *GetBVTree() { return &m_Tree; }
virtual int UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact);
virtual int GetPrimitive(int iPrim, primitive *pprim) { *(cylinder*)pprim = m_cyl; return sizeof(cylinder); }
virtual void DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false);
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId);
virtual int GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest);
virtual int PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
virtual int PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm);
cylinder m_cyl;
int m_nTesselation;
CSingleBoxTree m_Tree;
};
#endif

158
CryPhysics/geoman.cpp Normal file
View File

@@ -0,0 +1,158 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "geometries.h"
#include "geoman.h"
void CGeomManager::InitGeoman()
{
m_nGeomChunks=0; m_nGeomsInLastChunk=0;
}
void CGeomManager::ShutDownGeoman()
{
int i,j;
for(i=0;i<m_nGeomChunks;i++) {
for(j=0;j<GEOM_CHUNK_SZ;j++) if (m_pGeoms[i][j].pGeom)
delete m_pGeoms[i][j].pGeom;
delete[] m_pGeoms[i];
}
if (m_nGeomChunks)
delete[] m_pGeoms;
m_nGeomChunks=0; m_nGeomsInLastChunk=0;
}
IGeometry *CGeomManager::CreateMesh(strided_pointer<const vectorf> pVertices,index_t *pIndices,const short *pIds,int nTris, int flags,bool bCopyTriangles,
bool bCopyVertices, float approx_tolerance, int nMinTrisPerNode,int nMaxTrisPerNode, float favorAABB)
{
vectorr axes[3],center;
primitive *pprim;
int i,itype = triangle::type;
if (nTris<=0)
return 0;
if (pIds) {
if (flags & mesh_uchar_ids)
for(i=1;i<nTris && ((char*)pIds)[i]==*(char*)pIds;i++);
else
for(i=1;i<nTris && pIds[i]==*pIds;i++);
} else i = nTris;
if (i==nTris) { // only try approximation if the mesh has one material
ComputeMeshEigenBasis(pVertices,pIndices,nTris, axes,center);
itype = ChoosePrimitiveForMesh(pVertices,pIndices,nTris, axes,center, flags,approx_tolerance, pprim);
}
if (itype==triangle::type)
return (new CTriMesh)->CreateTriMesh(pVertices,pIndices,pIds,nTris, flags,bCopyTriangles,bCopyVertices, nMinTrisPerNode,nMaxTrisPerNode, favorAABB);
else
return CreatePrimitive(itype,pprim);
}
IGeometry *CGeomManager::CreatePrimitive(int type, const primitive *pprim)
{
switch (type) {
case cylinder::type: return (new CCylinderGeom)->CreateCylinder((cylinder*)pprim);
case sphere::type: return (new CSphereGeom)->CreateSphere((sphere*)pprim);
case box::type: return (new CBoxGeom)->CreateBox((box*)pprim);
case heightfield::type: return (new CHeightfield)->CreateHeightfield((heightfield*)pprim);
case ray::type: return new CRayGeom((ray*)pprim);
}
return 0;
}
void CGeomManager::DestroyGeometry(IGeometry *pGeom)
{
pGeom->Release();
}
int CGeomManager::AddRefGeometry(phys_geometry *pgeom)
{
return ++pgeom->nRefCount;
}
int CGeomManager::UnregisterGeometry(phys_geometry *pgeom)
{
if (--pgeom->nRefCount!=0)
return pgeom->nRefCount;
pgeom->pGeom->Release();
pgeom->pGeom = 0;
if (pgeom-m_pGeoms[m_nGeomChunks-1]==m_nGeomsInLastChunk-1)
m_nGeomsInLastChunk--;
return 0;
}
phys_geometry *CGeomManager::GetFreeGeomSlot()
{
int i,j;
for(i=0;i<m_nGeomChunks-1;i++) {
for(j=0;j<GEOM_CHUNK_SZ && m_pGeoms[i][j].pGeom;j++);
if (j<GEOM_CHUNK_SZ) break;
}
if (i>=m_nGeomChunks-1)
for(j=0;j<m_nGeomsInLastChunk && m_pGeoms[i][j].pGeom;j++);
if (m_nGeomChunks==0 || j==GEOM_CHUNK_SZ) {
phys_geometry **t = m_pGeoms;
m_pGeoms = new phys_geometry*[m_nGeomChunks+1];
if (m_nGeomChunks) {
memcpy(m_pGeoms, t, sizeof(phys_geometry*)*m_nGeomChunks);
delete[] t;
}
i=m_nGeomChunks++; j=0;
memset(m_pGeoms[i] = new phys_geometry[GEOM_CHUNK_SZ], 0, sizeof(phys_geometry)*GEOM_CHUNK_SZ);
}
if (i==m_nGeomChunks-1 && j==m_nGeomsInLastChunk)
m_nGeomsInLastChunk++;
return m_pGeoms[i]+j;
}
phys_geometry *CGeomManager::RegisterGeometry(IGeometry *pGeom,int defSurfaceIdx)
{
phys_geometry *pgeom = GetFreeGeomSlot();
pGeom->CalcPhysicalProperties(pgeom);
pgeom->nRefCount = 1;
pgeom->surface_idx = defSurfaceIdx;
return pgeom;
}
void CGeomManager::SaveGeometry(CMemStream &stm, IGeometry *pGeom)
{
stm.Write(pGeom->GetType());
pGeom->Save(stm);
}
IGeometry *CGeomManager::LoadGeometry(CMemStream &stm)
{
int itype; stm.Read(itype);
IGeometry *pGeom;
switch (itype) {
case GEOM_TRIMESH: pGeom = new CTriMesh; break;
case GEOM_CYLINDER: pGeom = new CCylinderGeom; break;
case GEOM_SPHERE: pGeom = new CSphereGeom; break;
case GEOM_BOX: pGeom = new CBoxGeom; break;
}
pGeom->Load(stm);
return pGeom;
}
void CGeomManager::SavePhysGeometry(CMemStream &stm, phys_geometry *pgeom)
{
stm.Write(*pgeom);
SaveGeometry(stm,pgeom->pGeom);
}
phys_geometry *CGeomManager::LoadPhysGeometry(CMemStream &stm)
{
phys_geometry *pgeom = GetFreeGeomSlot();
stm.Read(*pgeom);
pgeom->pGeom = LoadGeometry(stm);
return pgeom;
}

36
CryPhysics/geoman.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef geoman_h
#define geoman_h
#pragma once
const int GEOM_CHUNK_SZ = 64;
class CGeomManager : public IGeomManager {
public:
CGeomManager() { InitGeoman(); }
~CGeomManager() { ShutDownGeoman(); }
void InitGeoman();
void ShutDownGeoman();
virtual IGeometry *CreateMesh(strided_pointer<const vectorf> pVertices,index_t *pIndices,const short *pIds,int nTris, int flags,bool bCopyTriangles=true,
bool bCopyVertices=true, float approx_tolerance=0.05f, int nMinTrisPerNode=2,int nMaxTrisPerNode=4, float favorAABB=1.0f);
virtual IGeometry *CreatePrimitive(int type, const primitive *pprim);
virtual void DestroyGeometry(IGeometry *pGeom);
virtual phys_geometry *RegisterGeometry(IGeometry *pGeom,int defSurfaceIdx=0);
virtual int AddRefGeometry(phys_geometry *pgeom);
virtual int UnregisterGeometry(phys_geometry *pgeom);
virtual void SaveGeometry(CMemStream &stm, IGeometry *pGeom);
virtual IGeometry *LoadGeometry(CMemStream &stm);
virtual void SavePhysGeometry(CMemStream &stm, phys_geometry *pgeom);
virtual phys_geometry *LoadPhysGeometry(CMemStream &stm);
virtual void RemapPhysGeometryFaceIds(phys_geometry *pgeom,short *pMap,int sz) { pgeom->pGeom->RemapFaceIds(pMap,sz); }
phys_geometry *GetFreeGeomSlot();
phys_geometry **m_pGeoms;
int m_nGeomChunks,m_nGeomsInLastChunk;
};
#endif

19
CryPhysics/geometries.h Normal file
View File

@@ -0,0 +1,19 @@
#ifndef geometries_h
#define geometries_h
#pragma once
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "trimesh.h"
#include "heightfieldbv.h"
#include "heightfieldgeom.h"
#include "singleboxtree.h"
#include "cylindergeom.h"
#include "spheregeom.h"
#include "raybv.h"
#include "raygeom.h"
#include "boxgeom.h"
#include "geoman.h"
#endif

788
CryPhysics/geometry.cpp Normal file
View File

@@ -0,0 +1,788 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "overlapchecks.h"
#include "intersectionchecks.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "geometry.h"
#include "singleboxtree.h"
#include "spheregeom.h"
indexed_triangle g_IdxTriBuf[256];
int g_IdxTriBufPos;
cylinder g_CylBuf[2];
int g_CylBufPos;
sphere g_SphBuf[2];
int g_SphBufPos;
box g_BoxBuf[2];
int g_BoxBufPos;
BBox g_BBoxBuf[128];
int g_BBoxBufPos;
surface_desc g_SurfaceDescBuf[64];
int g_SurfaceDescBufPos;
edge_desc g_EdgeDescBuf[64];
int g_EdgeDescBufPos;
int g_iFeatureBuf[64];
int g_iFeatureBufPos;
short g_IdBuf[256];
int g_IdBufPos;
int g_UsedNodesMap[8192];
int g_UsedNodesMapPos;
int g_UsedNodesIdx[64];
int g_UsedNodesIdxPos;
geom_contact g_Contacts[64];
int g_nTotContacts;
geom_contact_area g_AreaBuf[32];
int g_nAreas;
vectorf g_AreaPtBuf[256];
int g_AreaPrimBuf0[256],g_AreaFeatureBuf0[256],g_AreaPrimBuf1[256],g_AreaFeatureBuf1[256];
int g_nAreaPt;
extern vectorf g_BrdPtBuf[2048]; // from trimesh.cpp
extern int g_BrdPtBufPos; //
struct InitGeometryGlobals {
InitGeometryGlobals() {
memset(g_UsedNodesMap, 0, sizeof(g_UsedNodesMap));
g_EdgeDescBufPos = 0; g_IdBufPos = 0; g_IdxTriBufPos = 0;
g_SurfaceDescBufPos = 0; g_UsedNodesMapPos = 0; g_UsedNodesIdxPos = 0;
g_iFeatureBufPos = 0;
g_nAreas = 0; g_nAreaPt = 0; g_nTotContacts = 0;
}
};
static InitGeometryGlobals now;
#include "raybv.h"
#include "raygeom.h"
int IntersectBVs(geometry_under_test *pGTest, BV *pBV1,BV *pBV2)
{
intptr_t mask;
int iNode[2],*pUsedNodesMap[2],res=0,nullmap=0,bNodeUsed[2];
mask = iszero_mask(pGTest[0].pUsedNodesMap); // mask = -(pUsedNodeMap==0)
pUsedNodesMap[0] = (int*)((intptr_t)pGTest[0].pUsedNodesMap&~mask | (intptr_t)&nullmap&mask);
iNode[0] = pBV1->iNode&~mask;
mask = iszero_mask(pGTest[1].pUsedNodesMap); // mask = -(pUsedNodeMap==0)
pUsedNodesMap[1] = (int*)((intptr_t)pGTest[1].pUsedNodesMap&~mask | (intptr_t)&nullmap&mask);
iNode[1] = pBV2->iNode&~mask;
bNodeUsed[0] = pUsedNodesMap[0][iNode[0]>>5]>>(iNode[0]&31) & 1;
bNodeUsed[1] = pUsedNodesMap[1][iNode[1]>>5]>>(iNode[1]&31) & 1;
if (bNodeUsed[0] & bNodeUsed[1])
return 0; // skip check only if both nodes are marked used
//if (pUsedNodesMap[0][iNode[0]>>5] & 1<<(iNode[0]&31) | pUsedNodesMap[1][iNode[1]>>5] & 1<<(iNode[1]&31))
// return 0; // skip check if at least one BV node is marked checked
if (!g_Overlapper.Check(pBV1->type,pBV2->type, *pBV1,*pBV2))
return 0;
float split1,split2;
split1 = pGTest[0].pBVtree->SplitPriority(pBV1);
split2 = pGTest[1].pBVtree->SplitPriority(pBV2);
BV *pBV_split1,*pBV_split2;
if (split1>split2 && split1>0) { // split the first BV
pGTest[0].pBVtree->GetNodeChildrenBVs(pBV1, pBV_split1,pBV_split2);
res = IntersectBVs(pGTest, pBV_split1,pBV2);
if (pGTest[0].bStopIntersection + pGTest[1].bStopIntersection > 0)
return res;
res += IntersectBVs(pGTest, pBV_split2,pBV2);
pGTest[0].pBVtree->ReleaseLastBVs();
} else
if (split2>0) { // split the second BV
pGTest[1].pBVtree->GetNodeChildrenBVs(pGTest[1].R_rel,pGTest[1].offset_rel,pGTest[1].scale_rel, pBV2, pBV_split1,pBV_split2);
res = IntersectBVs(pGTest, pBV1,pBV_split1);
if (pGTest[0].bStopIntersection + pGTest[1].bStopIntersection > 0)
return res;
res += IntersectBVs(pGTest, pBV1,pBV_split2);
pGTest[1].pBVtree->ReleaseLastBVs();
} else {
int nPrims1,nPrims2,iClient,i,j;
primitive *ptr[2];
prim_inters inters;
static vectorf ptborder[16];
inters.minPtDist2 = sqr(min(pGTest[0].pGeometry->m_minVtxDist, pGTest[1].pGeometry->m_minVtxDist));
iClient = -(pGTest[1].pGeometry->m_iCollPriority-pGTest[0].pGeometry->m_iCollPriority>>31);
inters.pt[1].Set(0,0,0);
inters.ptborder = ptborder;
inters.nborderpt = inters.nBestPtVal = 0;
inters.nbordersz = sizeof(ptborder)/sizeof(ptborder[0]);
nPrims1 = pGTest[0].pBVtree->GetNodeContents(pBV1->iNode, pBV2,bNodeUsed[1],1, pGTest+0,pGTest+1);
nPrims2 = pGTest[1].pBVtree->GetNodeContents(pBV2->iNode, pBV1,bNodeUsed[0],0, pGTest+1,pGTest+0);
for(i=0,ptr[0]=pGTest[0].primbuf; i<nPrims1; i++,ptr[0]=(primitive*)((char*)ptr[0]+pGTest[0].szprim))
for(j=0,ptr[1]=pGTest[1].primbuf; j<nPrims2; j++,ptr[1]=(primitive*)((char*)ptr[1]+pGTest[1].szprim))
if (g_Intersector.Check(pGTest[iClient].typeprim,pGTest[iClient^1].typeprim, ptr[iClient],ptr[iClient^1], &inters))
{
inters.id[0] = pGTest[iClient].idbuf[i&-(iClient^1)|j&-iClient];
inters.id[1] = pGTest[iClient^1].idbuf[i&-iClient|j&-(iClient^1)];
inters.iNode[0] = pBV1->iNode;
inters.iNode[1] = pBV2->iNode;
pGTest[0].bCurNodeUsed = pGTest[1].bCurNodeUsed = 0;
res += pGTest[iClient].pGeometry->RegisterIntersection(ptr[iClient],ptr[iClient^1], pGTest+iClient,pGTest+(iClient^1), &inters);
if (pGTest[0].bStopIntersection+pGTest[1].bStopIntersection + pGTest[0].bCurNodeUsed+pGTest[1].bCurNodeUsed> 0)
return res;
inters.nborderpt = inters.nBestPtVal = 0;
inters.nbordersz = sizeof(ptborder)/sizeof(ptborder[0]);
}
}
return res;
}
int SweepTestBVs(geometry_under_test *pGTest, BV *pBV1,BV *pBV2)
{
if (!g_Overlapper.Check(pBV1->type,pBV2->type, *pBV1,*pBV2))
return 0;
int res = 0;
float split1,split2;
split1 = pGTest[0].pBVtree->SplitPriority(pBV1);
split2 = pGTest[1].pBVtree->SplitPriority(pBV2);
BV *pBV_split1,*pBV_split2;
if (split1>split2 && split1>0) { // split the first BV
pGTest[0].pBVtree->GetNodeChildrenBVs(pBV1, pGTest[0].sweepdir_loc,pGTest[0].sweepstep_loc, pBV_split1,pBV_split2);
res = SweepTestBVs(pGTest, pBV_split1,pBV2) + SweepTestBVs(pGTest, pBV_split2,pBV2);
pGTest[0].pBVtree->ReleaseLastSweptBVs();
} else
if (split2>0) { // split the second BV
pGTest[1].pBVtree->GetNodeChildrenBVs(pGTest[1].R_rel,pGTest[1].offset_rel,pGTest[1].scale_rel, pBV2, pBV_split1,pBV_split2);
res = SweepTestBVs(pGTest, pBV1,pBV_split1) + SweepTestBVs(pGTest, pBV1,pBV_split2);
pGTest[1].pBVtree->ReleaseLastBVs();
} else {
int nPrims1,nPrims2,i,j;
primitive *ptr[2];
contact contact_cur;
geom_contact *pcontact = pGTest[0].contacts;
unprojection_mode unproj;
vectorf startoffs = pGTest[0].offset;
pGTest[0].offset += pGTest[0].sweepdir*pGTest[0].sweepstep;
unproj.imode = 0;
unproj.dir = -pGTest[0].sweepdir;
unproj.tmax = pGTest[0].sweepstep;
unproj.minPtDist = min(pGTest[0].pGeometry->m_minVtxDist, pGTest[1].pGeometry->m_minVtxDist);
nPrims1 = pGTest[0].pBVtree->GetNodeContents(pBV1->iNode, pBV2,0,-1, pGTest+0,pGTest+1);
nPrims2 = pGTest[1].pBVtree->GetNodeContents(pBV2->iNode, pBV1,0,0, pGTest+1,pGTest+0);
for(i=0,ptr[0]=pGTest[0].primbuf; i<nPrims1; i++,ptr[0]=(primitive*)((char*)ptr[0]+pGTest[0].szprim))
for(j=0,ptr[1]=pGTest[1].primbuf; j<nPrims2; j++,ptr[1]=(primitive*)((char*)ptr[1]+pGTest[1].szprim))
if (g_Unprojector.Check(&unproj, pGTest[0].typeprim,pGTest[1].typeprim, ptr[0],-1,ptr[1],-1, &contact_cur) &&
contact_cur.n*pGTest[0].sweepdir>0 && contact_cur.t<=pGTest[0].sweepstep && pGTest[0].sweepstep-contact_cur.t<pcontact->t)
{
pcontact->t = pGTest[0].sweepstep-contact_cur.t;
pcontact->pt = contact_cur.pt;
pcontact->n = contact_cur.n.normalized();
pcontact->iFeature[0] = contact_cur.iFeature[0];
pcontact->iFeature[1] = contact_cur.iFeature[1];
pcontact->id[0] = pGTest[0].idbuf[i];
pcontact->id[1] = pGTest[1].idbuf[j];
pcontact->iNode[0] = pBV1->iNode;
pcontact->iNode[1] = pBV2->iNode;
res++; *(pGTest[0].pnContacts) = 1;
}
pGTest[0].offset = startoffs;
}
return res;
}
int CGeometry::Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts)
{
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
geometry_under_test gtest[2];
geom_world_data *pdata[2] = { pdata1,pdata2 };
int i,j,jmax,mask,nContacts=0,iStartNode[2];
intersection_params defip;
vectorr offsetWorld;
if (!pparams)
pparams = &defip;
// zero all global buffer pointers
ResetGlobalPrimsBuffers();
g_Overlapper.Init();
if (!pparams->bKeepPrevContacts) {
g_nAreas = 0; g_nAreaPt = 0; g_nTotContacts = 0;
}
if (g_nTotContacts>=sizeof(g_Contacts)/sizeof(g_Contacts[0]))
return 0;
g_SurfaceDescBufPos = 0;
g_EdgeDescBufPos = 0;
g_IdBufPos = 0;
g_iFeatureBufPos = 0;
g_UsedNodesMapPos = 0;
g_UsedNodesIdxPos = 0;
pcontacts = g_Contacts+g_nTotContacts;
pparams->pGlobalContacts = g_Contacts;
if (!pparams->bNoAreaContacts && g_nAreas<sizeof(g_AreaBuf)/sizeof(g_AreaBuf[0]) && g_nAreaPt<sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])) {
pcontacts[0].parea = g_AreaBuf+g_nAreas;
pcontacts[0].parea->pt = g_AreaPtBuf+g_nAreaPt;
pcontacts[0].parea->piPrim[0] = g_AreaPrimBuf0+g_nAreaPt;
pcontacts[0].parea->piFeature[0] = g_AreaFeatureBuf0+g_nAreaPt;
pcontacts[0].parea->piPrim[1] = g_AreaPrimBuf1+g_nAreaPt;
pcontacts[0].parea->piFeature[1] = g_AreaFeatureBuf1+g_nAreaPt;
pcontacts[0].parea->npt = 0; pcontacts[0].parea->nmaxpt = sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])-g_nAreaPt;
/*for(i=0;i<pcontacts[0].parea->nmaxpt;i++) {
pcontacts[0].parea->piPrim[0][i]=pcontacts[0].parea->piFeature[0][i]=-2;
pcontacts[0].parea->pt[i](777.0f,777.0f,777.0f); }*/
pcontacts[0].parea->minedge = min(gtest[0].minAreaEdge, gtest[1].minAreaEdge);
} else
pcontacts[0].parea = 0;
if (pparams->bSweepTest && pdata1->v.len2()==0) {
pparams->bSweepTest = 0; pdata1->v.Set(0,0,1); pparams->vrel_min = 0;
}
for(i=0;i<2;i++) {
if (pdata[i]) {
gtest[i].offset = pdata[i]->offset;
gtest[i].R = pdata[i]->R;
gtest[i].scale = pdata[i]->scale;
gtest[i].rscale = 1.0f/pdata[i]->scale;
gtest[i].v = pdata[i]->v;
gtest[i].w = pdata[i]->w;
gtest[i].centerOfMass = pdata[i]->centerOfMass;
iStartNode[i] = pdata[i]->iStartNode;
} else {
gtest[i].offset.Set(0,0,0);
gtest[i].R.SetIdentity();
gtest[i].scale = gtest[i].rscale = 1.0f;
gtest[i].v.Set(0,0,0);
gtest[i].w.Set(0,0,0);
iStartNode[i] = 0;
}
gtest[i].centerOfRotation = pparams->centerOfRotation;
gtest[i].axisContactNormal = pparams->axisContactNormal*(1-(i<<1));
gtest[i].contacts = pcontacts;
gtest[i].pnContacts = &nContacts;
gtest[i].nMaxContacts = sizeof(g_Contacts)/sizeof(g_Contacts[0])-g_nTotContacts;
gtest[i].bStopIntersection = 0;
gtest[i].sweepstep = 0;
gtest[i].ptOutsidePivot = pparams->ptOutsidePivot[i];
gtest[i].pParams = pparams;
}
for(i=0;i<2;i++) {
gtest[i].offset_rel = ((gtest[i].offset-gtest[i^1].offset)*gtest[i^1].R)*gtest[i^1].rscale;
(gtest[i].R_rel = gtest[i^1].R.T()) *= gtest[i].R;
gtest[i].scale_rel = gtest[i].scale*gtest[i^1].rscale;
gtest[i].rscale_rel = gtest[i^1].scale*gtest[i].rscale;
}
pparams->bBothConvex = m_bIsConvex & ((CGeometry*)pCollider)->m_bIsConvex;
gtest[0].offset -= (offsetWorld = gtest[1].offset);
gtest[0].centerOfMass -= offsetWorld; gtest[0].centerOfRotation -= offsetWorld; gtest[0].ptOutsidePivot -= offsetWorld;
gtest[1].centerOfMass -= offsetWorld; gtest[1].centerOfRotation -= offsetWorld; gtest[1].ptOutsidePivot -= offsetWorld;
gtest[1].offset.zero(); // use relative offset in calculations to improve accuracy
BV *pBV1,*pBV2;
if (!pparams->bSweepTest) {
if (!PrepareForIntersectionTest(gtest+0, (CGeometry*)pCollider,gtest+1, pparams->bKeepPrevContacts) ||
!((CGeometry*)pCollider)->PrepareForIntersectionTest(gtest+1, this,gtest+0, pparams->bKeepPrevContacts))
return 0;
if (iStartNode[0]+iStartNode[1]) {
gtest[0].pBVtree->GetNodeBV(pBV1, iStartNode[0]);
gtest[1].pBVtree->GetNodeBV(gtest[1].R_rel,gtest[1].offset_rel,gtest[1].scale_rel, pBV2, iStartNode[1]);
IntersectBVs(gtest, pBV1,pBV2);
}
if (nContacts==0) {
gtest[0].pBVtree->GetNodeBV(pBV1);
gtest[1].pBVtree->GetNodeBV(gtest[1].R_rel,gtest[1].offset_rel,gtest[1].scale_rel, pBV2);
IntersectBVs(gtest, pBV1,pBV2);
}
int iClient = -(gtest[1].pGeometry->m_iCollPriority-gtest[0].pGeometry->m_iCollPriority>>31);
if (iClient) for(i=0;i<nContacts;i++) {
if (pcontacts[i].parea && pcontacts[i].parea->npt) {
vectorf ntmp=pcontacts[i].n; pcontacts[i].n=pcontacts[i].parea->n1; pcontacts[i].parea->n1=ntmp;
int *pprim=pcontacts[i].parea->piPrim[0]; pcontacts[i].parea->piPrim[0]=pcontacts[i].parea->piPrim[1]; pcontacts[i].parea->piPrim[1]=pprim;
int *pfeat=pcontacts[i].parea->piFeature[0]; pcontacts[i].parea->piFeature[0]=pcontacts[i].parea->piFeature[1];
pcontacts[i].parea->piFeature[1]=pfeat;
if (pcontacts[i].iUnprojMode) {
pcontacts[i].n = pcontacts[i].n.rotated(pcontacts[i].dir,-pcontacts[i].t);
pcontacts[i].parea->n1 = pcontacts[i].parea->n1.rotated(pcontacts[i].dir,-pcontacts[i].t);
}
if (pcontacts[i].iUnprojMode) for(j=0;j<pcontacts[i].parea->npt;j++)
pcontacts[i].parea->pt[j] = pcontacts[i].parea->pt[j].rotated(gtest[0].centerOfRotation,pcontacts[i].dir,-pcontacts[i].t);
else for(j=0;j<pcontacts[i].parea->npt;j++)
pcontacts[i].parea->pt[j] -= pcontacts[i].dir*pcontacts[i].t;
} else {
if (pcontacts[i].iUnprojMode)
pcontacts[i].n = pcontacts[i].n.rotated(pcontacts[i].dir,-pcontacts[i].t);
pcontacts[i].n.Flip();
}
if (pcontacts[i].iUnprojMode)
pcontacts[i].pt = pcontacts[i].pt.rotated(gtest[0].centerOfRotation,pcontacts[i].dir,-pcontacts[i].t);
else
pcontacts[i].pt -= pcontacts[i].dir*pcontacts[i].t;
pcontacts[i].dir.Flip();
short id=pcontacts[i].id[0]; pcontacts[i].id[0]=pcontacts[i].id[1]; pcontacts[i].id[1]=id;
int iprim=pcontacts[i].iPrim[0]; pcontacts[i].iPrim[0]=pcontacts[i].iPrim[1]; pcontacts[i].iPrim[1]=iprim;
int ifeature=pcontacts[i].iFeature[0]; pcontacts[i].iFeature[0]=pcontacts[i].iFeature[1]; pcontacts[i].iFeature[1]=ifeature;
}
// sort contacts in descending t order
geom_contact tmpcontact;
for(i=0;i<nContacts-1;i++) {
for(jmax=i,j=i+1; j<nContacts; j++) {
mask = -isneg(pcontacts[jmax].t-pcontacts[j].t);
jmax = jmax&~mask | j&mask;
}
if (jmax!=i) {
if (pcontacts[i].ptborder==&pcontacts[i].center) pcontacts[i].ptborder = &pcontacts[jmax].center;
if (pcontacts[i].ptborder==&pcontacts[i].pt) pcontacts[i].ptborder = &pcontacts[jmax].pt;
if (pcontacts[jmax].ptborder==&pcontacts[jmax].center) pcontacts[jmax].ptborder = &pcontacts[i].center;
if (pcontacts[jmax].ptborder==&pcontacts[jmax].pt) pcontacts[jmax].ptborder = &pcontacts[i].pt;
tmpcontact=pcontacts[i]; pcontacts[i]=pcontacts[jmax]; pcontacts[jmax]=tmpcontact;
}
}
} else {
gtest[0].sweepstep = pcontacts[0].vel = gtest[0].v.len();
gtest[0].sweepdir = gtest[0].v/gtest[0].sweepstep;
gtest[0].sweepstep *= pparams->time_interval;
gtest[0].sweepdir_loc = gtest[0].sweepdir*gtest[0].R;
gtest[0].sweepstep_loc = gtest[0].sweepstep*gtest[0].rscale;
if (!PrepareForIntersectionTest(gtest+0, (CGeometry*)pCollider,gtest+1, pparams->bKeepPrevContacts) ||
!((CGeometry*)pCollider)->PrepareForIntersectionTest(gtest+1, this,gtest+0, pparams->bKeepPrevContacts))
return 0;
pcontacts[0].ptborder = &g_Contacts[0].pt;
pcontacts[0].nborderpt = 1;
pcontacts[0].parea = 0;
pcontacts[0].dir = -gtest[0].sweepdir;
pcontacts[0].t = gtest[0].sweepstep;
gtest[0].pBVtree->GetNodeBV(pBV1, gtest[0].sweepdir_loc,gtest[0].sweepstep_loc);
gtest[1].pBVtree->GetNodeBV(gtest[1].R_rel,gtest[1].offset_rel,gtest[1].scale_rel, pBV2);
SweepTestBVs(gtest, pBV1,pBV2);
}
for(i=0;i<nContacts;i++) {
pcontacts[i].pt += offsetWorld;
pcontacts[i].center += offsetWorld;
if (pcontacts[i].ptborder!=&pcontacts[i].pt && pcontacts[i].ptborder!=&pcontacts[i].center)
for(j=0;j<pcontacts[i].nborderpt;j++)
pcontacts[i].ptborder[j] += offsetWorld;
if (pcontacts[i].parea) for(j=0;j<pcontacts[i].parea->npt;j++)
pcontacts[i].parea->pt[j] += offsetWorld;
}
CleanupAfterIntersectionTest(gtest+0);
((CGeometry*)pCollider)->CleanupAfterIntersectionTest(gtest+1);
g_nTotContacts += nContacts;
return nContacts;
}
int CGeometry::RegisterIntersection(primitive *pprim1,primitive *pprim2, geometry_under_test *pGTest1,geometry_under_test *pGTest2,
prim_inters *pinters)
{
unprojection_mode unproj;
contact contact_cur;
geom_contact *pres = pGTest1->contacts + *pGTest1->pnContacts;
primitive *pprims[2] = { pprim1, pprim2 };
pres->ptborder = &pres->pt;
pres->nborderpt = 1;
if (pGTest1->pParams->iUnprojectionMode==0) {
// for linear unprojection, find point with maximum relative normal velocity
unproj.imode = 0;
unproj.dir = pGTest2->v+(pGTest2->w^pinters->pt[1]-pGTest2->centerOfMass) -
pGTest1->v-(pGTest1->w^pinters->pt[1]-pGTest1->centerOfMass);
unproj.vel = unproj.dir.len();
if (unproj.vel < pGTest1->pParams->vrel_min)
goto use_normal;
unproj.dir /= unproj.vel;
} else if (pGTest1->pParams->iUnprojectionMode==1) {
unproj.imode = 1;
unproj.dir = pGTest2->w-pGTest1->w;
unproj.center = pGTest1->centerOfRotation.len2()>pGTest2->centerOfRotation.len2() ? pGTest1->centerOfRotation : pGTest2->centerOfRotation;
unproj.vel = unproj.dir.len();
unproj.dir /= unproj.vel;
}
unproj.tmax = pGTest1->pParams->time_interval*unproj.vel;
if (!g_Unprojector.Check(&unproj, pGTest1->typeprim,pGTest2->typeprim, pprims[0],-1,pprims[1],-1, &contact_cur, pres->parea))
return 0;
if (contact_cur.t > pGTest1->pParams->time_interval*unproj.vel) {
use_normal:
unproj.imode = 0;
unproj.dir = pinters->n.normalize();
unproj.vel = 0;
if (!g_Unprojector.Check(&unproj, pGTest1->typeprim,pGTest2->typeprim, pprims[0],-1,pprims[1],-1, &contact_cur, pres->parea))
return 0;
}
pres->t = contact_cur.t;
pres->pt = contact_cur.pt;
pres->n = contact_cur.n;
pres->dir = unproj.dir;
pres->vel = unproj.vel;
pres->iUnprojMode = unproj.imode;
pres->id[0] = pinters->id[0];
pres->id[1] = pinters->id[1];
pres->iNode[0] = pinters->iNode[0];
pres->iNode[1] = pinters->iNode[1];
if (pres->parea->npt>0) {
g_nAreaPt += pres->parea->npt;
g_nAreas++;
} else
pres->parea = 0;
// allocate slots for the next intersection
(*pGTest1->pnContacts)++;
pres = pGTest1->contacts + *pGTest1->pnContacts;
if (g_nAreas<sizeof(g_AreaBuf)/sizeof(g_AreaBuf[0]) && g_nAreaPt<sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])) {
pres->parea = g_AreaBuf+g_nAreas;
pres->parea->pt = g_AreaPtBuf+g_nAreaPt;
pres->parea->piPrim[0] = g_AreaPrimBuf0+g_nAreaPt; pres->parea->piFeature[0] = g_AreaFeatureBuf0+g_nAreaPt;
pres->parea->piPrim[1] = g_AreaPrimBuf1+g_nAreaPt; pres->parea->piFeature[1] = g_AreaFeatureBuf1+g_nAreaPt;
pres->parea->npt = 0; pres->parea->nmaxpt = sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])-g_nAreaPt;
pres->parea->minedge = min(pGTest1->minAreaEdge, pGTest2->minAreaEdge);
} else
pres->parea = 0;
if (*pGTest1->pnContacts >= pGTest1->nMaxContacts)
pGTest1->bStopIntersection = 1;
return 0;
}
struct radius_check_data {
CGeometry *pGeom;
CBVTree *pBVtree;
geom_world_data *pgwd;
sphere sph;
float rmin,rmax;
float zscale;
int **pGrid;
int nRes;
int nGrow;
int iPass;
};
int RadiusCheckBVs(radius_check_data *prcd, BV *pBV)
{
if (!g_Overlapper.Check(pBV->type,sphere::type, *pBV,&prcd->sph))
return 0;
if (prcd->pBVtree->SplitPriority(pBV)>0) {
BV *pBV_split1,*pBV_split2;
prcd->pBVtree->GetNodeChildrenBVs(pBV, pBV_split1,pBV_split2);
int res = RadiusCheckBVs(prcd, pBV_split1)+RadiusCheckBVs(prcd, pBV_split2);
prcd->pBVtree->ReleaseLastBVs();
return res;
} else {
int iStartPrim,nPrims;
nPrims = prcd->pBVtree->GetNodeContentsIdx(pBV->iNode, iStartPrim);
return prcd->pGeom->DrawToOcclusionCubemap(prcd->pgwd, iStartPrim,nPrims, prcd->iPass,
prcd->pGrid,prcd->nRes, prcd->rmin,prcd->rmax,prcd->zscale);
}
}
float CGeometry::BuildOcclusionCubemap(geom_world_data *pgwd, int iMode, int *pGrid0[6],int *pGrid1[6],int nRes, float rmin,float rmax, int nGrow)
{
radius_check_data rcd;
float rscale = pgwd->scale==1.0f ? 1.0f:1.0f/pgwd->scale;
int i,j,irmin;
ResetGlobalPrimsBuffers();
rcd.pgwd = pgwd;
rcd.sph.center = (-pgwd->offset*rscale)*pgwd->R;
rcd.sph.r = rmax*rscale;
rcd.rmin = rmin;
rcd.rmax = rmax;
rcd.zscale = 65535.0f/rmax;
rcd.nRes = nRes;
rcd.nGrow = nGrow;
rcd.iPass = 0;
if (iMode==0)
rcd.pGrid = pGrid0;
else {
rcd.pGrid = pGrid1;
for(i=0;i<6;i++) for(j=nRes*nRes-1;j>=0;j--)
pGrid1[i][j] = (1u<<31)-1;
}
rcd.pGeom = this;
rcd.pBVtree = GetBVTree();
BV *pBV;
rcd.pBVtree->GetNodeBV(pBV);
if (RadiusCheckBVs(&rcd,pBV)) {// && iMode==0) {
rcd.iPass = 1;
ResetGlobalPrimsBuffers();
RadiusCheckBVs(&rcd,pBV);
}
irmin = float2int(rmin*rcd.zscale);
for(i=0;i<6;i++) for(j=nRes*nRes-1;j>=0;j--) // if the highest bit is set, change cell z to irmin
rcd.pGrid[i][j] = irmin&rcd.pGrid[i][j]>>31 | rcd.pGrid[i][j]&~(rcd.pGrid[i][j]>>31);
if (iMode) {
int nCells,nOccludedCells;
GrowAndCompareCubemaps(pGrid0,pGrid1,nRes, nGrow, nCells,nOccludedCells);
return nCells>0 ? (float)(nCells-nOccludedCells)/nCells : 0.0f;
}
return 0.0f;
}
void DrawBBox(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, CBVTree *pTree,BBox *pbbox,int maxlevel,int level)
{
if (level<maxlevel && pTree->SplitPriority(pbbox)>0) {
BV *pbbox1,*pbbox2;
pTree->GetNodeChildrenBVs(gwd->R,gwd->offset,gwd->scale, pbbox, pbbox1,pbbox2);
DrawBBox(DrawLineFunc,gwd, pTree,(BBox*)pbbox1,maxlevel,level+1);
DrawBBox(DrawLineFunc,gwd, pTree,(BBox*)pbbox2,maxlevel,level+1);
pTree->ReleaseLastBVs();
return;
}
vectorf pts[8],sz;
int i,j;
for(i=0;i<8;i++) {
for(j=0;j<3;j++) sz[j]=pbbox->abox.size[j]*(((i>>j&1)<<1)-1);
pts[i] = pbbox->abox.Basis.T()*sz + pbbox->abox.center;
}
for(i=0;i<8;i++) for(j=0;j<3;j++) if (i&1<<j)
DrawLineFunc(pts[i],pts[i^1<<j]);
}
int CPrimitive::Intersect(IGeometry *_pCollider, geom_world_data *pdata1,geom_world_data *pdata2,
intersection_params *pparams, geom_contact *&pcontacts)
{
CGeometry *pCollider = (CGeometry*)_pCollider;
if (!pCollider->IsAPrimitive())
return CGeometry::Intersect(pCollider,pdata1,pdata2,pparams,pcontacts);
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
static geom_world_data defgwd;
static intersection_params defip;
pdata1 = (geom_world_data*)((intptr_t)pdata1 | -iszero((intptr_t)pdata1) & (intptr_t)&defgwd);
pdata2 = (geom_world_data*)((intptr_t)pdata2 | -iszero((intptr_t)pdata2) & (intptr_t)&defgwd);
pparams = (intersection_params*)((intptr_t)pparams | -iszero((intptr_t)pparams) & (intptr_t)&defip);
if (!pparams->bKeepPrevContacts)
g_nAreas = g_nAreaPt = g_nTotContacts = g_BrdPtBufPos = 0;
if (g_nTotContacts>=sizeof(g_Contacts)/sizeof(g_Contacts[0]))
return 0;
pcontacts = g_Contacts+g_nTotContacts;
pcontacts->ptborder = g_BrdPtBuf+g_BrdPtBufPos;
pcontacts->nborderpt = 0;
pparams->pGlobalContacts = g_Contacts;
BV *pBV1,*pBV2;
prim_inters inters;
vectorf sweepdir(zero); float sweepstep=0;
ResetGlobalPrimsBuffers();
g_Overlapper.Init();
if (!pparams->bSweepTest)
GetBVTree()->GetNodeBV(pdata1->R,pdata1->offset,pdata1->scale, pBV1);
else {
sweepstep = pdata1->v.len();
sweepdir = pdata1->v/sweepstep;
sweepstep *= pparams->time_interval;
GetBVTree()->GetNodeBV(pdata1->R,pdata1->offset,pdata1->scale, pBV1, sweepdir,sweepstep);
}
pCollider->GetBVTree()->GetNodeBV(pdata2->R,pdata2->offset,pdata2->scale, pBV2);
if (!g_Overlapper.Check(pBV1->type,pBV2->type, *pBV1,*pBV2))
return 0;
// get primitives in world space
int i,itype[2],bUnprojected=0;
primitive *pprim[2];
pdata1->offset += sweepdir*sweepstep;
itype[0] = PreparePrimitive(pdata1,pprim[0]);
itype[1] = pCollider->PreparePrimitive(pdata2,pprim[1]);
pdata1->offset -= sweepdir*sweepstep;
if (pCollider->m_iCollPriority==0) { // probably the other geometry is a ray
if (!g_Intersector.Check(itype[0],itype[1], pprim[0],pprim[1], &inters))
return 0;
geometry_under_test gtest;
gtest.contacts = g_Contacts+g_nTotContacts;
gtest.pnContacts = &g_nTotContacts;
gtest.pParams = pparams;
pCollider->RegisterIntersection(pprim[0],pprim[1],&gtest,0,&inters);
pcontacts->n.Flip();
return 1;
}
unprojection_mode unproj;
contact contact_best;
contact_best.t = 0;
geom_contact_area *parea;
unproj.minPtDist = min(m_minVtxDist,pCollider->m_minVtxDist);
if (!pparams->bNoAreaContacts && g_nAreas<sizeof(g_AreaBuf)/sizeof(g_AreaBuf[0]) && g_nAreaPt<sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])) {
parea = g_AreaBuf+g_nAreas;
parea->pt = g_AreaPtBuf+g_nAreaPt;
parea->piPrim[0] = g_AreaPrimBuf0+g_nAreaPt;
parea->piFeature[0] = g_AreaFeatureBuf0+g_nAreaPt;
parea->piPrim[1] = g_AreaPrimBuf1+g_nAreaPt;
parea->piFeature[1] = g_AreaFeatureBuf1+g_nAreaPt;
parea->npt = 0; parea->nmaxpt = sizeof(g_AreaPtBuf)/sizeof(g_AreaPtBuf[0])-g_nAreaPt;
parea->minedge = 0;
} else
parea = 0;
if (!pparams->bSweepTest) {
vectorf ptm;
if (!pparams->bNoIntersection) {
if (g_Intersector.CheckExists(itype[0],itype[1])) {
inters.ptborder = pcontacts->ptborder;
inters.nborderpt = 0;
if (!g_Intersector.Check(itype[0],itype[1], pprim[0],pprim[1], &inters))
return 0;
ptm = inters.ptborder[0];
pcontacts->nborderpt = inters.nborderpt;
for(i=0,pcontacts->center.zero();i<inters.nborderpt;i++) pcontacts->center += pcontacts->ptborder[i];
pcontacts->center /= inters.nborderpt;
}
} else if (!g_Overlapper.Check(itype[0],itype[1], pprim[0],pprim[1]))
return 0;
if (pparams->iUnprojectionMode==0) {
if (pparams->vrel_min<1E9f) {
vectorf vrel;
if (!pcontacts->nborderpt) {
vectorf center[2],pt[3]; box bbox;
int iPrim,iFeature; // dummy parameters
if (pBV1->type==box::type) center[0] = ((box*)(primitive*)*pBV1)->center;
else { GetBBox(&bbox); center[0] = pdata1->R*bbox.center+pdata1->offset; }
if (pBV1->type==box::type) center[1] = ((box*)(primitive*)*pBV2)->center;
else { pCollider->GetBBox(&bbox); center[1] = pdata2->R*bbox.center+pdata2->offset; }
FindClosestPoint(pdata1,iPrim,iFeature,center[1],center[1],pt+0);
pCollider->FindClosestPoint(pdata2,iPrim,iFeature,center[0],center[0],pt+1);
if (pCollider->PointInsideStatus(((pt[0]-pdata2->offset)*pdata2->R)*(pdata2->scale==1.0f ? 1.0f:1.0f/pdata2->scale)))
ptm = pt[0];
else if (PointInsideStatus(((pt[1]-pdata1->offset)*pdata1->R)*(pdata1->scale==1.0f ? 1.0f:1.0f/pdata1->scale)))
ptm = pt[1];
else
ptm = (pt[0]+pt[1])*0.5f;
}
vrel = pdata1->v+(pdata1->w^ptm-pdata1->centerOfMass)-pdata2->v-(pdata2->w^ptm-pdata2->centerOfMass);
if (vrel.len2()>sqr(pparams->vrel_min)) {
unproj.imode = 0; // unproject along vrel
(unproj.dir=-vrel) /= (unproj.vel=vrel.len());
unproj.tmax = pparams->time_interval*unproj.vel;
bUnprojected = g_Unprojector.Check(&unproj, itype[0],itype[1], pprim[0],-1,pprim[1],-1, &contact_best, parea);
bUnprojected &= isneg(contact_best.t-unproj.tmax);
}
}
} else {
unproj.imode = 1;
unproj.center = pparams->centerOfRotation;
if (pparams->axisOfRotation.len2()==0) {
unproj.dir = inters.n^inters.pt[0]-unproj.center;
if (unproj.dir.len2()<1E-6f)
unproj.dir = GetOrthogonal(inters.n);
unproj.dir.normalize();
} else
unproj.dir = pparams->axisOfRotation;
bUnprojected = g_Unprojector.Check(&unproj, itype[0],itype[1], pprim[0],-1,pprim[1],-1, &contact_best, parea);
if (bUnprojected)
contact_best.t = atan2(contact_best.t,contact_best.taux);
}
if (!bUnprojected) {
unproj.imode = 0;
unproj.dir.zero(); // zero requested direction - means minimum direction will be found
unproj.vel = 0; unproj.tmax = pparams->maxUnproj;
bUnprojected = g_Unprojector.Check(&unproj, itype[0],itype[1], pprim[0],-1,pprim[1],-1, &contact_best, parea);
}
} else {
unproj.imode = 0;
unproj.dir = -sweepdir;
unproj.tmax = sweepstep;
unproj.bCheckContact = 1;
contact_best.t = 0;
bUnprojected = g_Unprojector.Check(&unproj, itype[0],itype[1], pprim[0],-1,pprim[1],-1, &contact_best, parea);
bUnprojected &= isneg(contact_best.t-unproj.tmax);
if (bUnprojected && contact_best.n*unproj.dir>0) {
// if we hit something with the back side, 1st primitive probably just passed through the 2nd one,
// so move a little deeper than the contact and unproject again (some primitives check for minimal separating
// unprojection, and some - for maximum contacting; the former can cause this)
real t = contact_best.t;
box bbox; pCollider->GetBBox(&bbox);
vectorf dirloc,dirsz;
dirloc = bbox.Basis*(sweepdir*pdata2->R);
dirsz(fabs_tpl(dirloc.x)*bbox.size.y*bbox.size.z, fabs_tpl(dirloc.y)*bbox.size.x*bbox.size.z, fabs_tpl(dirloc.z)*bbox.size.x*bbox.size.y);
i = idxmax3((float*)&dirsz);
t += bbox.size[i]/fabs_tpl(dirloc[i])*0.1f;
unproj.tmax = sweepstep-t;
pdata1->offset += sweepdir*unproj.tmax;
itype[0] = PreparePrimitive(pdata1,pprim[0]);
pdata1->offset -= sweepdir*unproj.tmax;
bUnprojected = g_Unprojector.Check(&unproj, itype[0],itype[1], pprim[0],-1,pprim[1],-1, &contact_best, parea);
bUnprojected &= isneg(contact_best.t-unproj.tmax) & isneg(contact_best.n*unproj.dir);
contact_best.t += t;
unproj.tmax = sweepstep;
}
}
if (bUnprojected) {
pcontacts->t = contact_best.t;
if (pparams->bSweepTest)
pcontacts->t = unproj.tmax-pcontacts->t;
pcontacts->pt = contact_best.pt;
pcontacts->n = contact_best.n.normalized();
pcontacts->dir = unproj.dir;
pcontacts->iUnprojMode = unproj.imode;
pcontacts->vel = unproj.vel;
pcontacts->id[0] = pcontacts->id[1] = -1;
pcontacts->iPrim[0] = pcontacts->iPrim[1] = 0;
pcontacts->iFeature[0] = contact_best.iFeature[0];
pcontacts->iFeature[1] = contact_best.iFeature[1];
pcontacts->iNode[0] = pcontacts->iNode[1] = 0;
if (!parea || parea->npt==0)
pcontacts->parea = 0;
else {
pcontacts->parea = parea;
g_nAreas++; g_nAreaPt += parea->npt;
if (pcontacts->nborderpt<parea->npt && (pcontacts->nborderpt==0 || (iszero(itype[0]-cylinder::type) | iszero(itype[1]-cylinder::type)))) {
pcontacts->ptborder = parea->pt;
pcontacts->nborderpt = parea->npt;
for(i=0,pcontacts->center.zero(); i<parea->npt; i++) pcontacts->center += parea->pt[i];
pcontacts->center /= parea->npt;
}
}
if (pcontacts->nborderpt==0) {
pcontacts->ptborder[0]=pcontacts->center = pcontacts->pt;
pcontacts->nborderpt = 1;
}
pcontacts->bBorderConsecutive = false;
g_nTotContacts++;
g_BrdPtBufPos += pcontacts->nborderpt;
}
return bUnprojected;
}

96
CryPhysics/geometry.h Normal file
View File

@@ -0,0 +1,96 @@
#ifndef geometry_h
#define geometry_h
#pragma once
extern surface_desc g_SurfaceDescBuf[64];
extern int g_SurfaceDescBufPos;
extern edge_desc g_EdgeDescBuf[64];
extern int g_EdgeDescBufPos;
extern int g_iFeatureBuf[64];
extern int g_iFeatureBufPos;
extern short g_IdBuf[256];
extern int g_IdBufPos;
extern int g_UsedNodesMap[8192];
extern int g_UsedNodesMapPos;
extern int g_UsedNodesIdx[64];
extern int g_UsedNodesIdxPos;
extern geom_contact g_Contacts[64];
extern int g_nTotContacts;
extern geom_contact_area g_AreaBuf[32];
extern int g_nAreas;
extern vectorf g_AreaPtBuf[256];
extern int g_AreaPrimBuf0[256],g_AreaFeatureBuf0[256],g_AreaPrimBuf1[256],g_AreaFeatureBuf1[256];
extern int g_nAreaPt;
class CGeometry : public IGeometry {
public:
CGeometry() { m_bIsConvex=0; }
virtual ~CGeometry() {}
virtual int GetType() = 0;
virtual void Release() { delete this; }
virtual void GetBBox(box *pbox) = 0;
virtual int Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts);
virtual int CalcPhysicalProperties(phys_geometry *pgeom) { return 0; }
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters=10) { return 0; }
virtual int PointInsideStatus(const vectorf &pt) { return -1; }
virtual void DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel) {}
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L) {}
virtual float CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter) { massCenter.zero(); return 0; }
virtual void CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres) { dPres.zero(); dLres.zero(); }
virtual int GetPrimitiveId(int iPrim,int iFeature) { return -1; }
virtual int IsConvex(float tolerance) { return 1; }
virtual void PrepareForRayTest(float raylen) {}
virtual CBVTree *GetBVTree() { return 0; }
virtual int GetPrimitiveCount() { return 1; }
virtual int IsAPrimitive() { return 0; }
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim) { return -1; }
virtual int GetFeature(int iPrim,int iFeature, vectorf *pt) { return 0; }
virtual void RemapFaceIds(short *pMap,int sz) {}
virtual int UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact) { return 0; }
virtual float BuildOcclusionCubemap(geom_world_data *pgwd, int iMode, int *pGrid0[6],int *pGrid1[6],int nRes, float rmin,float rmax, int nGrow);
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale) { return 0; }
virtual void GetMemoryStatistics(ICrySizer *pSizer) {}
virtual void Save(CMemStream &stm) {}
virtual void Load(CMemStream &stm) {}
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false)
{ return 1; };
virtual int RegisterIntersection(primitive *pprim1,primitive *pprim2, geometry_under_test *pGTest1,geometry_under_test *pGTest2,
prim_inters *pinters);
virtual void CleanupAfterIntersectionTest(geometry_under_test *pGTest) {}
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal, geometry_under_test *pGTest,
geometry_under_test *pGTestOp, primitive *pRes,short *pResId) { return 0; }
virtual int GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest) { return 0; }
virtual int PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf) { return 0; }
virtual int PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf) { return 0; }
float m_minVtxDist;
int m_iCollPriority;
int m_bIsConvex;
};
class CPrimitive : public CGeometry {
public:
CPrimitive() { m_bIsConvex=1; }
virtual int Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts);
virtual int IsAPrimitive() { return 1; }
};
void DrawBBox(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, CBVTree *pTree,BBox *pbbox,int maxlevel,int level=0);
#endif

45
CryPhysics/grid.h Normal file
View File

@@ -0,0 +1,45 @@
//////////////////////////////////////////////////////////////////////
//
// Grid
//
// File: grid.h
// Description : Grid class declaration and inlined implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef grid_h
#define grid_h
#pragma once
#include "vector3d.h"
#include "vector2d.h"
struct grid {
vector_tpl<int> ax;
vectorf org;
vector2df step,stepr;
vector2d_tpl<unsigned int> sz;
float parity;
float scale;
unsigned short *pflags;
unsigned short holeflag;
int holepower;
int inrange(unsigned int ix, unsigned int iy) {
return ix<sz.x && iy<sz.y;
}
int inrange(int ix, int iy) {
return (unsigned int)ix<sz.x && (unsigned int)iy<sz.y;
}
int getcell(unsigned int ix, unsigned int iy) {
return ix<sz.x && iy<sz.y ? iy*sz.x+ix : sz.x*sz.y;
}
int getcell(int ix,int iy) {
return (unsigned int)ix<sz.x && (unsigned int)iy<sz.y ? iy*sz.x+ix : sz.x*sz.y;
}
};
#endif

131
CryPhysics/heightfieldbv.cpp Normal file
View File

@@ -0,0 +1,131 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "trimesh.h"
#include "heightfieldbv.h"
BVheightfield CHeightfieldBV::g_BVhf;
struct InitHeightfieldGlobals {
InitHeightfieldGlobals() {
CHeightfieldBV::g_BVhf.iNode = 0;
CHeightfieldBV::g_BVhf.type = heightfield::type;
}
};
static InitHeightfieldGlobals now;
float CHeightfieldBV::Build(CGeometry *pGeom)
{
m_pMesh = (CTriMesh*)pGeom;
return (m_phf->size.x*m_phf->step.x*m_phf->size.y*m_phf->step.y*(m_maxHeight-m_minHeight));
}
void CHeightfieldBV::SetHeightfield(heightfield *phf)
{
m_phf = phf;
m_PatchStart.set(0,0);
m_PatchSize = m_phf->size;
m_minHeight = -1;
m_maxHeight = 1;
}
void CHeightfieldBV::GetBBox(box *pbox)
{
pbox->size.x = m_PatchSize.x*m_phf->step.x*0.5f;
pbox->size.y = m_PatchSize.y*m_phf->step.y*0.5f;
pbox->size.z = (m_maxHeight-m_minHeight)*0.5f;
pbox->center.x = m_PatchStart.x*m_phf->step.x + pbox->size.x;
pbox->center.y = m_PatchStart.y*m_phf->step.y + pbox->size.y;
pbox->center.z = m_minHeight + pbox->size.z;
pbox->Basis.SetIdentity();
pbox->bOriented = 0;
}
void CHeightfieldBV::GetNodeBV(BV *&pBV,int iNode)
{
pBV = &g_BVhf;
g_BVhf.hf = *m_phf;
g_BVhf.hf.Basis.SetIdentity();
g_BVhf.hf.bOriented = 0;
g_BVhf.hf.origin.zero();
}
void CHeightfieldBV::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode)
{
pBV = &g_BVhf;
g_BVhf.hf = *m_phf;
g_BVhf.hf.Basis = Rw.T();
g_BVhf.hf.bOriented = 1;
g_BVhf.hf.origin = offsw;
g_BVhf.hf.step = m_phf->step*scalew;
g_BVhf.hf.stepr = m_phf->stepr/scalew;
g_BVhf.hf.heightscale *= scalew;
}
void project_box_on_grid(box *pbox,grid *pgrid, geometry_under_test *pGTest, int &ix,int &iy,int &sx,int &sy,float &minz)
{
vectorf center; vectorf dim;
if (!pGTest) {
matrix3x3f Basis = pbox->Basis;
dim = pbox->size*Basis.Fabs();
center = pbox->center;
} else {
matrix3x3f Basis;
if (pbox->bOriented)
Basis = pbox->Basis*pGTest->R_rel;
else
Basis = pGTest->R_rel;
dim = (pbox->size*pGTest->rscale_rel)*Basis.Fabs();
center = ((pbox->center-pGTest->offset_rel)*pGTest->R_rel)*pGTest->rscale_rel;
}
ix = float2int((center.x-dim.x)*pgrid->stepr.x-0.5f); ix &= ~(ix>>31);
iy = float2int((center.y-dim.y)*pgrid->stepr.y-0.5f); iy &= ~(iy>>31);
sx = min(float2int((center.x+dim.x)*pgrid->stepr.x+0.5f),pgrid->size.x)-ix;
sy = min(float2int((center.y+dim.y)*pgrid->stepr.y+0.5f),pgrid->size.y)-iy;
minz = center.z-dim.z;
}
void CHeightfieldBV::MarkUsedTriangle(int itri, geometry_under_test *pGTest)
{
m_pUsedTriMap[itri>>5] |= 1u<<(itri&31);
}
int CHeightfieldBV::GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp)
{
int iStart,nCols,nRows,nPrims,ix,iy,ix0,iy0,i,j;
float minz;
if (pBVCollider->type==box::type) {
project_box_on_grid((box*)(primitive*)*pBVCollider,m_phf, (geometry_under_test*)((intptr_t)pGTest & -((intptr_t)bColliderLocal^1)),
ix0,iy0,nCols,nRows,minz);
ix = ix0-m_PatchStart.x; iy = iy0-m_PatchStart.y; ix &= ~(ix>>31); iy &= ~(iy>>31);
nCols = min(ix0+nCols,m_PatchStart.x+m_PatchSize.x)-ix-m_PatchStart.x; // don't forget to crop values to the current patch
nRows = min(iy0+nRows,m_PatchStart.y+m_PatchSize.y)-iy-m_PatchStart.y;
} else {
nCols=m_PatchSize.x; nRows=m_PatchSize.y; ix=iy=0;
}
//if (m_phf->gettype(ix,iy)==-1)
// return 0;
iStart = ix+iy*m_PatchSize.x;
if (!bColliderUsed) {
for(i=nPrims=0; i<nRows; i++,iStart+=m_PatchSize.x)
nPrims += m_pMesh->GetPrimitiveList(iStart*2,nCols*2, pBVCollider->type,*pBVCollider,bColliderLocal, pGTest,pGTestOp,
(primitive*)((char*)pGTest->primbuf+nPrims*pGTest->szprim), pGTest->idbuf+nPrims);
} else {
for(i=nPrims=0; i<nRows; i++,iStart+=m_PatchSize.x) for(j=0;j<nCols*2;j++) {
int itri = iStart*2+j;
if (!(m_pUsedTriMap[itri>>5]>>itri & 1))
nPrims += m_pMesh->GetPrimitiveList(itri,1, pBVCollider->type,*pBVCollider,bColliderLocal, pGTest,pGTestOp,
(primitive*)((char*)pGTest->primbuf+nPrims*pGTest->szprim), pGTest->idbuf+nPrims);
}
}
return nPrims;
}

36
CryPhysics/heightfieldbv.h Normal file
View File

@@ -0,0 +1,36 @@
#ifndef heightfieldbv_h
#define heightfieldbv_h
#pragma once
class CHeightfieldBV : public CBVTree {
public:
CHeightfieldBV() { m_pUsedTriMap=0; }
virtual ~CHeightfieldBV() { if (m_pUsedTriMap) delete[] m_pUsedTriMap; }
virtual int GetType() { return BVT_HEIGHTFIELD; }
virtual float Build(CGeometry *pGeom);
void SetHeightfield(heightfield *phf);
virtual void GetBBox(box *pbox);
virtual int MaxPrimsInNode() { return m_PatchSize.x*m_PatchSize.y*2; }
virtual void GetNodeBV(BV *&pBV, int iNode=0);
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp);
virtual void MarkUsedTriangle(int itri, geometry_under_test *pGTest);
CTriMesh *m_pMesh;
heightfield *m_phf;
vector2di m_PatchStart;
vector2di m_PatchSize;
float m_minHeight,m_maxHeight;
unsigned int *m_pUsedTriMap;
static BVheightfield g_BVhf;
};
void project_box_on_grid(box *pbox,grid *pgrid, geometry_under_test *pGTest, int &ix,int &iy,int &sx,int &sy,float &minz);
#endif

375
CryPhysics/heightfieldgeom.cpp Normal file
View File

@@ -0,0 +1,375 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "intersectionchecks.h"
#include "overlapchecks.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "geometry.h"
#include "trimesh.h"
#include "raybv.h"
#include "raygeom.h"
#include "heightfieldbv.h"
#include "heightfieldgeom.h"
CHeightfield* CHeightfield::CreateHeightfield(heightfield *phf)
{
m_hf = *phf;
m_hf.Basis.SetIdentity();
m_hf.origin.zero();
m_hf.stepr.x = 1.0f/phf->step.x; m_hf.stepr.y = 1.0f/phf->step.y;
int i = (phf->typemask^phf->typemask-1)+1>>1;
for(m_hf.typepower=0; !(i&1); i>>=1,m_hf.typepower++);
m_minHeight = 1E10f; m_maxHeight = -1E10f;
for(i=(phf->size.x+1)*(phf->size.y+1)-1; i>=0; i--) {
m_minHeight = min(m_minHeight, phf->getheight(i));
m_maxHeight = max(m_maxHeight, phf->getheight(i));
}
m_Tree.m_phf = &m_hf;
m_Tree.Build(this);
m_pTree = &m_Tree;
m_pVertices = new vectorf[m_nVerticesAlloc = 32];
m_pNormals = new vectorf[m_nTrisAlloc = 64];
m_pIndices = new index_t[m_nTrisAlloc*3];
m_pIds = new short[m_nTrisAlloc];
m_pTopology = new trinfo[m_nTrisAlloc];
m_Tree.m_pUsedTriMap = new unsigned int[(m_nTrisAlloc-1>>5)+1];
m_minVtxDist = (m_hf.step.x+m_hf.step.y)*1E-3f;
m_nVertices = m_nTris = 0;
m_flags = 3;
return this;
}
struct hf_cell_checker {
float dir2d_len,max_zcell,maxt;
int idcontact;
triangle hftri;
ray hfray;
heightfield *phf;
prim_inters inters;
vector2df org2d,dir2d;
int check_cell(const vector2di &icell, int &ilastcell) {
quotientf t((org2d+icell)*dir2d, dir2d_len*dir2d_len);
if (t.x>maxt || !phf->inrange(icell.x,icell.y))
return 1;
float h[4], zlowest = hfray.origin.z*t.y + hfray.dir.z*t.x - max_zcell;
int idcell = icell.x*phf->stride.x + icell.y*phf->stride.y, itype = phf->gettype(idcell);
h[0] = phf->getheight(idcell);
h[1] = phf->getheight(idcell+phf->stride.x);
h[2] = phf->getheight(idcell+phf->stride.y);
h[3] = phf->getheight(idcell+phf->stride.x+phf->stride.y);
if (zlowest<=max(max(max(h[0],h[1]),h[2]),h[3])*t.y && itype>=0) {
hftri.pt[0].x = hftri.pt[2].x = icell.x*phf->step.x; hftri.pt[1].x = hftri.pt[0].x+phf->step.x;
hftri.pt[0].y = hftri.pt[1].y = icell.y*phf->step.y; hftri.pt[2].y = hftri.pt[0].y+phf->step.y;
hftri.pt[0].z = h[0]; hftri.pt[1].z = h[1]; hftri.pt[2].z = h[2];
hftri.n = hftri.pt[1]-hftri.pt[0] ^ hftri.pt[2]-hftri.pt[0];
if (ray_tri_intersection(&hfray,&hftri, &inters) && hftri.n*hfray.dir<0)
{ idcontact = itype; return 1; }
hftri.pt[0] = hftri.pt[2]; hftri.pt[2].x += phf->step.x; hftri.pt[2].z = h[3];
hftri.n = hftri.pt[1]-hftri.pt[0] ^ hftri.pt[2]-hftri.pt[0];
if (ray_tri_intersection(&hfray,&hftri, &inters) && hftri.n*hfray.dir<0)
{ idcontact = itype; return 1; }
}
return 0;
}
};
int CHeightfield::Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts)
{
if (pCollider->GetType()==GEOM_RAY) {
geometry_under_test GRay;
bool bKeepPrevContacts = pparams ? pparams->bKeepPrevContacts : false;
((CGeometry*)pCollider)->PrepareForIntersectionTest(&GRay, this,0, bKeepPrevContacts);
ray *pray = (ray*)GRay.primbuf;
float rscale;
hf_cell_checker hcc;
vectorf dirn = ((CRayGeom*)pCollider)->m_dirn, origin,dir;
unprojection_mode unproj;
contact contact_best;
hcc.phf = &m_hf;
if (!bKeepPrevContacts)
g_nTotContacts = 0;
// transform ray to grid coordinates
rscale = 1.0f/pdata1->scale;
hcc.hfray.origin = origin = ((pray->origin-pdata1->offset)*pdata1->R)*rscale;
hcc.hfray.dir = dir = (pray->dir*pdata1->R)*rscale;
hcc.inters.minPtDist2 = sqr(m_minVtxDist);
hcc.org2d.set(0.5f-hcc.hfray.origin.x*m_hf.stepr.x,0.5f-hcc.hfray.origin.y*m_hf.stepr.y);
hcc.dir2d.set(hcc.hfray.dir.x*m_hf.stepr.x,hcc.hfray.dir.y*m_hf.stepr.y);
hcc.dir2d_len = hcc.dir2d.len();
hcc.max_zcell = fabs_tpl(hcc.hfray.dir.z)*hcc.dir2d_len*(sqrt2*0.5f);
hcc.maxt = hcc.dir2d_len*(hcc.dir2d_len+sqrt2);
hcc.idcontact = -2;
if (DrawRayOnGrid(&m_hf, origin,dir, hcc) && hcc.idcontact!=-2) {
g_Contacts[g_nTotContacts].ptborder = &g_Contacts[g_nTotContacts].center;
g_Contacts[g_nTotContacts].center = (pdata1->R*hcc.inters.pt[0])*pdata1->scale + pdata1->offset;
g_Contacts[g_nTotContacts].nborderpt = 1;
g_Contacts[g_nTotContacts].parea = 0;
if (((CGeometry*)pCollider)->m_iCollPriority<m_iCollPriority || !pparams)
unproj.imode = -1;
else {
if (unproj.imode = pparams->iUnprojectionMode) {
if ((unproj.dir = pparams->axisOfRotation).len2()==0) {
unproj.dir = g_Contacts[g_nTotContacts].n^pray->dir;
//if (unproj.dir.len2()<0.002) unproj.dir = aray.dir.orthogonal();
if (unproj.dir.len2()<0.002) unproj.dir.SetOrthogonal(pray->dir);
unproj.dir.normalize();
}
unproj.center = pparams->centerOfRotation;
unproj.tmax = pi*0.5;
} else {
unproj.dir = pdata2->v-pdata1->v;
unproj.tmax = pparams->time_interval;
if (fabsf(unproj.dir.len2()-1.0f)>0.001f)
unproj.dir.normalize();
}
unproj.minPtDist = m_minVtxDist;
for(int i=0;i<3;i++) hcc.hftri.pt[i] = pdata1->R*hcc.hftri.pt[i]*pdata1->scale + pdata1->offset;
hcc.hftri.n = pdata1->R*hcc.hftri.n;
}
if (unproj.imode<0 || !g_Unprojector.Check(&unproj, triangle::type,ray::type, &hcc.hftri,-1,pray,-1, &contact_best)) {
g_Contacts[g_nTotContacts].t = ((pdata1->R*(hcc.inters.pt[0]-hcc.hfray.origin))*dirn)*pdata1->scale;
g_Contacts[g_nTotContacts].pt = g_Contacts[g_nTotContacts].center;
g_Contacts[g_nTotContacts].n = pdata1->R*hcc.inters.n.normalized();
g_Contacts[g_nTotContacts].dir.zero();
g_Contacts[g_nTotContacts].iUnprojMode = -1;
} else {
if (unproj.imode)
contact_best.t = atan2(contact_best.t,contact_best.taux);
g_Contacts[g_nTotContacts].t = contact_best.t;
g_Contacts[g_nTotContacts].pt = contact_best.pt;
g_Contacts[g_nTotContacts].n = contact_best.n;
g_Contacts[g_nTotContacts].dir = unproj.dir;
g_Contacts[g_nTotContacts].iUnprojMode = unproj.imode;
}
g_Contacts[g_nTotContacts].vel = 0;
g_Contacts[g_nTotContacts].id[0] = hcc.idcontact;
g_Contacts[g_nTotContacts].id[1] = -1;
pcontacts = g_Contacts+g_nTotContacts++;
if (pparams)
pparams->pGlobalContacts = g_Contacts;
return 1;
}
return 0;
}
return CTriMesh::Intersect(pCollider,pdata1,pdata2,pparams,pcontacts);
}
int CHeightfield::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
box abox,aboxext,*pbox; pCollider->GetBBox(&abox);
if (pGTestColl->sweepstep>0) {
ExtrudeBox(&abox, pGTestColl->sweepdir_loc,pGTestColl->sweepstep_loc, &aboxext);
pbox = &aboxext;
} else pbox = &abox;
int ix,iy,sx,sy,i,j,itri,icol,irow,icell;
float curx,cury,maxh,minz;
index_t *pIdx;
trinfo *pTop;
vectorf *pVtx;
project_box_on_grid(pbox,&m_hf, pGTest, ix,iy,sx,sy,minz);
for(i=ix,maxh=m_minHeight;i<=ix+sx;i++) for(j=iy;j<=iy+sy;j++)
maxh = max(maxh,m_hf.getheight(i,j));
if (minz>maxh || sx<=0 || sy<=0)
return 0;
m_Tree.m_PatchStart.set(ix,iy);
m_Tree.m_PatchSize.set(sx,sy);
m_nVertices = (sx+1)*(sy+1);
m_nTris = sx*sy*2;
m_nMaxVertexValency = 6;
if (m_nVerticesAlloc<m_nVertices) {
delete[] m_pVertices;
m_pVertices = new vectorf[m_nVerticesAlloc = (m_nVertices-1 & ~15)+16];
}
if (m_nTrisAlloc<m_nTris) {
delete[] m_pIndices; delete[] m_pNormals; delete[] m_pIds; delete[] m_pTopology; delete[] m_Tree.m_pUsedTriMap;
m_pNormals = new vectorf[m_nTrisAlloc = (m_nTris-1 & ~15)+16];
m_pIndices = new index_t[m_nTrisAlloc*3];
m_pIds = new short[m_nTrisAlloc];
m_pTopology = new trinfo[m_nTrisAlloc];
m_Tree.m_pUsedTriMap = new unsigned int[(m_nTrisAlloc-1>>5)+1];
}
m_Tree.m_minHeight = m_Tree.m_minHeight = m_hf.getheight(ix,iy);
for(j=0,pVtx=m_pVertices.data; j<=sy; j++) {
icell = ix*m_hf.stride.x + (iy+j)*m_hf.stride.y;
curx = m_hf.step.x*ix; cury = m_hf.step.y*(iy+j);
for(i=0; i<=sx; i++,pVtx++,icell+=m_hf.stride.x,curx+=m_hf.step.x) {
pVtx->Set(curx,cury,m_hf.getheight(icell));
m_Tree.m_minHeight = min(m_Tree.m_minHeight, pVtx->z);
m_Tree.m_maxHeight = max(m_Tree.m_maxHeight, pVtx->z);
}
}
for(irow=i=itri=0,pIdx=m_pIndices,pTop=m_pTopology; irow<sy; irow++,i++)
for(icol=0,icell=ix*m_hf.stride.x+(irow+iy)*m_hf.stride.y; icol<sx; icol++,itri+=2,i++,icell+=m_hf.stride.x) {
*pIdx++=i; *pIdx++=i+1; *pIdx++=i+sx+1;
m_pNormals[itri] = (m_pVertices[pIdx[1-3]]-m_pVertices[pIdx[0-3]] ^ m_pVertices[pIdx[2-3]]-m_pVertices[pIdx[0-3]]).normalized();
*pIdx++=i+sx+1; *pIdx++=i+1; *pIdx++=i+sx+2;
m_pNormals[itri+1] = (m_pVertices[pIdx[1-3]]-m_pVertices[pIdx[0-3]] ^ m_pVertices[pIdx[2-3]]-m_pVertices[pIdx[0-3]]).normalized();
pTop->ibuddy[0] = itri-sx*2+1 | irow-1>>31; pTop->ibuddy[1] = itri+1; pTop++->ibuddy[2] = itri-1 | icol-1>>31;
pTop->ibuddy[0] = itri; pTop->ibuddy[1] = itri+2 | sx-2-icol>>31; pTop++->ibuddy[2] = itri+sx*2 | sy-2-irow>>31;
m_pIds[itri] = m_pIds[itri+1] = m_hf.gettype(icell);
}
for(i=m_nTris-1>>5;i>=0;i--)
m_Tree.m_pUsedTriMap[i] = 0;
for(itri=0;itri<m_nTris;itri++) if (m_pIds[itri]==-1)
for(j=0;j<3;j++) if ((i=m_pTopology[itri].ibuddy[0])>=0) // remove connectivity for 'hole' triangles
m_pTopology[i].ibuddy[GetEdgeByBuddy(i,itri)] = -1;
return CTriMesh::PrepareForIntersectionTest(pGTest, pCollider,pGTestColl, bKeepPrevContacts);
}
inline void CheckPtEdgeDist(const vectorf *pte, const vectorf &pt, vectorf &ptres,int &iFeature)
{
vectorf edge = pte[1]-pte[0], dp = pt-pte[0];
float dp_edge = dp*edge, edge_len2 = edge.len2();
if ((dp^edge).len2()<(pt-ptres).len2()*edge_len2 && dp_edge>0 && (pt-pte[1])*edge<0) {
ptres = pte[0] + edge*(dp_edge/edge_len2);
iFeature = 0x20;
}
}
inline void CheckPtTriDist(const triangle &tri, const vectorf &pt, vectorf &ptres,int &iFeature)
{
float dist = (pt-tri.pt[0])*tri.n;
if (dist>0 && sqr(dist)<(pt-ptres).len2()*tri.n.len2() && (tri.pt[1]-tri.pt[0]^pt-tri.pt[0])*tri.n>0 &&
(tri.pt[2]-tri.pt[1]^pt-tri.pt[1])*tri.n>0 && (tri.pt[0]-tri.pt[2]^pt-tri.pt[2])*tri.n>0)
{
vectorf n = tri.n.normalized();
ptres = pt-n*(n*(pt-tri.pt[0]));
iFeature = 0x40;
}
}
int CHeightfield::FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters)
{
triangle tri;
float h[4];
vectorf pt,ptres_loc;
int ix,iy,sx,sy,icell;
ptres[1] = ptdst0;
pt = ((ptdst0-pgwd->offset)*pgwd->R);
if (pgwd->scale!=1.0f) pt/=pgwd->scale;
ix = float2int(pt.x*m_hf.stepr.x-0.5f); iy = float2int(pt.y*m_hf.stepr.y-0.5f);
icell = ix*m_hf.stride.x + iy*m_hf.stride.y;
if ((unsigned int)ix>(unsigned int)m_hf.size.x-2 || (unsigned int)iy>(unsigned int)m_hf.size.y-2 || m_hf.gettype(icell)<0) {
ptres[0].Set(1E10,1E10,1E10); return -1;
}
h[0] = m_hf.getheight(icell);
h[1] = m_hf.getheight(icell+m_hf.stride.x);
h[2] = m_hf.getheight(icell+m_hf.stride.y);
h[3] = m_hf.getheight(icell+m_hf.stride.x+m_hf.stride.y);
sx = isneg(m_hf.step.x*ix+0.5f-pt.x);
sy = isneg(m_hf.step.y*iy+0.5f-pt.y);
ptres_loc.Set((ix+sx)*m_hf.step.x, (iy+sy)*m_hf.step.y, h[sx+sy*2]);
iFeature = 0;
tri.pt[0].x = tri.pt[2].x = ix*m_hf.step.x; tri.pt[1].x = tri.pt[0].x+m_hf.step.x;
tri.pt[0].y = tri.pt[1].y = iy*m_hf.step.y; tri.pt[2].y = tri.pt[0].y+m_hf.step.y;
tri.pt[0].z = h[0]; tri.pt[1].z = h[1]; tri.pt[2].z = h[2];
tri.n = tri.pt[1]-tri.pt[0] ^ tri.pt[2]-tri.pt[0];
CheckPtTriDist(tri,pt, ptres_loc,iFeature);
tri.pt[0] = tri.pt[2]; tri.pt[2].x += m_hf.step.x; tri.pt[2].z = h[3];
tri.n = tri.pt[1]-tri.pt[0] ^ tri.pt[2]-tri.pt[0];
CheckPtTriDist(tri,pt, ptres_loc,iFeature);
tri.pt[1].x = tri.pt[0].x = (ix+sx)*m_hf.step.x;
tri.pt[1].y = (tri.pt[0].y = iy*m_hf.step.y) + m_hf.step.y;
tri.pt[0].z = h[sx]; tri.pt[1].z = h[sx+2];
CheckPtEdgeDist(tri.pt, pt, ptres_loc,iFeature);
tri.pt[1].y = tri.pt[0].y = (iy+sy)*m_hf.step.y;
tri.pt[1].x = (tri.pt[0].x = ix*m_hf.step.x) + m_hf.step.x;
tri.pt[0].z = h[sy*2]; tri.pt[1].z = h[sy*2+1];
CheckPtEdgeDist(tri.pt, pt, ptres_loc,iFeature);
tri.pt[0].Set((ix+1)*m_hf.step.x, iy*m_hf.step.y, h[1]);
tri.pt[1].Set(ix*m_hf.step.x, (iy+1)*m_hf.step.y, h[2]);
CheckPtEdgeDist(tri.pt, pt, ptres_loc,iFeature);
iPrim = 0;
ptres[0] = pgwd->R*ptres_loc*pgwd->scale + pgwd->offset;
return 1;
}
void CHeightfield::CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L)
{
return;
}
int CHeightfield::DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale)
{
int nTris,ix,iy,idcell;
float rscale = pgwd->scale==1.0f ? 1.0f:1.0f/pgwd->scale, h[4];
vector2di irect[2];
triangle tri;
sphere sph;
sph.center = (-pgwd->offset*rscale)*pgwd->R;
sph.r = rmax*rscale;
irect[0].x = max(0,min(m_hf.size.x, float2int((sph.center.x-sph.r)*m_hf.stepr.x-0.5f)));
irect[0].y = max(0,min(m_hf.size.y, float2int((sph.center.y-sph.r)*m_hf.stepr.y-0.5f)));
irect[1].x = max(0,min(m_hf.size.x, float2int((sph.center.x+sph.r)*m_hf.stepr.x+0.5f)));
irect[1].y = max(0,min(m_hf.size.y, float2int((sph.center.y+sph.r)*m_hf.stepr.y+0.5f)));
sph.center.zero();
sph.r = rmax;
for(ix=irect[0].x,nTris=0;ix<irect[1].x;ix++) for(iy=irect[0].y;iy<irect[1].y;iy++) {
idcell = ix*m_hf.stride.x + iy*m_hf.stride.y;
if (m_hf.gettype(idcell)>=0) {
h[0] = m_hf.getheight(idcell);
h[1] = m_hf.getheight(idcell+m_hf.stride.x);
h[2] = m_hf.getheight(idcell+m_hf.stride.y);
h[3] = m_hf.getheight(idcell+m_hf.stride.x+m_hf.stride.y);
tri.pt[0].x = tri.pt[2].x = ix*m_hf.step.x; tri.pt[1].x = tri.pt[0].x+m_hf.step.x;
tri.pt[0].y = tri.pt[1].y = iy*m_hf.step.y; tri.pt[2].y = tri.pt[0].y+m_hf.step.y;
tri.pt[0].z = h[0]; tri.pt[1].z = h[1]; tri.pt[2].z = h[2];
tri.pt[0] = pgwd->R*tri.pt[0]*pgwd->scale+pgwd->offset;
tri.pt[1] = pgwd->R*tri.pt[1]*pgwd->scale+pgwd->offset;
tri.pt[2] = pgwd->R*tri.pt[2]*pgwd->scale+pgwd->offset;
tri.n = tri.pt[1]-tri.pt[0] ^ tri.pt[2]-tri.pt[0];
if (tri_sphere_overlap_check(&tri,&sph)) {
RasterizePolygonIntoCubemap(tri.pt,3, iPass, pGrid,nRes, rmin,rmax,zscale);
nTris++;
}
tri.pt[0] = tri.pt[2]; tri.pt[2] += pgwd->R*vectorf(pgwd->scale*m_hf.step.x,0,0);
tri.pt[2] += pgwd->R*vectorf(0,0,pgwd->scale*(h[3]-h[2]));
tri.n = tri.pt[1]-tri.pt[0] ^ tri.pt[2]-tri.pt[0];
if (tri_sphere_overlap_check(&tri,&sph)) {
RasterizePolygonIntoCubemap(tri.pt,3, iPass, pGrid,nRes, rmin,rmax,zscale);
nTris++;
}
}
}
return nTris;
}
void CHeightfield::GetMemoryStatistics(ICrySizer *pSizer)
{
CTriMesh::GetMemoryStatistics(pSizer);
pSizer->AddObject(this, sizeof(CHeightfield));
}

34
CryPhysics/heightfieldgeom.h Normal file
View File

@@ -0,0 +1,34 @@
#ifndef heightfieldgeom_h
#define heightfieldgeom_h
#pragma once
class CHeightfield : public CTriMesh {
public:
CHeightfield() {}
virtual ~CHeightfield() { m_pTree=0; }
CHeightfield* CreateHeightfield(heightfield *phf);
virtual int GetType() { return GEOM_HEIGHTFIELD; }
virtual int Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false);
virtual int PointInsideStatus(const vectorf &pt) { return -1; }
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters=10);
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L);
virtual int IsConvex(float tolerance) { return 0; }
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale);
virtual void PrepareForRayTest(float raylen) {}
virtual CBVTree *GetBVTree() { return &m_Tree; }
virtual int GetPrimitive(int iPrim, primitive *pprim) { *(heightfield*)pprim = m_hf; return sizeof(heightfield); }
virtual void GetMemoryStatistics(ICrySizer *pSizer);
heightfield m_hf;
CHeightfieldBV m_Tree;
float m_minHeight,m_maxHeight;
int m_nVerticesAlloc,m_nTrisAlloc;
};
#endif

1202
CryPhysics/intersectionchecks.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

55
CryPhysics/intersectionchecks.h Normal file
View File

@@ -0,0 +1,55 @@
#ifndef intersectionchecks_h
#define intersectionchecks_h
typedef int (*intersection_check)(const primitive*,const primitive*,prim_inters*);
int default_intersection(const primitive*,const primitive*, prim_inters *pinters);
int tri_tri_intersection(const triangle *ptri1, const triangle *ptri2, prim_inters *pinters);
int tri_box_intersection(const triangle *ptri, const box *pbox, prim_inters *pinters);
int box_tri_intersection(const box *pbox, const triangle *ptri, prim_inters *pinters);
int tri_cylinder_intersection(const triangle *ptri, const cylinder *pcyl, prim_inters *pinters);
int cylinder_tri_intersection(const cylinder *pcyl, const triangle *ptri, prim_inters *pinters);
int tri_sphere_intersection(const triangle *ptri, const sphere *psphere, prim_inters *pinters);
int sphere_tri_intersection(const sphere *psphere, const triangle *ptri, prim_inters *pinters);
int tri_ray_intersection(const triangle *ptri, const ray *pray, prim_inters *pinters);
int ray_tri_intersection(const ray *pray, const triangle *ptri, prim_inters *pinters);
int tri_plane_intersection(const triangle *ptri, const plane *pplane, prim_inters *pinters);
int plane_tri_intersection(const plane *pplane, const triangle *ptri, prim_inters *pinters);
int box_box_intersection(const box *pbox1, const box *pbox2, prim_inters *pinters);
int box_cylinder_intersection(const box *pbox, const cylinder *pcyl, prim_inters *pinters);
int cylinder_box_intersection(const cylinder *pcyl, const box *pbox, prim_inters *pinters);
int box_sphere_intersection(const box *pbox, const sphere *psphere, prim_inters *pinters);
int sphere_box_intersection(const sphere *psphere, const box *pbox, prim_inters *pinters);
int box_ray_intersection(const box *pbox, const ray *pray, prim_inters *pinters);
int ray_box_intersection(const ray *pray, const box *pbox, prim_inters *pinters);
int box_plane_intersection(const box *pbox, const plane *pplane, prim_inters *pinters);
int plane_box_intersection(const plane *pplane, const box *pbox, prim_inters *pinters);
int cylinder_cylinder_intersection(const cylinder *pcyl1, const cylinder *pcyl2, prim_inters *pinters);
int cylinder_sphere_intersection(const cylinder *pcyl, const sphere *psphere, prim_inters *pinters);
int sphere_cylinder_intersection(const sphere *psphere, const cylinder *pcyl, prim_inters *pinters);
int cylinder_ray_intersection(const cylinder *pcyl, const ray *pray, prim_inters *pinters);
int ray_cylinder_intersection(const ray *pray, const cylinder *pcyl, prim_inters *pinters);
int cylinder_plane_intersection(const cylinder *pcyl, const plane *pplane, prim_inters *pinters);
int plane_cylinder_intersection(const plane *pplane, const cylinder *pcyl, prim_inters *pinters);
int sphere_sphere_intersection(const sphere *psphere1, const sphere *psphere2, prim_inters *pinters);
int sphere_ray_intersection(const sphere *psphere, const ray *pray, prim_inters *pinters);
int ray_sphere_intersection(const ray *pray, const sphere *psphere, prim_inters *pinters);
int sphere_plane_intersection(const sphere *psphere, const plane *pplane, prim_inters *pinters);
int plane_sphere_intersection(const plane *pplane, const sphere *psphere, prim_inters *pinters);
int ray_plane_intersection(const ray *pray, const plane *pplane, prim_inters *pinters);
int plane_ray_intersection(const plane *pplane, const ray *pray, prim_inters *pinters);
class CIntersectionChecker {
public:
CIntersectionChecker();
int Check(int type1,int type2, const primitive* prim1,const primitive *prim2, prim_inters* pinters) {
return table[type1][type2](prim1,prim2, pinters);
}
int CheckExists(int type1,int type2) {
return table[type1][type2]!=default_intersection;
}
intersection_check table[NPRIMS][NPRIMS];
};
extern CIntersectionChecker g_Intersector;
#endif

1978
CryPhysics/linunprojectionchecks.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

1731
CryPhysics/livingentity.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

146
CryPhysics/livingentity.h Normal file
View File

@@ -0,0 +1,146 @@
//////////////////////////////////////////////////////////////////////
//
// Living Entity header
//
// File: livingentity.h
// Description : CLivingEntity class header
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef livingentity_h
#define livingentity_h
#pragma once
const int SZ_ACTIONS = 128;
const int SZ_HISTORY = 128;
struct le_history_item {
vectorf pos;
quaternionf q;
vectorf v;
int bFlying;
vectorf nslope;
float timeFlying;
float minFlyTime;
float timeUseLowCap;
int idCollider;
int iColliderPart;
vectorf posColl;
float dt;
};
struct le_contact {
CPhysicalEntity *pent;
int ipart;
vectorf pt,ptloc;
vectorf n;
float penetration;
vectorf center;
};
class CLivingEntity : public CPhysicalEntity {
public:
CLivingEntity(CPhysicalWorld *pWorld);
virtual ~CLivingEntity();
virtual pe_type GetType() { return PE_LIVING; }
virtual int SetParams(pe_params*);
virtual int GetParams(pe_params*);
virtual int GetStatus(pe_status*);
virtual int Action(pe_action*);
virtual void StartStep(float time_interval);
virtual float GetMaxTimeStep(float time_interval);
virtual int Step(float time_interval);
void StepBackEx(float time_interval,bool bRollbackHistory=true);
virtual void StepBack(float time_interval) { StepBackEx(time_interval); }
virtual float CalcEnergy(float time_interval);
virtual int RegisterContacts(float time_interval,int nMaxPlaneContacts);
virtual int Update(float time_interval, float damping);
virtual int Awake(int bAwake=1,int iSource=0);
virtual void AlertNeighbourhoodND() { ReleaseGroundCollider(); CPhysicalEntity::AlertNeighbourhoodND(); }
virtual void ComputeBBox() {
CPhysicalEntity::ComputeBBox(); m_BBox[0].z = min(m_BBox[0].z,m_pos.z-m_hPivot);
m_BBox[1].z = max(m_BBox[1].z,m_pos.z-m_hPivot+(m_hHead+m_HeadGeom.m_sphere.r)*isneg(0.001f-m_HeadGeom.m_sphere.r));
}
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
enum snapver { SNAPSHOT_VERSION = 2 };
virtual int GetStateSnapshot(class CStream &stm, float time_back=0, int flags=0);
virtual int SetStateFromSnapshot(class CStream &stm, int flags=0);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual float GetMassInv() { return m_massinv; }
virtual void GetContactMatrix(const vectorf &pt, int ipart, matrix3x3f &K) {
K(0,0)+=m_massinv; K(1,1)+=m_massinv; K(2,2)+=m_massinv;
}
virtual void GetSpatialContactMatrix(const vectorf &pt, int ipart, float Ibuf[][6]) {
Ibuf[3][0]+=m_massinv; Ibuf[4][1]+=m_massinv; Ibuf[5][2]+=m_massinv;
}
float ShootRayDown(CPhysicalEntity **pentlist,int nents, const vectorf &pos,vectorf &nslope, float time_interval=0,
bool bUseRotation=false,bool bUpdateGroundCollider=false,bool bIgnoreSmallObjects=true);
void AddLegsImpulse(const vectorf &vel, const vectorf &nslope, bool bInstantChange);
void ReleaseGroundCollider();
void SetGroundCollider(CPhysicalEntity *pCollider);
void SyncWithGroundCollider(float time_interval);
void RegisterContact(const vectorf& pt,const vectorf& n, CPhysicalEntity *pCollider, int ipart,int idmat);
void RegisterUnprojContact(const le_contact &unproj);
int IsPositionFree(const vectorf *BBox,float newh,const vectorf &newdim);
void AllocateExtendedHistory();
vectorf m_vel,m_velRequested,m_gravity,m_nslope;
float m_kInertia,m_kAirControl,m_kAirResistance, m_hCyl,m_hEye,m_hPivot,m_hHead;
vectorf m_size;
float m_dh,m_dhSpeed,m_dhAcc,m_stablehTime,m_hLatest,m_nodSpeed;
float m_mass,m_massinv;
int m_bFlying,m_bJumpRequested,m_bSwimming, m_surface_idx, m_lastGroundSurfaceIdx;
float m_timeFlying,m_minFlyTime,m_timeForceInertia;
float m_slopeSlide,m_slopeClimb,m_slopeJump,m_slopeFall;
float m_maxVelGround;
CCylinderGeom m_CylinderGeom;
CSphereGeom m_SphereGeom,m_HeadGeom;
phys_geometry m_CylinderGeomPhys;
int m_bIgnoreCommands;
int m_bStateReading;
int m_bActive;
float m_timeUseLowCap;
float m_timeSinceStanceChange;
float m_timeSinceImpulseContact;
int m_bActiveEnvironment;
int m_bStuck;
float m_dhHist[2],m_timeOnStairs;
float m_timeStepFull,m_timeStepPerformed;
int m_iSnapshot;
int m_iTimeLastSend;
CPhysicalEntity *m_pLastGroundCollider;
int m_iLastGroundColliderPart;
vectorf m_posLastGroundColl;
vectorf m_velGround;
vectorf m_deltaPos,m_posLocal;
float m_timeSmooth;
int m_bUseSphere;
le_history_item *m_history,m_history_buf[4];
int m_szHistory,m_iHist;
pe_action_move *m_actions,m_actions_buf[16];
int m_szActions,m_iAction;
coll_history_item m_collision;
le_contact *m_pContacts;
int m_nContacts,m_nContactsAlloc;
int m_nSensors;
vectorf *m_pSensors,*m_pSensorsPoints,*m_pSensorsSlopes;
int m_iSensorsActive;
};
#endif

764
CryPhysics/matrixnm.cpp Normal file
View File

@@ -0,0 +1,764 @@
//////////////////////////////////////////////////////////////////////
//
// NxM Matrix
//
// File: matrixnm.cpp
// Description : matrixnm template class implementation for float and real
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "utils.h"
#ifdef USE_MATRIX_POOLS
DECLARE_MTXNxM_POOL(float,512)
DECLARE_VECTORN_POOL(float,256)
#ifndef REAL_IS_FLOAT
DECLARE_MTXNxM_POOL(real,512)
DECLARE_VECTORN_POOL(real,256)
#endif
#endif
int g_bHasSSE;
inline float getlothresh(float) { return 1E-10f; }
inline float gethithresh(float) { return 1E10f; }
inline double getlothresh(double) { return 1E-20; }
inline double gethithresh(double) { return 1E20; }
template<class ftype>
int matrix_tpl<ftype>::jacobi_transformation(matrix_tpl<ftype> &evec, ftype *eval, ftype prec) const
{
if (!(flags & mtx_symmetric) || nCols!=nRows) return 0;
matrix_tpl a(*this);
int n = nRows, p,q,r,iter,pmax,qmax,sz=nRows*nCols;
ftype theta,t,s,c,apr,aqr,arp,arq,thresh=prec,amax;
evec.identity();
evec.flags = (evec.flags & mtx_foreign_data) | mtx_orthogonal | mtx_normal;
for(iter=0; iter<nRows*nCols*10; iter++) {
for(p=0,amax=thresh,pmax=-1;p<n-1;p++) for(q=p+1;q<n;q++) if (sqr(a[p][q])>amax)
{ amax=sqr(a[p][q]); pmax=p; qmax=q; }
if (pmax==-1)
goto exitjacobi;
p=pmax; q=qmax;
theta = (ftype)0.5*(a[q][q]-a[p][p])/a[p][q];
if (fabs_tpl(theta)<gethithresh(theta)) {
t = sqrt_tpl(theta*theta+1);
if (theta>0) t = -theta-t;
else t -= theta;
c = 1/sqrt_tpl(1+t*t); s = t*c;
for(r=0;r<n;r++) { arp=a[r][p];arq=a[r][q]; a[r][p]=c*arp-s*arq; a[r][q]=c*arq+s*arp; }
for(r=0;r<n;r++) { apr=a[p][r];aqr=a[q][r]; a[p][r]=c*apr-s*aqr; a[q][r]=c*aqr+s*apr; }
for(r=0;r<n;r++) { apr=evec[p][r];aqr=evec[q][r]; evec[p][r]=c*apr-s*aqr; evec[q][r]=c*aqr+s*apr; }
}
a[p][q] = 0;
if (iter==sz+1) thresh += getlothresh(thresh);
}
iter=0;
exitjacobi:
for(p=0;p<n*n;p++) {
t = fabs_tpl(evec.data[p]);
if (t<(ftype)1E-6) evec.data[p]=0;
else if (fabs_tpl(t-1)<getlothresh(t))
evec.data[p]=sgnnz(evec.data[p]);
}
for(p=0;p<n;p++) eval[p] = a[p][p];
return iter; // not converged during iterations limit
}
template<class ftype>
int matrix_tpl<ftype>::conjugate_gradient(ftype *startx,ftype *rightside, ftype minlen,ftype minel) const
{
ftype a,b,r2,r2new,denom,maxel;
int i,iter=nRows*3;
minlen*=minlen;
ftype buf[24],*pbuf = nRows>8 ? new ftype[nRows*3]:buf;
vectorn_tpl<ftype> x(nRows,startx),rh(nRows,rightside),r(nRows,pbuf),p(nRows,pbuf+nRows),Ap(nRows,pbuf+nRows*2);
(r=rh)-=*this*x; p=r;
r2=r.len2();
do {
Ap = *this*p; denom = p*Ap;
if (sqr(denom)<1E-30) break;
a = r2/denom;
#if defined(LINUX)
r = ::operator-=(r, (const vectorn_tpl<ftype>&)(Ap*a));
#else
r -= Ap*a;
#endif
r2new=r.len2();
if (r2new>r2*500)
break;
#if defined(LINUX)
x = ::operator+=(x, (const vectorn_tpl<ftype>&)(p*a));
#else
x += p*a;
#endif
b = r2new/r2; r2=r2new;
(p*=b)+=r;
for(i=0,maxel=0;i<nRows;i++) maxel = max(maxel,fabs_tpl(r[i]));
} while (--iter && (r2new>minlen || maxel>minel));
if (pbuf!=buf) delete pbuf;
return nRows-iter;
}
template<class ftype>
int matrix_tpl<ftype>::biconjugate_gradient(ftype *startx,ftype *rightside, ftype minlen,ftype minel) const
{
ftype a,b,r2,r2new,err,denom,maxel;
int i,iter=nRows*3;
minlen*=minlen;
ftype buf[40],*pbuf = nRows>8 ? new ftype[nRows*5]:buf;
vectorn_tpl<ftype> x(nRows,startx),rh(nRows,rightside),r(nRows,pbuf),rc(nRows,pbuf+nRows),p(nRows,pbuf+nRows*2),
pc(nRows,pbuf+nRows*3),Ap(nRows,pbuf+nRows*4);
(r=rh)-=*this*x; rc=r; p=r; pc=rc; r2 = r.len2();
do {
Ap = *this*p; denom = pc*Ap;
if (sqr(denom)<1E-30) break;
a = r2/denom;
#if defined(LINUX)
r = ::operator-=(r, (const vectorn_tpl<ftype>&)(Ap*a));
rc = ::operator-=(rc, (const vectorn_tpl<ftype>&)((pc**this)*a));
x = ::operator+=(x, (const vectorn_tpl<ftype>&)(p*a));
#else
r -= Ap*a;
rc -= (pc**this)*a;
x += p*a;
#endif
r2new = rc*r;
b = r2new/r2; r2 = r2new;
(p*=b)+=r; (pc*=b)+=rc;
for(err=maxel=0,i=0;i<nRows;i++) {
err += sqr(r.data[i]);
maxel = max(maxel,fabs_tpl(r[i]));
}
} while(--iter && (err>minlen || maxel>minel) && sqr(r2)>1E-30);
if (pbuf!=buf) delete pbuf;
return nRows-iter;
}
template<class ftype>
int matrix_tpl<ftype>::minimum_residual(ftype *startx,ftype *rightside, ftype minlen,ftype minel) const
{
ftype a,b,r2,r2new,err,denom,maxel;
int i,iter=nRows*3;
minlen*=minlen;
ftype buf[40],*pbuf = nRows>8 ? new ftype[nRows*5]:buf;
vectorn_tpl<ftype> x(nRows,startx),rh(nRows,rightside),r(nRows,pbuf),rc(nRows,pbuf+nRows),p(nRows,pbuf+nRows*2),
Ap(nRows,pbuf+nRows*4);
(r=rh)-=*this*x;
rc=*this*r;
p=r;
r2 = rc*r;
do {
Ap = *this*p; denom = Ap.len2();
if (sqr(denom)<1E-30) break;
a = r2/denom;
#if defined(LINUX)
r = ::operator-=(r, (const vectorn_tpl<ftype>&)(Ap*a));
#else
r -= Ap*a;
#endif
rc = *this*r;
#if defined(LINUX)
x = ::operator+=(x, (const vectorn_tpl<ftype>&)(p*a));
#else
x += p*a;
#endif
r2new = rc*r;
b = r2new/r2; r2 = r2new;
(p*=b)+=r;
for(err=maxel=0,i=0;i<nRows;i++) {
err += sqr(r.data[i]);
maxel = max(maxel,fabs_tpl(r[i]));
}
} while(--iter && (err>minlen || maxel>minel) && sqr(r2)>1E-30);
if (pbuf!=buf) delete pbuf;
return nRows-iter;
}
template<class ftype>
int matrix_tpl<ftype>::LUdecomposition(ftype *&LUdata,int *&LUidx) const
{
if (nRows!=nCols) return 0;
int i,j,k,imax,alloc=(LUdata==0 | (LUidx==0)<<1);
ftype aij,bij,maxaij,t;
if (alloc & 1) LUdata = new ftype[nRows*nRows];
if (alloc & 2) LUidx = new int[nRows];
matrix_tpl<ftype> LU(nRows,nRows,0,LUdata);
LU = *this;
for(j=0;j<nRows;j++) {
for(i=0;i<=j;i++) {
for(k=0,bij=LU[i][j]; k<i; k++) bij -= LU[i][k]*LU[k][j];
LU[i][j] = bij;
}
for(maxaij=0,imax=j ;i<nRows;i++) {
for(k=0,aij=LU[i][j]; k<j; k++) aij -= LU[i][k]*LU[k][j];
LU[i][j] = aij;
if (aij*aij > maxaij*maxaij)
{ maxaij=aij; imax=i; }
}
LUidx[j] = imax;
if (j==nRows-1 && LU[j][j]!=0) break; // no aij in this case
if (maxaij==0) { // the matrix is singular
if (alloc & 1) delete[] LUdata;
if (alloc & 2) delete[] LUidx;
return 0;
}
if (imax!=j) for(k=0;k<nRows;k++)
{ t=LU[imax][k]; LU[imax][k]=LU[j][k]; LU[j][k]=t; }
maxaij = (ftype)1.0/maxaij;
for(i=j+1;i<nRows;i++)
LU[i][j] *= maxaij;
}
return 1;
}
template<class ftype>
int matrix_tpl<ftype>::solveAx_b(ftype *x,ftype *b, ftype *LUdata,int *LUidx) const
{
int LUidx_buf[16],alloc=0;
if (!LUdata) {
if (nRows*nRows*2<sizeof(mtx_pool)/sizeof(mtx_pool[0])) {
if (mtx_pool_pos+nRows*nRows > sizeof(mtx_pool)/sizeof(mtx_pool[0]))
mtx_pool_pos = 0;
LUdata = mtx_pool+mtx_pool_pos; mtx_pool_pos += nRows*nRows;
}
if (nRows<=sizeof(LUidx_buf)/sizeof(LUidx_buf[0])) LUidx = LUidx_buf;
alloc = LUdata==0 | (LUidx==0)<<1;
if (!LUdecomposition(LUdata,LUidx)) return 0;
}
int i,j; ftype xi;
matrix_tpl<ftype> LU(nRows,nRows,0,LUdata);
for(i=0;i<nRows;i++) x[i]=b[i];
for(i=0;i<nRows;i++) {
xi=x[i]; x[i]=x[LUidx[i]]; x[LUidx[i]]=xi;
for(j=0;j<i;j++) x[i]-=LU[i][j]*x[j];
}
for(i=nRows-1;i>=0;i--) {
for(j=i+1;j<nRows;j++) x[i]-=LU[i][j]*x[j];
x[i] /= LU[i][i];
}
if (alloc & 1) delete[] LUdata;
if (alloc & 2) delete[] LUidx;
return 1;
}
template<class ftype>
matrix_tpl<ftype>& matrix_tpl<ftype>::invert()
{
if (flags & mtx_orthogonal)
return transpose();
if (nRows!=nCols)
return *this;
int i,j; ftype det=0;
if (nRows==1)
data[0]=(ftype)1.0/data[0];
else if (nRows==2) {
det = data[0]*data[3]-data[1]*data[2];
if (det==0) return *this; det = (ftype)1.0/det;
ftype oldata[4]; for(i=0;i<4;i++) oldata[i]=data[i];
data[0]=oldata[3]*det; data[1]=-oldata[1]*det;
data[2]=-oldata[2]*det; data[3]=oldata[0]*det;
} else if (nRows==3) {
for(i=0;i<3;i++) det+=data[i]*data[inc_mod3[i]+3]*data[dec_mod3[i]+6];
for(i=0;i<3;i++) det-=data[dec_mod3[i]]*data[inc_mod3[i]+3]*data[i+6];
if (det==0) return *this; det = (ftype)1.0/det;
ftype oldata[9]; for(i=0;i<9;i++) oldata[i]=data[i];
for(i=0;i<3;i++) for(j=0;j<3;j++)
data[i+j*3] = (oldata[dec_mod3[i]*3+dec_mod3[j]]*oldata[inc_mod3[i]*3+inc_mod3[j]]-
oldata[dec_mod3[i]*3+inc_mod3[j]]*oldata[inc_mod3[i]*3+dec_mod3[j]])*det;
} else {
ftype *LUdata=0,*colmark; int *LUidx=0,LUidx_buf[32], alloc=0;
if (nRows*nRows*2<sizeof(mtx_pool)/sizeof(mtx_pool[0])) {
if (mtx_pool_pos+nRows*nRows > sizeof(mtx_pool)/sizeof(mtx_pool[0]))
mtx_pool_pos = 0;
LUdata = mtx_pool+mtx_pool_pos; mtx_pool_pos += nRows*nRows;
} else alloc = 1;
if (nRows<=sizeof(LUidx_buf)/sizeof(LUidx_buf[0])) LUidx = LUidx_buf;
else alloc |= 2;
if (!LUdecomposition(LUdata,LUidx)) return *this;
if (nRows*2<sizeof(mtx_pool)/sizeof(mtx_pool[0])) {
if (mtx_pool_pos+nRows > sizeof(mtx_pool)/sizeof(mtx_pool[0]))
mtx_pool_pos=0;
colmark=mtx_pool+mtx_pool_pos; mtx_pool_pos+=nRows;
} else {
colmark = new ftype[nRows]; alloc |= 4;
}
for(i=0;i<nRows;i++) colmark[i]=0;
for(i=0;i<nRows;i++) {
colmark[i]=1; solveAx_b(data+i*nRows,colmark, LUdata,LUidx); colmark[i]=0;
}
transpose();
if (alloc & 1) delete[] LUdata;
if (alloc & 2) delete[] LUidx;
if (alloc & 4) delete[] colmark;
}
flags = flags & (mtx_normal | mtx_orthogonal | mtx_symmetric | mtx_PD | mtx_PSD | mtx_diagonal | mtx_identity | mtx_foreign_data);
return *this;
}
template<class ftype>
ftype matrix_tpl<ftype>::determinant(ftype *LUdata,int *LUidx) const
{
if (nRows!=nCols) return 0;
int i,j; ftype det=0;
if (nRows==1)
det = data[0];
else if (nRows==2)
det = data[0]*data[3]-data[1]*data[2];
else if (nRows==3) {
for(i=0;i<3;i++) det+=data[i]*data[inc_mod3[i]+3]*data[dec_mod3[i]+6];
for(i=0;i<3;i++) det-=data[dec_mod3[i]]*data[inc_mod3[i]+3]*data[i+6];
} else if (LUdecomposition(LUdata,LUidx)) {
ftype *LUdata=0; int *LUidx=0,LUidx_buf[32], alloc=0;
if (nRows*nRows*2<sizeof(mtx_pool)/sizeof(mtx_pool[0])) {
if (mtx_pool_pos+nRows*nRows > sizeof(mtx_pool)/sizeof(mtx_pool[0]))
mtx_pool_pos = 0;
LUdata = mtx_pool+mtx_pool_pos; mtx_pool_pos += nRows*nRows;
} else alloc = 1;
if (nRows<=sizeof(LUidx_buf)/sizeof(LUidx_buf[0])) LUidx = LUidx_buf;
else alloc |= 2;
if (!LUdecomposition(LUdata,LUidx)) return 0;
for(i=j=0,det=1;i<nRows;i++,j+=nRows+1) det*=LUdata[j];
for(i=j=0;i<nRows;i++) j+=LUidx[i]!=i;
if (j&2) det*=-1;
if (alloc & 1) delete[] LUdata;
if (alloc & 2) delete[] LUidx;
}
return det;
}
// Linear programming with simplex method
template<class ftype>
int matrix_tpl<ftype>::LPsimplex(int m1,int m2, ftype &objfunout,ftype *xout, int nvars, ftype e) const
{
int M=nRows-2,N=nCols-1,i,j,imax,jmax,iobjfun=M, res=0,iter=(M+N)*8;
ftype rpivot,t;
const int imask=0x7FFFFFFF;
int *irow=new int[M],*icol=new int[N];
#define a (*this)
if (e<0) {
for(i=nRows*nCols-1,e=0;i>=0;i--) e+=data[i];
e *= (ftype)1E-6/(nRows*nCols);
}
if (nvars<0) nvars = N+m1+m2;
if (m1<M) {
iobjfun++;
for(j=0;j<N;j++) {
for(t=0,i=0;i<M;i++) t-=a[i][j];
a[iobjfun][j] = t;
}
}
for(i=0;i<N;i++) icol[i]=i;
for(i=0;i<M;i++) irow[i]=N+i|~imask;
do {
for(t=0,j=0,jmax=-1;j<N;j++) if(icol[j]<nvars && a[iobjfun][j]>t)
{ jmax=j; t=a[iobjfun][j]; }
if (jmax<0)
{ res=1; break; }
for(imax=0; imax<M && a[imax][jmax]>0; imax++);
if (imax==M)
{ res=2; break; }
for(i=imax+1;i<M;i++) if (a[i][jmax]<0 && a[i][N]*a[imax][jmax] < a[imax][N]*a[i][jmax])
imax=i;
varexchange:
rpivot = (ftype)1.0/a[imax][jmax];
for(j=0;j<nCols;j++) a[imax][j]*=-rpivot;
a[imax][jmax] = rpivot;
for(i=0;i<nRows;i++) if(i!=imax) {
a[i][jmax] *= rpivot;
for(j=0;j<nCols;j++) if(j!=jmax)
a[i][j] -= a[imax][j]*a[i][jmax];
}
if ((irow[imax]&imask)>=N+m1 && (irow[imax]&imask)<N+m1+m2 && (irow[imax]&~imask)!=0) {
a[iobjfun][jmax] += 1; // remove slack variable from objective function, nonnegativity constraint will enforce feasibility
for(i=0;i<=M;i++) a[i][jmax]*=-1;
}
i=irow[imax]&imask; irow[imax]=icol[jmax]; icol[jmax]=i;
} while(--iter);
if (m1<M) {
if (res==2 || sqr(a[M][N])<e*e)
res = 0;
else {
for(i=0;i<M;i++) {
if ((irow[i]&imask)>M+m1+m2) {
for(jmax=0;jmax<N && icol[jmax]>=nvars;jmax++);
imax=i; goto varexchange;
} else if ((irow[i]&imask)>N+m1 && (irow[i]&~imask)!=0) {
for(j=0;j<N;j++) a[i][j]*=-1;
irow[i]&=imask;
}
}
res = LPsimplex(M,0, objfunout,xout, nvars, e);
}
} else {
if (xout) {
for(i=0;i<N;i++) xout[i]=0;
for(i=0;i<M;i++) if((irow[i]&imask)<N)
xout[irow[i]&imask] = a[i][N];
}
objfunout = a[iobjfun][N];
}
delete[] irow; delete[] icol;
return res;
#undef a
}
#ifdef PIII_SSE
// TODO: AMD64: put into a separate file
// SSE-optimized code from "Streaming SIMD Extensions - Matrix Multiplication" document by Intel Corp.
__declspec(naked) void PIII_Mult00_6x6_6x6(float *src1, float *src2, float *dst)
{
__asm {
mov ecx, dword ptr [esp+8] // src2
movlps xmm3, qword ptr [ecx+72]
mov edx, dword ptr [esp+4] // src1
// Loading first 4 columns (upper 4 rows) of src2.
movaps xmm0, xmmword ptr [ecx]
movlps xmm1, qword ptr [ecx+24]
movhps xmm1, qword ptr [ecx+32]
movaps xmm2, xmmword ptr [ecx+48]
movhps xmm3, qword ptr [ecx+80]
// Calculating first 4 elements in the first row of the destination matrix.
movss xmm4, dword ptr [edx]
movss xmm5, dword ptr [edx+4]
mov eax, dword ptr [esp+0Ch] // dst
shufps xmm4, xmm4, 0
movss xmm6, dword ptr [edx+8]
shufps xmm5, xmm5, 0
movss xmm7, dword ptr [edx+12]
mulps xmm4, xmm0
shufps xmm6, xmm6, 0
shufps xmm7, xmm7, 0
mulps xmm5, xmm1
mulps xmm6, xmm2
addps xmm5, xmm4
mulps xmm7, xmm3
addps xmm6, xmm5
addps xmm7, xmm6
movaps xmmword ptr [eax], xmm7
// Calculating first 4 elements in the second row of the destination matrix.
movss xmm4, dword ptr [edx+24]
shufps xmm4, xmm4, 0
mulps xmm4, xmm0
movss xmm5, dword ptr [edx+28]
shufps xmm5, xmm5, 0
mulps xmm5, xmm1
movss xmm6, dword ptr [edx+32]
shufps xmm6, xmm6, 0
movss xmm7, dword ptr [edx+36]
shufps xmm7, xmm7, 0
mulps xmm6, xmm2
mulps xmm7, xmm3
addps xmm7, xmm6
addps xmm5, xmm4
addps xmm7, xmm5
// Calculating first 4 elements in the third row of the destination matrix.
movss xmm4, dword ptr [edx+48]
movss xmm5, dword ptr [edx+52]
movlps qword ptr [eax+24], xmm7 // save 2nd
movhps qword ptr [eax+32], xmm7 // row
movss xmm6, dword ptr [edx+56]
movss xmm7, dword ptr [edx+60]
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
shufps xmm6, xmm6, 0
shufps xmm7, xmm7, 0
mulps xmm4, xmm0
mulps xmm5, xmm1
mulps xmm6, xmm2
mulps xmm7, xmm3
addps xmm5, xmm4
addps xmm7, xmm6
addps xmm7, xmm5
movaps xmmword ptr [eax+48], xmm7
// Calculating first 4 elements in the fourth row of the destination matrix.
movss xmm4, dword ptr [edx+72]
movss xmm5, dword ptr [edx+76]
movss xmm6, dword ptr [edx+80]
movss xmm7, dword ptr [edx+84]
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
shufps xmm6, xmm6, 0
shufps xmm7, xmm7, 0
mulps xmm4, xmm0
mulps xmm5, xmm1
mulps xmm6, xmm2
mulps xmm7, xmm3
addps xmm4, xmm5
addps xmm6, xmm4
addps xmm7, xmm6
movlps qword ptr [eax+72], xmm7
movhps qword ptr [eax+80], xmm7
// Calculating first 4 elements in the fifth row of the destination matrix.
movss xmm4, dword ptr [edx+96]
movss xmm5, dword ptr [edx+100]
movss xmm6, dword ptr [edx+104]
movss xmm7, dword ptr [edx+108]
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
shufps xmm6, xmm6, 0
shufps xmm7, xmm7, 0
mulps xmm4, xmm0
mulps xmm5, xmm1
mulps xmm6, xmm2
mulps xmm7, xmm3
addps xmm5, xmm4
addps xmm7, xmm6
addps xmm7, xmm5
movaps xmmword ptr [eax+96], xmm7
// Calculating first 4 elements in the sixth row of the destination matrix.
movss xmm4, dword ptr [edx+120]
movss xmm5, dword ptr [edx+124]
movss xmm6, dword ptr [edx+128]
movss xmm7, dword ptr [edx+132]
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
shufps xmm6, xmm6, 0
shufps xmm7, xmm7, 0
mulps xmm4, xmm0
mulps xmm5, xmm1
mulps xmm6, xmm2
mulps xmm7, xmm3
addps xmm4, xmm5
addps xmm6, xmm4
addps xmm7, xmm6
movhps qword ptr [eax+128], xmm7
movlps qword ptr [eax+120], xmm7
// Loading first 4 columns (lower 2 rows) of src2.
movlps xmm0, qword ptr [ecx+96]
movhps xmm0, qword ptr [ecx+104]
movlps xmm1, qword ptr [ecx+120]
movhps xmm1, qword ptr [ecx+128]
// Calculating first 4 elements in the first row of the destination matrix.
movss xmm2, dword ptr [edx+16]
shufps xmm2, xmm2, 0
movss xmm4, dword ptr [edx+40]
movss xmm3, dword ptr [edx+20]
movss xmm5, dword ptr [edx+44]
movaps xmm6, xmmword ptr [eax]
movlps xmm7, qword ptr [eax+24]
shufps xmm3, xmm3, 0
shufps xmm5, xmm5, 0
movhps xmm7, qword ptr [eax+32]
shufps xmm4, xmm4, 0
mulps xmm5, xmm1
mulps xmm2, xmm0
mulps xmm3, xmm1
mulps xmm4, xmm0
addps xmm6, xmm2
addps xmm7, xmm4
addps xmm7, xmm5
addps xmm6, xmm3
movlps qword ptr [eax+24], xmm7
movaps xmmword ptr [eax], xmm6
movhps qword ptr [eax+32], xmm7
// Calculating first 4 elements in the third row of the destination matrix.
movss xmm2, dword ptr [edx+64]
movss xmm4, dword ptr [edx+88]
movss xmm5, dword ptr [edx+92]
movss xmm3, dword ptr [edx+68]
movaps xmm6, xmmword ptr [eax+48]
movlps xmm7, qword ptr [eax+72]
movhps xmm7, qword ptr [eax+80]
shufps xmm2, xmm2, 0
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
shufps xmm3, xmm3, 0
mulps xmm2, xmm0
mulps xmm4, xmm0
mulps xmm5, xmm1
mulps xmm3, xmm1
addps xmm6, xmm2
addps xmm6, xmm3
addps xmm7, xmm4
addps xmm7, xmm5
movlps qword ptr [eax+72], xmm7
movaps xmmword ptr [eax+48], xmm6
movhps qword ptr [eax+80], xmm7
// Calculating first 4 elements in the fifth row of the destination matrix.
movss xmm2, dword ptr [edx+112]
movss xmm3, dword ptr [edx+116]
movaps xmm6, xmmword ptr [eax+96]
shufps xmm2, xmm2, 0
shufps xmm3, xmm3, 0
mulps xmm2, xmm0
mulps xmm3, xmm1
addps xmm6, xmm2
addps xmm6, xmm3
movaps xmmword ptr [eax+96], xmm6
// Calculating first 4 elements in the sixth row of the destination matrix.
movss xmm4, dword ptr [edx+136]
movss xmm5, dword ptr [edx+140]
movhps xmm7, qword ptr [eax+128]
movlps xmm7, qword ptr [eax+120]
shufps xmm4, xmm4, 0
shufps xmm5, xmm5, 0
mulps xmm4, xmm0
mulps xmm5, xmm1
addps xmm7, xmm4
addps xmm7, xmm5
// Calculating last 2 columns of the destination matrix.
movlps xmm0, qword ptr [ecx+16]
movhps xmm0, qword ptr [ecx+40]
movhps qword ptr [eax+128], xmm7
movlps qword ptr [eax+120], xmm7
movlps xmm2, qword ptr [ecx+64]
movhps xmm2, qword ptr [ecx+88]
movaps xmm3, xmm2
shufps xmm3, xmm3, 4Eh
movlps xmm4, qword ptr [ecx+112]
movhps xmm4, qword ptr [ecx+136]
movaps xmm5, xmm4
shufps xmm5, xmm5, 4Eh
movlps xmm6, qword ptr [edx]
movhps xmm6, qword ptr [edx+24]
movaps xmm7, xmm6
shufps xmm7, xmm7, 0F0h
mulps xmm7, xmm0
shufps xmm6, xmm6, 0A5h
movaps xmm1, xmm0
shufps xmm1, xmm1, 4Eh
mulps xmm1, xmm6
addps xmm7, xmm1
movlps xmm6, qword ptr [edx+8]
movhps xmm6, qword ptr [edx+32]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm2
mulps xmm6, xmm3
addps xmm7, xmm1
addps xmm7, xmm6
movhps xmm6, qword ptr [edx+40]
movlps xmm6, qword ptr [edx+16]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm4
mulps xmm6, xmm5
addps xmm7, xmm1
addps xmm7, xmm6
movlps qword ptr [eax+16], xmm7
movhps qword ptr [eax+40], xmm7
movlps xmm6, qword ptr [edx+48]
movhps xmm6, qword ptr [edx+72]
movaps xmm7, xmm6
shufps xmm7, xmm7, 0F0h
mulps xmm7, xmm0
shufps xmm6, xmm6, 0A5h
movaps xmm1, xmm0
shufps xmm1, xmm1, 4Eh
mulps xmm1, xmm6
addps xmm7, xmm1
movhps xmm6, qword ptr [edx+80]
movlps xmm6, qword ptr [edx+56]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm2
mulps xmm6, xmm3
addps xmm7, xmm1
addps xmm7, xmm6
movlps xmm6, qword ptr [edx+64]
movhps xmm6, qword ptr [edx+88]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm4
mulps xmm6, xmm5
addps xmm7, xmm1
addps xmm7, xmm6
movlps qword ptr [eax+64], xmm7
movhps qword ptr [eax+88], xmm7
movlps xmm6, qword ptr [edx+96]
movhps xmm6, qword ptr [edx+120]
movaps xmm7, xmm6
shufps xmm7, xmm7, 0F0h
mulps xmm7, xmm0
shufps xmm6, xmm6, 0A5h
movaps xmm1, xmm0
shufps xmm1, xmm1, 4Eh
mulps xmm1, xmm6
addps xmm7, xmm1
movlps xmm6, qword ptr [edx+104]
movhps xmm6, qword ptr [edx+128]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm2
mulps xmm6, xmm3
addps xmm7, xmm1
addps xmm7, xmm6
movlps xmm6, qword ptr [edx+112]
movhps xmm6, qword ptr [edx+136]
movaps xmm1, xmm6
shufps xmm1, xmm1, 0F0h
shufps xmm6, xmm6, 0A5h
mulps xmm1, xmm4
mulps xmm6, xmm5
addps xmm7, xmm1
addps xmm7, xmm6
movlps qword ptr [eax+112], xmm7
movhps qword ptr [eax+136], xmm7
ret }
}
#endif
void ForceCompilerToGenerateCodeForMatrixTemplates()
{
matrixf mtxf; float f,*pf; int *pi;
mtxf.jacobi_transformation(mtxf,0);
mtxf.conjugate_gradient(0,0);
mtxf.biconjugate_gradient(0,0);
mtxf.minimum_residual(0,0);
mtxf.LPsimplex(0,0,f);
mtxf.LUdecomposition(pf,pi);
mtxf.solveAx_b(pf,pf);
mtxf.determinant();
mtxf.invert();
matrix mtxr; real r,*pr;
mtxr.jacobi_transformation(mtxr,0);
mtxr.conjugate_gradient(0,0);
mtxr.biconjugate_gradient(0,0);
mtxr.minimum_residual(0,0);
mtxr.LPsimplex(0,0,r);
mtxr.LUdecomposition(pr,pi);
mtxr.solveAx_b(pr,pr);
mtxr.determinant();
mtxr.invert();
}

276
CryPhysics/matrixnm.h Normal file
View File

@@ -0,0 +1,276 @@
//////////////////////////////////////////////////////////////////////
//
// NxM Matrix header
//
// File: matrixnm.h
// Description : matrixnm template class declaration and inlined implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef matrixnm_h
#define matrixnm_h
#pragma once
enum mtxflags {
mtx_invalid=1, mtx_normal=2, mtx_orthogonal=4, mtx_PSD=8, mtx_PD_flag=16, mtx_PD=mtx_PSD|mtx_PD_flag, mtx_symmetric=32,
mtx_diagonal_flag=64, mtx_diagonal=mtx_symmetric|mtx_normal|mtx_diagonal_flag, mtx_identity_flag=128,
mtx_identity=mtx_PD|mtx_diagonal|mtx_orthogonal|mtx_normal|mtx_symmetric|mtx_identity_flag, mtx_singular=256,
mtx_foreign_data=1024, // this flag means that data was not allocated by matrix
mtx_allocate=32768 // prohibts using matrix pool for data
};
template<class ftype> class matrix_product_tpl {
public:
matrix_product_tpl(int nrows1,int ncols1,ftype *pdata1,int flags1, int ncols2,ftype *pdata2,int flags2) {
data1=pdata1; data2=pdata2; nRows1=nrows1;nCols1=ncols1; nCols2=ncols2;
flags = flags1 & flags2 & (mtx_orthogonal | mtx_PD) & ~mtx_foreign_data;
}
void assign_to(ftype *pdst) const {
int i,j,k; ftype sum;
for(i=0;i<nRows1;i++) for(j=0;j<nCols2;j++) {
for(sum=0,k=0; k<nCols1; k++)
sum += data1[i*nCols1+k]*data2[k*nCols2+j];
pdst[i*nCols2+j] = sum;
}
}
void add_assign_to(ftype *pdst) const {
for(int i=0;i<nRows1;i++) for(int j=0;j<nCols2;j++)
for(int k=0; k<nCols1; k++)
pdst[i*nCols2+j] += data1[i*nCols1+k]*data2[k*nCols2+j];
}
void sub_assign_to(ftype *pdst) const {
for(int i=0;i<nRows1;i++) for(int j=0;j<nCols2;j++)
for(int k=0; k<nCols1; k++)
pdst[i*nCols2+j] -= data1[i*nCols1+k]*data2[k*nCols2+j];
}
int nRows1,nCols1,nCols2;
ftype *data1,*data2;
int flags;
};
template <class ftype> class matrix_tpl {
public:
matrix_tpl() {
nRows=nCols=3; flags=mtx_foreign_data; data=0;
}
matrix_tpl( int nrows, int ncols, int _flags=0, ftype *pdata=(ftype*)-1 ) {
nRows = nrows; nCols = ncols;
flags = _flags & ~mtx_allocate;
int sz = nRows*nCols;
if (pdata!=(ftype*)-1) {
data = pdata; flags |= mtx_foreign_data;
#ifdef USE_MATRIX_POOLS
} else if (sz<=36 && !(_flags & mtx_allocate)) {
if (mtx_pool_pos+sz > mtx_pool_size)
mtx_pool_pos = 0;
data = mtx_pool+mtx_pool_pos;
mtx_pool_pos += sz;
flags |= mtx_foreign_data;
#endif
} else
data = new ftype[sz];
}
matrix_tpl(const matrix_tpl& src) {
if (src.flags & mtx_foreign_data) {
nRows=src.nRows; nCols=src.nCols;
flags=src.flags; data=src.data;
} else {
matrix_tpl(src.nRows,src.nCols,src.flags,0);
for(int i=nRows*nCols-1;i>=0;i--) data[i]=src.data[i];
}
}
~matrix_tpl() {
if (data && !(flags & mtx_foreign_data)) delete [] data;
}
matrix_tpl& operator=(const matrix_tpl<ftype> &src) {
if (!data || !(flags & mtx_foreign_data) && nRows*nCols<src.nRows*src.nCols) {
delete[] data; data = new ftype[src.nRows*src.nCols];
}
nRows=src.nRows; nCols=src.nCols;
flags = flags&mtx_foreign_data | src.flags&~mtx_foreign_data;
for(int i=nRows*nCols-1;i>=0;i--) data[i] = src.data[i];
return *this;
}
template<class ftype1> matrix_tpl& operator=(const matrix_tpl<ftype1> &src) {
if (!data || !(flags & mtx_foreign_data) && nRows*nCols<src.nRows*src.nCols) {
delete[] data; data = new ftype[src.nRows*src.nCols];
}
nRows=src.nRows; nCols=src.nCols;
flags = flags&mtx_foreign_data | src.flags&~mtx_foreign_data;
for(int i=nRows*nCols-1;i>=0;i--) data[i] = src.data[i];
return *this;
}
matrix_tpl& operator=(const matrix_product_tpl<ftype> &src) {
nRows=src.nRows1; nCols=src.nCols2;
flags = flags&mtx_foreign_data | src.flags;
src.assign_to(data);
return *this;
}
matrix_tpl& operator+=(const matrix_product_tpl<ftype> &src) {
src.add_assign_to(data); return *this;
}
matrix_tpl& operator-=(const matrix_product_tpl<ftype> &src) {
src.sub_assign_to(data); return *this;
}
matrix_tpl& allocate() {
int i,sz=nRows*nCols; ftype *prevdata = data;
if (!data) data = new ftype[sz];
if (flags & mtx_foreign_data) for(i=0;i<sz;i++) data[i] = prevdata[i];
return *this;
}
matrix_tpl& zero() { for(int i=nRows*nCols-1;i>=0;i--) data[i]=0; return *this; }
matrix_tpl& identity() {
zero(); for(int i=min(nRows,nCols)-1;i>=0;i--) data[i*(nCols+1)] = 1;
return *this;
}
matrix_tpl& invert(); // in-place inversion
matrix_tpl operator!() const { // returns inverted matrix
if (flags & mtx_orthogonal)
return T();
matrix_tpl<ftype> res = *this;
res.invert();
return res;
}
matrix_tpl& transpose() { // in-place transposition
if (nRows==nCols) {
if (flags & mtx_symmetric) return *this;
int i,j; ftype t; for(i=0;i<nRows;i++) for(j=0;j<i;j++) {
t=(*this)[i][j]; (*this)[i][j]=(*this)[j][i]; (*this)[j][i]=t;
}
} else
*this = T();
return *this;
}
matrix_tpl T() const { // returns transposed matrix
if (flags & mtx_symmetric)
return matrix_tpl<ftype>(*this);
int i,j; matrix_tpl<ftype> res(nCols,nRows, flags & ~mtx_foreign_data);
for(i=0;i<nRows;i++) for(j=0;j<nCols;j++) res[j][i] = (*this)[i][j];
return res;
}
int LUdecomposition(ftype *&LUdata,int *&LUidx) const;
int solveAx_b(ftype *x,ftype *b, ftype *LUdata=0,int *LUidx=0) const; // finds x that satisfies Ax=b
ftype determinant(ftype *LUdata=0,int *LUidx=0) const;
int jacobi_transformation(matrix_tpl &evec, ftype* eval, ftype prec=0) const;
int conjugate_gradient(ftype *startx,ftype *rightside, ftype minlen=0,ftype minel=0) const;
int biconjugate_gradient(ftype *startx,ftype *rightside, ftype minlen=0,ftype minel=0) const;
int minimum_residual(ftype *startx,ftype *rightside, ftype minlen=0,ftype minel=0) const;
int LPsimplex(int m1,int m2, ftype &objfunout,ftype *xout=0, int nvars=-1, ftype e=-1) const;
ftype *operator[](int iRow) const { return data + iRow*nCols; }
int nRows,nCols;
int flags;
ftype *data;
#ifdef USE_MATRIX_POOLS
static ftype mtx_pool[];
static int mtx_pool_pos;
static int mtx_pool_size;
#endif
};
/*template<class ftype1,class ftype2>
matrix_tpl<ftype1> operator*(const matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
matrix_tpl<ftype1> res(op1.nRows, op2.nCols);
res.flags = res.flags & mtx_foreign_data | op1.flags & op2.flags & (mtx_orthogonal | mtx_PD) & ~mtx_foreign_data;
int i,j,k; ftype1 sum;
for(i=0;i<op1.nRows;i++) for(j=0;j<op2.nCols;j++) {
for(sum=0,k=0; k<op1.nCols; k++) sum += op1[i][k]*op2[k][j];
res[i][j] = sum;
}
return res;
}*/
template<class ftype>
matrix_product_tpl<ftype> operator*(const matrix_tpl<ftype> &op1, const matrix_tpl<ftype> &op2) {
return matrix_product_tpl<ftype>(op1.nRows,op1.nCols,op1.data,op1.flags, op2.nCols,op2.data,op2.flags);
}
/*template<class ftype1,class ftype2>
matrix_tpl<ftype1>& operator*=(matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
return op1 = (op1 * op2);
}*/
template<class ftype>
matrix_tpl<ftype>& operator*=(matrix_tpl<ftype> &op1,ftype op2) {
for(int i=op1.nRows*op1.nCols-1; i>=0; i--) op1.data[i]*=op2;
op1.flags &= ~(mtx_identity_flag | mtx_PD);
return op1;
}
template<class ftype1,class ftype2>
matrix_tpl<ftype1>& operator+=(matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
for(int i=op1.nRows*op1.nCols-1; i>=0; i--) op1.data[i] += op2.data[i];
op1.flags = op1.flags & mtx_foreign_data | op1.flags & op2.flags & (mtx_symmetric | mtx_PD);
return op1;
}
template<class ftype1,class ftype2>
matrix_tpl<ftype1>& operator-=(matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
for(int i=op1.nRows*op1.nCols-1; i>=0; i--) op1.data[i] -= op2.data[i];
op1.flags = op1.flags & mtx_foreign_data | op1.flags & op2.flags & mtx_symmetric;
return op1;
}
template<class ftype1,class ftype2>
matrix_tpl<ftype1> operator+(const matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
matrix_tpl<ftype1> res; res=op1; res+=op2;
return res;
}
template<class ftype1,class ftype2>
matrix_tpl<ftype1> operator-(const matrix_tpl<ftype1> &op1, const matrix_tpl<ftype2> &op2) {
matrix_tpl<ftype1> res; res=op1; res-=op2;
return res;
}
template<class ftype1,class ftype2,class ftype3>
ftype3 *mul_vector_by_matrix(const matrix_tpl<ftype1> &mtx, const ftype2 *psrc,ftype3 *pdst) {
int i,j;
for(i=0;i<mtx.nRows;i++) for(pdst[i]=0,j=0;j<mtx.nCols;j++)
pdst[i] += mtx.data[i*mtx.nCols+j]*psrc[j];
return pdst;
}
typedef matrix_tpl<real> matrix;
typedef matrix_tpl<float> matrixf;
#if defined(LINUX)
#define DECLARE_MTXNxM_POOL(ftype,sz) template<> ftype matrix_tpl<ftype>::mtx_pool[sz] = {}; template<> int matrix_tpl<ftype>::mtx_pool_pos=0; \
template<> int matrix_tpl<ftype>::mtx_pool_size=sz;
#else
#define DECLARE_MTXNxM_POOL(ftype,sz) template<> ftype matrix_tpl<ftype>::mtx_pool[sz]; template<> int matrix_tpl<ftype>::mtx_pool_pos=0; \
template<> int matrix_tpl<ftype>::mtx_pool_size=sz;
#endif //LINUX
extern int g_bHasSSE;
#ifdef PIII_SSE
void PIII_Mult00_6x6_6x6(float *src1, float *src2, float *dst);
inline void matrix_product_tpl<float>::assign_to(float *pdst) const {
if ((g_bHasSSE^1|nRows1-6|nCols1-6|nCols2-6)==0)
PIII_Mult00_6x6_6x6(data1,data2, pdst);
else {
int i,j,k; float sum;
for(i=0;i<nRows1;i++) for(j=0;j<nCols2;j++) {
for(sum=0,k=0; k<nCols1; k++)
sum += data1[i*nCols1+k]*data2[k*nCols2+j];
pdst[i*nCols2+j] = sum;
}
}
}
#endif
#endif

428
CryPhysics/obbtree.cpp Normal file
View File

@@ -0,0 +1,428 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "obbtree.h"
#include "trimesh.h"
void COBBTree::SetParams(int nMinTrisPerNode, int nMaxTrisPerNode, float skipdim)
{
m_nMinTrisPerNode = nMinTrisPerNode; m_nMaxTrisPerNode = nMaxTrisPerNode;
m_maxSkipDim = skipdim;
}
float COBBTree::Build(CGeometry *pMesh)
{
m_pMesh = (CTriMesh*)pMesh;
m_pNodes = new OBBnode[m_nNodesAlloc=256];
memset(m_pMapVtxUsed = new int[(m_pMesh->m_nVertices-1>>5)+1], 0, (m_pMesh->m_nVertices-1>>3)+1);
m_pVtxUsed = new vectorf[m_pMesh->m_nVertices];
m_pNodes[0].iparent = -1;
m_pTri2Node = new index_t[m_pMesh->m_nTris];
m_nMaxTrisInNode = 0;
m_nNodes = 2;
m_pNodes[0].iparent = m_pNodes[1].iparent = -1;
m_pNodes[0].ntris = m_pNodes[1].ntris = 0;
float volume = BuildNode(0, 0,m_pMesh->m_nTris, 0);
if (m_nNodesAlloc>m_nNodes) {
OBBnode *pNodes = m_pNodes;
memcpy(m_pNodes = new OBBnode[m_nNodesAlloc=m_nNodes], pNodes, sizeof(OBBnode)*m_nNodes);
delete[] pNodes;
}
delete[] m_pMapVtxUsed;
delete[] m_pVtxUsed;
m_maxSkipDim *= max(max(m_pNodes[0].size.x,m_pNodes[0].size.y),m_pNodes[0].size.z);
return volume*8;
}
float COBBTree::BuildNode(int iNode, int iTriStart,int nTris, int nDepth)
{
int nHullTris,i,j,nPts,iStart=1<<30,iEnd=-1,idx0=iTriStart*3,nidx=nTris*3;
index_t *pHullTris=0;
// calculate convex hull of vertices
// mark all vertices used by this set of triangles
for(i=0; i<nidx; i++) {
m_pMapVtxUsed[m_pMesh->m_pIndices[idx0+i]>>5] |= 1<<(m_pMesh->m_pIndices[idx0+i]&31);
iStart = min(iStart, m_pMesh->m_pIndices[idx0+i]);
iEnd = max(iEnd, m_pMesh->m_pIndices[idx0+i]);
}
// group these vertices in one array
for(i=iStart,nPts=0; i<=iEnd; i++) if (m_pMapVtxUsed[i>>5] & 1<<(i&31)) {
m_pVtxUsed[nPts++]=m_pMesh->m_pVertices[i]; m_pMapVtxUsed[i>>5] &= ~(1<<(i&31));
}
// give these vertices to qhull routine
if (nDepth>4 || nTris<50 || !(nHullTris = qhull(m_pVtxUsed,nPts, pHullTris))) {
nHullTris=nTris; pHullTris=m_pMesh->m_pIndices+idx0;
}
vectorr axes[3],mean;
matrix3x3 Basis;
if (nHullTris) {
ComputeMeshEigenBasis(m_pMesh->m_pVertices,pHullTris,nHullTris, axes,mean);
Basis = (matrix3x3RM&)axes[0];
} else // convex hull not computed. probably we have some degenerate case, so use AABB
Basis.SetIdentity();
if (pHullTris && pHullTris!=m_pMesh->m_pIndices+idx0)
delete[] pHullTris;
// calculate bounding box center and dimensions
vectorr pt,ptmin(MAX),ptmax(MIN);
for(i=0;i<nPts;i++) {
pt = Basis*m_pVtxUsed[i];
for(j=0;j<3;j++) {
ptmin[j] = min_safe(ptmin[j], pt[j]);
ptmax[j] = max_safe(ptmax[j], pt[j]);
}
}
(matrix3x3RMf&)m_pNodes[iNode].axes[0] = Basis;
m_pNodes[iNode].size = (ptmax-ptmin)*0.5f;
m_pNodes[iNode].center = ((ptmax+ptmin)*(real)0.5)*Basis;
float mindim = max(max(m_pNodes[iNode].size.x,m_pNodes[iNode].size.y),m_pNodes[iNode].size.z)*0.001f;
for(i=0;i<3;i++) {
m_pNodes[iNode].axes[i] = axes[i];
m_pNodes[iNode].size[i] = max(mindim, m_pNodes[iNode].size[i]);
}
if (nTris<=m_nMaxTrisPerNode) {
m_pNodes[iNode].ichild = iTriStart;
m_pNodes[iNode].ntris = nTris;
m_nMaxTrisInNode = max(m_nMaxTrisInNode, nTris);
for(i=iTriStart;i<iTriStart+nTris;i++)
m_pTri2Node[i] = iNode;
return m_pNodes[iNode].size.volume();
}
// separate geometry into two parts
#if defined(WIN64) || defined(LINUX64)
volatile // compiler bug workaround?
#endif
int iAxis;
int numtris[3],nTrisAx[3],iPart,iMode[3],idx;
float x0,x1,x2,cx,sz,xlim[2],bounds[3][2],diff[3],axdiff[3];
vectorf axis,center;
for(iAxis=0;iAxis<3;iAxis++) {
sz = m_pNodes[iNode].size[iAxis];
if (sz<mindim*10) {
axdiff[iAxis] = -1E10f; nTrisAx[iAxis] = 0;
continue;
}
axis = m_pNodes[iNode].axes[iAxis];
cx = axis*m_pNodes[iNode].center;
bounds[0][0]=bounds[1][0]=bounds[2][0] = -sz;
bounds[0][1]=bounds[1][1]=bounds[2][1] = sz;
numtris[0]=numtris[1]=numtris[2] = 0;
for(i=iTriStart;i<iTriStart+nTris;i++) {
center = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3]]+m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]+
m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]];
x0 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+0]]*axis-cx;
x1 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]*axis-cx;
x2 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]]*axis-cx;
xlim[0] = min(min(x0,x1),x2); xlim[1] = max(max(x0,x1),x2);
/*for(j=0,center.zero(),xlim[0]=sz,xlim[1]=-sz;j<3;j++) {
center += m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+j]];
x = axis*m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+j]]-cx;
xlim[0] = min(xlim[0],x); xlim[1] = max(xlim[1],x);
}*/
iPart = isneg(xlim[1])^1; // mode0: group all triangles that are entirely below center
bounds[0][iPart] = minmax(bounds[0][iPart],xlim[iPart^1],iPart^1); numtris[0]+=iPart;
iPart = isnonneg(xlim[0]); // mode1: group all triangles that are entirely above center
bounds[1][iPart] = minmax(bounds[1][iPart],xlim[iPart^1],iPart^1); numtris[1]+=iPart;
iPart = isnonneg(((center*axis)*(1.0f/3)-cx)); // mode2: sort triangles basing on centroids only
bounds[2][iPart] = minmax(bounds[2][iPart],xlim[iPart^1],iPart^1); numtris[2]+=iPart;
}
for(i=0;i<3;i++) diff[i] = bounds[i][1]-bounds[i][0]-sz*(isneg(numtris[i]-m_nMinTrisPerNode)|isneg(nTris-numtris[i]-m_nMinTrisPerNode));
iMode[iAxis] = idxmax3(diff); nTrisAx[iAxis] = numtris[iMode[iAxis]];
axdiff[iAxis] = diff[iMode[iAxis]]*m_pNodes[iNode].size[dec_mod3[iAxis]]*m_pNodes[iNode].size[inc_mod3[iAxis]];
}
iAxis = idxmax3(axdiff);
axis = m_pNodes[iNode].axes[iAxis];
cx = axis*m_pNodes[iNode].center;
for(i=j=iTriStart;i<iTriStart+nTris;i++) {
x0 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+0]]*axis-cx;
x1 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+1]]*axis-cx;
x2 = m_pMesh->m_pVertices[m_pMesh->m_pIndices[i*3+2]]*axis-cx;
#if 0//def WIN64
if ((unsigned)iAxis >= 3)
OutputDebugString ("iAxis>=3!");
else
if ((unsigned)iMode[iAxis] >= 3)
OutputDebugString("iMode[iAxis]>=3!");
#endif
switch(iMode[iAxis]) {
case 0: iPart = isneg(max(max(x0,x1),x2))^1; break;
case 1: iPart = isnonneg(min(min(x0,x1),x2)); break;
case 2: iPart = isnonneg(x0+x1+x2);
}
if (iPart==0) {
// swap triangles
idx=m_pMesh->m_pIndices[i*3+0]; m_pMesh->m_pIndices[i*3+0]=m_pMesh->m_pIndices[j*3+0]; m_pMesh->m_pIndices[j*3+0]=idx;
idx=m_pMesh->m_pIndices[i*3+1]; m_pMesh->m_pIndices[i*3+1]=m_pMesh->m_pIndices[j*3+1]; m_pMesh->m_pIndices[j*3+1]=idx;
idx=m_pMesh->m_pIndices[i*3+2]; m_pMesh->m_pIndices[i*3+2]=m_pMesh->m_pIndices[j*3+2]; m_pMesh->m_pIndices[j*3+2]=idx;
if (m_pMesh->m_pIds) {
idx=m_pMesh->m_pIds[i]; m_pMesh->m_pIds[i]=m_pMesh->m_pIds[j]; m_pMesh->m_pIds[j]=idx;
}
j++;
}
}
j -= iTriStart;
if (j<m_nMinTrisPerNode || j>nTris-m_nMinTrisPerNode) {
m_pNodes[iNode].ichild = iTriStart;
m_pNodes[iNode].ntris = nTris;
m_nMaxTrisInNode = max(m_nMaxTrisInNode, nTris);
for(i=iTriStart;i<iTriStart+nTris;i++)
m_pTri2Node[i] = iNode;
return m_pNodes[iNode].size.volume();
}
// proceed with the children
if (m_nNodes+2 > m_nNodesAlloc) {
OBBnode *pNodes = m_pNodes;
memcpy(m_pNodes = new OBBnode[m_nNodesAlloc+=256], pNodes, m_nNodes*sizeof(OBBnode));
delete[] pNodes;
}
m_pNodes[iNode].ichild = m_nNodes;
m_pNodes[m_nNodes].iparent = m_pNodes[m_nNodes+1].iparent = iNode;
m_pNodes[m_nNodes].ntris = m_pNodes[m_nNodes+1].ntris = 0;
iNode = m_nNodes; m_nNodes += 2;
float res = BuildNode(iNode+1, iTriStart+j,nTris-j, nDepth+1);
res += BuildNode(iNode, iTriStart,j, nDepth+1);
return res;
}
void COBBTree::SetGeomConvex()
{
m_maxSkipDim = (m_pNodes[0].size.x+m_pNodes[0].size.y+m_pNodes[0].size.z)*10.0f;
}
void COBBTree::GetBBox(box *pbox)
{
pbox->Basis = (matrix3x3RMf&)m_pNodes[0].axes[0];
pbox->bOriented = 1;
pbox->center = m_pNodes[0].center;
pbox->size = m_pNodes[0].size;
}
void COBBTree::GetNodeBV(BV *&pBV,int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
((BBox*)pBV)->iNode = iNode;
((BBox*)pBV)->abox.Basis = (matrix3x3RMf&)m_pNodes[iNode].axes[0];
((BBox*)pBV)->abox.bOriented = 1;
((BBox*)pBV)->abox.center = m_pNodes[iNode].center;
((BBox*)pBV)->abox.size = m_pNodes[iNode].size;
}
void COBBTree::GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
((BBox*)pBV)->iNode = iNode;
box boxstatic;
boxstatic.Basis = (matrix3x3RMf&)m_pNodes[iNode].axes[0];
boxstatic.bOriented = 1;
boxstatic.center = m_pNodes[iNode].center;
boxstatic.size = m_pNodes[iNode].size;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &((BBox*)pBV)->abox);
}
void COBBTree::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = iNode;
((BBox*)pBV)->abox.Basis = (matrix3x3RMf&)m_pNodes[iNode].axes[0]*Rw.T();
((BBox*)pBV)->abox.bOriented = 1;
((BBox*)pBV)->abox.center = Rw*m_pNodes[iNode].center*scalew + offsw;
((BBox*)pBV)->abox.size = m_pNodes[iNode].size*scalew;
}
float COBBTree::SplitPriority(const BV* pBV)
{
BBox *pbox = (BBox*)pBV;
return pbox->abox.size.volume()*(m_pNodes[pbox->iNode].ntris-1>>31 & 1);
}
void COBBTree::GetNodeChildrenBVs(const matrix3x3f &Rw,const vectorf &offsw,float scalew,
const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2)
{
BBox *bbox_parent=(BBox*)pBV_parent, *&bbox_child1=(BBox*&)pBV_child1, *&bbox_child2=(BBox*&)pBV_child2;
bbox_child1 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
OBBnode *pNode = m_pNodes + bbox_child1->iNode;
bbox_child1->abox.Basis = (matrix3x3RMf&)pNode->axes[0]*Rw.T();
bbox_child1->abox.bOriented = 1+bbox_child1->iNode;
bbox_child1->abox.center = Rw*pNode->center*scalew + offsw;
bbox_child1->abox.size = pNode++->size*scalew;
bbox_child2->abox.Basis = (matrix3x3RMf&)pNode->axes[0]*Rw.T();
bbox_child2->abox.bOriented = 1+bbox_child2->iNode;
bbox_child2->abox.center = Rw*pNode->center*scalew + offsw;
bbox_child2->abox.size = pNode->size*scalew;
}
void COBBTree::GetNodeChildrenBVs(const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2)
{
BBox *bbox_parent=(BBox*)pBV_parent, *&bbox_child1=(BBox*&)pBV_child1, *&bbox_child2=(BBox*&)pBV_child2;
bbox_child1 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
OBBnode *pNode = m_pNodes + bbox_child1->iNode;
bbox_child1->abox.Basis = (matrix3x3RMf&)pNode->axes[0];
bbox_child1->abox.bOriented = 1+bbox_child1->iNode;
bbox_child1->abox.center = pNode->center;
bbox_child1->abox.size = pNode++->size;
bbox_child2->abox.Basis = (matrix3x3RMf&)pNode->axes[0];
bbox_child2->abox.bOriented = 1+bbox_child2->iNode;
bbox_child2->abox.center = pNode->center;
bbox_child2->abox.size = pNode->size;
}
void COBBTree::GetNodeChildrenBVs(const BV *pBV_parent, const vectorf &sweepdir,float sweepstep, BV *&pBV_child1,BV *&pBV_child2)
{
BBox *bbox_parent=(BBox*)pBV_parent, *&bbox_child1=(BBox*&)pBV_child1, *&bbox_child2=(BBox*&)pBV_child2;
bbox_child1 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2 = g_BBoxBuf + g_BBoxBufPos++;
bbox_child2->iNode = (bbox_child1->iNode = m_pNodes[bbox_parent->iNode].ichild)+1;
bbox_child1->type = bbox_child2->type = box::type;
OBBnode *pNode = m_pNodes + bbox_child1->iNode;
box boxstatic;
boxstatic.bOriented = 1;
boxstatic.Basis = (matrix3x3RMf&)pNode->axes[0];
boxstatic.center = pNode->center;
boxstatic.size = pNode++->size;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &bbox_child1->abox);
bbox_child1->abox.bOriented = 1+bbox_child1->iNode;
boxstatic.Basis = (matrix3x3RMf&)pNode->axes[0];
boxstatic.center = pNode->center;
boxstatic.size = pNode->size;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &bbox_child2->abox);
bbox_child1->abox.bOriented = 1+bbox_child2->iNode;
}
void COBBTree::ReleaseLastBVs()
{
g_BBoxBufPos-=2;
}
void COBBTree::ReleaseLastSweptBVs()
{
g_BBoxBufPos-=2;
}
int COBBTree::GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal, geometry_under_test *pGTest,
geometry_under_test *pGTestOp)
{
return m_pMesh->GetPrimitiveList(m_pNodes[iNode].ichild,m_pNodes[iNode].ntris, pBVCollider->type,*pBVCollider,bColliderLocal, pGTest,pGTestOp,
pGTest->primbuf,pGTest->idbuf);
}
void COBBTree::MarkUsedTriangle(int itri, geometry_under_test *pGTest)
{
if (!pGTest->pUsedNodesMap) return;
int iNode = m_pTri2Node[itri];
if (!(pGTest->pUsedNodesMap[iNode>>5] & 1<<(iNode&31)) &&
max(max(m_pNodes[iNode].size.x,m_pNodes[iNode].size.y),m_pNodes[iNode].size.z) < m_maxSkipDim)
{
do {
pGTest->pUsedNodesMap[iNode>>5] |= 1<<(iNode&31);
pGTest->pUsedNodesIdx[pGTest->nUsedNodes = min(pGTest->nUsedNodes+1,pGTest->nMaxUsedNodes-1)] = iNode;
pGTest->bCurNodeUsed = 1;
iNode ^= 1; // the other child of the same parent
if (!(pGTest->pUsedNodesMap[iNode>>5] & 1<<(iNode&31)))
break;
iNode = m_pNodes[iNode].iparent;
} while (iNode>=0);
}
}
int COBBTree::PrepareForIntersectionTest(geometry_under_test *pGTest)
{
if (m_maxSkipDim<=0) {
pGTest->pUsedNodesMap = 0;
pGTest->pUsedNodesIdx = 0;
pGTest->nMaxUsedNodes = 0;
} else {
int mapsz = (m_nNodes-1>>5) + 1;
if (mapsz<=(int)(sizeof(g_UsedNodesMap)/sizeof(g_UsedNodesMap[0]))-g_UsedNodesMapPos) {
pGTest->pUsedNodesMap = g_UsedNodesMap+g_UsedNodesMapPos; g_UsedNodesMapPos += mapsz;
} else
pGTest->pUsedNodesMap = new int[mapsz];
pGTest->pUsedNodesIdx = g_UsedNodesIdx+g_UsedNodesIdxPos;
pGTest->nMaxUsedNodes = min(32,sizeof(g_UsedNodesIdx)/sizeof(g_UsedNodesIdx[0])-g_UsedNodesIdxPos);
g_UsedNodesIdxPos += pGTest->nMaxUsedNodes;
}
pGTest->nUsedNodes = -1;
return 1;
}
void COBBTree::CleanupAfterIntersectionTest(geometry_under_test *pGTest)
{
if (!pGTest->pUsedNodesMap)
return;
if ((unsigned int)(pGTest->pUsedNodesMap-g_UsedNodesMap) > (unsigned int)sizeof(g_UsedNodesMap)/sizeof(g_UsedNodesMap[0])) {
delete[] pGTest->pUsedNodesMap; return;
}
if (pGTest->nUsedNodes < pGTest->nMaxUsedNodes-1) {
for(int i=0;i<=pGTest->nUsedNodes;i++)
pGTest->pUsedNodesMap[pGTest->pUsedNodesIdx[i]>>5] &= ~(1<<(pGTest->pUsedNodesIdx[i]&31));
} else
memset(pGTest->pUsedNodesMap, 0, ((m_nNodes-1>>5)+1)*4);
}
void COBBTree::GetMemoryStatistics(ICrySizer *pSizer)
{
SIZER_COMPONENT_NAME(pSizer, "OBB trees");
pSizer->AddObject(this, sizeof(COBBTree));
pSizer->AddObject(m_pNodes, m_nNodesAlloc*sizeof(m_pNodes[0]));
if (m_pTri2Node)
pSizer->AddObject(m_pTri2Node, m_pMesh->m_nTris*sizeof(m_pTri2Node[0]));
}
void COBBTree::Save(CMemStream &stm)
{
stm.Write(m_nNodes);
stm.Write(m_pNodes, m_nNodes*sizeof(m_pNodes[0]));
stm.Write(m_nMaxTrisInNode);
stm.Write(m_nMinTrisPerNode);
stm.Write(m_nMaxTrisPerNode);
stm.Write(m_maxSkipDim);
}
void COBBTree::Load(CMemStream &stm, CGeometry *pGeom)
{
m_pMesh = (CTriMesh*)pGeom;
stm.Read(m_nNodes);
m_pNodes = new OBBnode[m_nNodesAlloc=m_nNodes];
stm.Read(m_pNodes, m_nNodes*sizeof(m_pNodes[0]));
m_pTri2Node = new index_t[m_pMesh->m_nTris];
for(int i=0;i<m_nNodes;i++) for(int j=0;j<m_pNodes[i].ntris;j++)
m_pTri2Node[m_pNodes[i].ichild+j] = i;
stm.Read(m_nMaxTrisInNode);
stm.Read(m_nMinTrisPerNode);
stm.Read(m_nMaxTrisPerNode);
stm.Read(m_maxSkipDim);
}

68
CryPhysics/obbtree.h Normal file
View File

@@ -0,0 +1,68 @@
#ifndef obbtree_h
#define obbtree_h
#pragma once
struct OBBnode {
vectorf axes[3];
vectorf center;
vectorf size;
int iparent;
int ichild;
int ntris;
};
class CTriMesh;
class COBBTree : public CBVTree {
public:
COBBTree() { m_nMinTrisPerNode=2; m_nMaxTrisPerNode=4; m_maxSkipDim=0; m_pNodes=0; m_pTri2Node=0; }
virtual ~COBBTree() {
if (m_pNodes) delete[] m_pNodes; m_pNodes=0;
if (m_pTri2Node) delete[] m_pTri2Node; m_pTri2Node=0;
}
virtual int GetType() { return BVT_OBB; }
virtual float Build(CGeometry *pMesh);
virtual void SetGeomConvex();
void SetParams(int nMinTrisPerNode,int nMaxTrisPerNode, float skipdim);
float BuildNode(int iNode, int iTriStart,int nTris, int nDepth);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest);
virtual void CleanupAfterIntersectionTest(geometry_under_test *pGTest);
virtual void GetBBox(box *pbox);
virtual int MaxPrimsInNode() { return m_nMaxTrisInNode; }
virtual void GetNodeBV(BV *&pBV, int iNode=0);
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual float SplitPriority(const BV* pBV);
virtual void GetNodeChildrenBVs(const matrix3x3f &Rw,const vectorf &offsw,float scalew, const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2);
virtual void GetNodeChildrenBVs(const BV *pBV_parent, BV *&pBV_child1,BV *&pBV_child2);
virtual void GetNodeChildrenBVs(const BV *pBV_parent, const vectorf &sweepdir,float sweepstep, BV *&pBV_child1,BV *&pBV_child2);
virtual void ReleaseLastBVs();
virtual void ReleaseLastSweptBVs();
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp);
virtual int GetNodeContentsIdx(int iNode, int &iStartPrim) { iStartPrim = m_pNodes[iNode].ichild; return m_pNodes[iNode].ntris; }
virtual void MarkUsedTriangle(int itri, geometry_under_test *pGTest);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm, CGeometry *pGeom);
CTriMesh *m_pMesh;
OBBnode *m_pNodes;
int m_nNodes;
int m_nNodesAlloc;
index_t *m_pTri2Node;
int m_nMaxTrisInNode;
int m_nMinTrisPerNode;
int m_nMaxTrisPerNode;
float m_maxSkipDim;
int *m_pMapVtxUsed;
vectorf *m_pVtxUsed;
};
#endif

430
CryPhysics/overlapchecks.cpp Normal file
View File

@@ -0,0 +1,430 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "overlapchecks.h"
COverlapChecker g_Overlapper;
COverlapChecker::COverlapChecker()
{
for(int i=0;i<NPRIMS;i++) for(int j=0;j<NPRIMS;j++)
table[i][j] = default_overlap_check;
table[box::type][box::type] = (overlap_check)box_box_overlap_check;
table[box::type][triangle::type] = (overlap_check)box_tri_overlap_check;
table[triangle::type][box::type] = (overlap_check)tri_box_overlap_check;
table[box::type][heightfield::type] = (overlap_check)box_heightfield_overlap_check;
table[heightfield::type][box::type] = (overlap_check)heightfield_box_overlap_check;
table[box::type][ray::type] = (overlap_check)box_ray_overlap_check;
table[ray::type][box::type] = (overlap_check)ray_box_overlap_check;
table[box::type][sphere::type] = (overlap_check)box_sphere_overlap_check;
table[sphere::type][box::type] = (overlap_check)sphere_box_overlap_check;
table[triangle::type][sphere::type] = (overlap_check)tri_sphere_overlap_check;
table[sphere::type][triangle::type] = (overlap_check)sphere_tri_overlap_check;
table[sphere::type][sphere::type] = (overlap_check)sphere_sphere_overlap_check;
table[heightfield::type][sphere::type] = (overlap_check)heightfield_sphere_overlap_check;
table[sphere::type][heightfield::type] = (overlap_check)sphere_heightfield_overlap_check;
}
int default_overlap_check(const primitive*, const primitive *)
{
return 1;
}
int box_box_overlap_check(const box *box1, const box *box2)
{
int i;
matrix3x3RMf &Basis21((matrix3x3RMf&)*(matrix3x3RMf*)g_Overlapper.Basis21);
if ((box1->bOriented|box2->bOriented<<16)!=g_Overlapper.iPrevCode) {
if (!box1->bOriented)
Basis21 = box2->Basis.T();
else if (box2->bOriented)
Basis21 = box1->Basis*box2->Basis.T();
else
Basis21 = box1->Basis;
for(i=0;i<9;i++)
g_Overlapper.Basis21abs[i] = fabsf(g_Overlapper.Basis21[i]);
g_Overlapper.iPrevCode = box1->bOriented|box2->bOriented<<16;
}
vectorf center21 = box2->center-box1->center;
if (box1->bOriented)
center21 = box1->Basis*center21;
const vectorf &a=box1->size, &b=box2->size;
float t1,t2,t3,e=(a.x+a.y+a.z)*1e-4f;
// node1 basis vectors
if (fabsf(center21.x) > a.x+b*vectorf(g_Overlapper.Basis21abs+0))
return 0;
if (fabsf(center21.y) > a.y+b*vectorf(g_Overlapper.Basis21abs+3))
return 0;
if (fabsf(center21.z) > a.z+b*vectorf(g_Overlapper.Basis21abs+6))
return 0;
// node2 basis vectors
if (fabsf(center21.x*g_Overlapper.Basis21[0]+center21.y*g_Overlapper.Basis21[3]+center21.z*g_Overlapper.Basis21[6]) >
a.x*g_Overlapper.Basis21abs[0]+a.y*g_Overlapper.Basis21abs[3]+a.z*g_Overlapper.Basis21abs[6]+b.x)
return 0;
if (fabsf(center21.x*g_Overlapper.Basis21[1]+center21.y*g_Overlapper.Basis21[4]+center21.z*g_Overlapper.Basis21[7]) >
a.x*g_Overlapper.Basis21abs[1]+a.y*g_Overlapper.Basis21abs[4]+a.z*g_Overlapper.Basis21abs[7]+b.y)
return 0;
if (fabsf(center21.x*g_Overlapper.Basis21[2]+center21.y*g_Overlapper.Basis21[5]+center21.z*g_Overlapper.Basis21[8]) >
a.x*g_Overlapper.Basis21abs[2]+a.y*g_Overlapper.Basis21abs[5]+a.z*g_Overlapper.Basis21abs[8]+b.z)
return 0;
// node1->axes[0] x node2->axes[0]
t1 = a.y*g_Overlapper.Basis21abs[6] + a.z*g_Overlapper.Basis21abs[3];
t2 = b.y*g_Overlapper.Basis21abs[2] + b.z*g_Overlapper.Basis21abs[1];
t3 = center21.z*g_Overlapper.Basis21[3] - center21.y*g_Overlapper.Basis21[6];
if (fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[0] x node2->axes[1]
t1 = a.y*g_Overlapper.Basis21abs[7] + a.z*g_Overlapper.Basis21abs[4];
t2 = b.x*g_Overlapper.Basis21abs[2] + b.z*g_Overlapper.Basis21abs[0];
t3 = center21.z*g_Overlapper.Basis21[4] - center21.y*g_Overlapper.Basis21[7];;
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[0] x node2->axes[2]
t1 = a.y*g_Overlapper.Basis21abs[8] + a.z*g_Overlapper.Basis21abs[5];
t2 = b.x*g_Overlapper.Basis21abs[1] + b.y*g_Overlapper.Basis21abs[0];
t3 = center21.z*g_Overlapper.Basis21[5] - center21.y*g_Overlapper.Basis21[8];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[1] x node2->axes[0]
t1 = a.x*g_Overlapper.Basis21abs[6] + a.z*g_Overlapper.Basis21abs[0];
t2 = b.y*g_Overlapper.Basis21abs[5] + b.z*g_Overlapper.Basis21abs[4];
t3 = center21.x*g_Overlapper.Basis21[6] - center21.z*g_Overlapper.Basis21[0];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[1] x node2->axes[1]
t1 = a.x*g_Overlapper.Basis21abs[7] + a.z*g_Overlapper.Basis21abs[1];
t2 = b.x*g_Overlapper.Basis21abs[5] + b.z*g_Overlapper.Basis21abs[3];
t3 = center21.x*g_Overlapper.Basis21[7] - center21.z*g_Overlapper.Basis21[1];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[1] x node2->axes[2]
t1 = a.x*g_Overlapper.Basis21abs[8] + a.z*g_Overlapper.Basis21abs[2];
t2 = b.x*g_Overlapper.Basis21abs[4] + b.y*g_Overlapper.Basis21abs[3];
t3 = center21.x*g_Overlapper.Basis21[8] - center21.z*g_Overlapper.Basis21[2];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[2] x node2->axes[0]
t1 = a.x*g_Overlapper.Basis21abs[3] + a.y*g_Overlapper.Basis21abs[0];
t2 = b.y*g_Overlapper.Basis21abs[8] + b.z*g_Overlapper.Basis21abs[7];
t3 = center21.y*g_Overlapper.Basis21[0] - center21.x*g_Overlapper.Basis21[3];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[2] x node2->axes[1]
t1 = a.x*g_Overlapper.Basis21abs[4] + a.y*g_Overlapper.Basis21abs[1];
t2 = b.x*g_Overlapper.Basis21abs[8] + b.z*g_Overlapper.Basis21abs[6];
t3 = center21.y*g_Overlapper.Basis21[1] - center21.x*g_Overlapper.Basis21[4];
if(fabsf(t3) > t1+t2+e)
return 0;
// node1->axes[2] x node2->axes[2]
t1 = a.x*g_Overlapper.Basis21abs[5] + a.y*g_Overlapper.Basis21abs[2];
t2 = b.x*g_Overlapper.Basis21abs[7] + b.y*g_Overlapper.Basis21abs[6];
t3 = center21.y*g_Overlapper.Basis21[2] - center21.x*g_Overlapper.Basis21[5];
if(fabsf(t3) > t1+t2+e)
return 0;
return 1;
}
int box_heightfield_overlap_check(const box *pbox, const heightfield *phf)
{
box boxtr;
vectorf vtx[4];
triangle hftri;
vector2df sz,ptmin,ptmax;
int ix,iy,ix0,iy0,ix1,iy1,icell;
float hmax;
// find the 3 lowest vertices
if (phf->bOriented) {
if (pbox->bOriented)
boxtr.Basis = pbox->Basis*phf->Basis.T();
else
boxtr.Basis = phf->Basis.T();
boxtr.center = phf->Basis*(pbox->center-phf->origin);
} else {
boxtr.Basis = pbox->Basis;
boxtr.center = pbox->center-phf->origin;
}
boxtr.bOriented = 1;
boxtr.Basis.SetRow(0,boxtr.Basis.GetRow(0)*-sgnnz(boxtr.Basis(0,2)));
boxtr.Basis.SetRow(1,boxtr.Basis.GetRow(1)*-sgnnz(boxtr.Basis(1,2)));
boxtr.Basis.SetRow(2,boxtr.Basis.GetRow(2)*-sgnnz(boxtr.Basis(2,2)));
boxtr.size = pbox->size;
vtx[0] = pbox->size*boxtr.Basis;
boxtr.Basis.SetRow(0,-boxtr.Basis.GetRow(0)); vtx[1]=pbox->size*boxtr.Basis; boxtr.Basis.SetRow(0,-boxtr.Basis.GetRow(0));
boxtr.Basis.SetRow(1,-boxtr.Basis.GetRow(1)); vtx[2]=pbox->size*boxtr.Basis; boxtr.Basis.SetRow(1,-boxtr.Basis.GetRow(1));
boxtr.Basis.SetRow(2,-boxtr.Basis.GetRow(2)); vtx[3]=pbox->size*boxtr.Basis; boxtr.Basis.SetRow(2,-boxtr.Basis.GetRow(2));
// find the underlying grid rectangle
sz.x = sz.y = 0;
sz.x = max(sz.x, fabsf(vtx[1].x)); sz.y = max(sz.y, fabsf(vtx[1].y));
sz.x = max(sz.x, fabsf(vtx[2].x)); sz.y = max(sz.y, fabsf(vtx[2].y));
sz.x = max(sz.x, fabsf(vtx[3].x)); sz.y = max(sz.y, fabsf(vtx[3].y));
ptmin.x = (boxtr.center.x-sz.x)*phf->stepr.x; ptmin.y = (boxtr.center.y-sz.y)*phf->stepr.y;
ptmax.x = (boxtr.center.x+sz.x)*phf->stepr.x; ptmax.y = (boxtr.center.y+sz.y)*phf->stepr.y;
ix0 = float2int(ptmin.x-0.5f); iy0 = float2int(ptmin.y-0.5f); ix0 &= ~(ix0>>31); iy0 &= ~(iy0>>31);
ix1 = min(float2int(ptmax.x+0.5f),phf->size.x); iy1 = min(float2int(ptmax.y+0.5f),phf->size.y);
//if (ix0>ix1 || iy0>iy1 || phf->gettype(ix0,iy0)==-1) // now checks are in CTriMesh on per-tri basis, using materials
// return 0;
vtx[0]+=boxtr.center; vtx[1]+=boxtr.center; vtx[2]+=boxtr.center; vtx[3]+=boxtr.center;
// check if all heightfield points are below the lowest box point
for(ix=ix0,hmax=0;ix<=ix1;ix++) for(iy=iy0;iy<=iy1;iy++)
hmax = max(hmax,phf->getheight(ix,iy));
if (hmax<vtx[0].z)
return 0;
if ((ix1-ix0)*(iy1-iy0)<=4) {
// check for intersection with each underlying triangle
for(ix=ix0;ix<ix1;ix++) for(iy=iy0;iy<iy1;iy++) {
icell = ix*phf->stride.x+iy*phf->stride.y;
hftri.pt[0].x=hftri.pt[2].x = ix*phf->step.x; hftri.pt[0].y=hftri.pt[1].y = iy*phf->step.y;
hftri.pt[1].x = hftri.pt[0].x+phf->step.x; hftri.pt[2].y = hftri.pt[0].y+phf->step.y;
hftri.pt[0].z = phf->getheight(icell); hftri.pt[1].z = phf->getheight(icell+phf->stride.x);
hftri.pt[2].z = phf->getheight(icell+phf->stride.y);
hftri.n = hftri.pt[1]-hftri.pt[0] ^ hftri.pt[2]-hftri.pt[0];
if (box_tri_overlap_check(&boxtr,&hftri))
return 1;
hftri.pt[0] = hftri.pt[2]; hftri.pt[2].x += phf->step.x; hftri.pt[2].z = phf->getheight(icell+phf->stride.x+phf->stride.y);
hftri.n = hftri.pt[1]-hftri.pt[0] ^ hftri.pt[2]-hftri.pt[0];
if (box_tri_overlap_check(&boxtr,&hftri))
return 1;
}
return 0;
}
return 1;
}
int heightfield_box_overlap_check(const heightfield *phf, const box *pbox)
{
return box_heightfield_overlap_check(pbox,phf);
}
int box_tri_overlap_check(const box *pbox, const triangle *ptri)
{
vectorf pt[3],n;
float l1,l2,l3,l,c;
if (pbox->bOriented) {
pt[0] = pbox->Basis*(ptri->pt[0]-pbox->center);
pt[1] = pbox->Basis*(ptri->pt[1]-pbox->center);
pt[2] = pbox->Basis*(ptri->pt[2]-pbox->center);
n = pbox->Basis*ptri->n;
} else {
pt[0] = ptri->pt[0]-pbox->center;
pt[1] = ptri->pt[1]-pbox->center;
pt[2] = ptri->pt[2]-pbox->center;
n = ptri->n;
}
// check box normals
l1=fabsf(pt[0].x-pt[1].x); l2=fabsf(pt[1].x-pt[2].x); l3=fabsf(pt[2].x-pt[0].x); l=l1+l2+l3; // half length = l/4
c = (l1*(pt[0].x+pt[1].x)+l2*(pt[1].x+pt[2].x)+l3*(pt[2].x+pt[0].x))*0.5f; // center = c/l
if (fabsf(c) > (pbox->size.x+l*0.25f)*l)
return 0;
l1=fabsf(pt[0].y-pt[1].y); l2=fabsf(pt[1].y-pt[2].y); l3=fabsf(pt[2].y-pt[0].y); l=l1+l2+l3;
c = (l1*(pt[0].y+pt[1].y)+l2*(pt[1].y+pt[2].y)+l3*(pt[2].y+pt[0].y))*0.5f;
if (fabsf(c) > (pbox->size.y+l*0.25f)*l)
return 0;
l1=fabsf(pt[0].z-pt[1].z); l2=fabsf(pt[1].z-pt[2].z); l3=fabsf(pt[2].z-pt[0].z); l=l1+l2+l3;
c = (l1*(pt[0].z+pt[1].z)+l2*(pt[1].z+pt[2].z)+l3*(pt[2].z+pt[0].z))*0.5f;
if (fabsf(c) > (pbox->size.z+l*0.25f)*l)
return 0;
// check triangle normal
if (fabsf(n.x)*pbox->size.x+fabsf(n.y)*pbox->size.y+fabsf(n.z)*pbox->size.z < fabsf(n*pt[0]))
return 0;
vectorf edge,triproj1,triproj2;
float boxproj;
// check triangle edges - box edges cross products
for(int i=0;i<3;i++) {
edge = pt[inc_mod3[i]]-pt[i]; triproj1 = edge^pt[i]; triproj2 = edge^pt[dec_mod3[i]];
boxproj = fabsf(pbox->size.y*edge.z) + fabsf(pbox->size.z*edge.y);
if (fabsf((triproj1.x+triproj2.x)*0.5f) > boxproj+fabsf(triproj1.x-triproj2.x)*0.5f)
return 0;
boxproj = fabsf(pbox->size.x*edge.z) + fabsf(pbox->size.z*edge.x);
if (fabsf((triproj1.y+triproj2.y)*0.5f) > boxproj+fabsf(triproj1.y-triproj2.y)*0.5f)
return 0;
boxproj = fabsf(pbox->size.x*edge.y) + fabsf(pbox->size.y*edge.x);
if (fabsf((triproj1.z+triproj2.z)*0.5f) > boxproj+fabsf(triproj1.z-triproj2.z)*0.5f)
return 0;
}
return 1;
}
int tri_box_overlap_check(const triangle *ptri, const box *pbox)
{
return box_tri_overlap_check(pbox,ptri);
}
int box_ray_overlap_check(const box *pbox, const ray *pray)
{
vectorf l,m,al;
if (pbox->bOriented) {
l = pbox->Basis*pray->dir*0.5f;
m = pbox->Basis*(pray->origin-pbox->center)+l;
} else {
l = pray->dir*0.5f;
m = pray->origin+l-pbox->center;
}
al.x=fabsf(l.x); al.y=fabsf(l.y); al.z=fabsf(l.z);
// separating axis check for line and box
if (fabsf(m.x) > pbox->size.x+al.x)
return 0;
if (fabsf(m.y) > pbox->size.y+al.y)
return 0;
if (fabsf(m.z) > pbox->size.z+al.z)
return 0;
if (fabsf(m.z*l.y-m.y*l.z) > pbox->size.y*al.z+pbox->size.z*al.y)
return 0;
if (fabsf(m.x*l.z-m.z*l.x) > pbox->size.x*al.z+pbox->size.z*al.x)
return 0;
if (fabsf(m.x*l.y-m.y*l.x) > pbox->size.x*al.y+pbox->size.y*al.x)
return 0;
return 1;
}
int ray_box_overlap_check(const ray *pray, const box *pbox)
{
return box_ray_overlap_check(pbox,pray);
}
int box_sphere_overlap_check(const box *pbox, const sphere *psph)
{
vectorf center=psph->center-pbox->center, dist;
if (pbox->bOriented)
center = pbox->Basis*center;
dist.x = max(0.0f, fabsf(center.x)-pbox->size.x);
dist.y = max(0.0f, fabsf(center.y)-pbox->size.y);
dist.z = max(0.0f, fabsf(center.z)-pbox->size.z);
return isneg(dist.len2()-sqr(psph->r));
}
int sphere_box_overlap_check(const sphere *psph, const box *pbox)
{
return box_sphere_overlap_check(pbox,psph);
}
quotientf tri_sphere_dist2(const triangle* ptri, const sphere* psph, int& bFace)
{
int i,bInside[2];
float rvtx[3],elen2[2],denom;
vectorf edge[2],dp;
bFace = 0;
rvtx[0] = (ptri->pt[0]-psph->center).len2();
rvtx[1] = (ptri->pt[1]-psph->center).len2();
rvtx[2] = (ptri->pt[2]-psph->center).len2();
i = idxmin3(rvtx);
dp = psph->center-ptri->pt[i];
edge[0] = ptri->pt[inc_mod3[i]]-ptri->pt[i]; elen2[0] = edge[0].len2();
edge[1] = ptri->pt[dec_mod3[i]]-ptri->pt[i]; elen2[1] = edge[1].len2();
bInside[0] = isneg((dp^edge[0])*ptri->n);
bInside[1] = isneg((edge[1]^dp)*ptri->n);
rvtx[i] = rvtx[i]*elen2[bInside[0]] - sqr(max(0.0f,dp*edge[bInside[0]]))*(bInside[0]|bInside[1]);
denom = elen2[bInside[0]];
if (bInside[0]&bInside[1]) {
if (edge[0]*edge[1]<0) {
edge[0] = ptri->pt[dec_mod3[i]]-ptri->pt[inc_mod3[i]];
dp = psph->center-ptri->pt[inc_mod3[i]];
if ((dp^edge[0])*ptri->n>0)
return quotientf(rvtx[inc_mod3[i]]*edge[0].len2()-sqr(dp*edge[0]), edge[0].len2());
}
bFace = 1;
return quotientf(sqr((psph->center-ptri->pt[0])*ptri->n),1.0f);
}
return quotientf(rvtx[i],denom);
}
int tri_sphere_overlap_check(const triangle *ptri, const sphere *psph)
{
int bFace;
return isneg(tri_sphere_dist2(ptri,psph,bFace)-sqr(psph->r));
/*float r2=sqr(psph->r),edgelen2;
vectorf edge,dp0,dp1;
if ((ptri->pt[0]-psph->center).len2()<r2)
return 1;
if ((ptri->pt[1]-psph->center).len2()<r2)
return 1;
if ((ptri->pt[2]-psph->center).len2()<r2)
return 1;
edgelen2 = (edge = ptri->pt[1]-ptri->pt[0]).len2(); dp0 = ptri->pt[0]-psph->center; dp1 = ptri->pt[1]-psph->center;
if (isneg((dp0^edge).len2()-r2*edgelen2) & isneg((dp0*edge)*(dp1*edge)))
return 1;
edgelen2 = (edge = ptri->pt[2]-ptri->pt[1]).len2(); dp0 = ptri->pt[1]-psph->center; dp1 = ptri->pt[2]-psph->center;
if (isneg((dp0^edge).len2()-r2*edgelen2) & isneg((dp0*edge)*(dp1*edge)))
return 1;
edgelen2 = (edge = ptri->pt[0]-ptri->pt[2]).len2(); dp0 = ptri->pt[2]-psph->center; dp1 = ptri->pt[0]-psph->center;
if (isneg((dp0^edge).len2()-r2*edgelen2) & isneg((dp0*edge)*(dp1*edge)))
return 1;
return 0;*/
}
int sphere_tri_overlap_check(const sphere *psph, const triangle *ptri)
{
return tri_sphere_overlap_check(ptri,psph);
}
int sphere_sphere_overlap_check(const sphere *psph1, const sphere *psph2)
{
return isneg((psph1->center-psph2->center).len2()-sqr(psph1->r+psph2->r));
}
int heightfield_sphere_overlap_check(const heightfield *phf, const sphere *psph)
{
vectorf center;
vector2di irect[2];
int ix,iy,bContact=0;
center = phf->Basis*(psph->center-phf->origin);
irect[0].x = min(phf->size.x,max(0,float2int((center.x-psph->r)*phf->stepr.x-0.5f)));
irect[0].y = min(phf->size.y,max(0,float2int((center.y-psph->r)*phf->stepr.y-0.5f)));
irect[1].x = min(phf->size.x,max(0,float2int((center.x+psph->r)*phf->stepr.x+0.5f)));
irect[1].y = min(phf->size.y,max(0,float2int((center.y+psph->r)*phf->stepr.y+0.5f)));
for(ix=irect[0].x;ix<irect[1].x;ix++) for(iy=irect[0].y;iy<irect[1].y;iy++)
bContact |= isneg(center.z-psph->r-phf->getheight(ix,iy));
return bContact;
}
int sphere_heightfield_overlap_check(const sphere *psph, const heightfield *phf)
{
return heightfield_sphere_overlap_check(phf,psph);
}

42
CryPhysics/overlapchecks.h Normal file
View File

@@ -0,0 +1,42 @@
#ifndef overlapchecks_h
#define overlapchecks_h
typedef int (*overlap_check)(const primitive*,const primitive*);
int default_overlap_check(const primitive*, const primitive*);
int box_box_overlap_check(const box *box1, const box *box2);
int box_heightfield_overlap_check(const box *pbox, const heightfield *phf);
int heightfield_box_overlap_check(const heightfield *phf, const box *pbox);
int box_tri_overlap_check(const box *pbox, const triangle *ptri);
int tri_box_overlap_check(const triangle *ptri, const box *pbox);
int box_ray_overlap_check(const box *pbox, const ray *pray);
int ray_box_overlap_check(const ray *pray, const box *pbox);
int box_sphere_overlap_check(const box *pbox, const sphere *psph);
int sphere_box_overlap_check(const sphere *psph, const box *pbox);
int tri_sphere_overlap_check(const triangle *ptri, const sphere *psph);
int sphere_tri_overlap_check(const sphere *psph, const triangle *ptri);
int heightfield_sphere_overlap_check(const heightfield *phf, const sphere *psph);
int sphere_heightfield_overlap_check(const sphere *psph, const heightfield *phf);
int sphere_sphere_overlap_check(const sphere *psph1, const sphere *psph2);
quotientf tri_sphere_dist2(const triangle *ptri, const sphere *psph, int &bFace);
class COverlapChecker {
public:
COverlapChecker();
void Init() { iPrevCode = -1; }
int Check(int type1,int type2, primitive *prim1,primitive *prim2) {
return table[type1][type2](prim1,prim2);
}
int CheckExists(int type1,int type2) {
return table[type1][type2]!=default_overlap_check;
}
overlap_check table[NPRIMS][NPRIMS];
int iPrevCode;
float Basis21[9];
float Basis21abs[9];
};
extern COverlapChecker g_Overlapper;
#endif

695
CryPhysics/particleentity.cpp Normal file
View File

@@ -0,0 +1,695 @@
//////////////////////////////////////////////////////////////////////
//
// Particle Entity
//
// File: particleentity.cpp
// Description : CParticleEntity class implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "bvtree.h"
#include "geometry.h"
#include "singleboxtree.h"
#include "raybv.h"
#include "raygeom.h"
#include "intersectionchecks.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "geoman.h"
#include "physicalworld.h"
#include "particleentity.h"
CParticleEntity::CParticleEntity(CPhysicalWorld *pWorld) : CPhysicalEntity(pWorld)
{
m_gravity = pWorld->m_vars.gravity; m_mass=0.2f; m_dim=m_dimLying=0.05f; m_rdim=20.0f;
m_heading.Set(1,0,0); m_vel.zero(); m_wspin.zero();
m_waterGravity = m_gravity*0.8f;
m_kAirResistance = 0;
m_kWaterResistance = 0.5f;
m_accThrust = 0;
m_kAccLift = 0;
m_qspin.SetIdentity();
m_surface_idx = 0;
m_dirdown.Set(0,0,-1);
m_normal.Set(0,0,1);
m_flags = 0;
m_iSimClass = 4;
for(int i=0;i<sizeof(m_CollHistory)/sizeof(m_CollHistory[0]);i++)
m_CollHistory[i].age = 1E10;
m_iCollHistory = 0;
m_bSliding = 0;
m_slide_normal.Set(0,0,1);
m_minBounceVel = 1.5f;
m_pColliderToIgnore = 0;
m_iPierceability = sf_max_pierceable;
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -3;
m_timeSurplus = 0;
m_defpart.flags = 0;
m_defpart.id = 0;
m_defpart.pos.zero();
m_defpart.q.SetIdentity();
m_defpart.scale = 1.0f;
m_defpart.mass = 0;
m_defpart.minContactDist = 0;
m_waterPlane.origin.zero();
m_waterPlane.n.Set(0,0,1);
m_waterDensity = 1000.0f;
m_bForceAwake = -1;
m_timeForceAwake = 0;
m_sleepTime = 0;
}
CParticleEntity::~CParticleEntity()
{
if (m_nParts>0 && m_pWorld)
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[0].pPhysGeom);
m_nParts = 0;
}
int CParticleEntity::SetParams(pe_params *_params)
{
int res;
if (res = CPhysicalEntity::SetParams(_params)) {
if (_params->type==pe_params_flags::type_id) {
pe_params_particle pp;
SetParams(&pp);
}
return res;
}
if (_params->type==pe_params_particle::type_id) {
pe_params_particle *params = (pe_params_particle*)_params;
ENTITY_VALIDATE("CParticleEntity:SetParams(pe_params_particle)",params);
if (!is_unused(params->mass)) m_mass = params->mass;
if (!is_unused(params->size)) {
m_rdim = 1.0f/(m_dim = params->size*0.5f);
m_dimLying = params->size*0.5f;
}
if (!is_unused(params->thickness)) m_dimLying = params->thickness*0.5f;
if (!is_unused(params->kAirResistance)) m_kAirResistance = params->kAirResistance;
if (!is_unused(params->kWaterResistance)) m_kWaterResistance = params->kWaterResistance;
if (!is_unused(params->accThrust)) m_accThrust = params->accThrust;
if (!is_unused(params->accLift) && !is_unused(params->velocity))
m_kAccLift = params->velocity!=0 ? params->accLift/fabs_tpl(params->velocity):0;
if (!is_unused(params->heading)) m_heading = params->heading;
if (!is_unused(params->velocity)) m_vel = m_heading*params->velocity;
if (!is_unused(params->wspin)) m_wspin = params->wspin;
if (!is_unused(params->gravity)) {
m_gravity = params->gravity;
if (m_gravity.len2()>0)
(m_dirdown=m_gravity).normalize();
else m_dirdown.Set(0,0,-1);
}
if (!is_unused(params->waterGravity)) m_waterGravity = params->waterGravity;
if (!is_unused(params->normal)) m_normal = params->normal;
if (!is_unused(params->q0)) m_qspin = params->q0;
if (!is_unused(params->minBounceVel)) m_minBounceVel = params->minBounceVel;
if (!is_unused(params->surface_idx)) m_surface_idx = params->surface_idx;
if (!is_unused(params->flags)) m_flags = params->flags;
if (m_flags & particle_traceable) {
if (m_ig[0].x==-3)
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -2;
if (m_pos.len2()>0)
m_pWorld->RepositionEntity(this,1);
} else {
if (m_ig[0].x!=-3) {
m_pWorld->DetachEntityGridThunks(this);
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -3;
}
}
if (!is_unused(params->pColliderToIgnore))
m_pColliderToIgnore = (CPhysicalEntity*)params->pColliderToIgnore;
if (!is_unused(params->iPierceability))
m_iPierceability = params->iPierceability;
if (!(m_flags & particle_constant_orientation)) {
if (!(m_flags & particle_no_path_alignment)) {
vectorf dirbuf[3];
dirbuf[0] = m_dirdown^m_heading; dirbuf[1] = m_heading;
// if (dirbuf[0].len2()<0.01f) dirbuf[0] = m_heading.orthogonal();
if (dirbuf[0].len2()<0.01f) dirbuf[0].SetOrthogonal(m_heading);
dirbuf[0].normalize(); dirbuf[2] = dirbuf[0]^dirbuf[1];
m_qrot = quaternionf((matrix3x3CMf&)dirbuf[0])*m_qspin;
} else
m_qrot = m_qspin;
}
m_BBox[0] = m_pos-vectorf(m_dim,m_dim,m_dim);
m_BBox[1] = m_pos+vectorf(m_dim,m_dim,m_dim);
return 1;
}
if (_params->type==pe_params_buoyancy::type_id) {
pe_params_buoyancy *params = (pe_params_buoyancy*)_params;
if (!is_unused(params->waterDensity)) m_waterDensity = params->waterDensity;
if (!is_unused(params->waterPlane.n)) m_waterPlane.n = params->waterPlane.n;
if (!is_unused(params->waterPlane.origin)) m_waterPlane.origin = params->waterPlane.origin;
return 1;
}
return 0;
}
int CParticleEntity::GetParams(pe_params *_params)
{
if (CPhysicalEntity::GetParams(_params))
return 1;
if (_params->type==pe_params_particle::type_id) {
pe_params_particle *params = (pe_params_particle*)_params;
params->mass = m_mass;
params->size = m_dim*2.0f;
params->thickness = m_dimLying*2.0f;
params->heading = m_heading;
params->velocity = m_vel.len();
params->wspin = m_wspin;
params->gravity = m_gravity;
params->normal = m_normal;
params->kAirResistance = m_kAirResistance;
params->accThrust = m_accThrust;
params->accLift = params->velocity*m_kAccLift;
params->q0 = m_qspin;
params->surface_idx = m_surface_idx;
params->flags = m_flags;
params->pColliderToIgnore = m_pColliderToIgnore;
params->iPierceability = m_iPierceability;
return 1;
}
if (_params->type==pe_params_buoyancy::type_id) {
pe_params_buoyancy *params = (pe_params_buoyancy*)_params;
params->waterDensity = m_waterDensity;
params->waterDamping = 0;
params->waterPlane = m_waterPlane;
params->waterFlow.zero();
params->waterResistance = 0;
params->waterEmin = 0;
return 1;
}
return 0;
}
int CParticleEntity::GetStateSnapshot(CStream &stm, float time_back, int flags)
{
stm.WriteNumberInBits(SNAPSHOT_VERSION, 4);
if (m_pWorld->m_vars.bMultiplayer) {
if (!IsAwake()) {
if (m_sleepTime>5.0f)
stm.Write(false);
else {
stm.Write(true);
stm.Write(m_pos);
stm.Write(false);
}
} else {
stm.Write(true); stm.Write(m_pos);
stm.Write(true);
stm.Write(m_vel);
if (!m_bSliding) stm.Write(false);
else {
stm.Write(true);
stm.Write(asin_tpl(m_slide_normal.z));
stm.Write(atan2_tpl(m_slide_normal.y,m_slide_normal.x));
}
}
} else {
stm.Write(m_pos);
stm.Write(m_vel);
if (!m_bSliding) stm.Write(false);
else {
stm.Write(true);
stm.Write(asin_tpl(m_slide_normal.z));
stm.Write(atan2_tpl(m_slide_normal.y,m_slide_normal.x));
}
}
/*if (m_qspin.v.len2()<0.01*0.01) stm.Write(false);
else {
stm.Write(true);
//CHANGED_BY_IVO (NOTE: order of angles is flipped!!!!)
//float angles[3]; m_qspin.get_Euler_angles_xyz(angles[0],angles[1],angles[2]);
//EULER_IVO
//Vec3 TempAng; m_qspin.GetEulerAngles_XYZ( TempAng );
vectorf angles = Ang3::GetAnglesXYZ(matrix3x3f(m_qspin));
for(int i=0;i<3;i++) stm.Write((unsigned short)float2int((angles[i]+pi)*(65535.0/2/pi)));
}
if (m_wspin.len2()==0) stm.Write(false);
else {
stm.Write(true); stm.Write(m_wspin);
}*/
return 1;
}
int CParticleEntity::SetStateFromSnapshot(CStream &stm, int flags)
{
bool bnz; int ver=0;
stm.ReadNumberInBits(ver,4);
if (ver!=SNAPSHOT_VERSION)
return 0;
if (!(flags & ssf_no_update)) {
if (m_pWorld->m_vars.bMultiplayer) {
stm.Read(bnz); if (bnz) {
stm.Read(m_pos);
stm.Read(bnz); if (bnz) {
stm.Read(m_vel);
stm.Read(bnz); if (bnz) {
m_bSliding = 1;
float yaw,pitch;
stm.Read(pitch); stm.Read(yaw);
m_slide_normal(cos_tpl(yaw)*cos_tpl(pitch),sin_tpl(yaw)*cos_tpl(pitch),sin_tpl(pitch));
} else m_bSliding = 0;
}
}
} else {
stm.Read(m_pos);
stm.Read(m_vel);
stm.Read(bnz); if (bnz) {
m_bSliding = 1;
float yaw,pitch;
stm.Read(pitch); stm.Read(yaw);
m_slide_normal(cos_tpl(yaw)*cos_tpl(pitch),sin_tpl(yaw)*cos_tpl(pitch),sin_tpl(pitch));
} else m_bSliding = 0;
}
if (m_bForceAwake!=0)
m_bForceAwake = -1;
} else {
if (m_pWorld->m_vars.bMultiplayer) {
stm.Read(bnz); if (bnz) {
stm.Seek(stm.GetReadPos()+sizeof(vectorf)*8);
stm.Read(bnz); if (bnz) {
stm.Seek(stm.GetReadPos()+sizeof(vectorf)*8);
stm.Read(bnz); if (bnz)
stm.Seek(stm.GetReadPos()+2*sizeof(float)*8);
}
}
} else {
stm.Seek(stm.GetReadPos()+2*sizeof(vectorf)*8);
stm.Read(bnz); if (bnz)
stm.Seek(stm.GetReadPos()+2*sizeof(float)*8);
}
}
/*stm.Read(bnz); if (bnz) {
unsigned short tmp; int i; vectorf axis(zero);
for(i=0,m_qspin.SetIdentity(); i<3; i++) {
axis[i] = 1;
//stm.Read(tmp); m_qspin*=quaternionf(tmp*(2*pi/65535.0)-pi, axis);
stm.Read(tmp); m_qspin = GetRotationAA((float)(tmp*(2*pi/65535.0)-pi), axis)*m_qspin;
axis[i] = 0;
}
}
stm.Read(bnz); if (bnz)
stm.Read(m_wspin);
else m_wspin.zero();*/
return 1;
}
int CParticleEntity::GetStatus(pe_status* _status)
{
if (CPhysicalEntity::GetStatus(_status))
return 1;
if (_status->type==pe_status_collisions::type_id) {
pe_status_collisions *status = (pe_status_collisions*)_status;
int i,n,nmax = min(status->len, sizeof(m_CollHistory)/sizeof(m_CollHistory[0]));
for(i=m_iCollHistory,n=0; n<nmax && m_CollHistory[i].age <= status->age; i=i-1&sizeof(m_CollHistory)/sizeof(m_CollHistory[0])-1,n++)
status->pHistory[n] = m_CollHistory[i];
if (status->bClearHistory) for(i=0;i<sizeof(m_CollHistory)/sizeof(m_CollHistory[0]);i++)
m_CollHistory[i].age = 1E10;
return status->len = n;
}
if (_status->type==pe_status_dynamics::type_id) {
pe_status_dynamics *status = (pe_status_dynamics*)_status;
vectorf gravity; float kAirResistance;
if (m_waterDensity>0 && (m_pos-m_waterPlane.origin)*m_waterPlane.n<0) {
gravity = m_waterGravity; kAirResistance = m_kWaterResistance;
} else {
gravity = m_gravity; kAirResistance = m_kAirResistance;
}
status->v = m_vel;
status->a = gravity+m_heading*m_accThrust-m_vel*kAirResistance+(m_heading^(m_heading^m_dirdown)).normalize()*m_kAccLift*m_vel.len();
status->w = m_wspin;
status->centerOfMass = m_pos;
return 1;
}
return 0;
}
int CParticleEntity::Action(pe_action* _action)
{
int res;
if (res = CPhysicalEntity::Action(_action))
return res;
if (_action->type==pe_action_impulse::type_id) {
pe_action_impulse *action = (pe_action_impulse*)_action;
vectorf P=action->impulse, L(zero);
if (!is_unused(action->momentum))
L = action->momentum;
else if (!is_unused(action->point))
L = action->point-m_pos^action->impulse;
m_vel += P/m_mass;
if (!(m_flags & particle_constant_orientation))
m_wspin += L/(0.4f*m_mass*m_dim);
return 1;
}
if (_action->type==pe_action_reset::type_id) {
m_vel.zero(); m_wspin.zero();
return 1;
}
return 0;
}
int CParticleEntity::Awake(int bAwake,int iSource)
{
if (bAwake)
m_bForceAwake = 1;
else {
m_bForceAwake = 0;
m_vel.zero();
}
return m_iSimClass;
}
int CParticleEntity::IsAwake(int ipart)
{
vectorf gravity = m_waterDensity>0 && (m_pos-m_waterPlane.origin)*m_waterPlane.n<0 ? m_waterGravity : m_gravity;
return m_vel.len2()<sqr(500.0f) && m_pos.z>-1000.0f && m_bForceAwake!=0 &&
(m_bForceAwake==1 || (m_vel.len2()>sqr(m_pWorld->m_vars.minSeparationSpeed) ||
m_CollHistory[m_iCollHistory].age>1E9 && !m_bSliding && gravity.len2()>0));
}
void CParticleEntity::StartStep(float time_interval)
{
m_timeStepPerformed = 0;
m_timeStepFull = time_interval;
}
float CParticleEntity::GetMaxTimeStep(float time_interval)
{
if (m_timeStepPerformed > m_timeStepFull-0.001f)
return time_interval;
return min_safe(m_timeStepFull-m_timeStepPerformed,time_interval);
}
int CParticleEntity::Step(float time_interval)
{
ray_hit hits[8];
pe_action_impulse ai;
pe_action_register_coll_event arce;
pe_status_dynamics sd;
vectorf pos0,vel0,heading0,vtang,vel_next;
float vn,vtang_len,rvtang_len,e,k,friction,vn0;
int i,nhits,bHit,flags;
vectorf gravity; float kAirResistance;
if (m_waterDensity>0 && (m_pos-m_waterPlane.origin)*m_waterPlane.n<0) {
gravity = m_waterGravity; kAirResistance = m_kWaterResistance;
} else {
gravity = m_gravity; kAirResistance = m_kAirResistance;
}
m_timeStepPerformed += time_interval;
if (m_pColliderToIgnore && m_pColliderToIgnore->m_iSimClass==7)
m_pColliderToIgnore = 0;
for(i=0;i<sizeof(m_CollHistory)/sizeof(m_CollHistory[0]);i++)
m_CollHistory[i].age += time_interval;
if (IsAwake()) {
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
PHYS_ENTITY_PROFILER
pos0 = m_pos; vel0 = m_vel;
if (!m_bSliding)
vel0 += gravity*time_interval*0.5f;
flags = m_flags;
m_pos += vel0*time_interval;
if (m_bSliding) {
if (m_pWorld->RayWorldIntersection(m_pos,m_slide_normal*(m_dim*-1.1f), ent_all,
m_iPierceability|(geom_colltype0|geom_colltype_ray)<<rwi_colltype_bit, hits,1, m_pColliderToIgnore,this))
{
m_slide_normal = hits[0].n;
m_pos = hits[0].pt+m_slide_normal*m_dimLying;
//if (m_dimLying!=m_dim)
// pos0 = m_pos;
if (m_flags&particle_no_roll || m_slide_normal.z<0.5f) { // always slide if the slope is steep enough
friction = max(0.0f, (m_pWorld->m_FrictionTable[m_surface_idx&NSURFACETYPES-1]+
m_pWorld->m_FrictionTable[hits[0].surface_idx&NSURFACETYPES-1])*0.5f);
if (m_slide_normal.z<0.5f)
friction = min(1.0f,friction); // limit sliding friction on slopes
vn = hits[0].n*vel0; vtang = vel0-hits[0].n*vn; vtang_len = vtang.len(); rvtang_len = vtang_len>1e-4 ? 1.0f/vtang_len:0;
if (((CPhysicalEntity*)hits[0].pCollider)->m_iSimClass>1) {
hits[0].pCollider->GetStatus(&sd);
vn0 = sd.v*hits[0].n;
} else vn0 = 0;
m_vel = vel0 = hits[0].n*max(vn0,vn) +
vtang*(max(0.0f,vtang_len-max(0.0f,-(vn+(m_gravity*hits[0].n)*time_interval))*friction)*rvtang_len);
m_wspin.zero(); m_qspin.SetIdentity();
if (!(m_flags & particle_constant_orientation))
(m_qrot = GetRotationV0V1(m_qrot*m_normal,hits[0].n)*m_qrot).Normalize();
//m_qrot.SetRotationV0V1(m_normal,hits[0].n); //m_qrot = quaternionf(m_normal,hits[0].n);
flags |= particle_constant_orientation;
} else {
friction = m_pWorld->m_FrictionTable[m_surface_idx&NSURFACETYPES-1];
vel0 = m_vel = (m_vel-m_slide_normal*(m_vel*m_slide_normal))*max(0.0f,1.0f-time_interval*friction);
m_wspin = m_slide_normal^m_vel*m_rdim;
}
if (m_flags & particle_single_contact)
gravity.zero();
else
gravity -= m_slide_normal*(m_slide_normal*gravity);
m_bForceAwake = ((CPhysicalEntity*)hits[0].pCollider)->m_iSimClass<=2 || m_timeForceAwake>40.0f ? -1:1;
} else {
m_bSliding = 0;
if (!(m_flags & particle_constant_orientation))
m_wspin = (m_heading^m_gravity).normalized()*((m_gravity*m_dirdown)*0.5f*m_rdim);
}
}
m_vel += (gravity + m_heading*m_accThrust - m_vel*kAirResistance +
(m_heading^(m_heading^m_dirdown)).normalize()*(m_kAccLift*m_vel.len()))*time_interval;
(m_heading=m_vel).normalize();
if (!(flags & particle_constant_orientation)) {
if (!(m_flags & particle_no_spin)) {
if (m_wspin.len2()*sqr(time_interval)<0.1f*0.1f) {
m_qspin += quaternionf(0,m_wspin*0.5f)*m_qspin*time_interval;
m_qspin.Normalize();
} else {
float wlen = m_wspin.len();
//m_qspin = quaternionf(wlen*time_interval,m_wspin/wlen)*m_qspin;
m_qspin = GetRotationAA(wlen*time_interval,m_wspin/wlen)*m_qspin;
}
} else
m_wspin.zero();
if (!(m_flags & particle_no_path_alignment)) {
vectorf dirbuf[3];
dirbuf[0] = m_dirdown^m_heading; dirbuf[1] = m_heading;
if (dirbuf[0].len2()<0.01f) dirbuf[0].SetOrthogonal(m_heading);
dirbuf[0].normalize(); dirbuf[2] = dirbuf[0]^dirbuf[1];
m_qrot = m_qspin*quaternionf((matrix3x3CMf&)dirbuf[0]);
} else
m_qrot = m_qspin;
}
bHit = 0;
if (m_pWorld->m_bWorldStep==2) {
CPhysicalEntity **pentlist;
pe_status_pos sp; sp.timeBack = 1;//time_interval;
vectorf posFixed;
int nents = m_pWorld->GetEntitiesAround(pos0-vectorf(m_dim,m_dim,m_dim),pos0+vectorf(m_dim,m_dim,m_dim),pentlist,ent_rigid);
hits[0].dist = hits[1].dist = 1E10f;
for(i=0;i<nents;i++) {
pentlist[i]->GetStatus(&sp);
posFixed = (pentlist[i]->m_qrot*!sp.q)*(pos0-sp.pos)+pentlist[i]->m_pos;
if (bHit = (m_pWorld->RayTraceEntity(pentlist[i],posFixed,pos0-posFixed+(pos0-posFixed).normalized()*(m_dim*0.97f),hits+1) &&
pentlist[i]->m_parts[hits[1].ipart].flags&geom_colltype0 && hits[1].dist<hits[0].dist))
hits[0] = hits[1];
}
if (bHit) { // ignore collisions with moving bodies if they push us through statics
heading0 = (hits[0].pt-pos0).normalized();
bHit ^= m_pWorld->RayWorldIntersection(pos0,hits[0].pt-pos0+heading0*m_dim,ent_terrain|ent_static,
m_iPierceability|(geom_colltype0|geom_colltype_ray)<<rwi_colltype_bit|rwi_ignore_back_faces, hits,1, m_pColliderToIgnore,this);
}
if (nhits = bHit)
pos0 = posFixed;
}
if (!bHit) {
heading0 = (m_pos-pos0).normalized();
nhits = m_pWorld->RayWorldIntersection(pos0,m_pos-pos0+heading0*m_dim, ent_all,
m_iPierceability|(geom_colltype0|geom_colltype_ray)<<rwi_colltype_bit|rwi_ignore_back_faces, hits,8, m_pColliderToIgnore,this);
bHit = isneg(-nhits) & (isneg(hits[0].dist+0.5f)^1);
for(i=nhits-1+(bHit^1);i>=(bHit^1);i--) { // register all hits in history
m_iCollHistory = m_iCollHistory+1 & sizeof(m_CollHistory)/sizeof(m_CollHistory[0])-1;
RigidBody *pbody = ((CPhysicalEntity*)hits[i].pCollider)->GetRigidBody(hits[i].ipart);
m_CollHistory[m_iCollHistory].pt = hits[i].pt-heading0*m_dim; // store not contact, but position of particle center at the time of contact
m_CollHistory[m_iCollHistory].n = hits[i].n; // it's better for explosions to be created at some distance from the wall
m_CollHistory[m_iCollHistory].v[0] = vel0;
m_CollHistory[m_iCollHistory].v[1] = pbody->v+(pbody->w^m_CollHistory[m_iCollHistory].pt-pbody->pos);
m_CollHistory[m_iCollHistory].mass[0] = m_mass;
m_CollHistory[m_iCollHistory].mass[1] = pbody->M;
m_CollHistory[m_iCollHistory].idCollider = m_pWorld->GetPhysicalEntityId(hits[i].pCollider);
m_CollHistory[m_iCollHistory].partid[0] = 0;
m_CollHistory[m_iCollHistory].partid[1] = hits[i].partid;
m_CollHistory[m_iCollHistory].idmat[0] = m_surface_idx;
m_CollHistory[m_iCollHistory].idmat[1] = hits[i].surface_idx;
m_CollHistory[m_iCollHistory].age = 0;
}
}
if (bHit) {
e = max(min((m_pWorld->m_BouncinessTable[m_surface_idx] + m_pWorld->m_BouncinessTable[hits[0].surface_idx])*0.5f, 1.0f), 0.0f);
float minv = ((CPhysicalEntity*)hits[0].pCollider)->GetMassInv();
k = m_mass*minv/(1+m_mass*minv);
if (((CPhysicalEntity*)hits[0].pCollider)->m_iSimClass>0) {
RigidBody *pbody = ((CPhysicalEntity*)hits[0].pCollider)->GetRigidBody(hits[0].ipart);
vectorf vrel = vel0-pbody->v-(pbody->w^hits[0].pt-pbody->pos);
if (vel0.len2()>sqr(m_minBounceVel)) {
ai.point = hits[0].pt;
ai.impulse = vrel*(m_mass*pbody->M/(m_mass+pbody->M)*(1+e));
ai.ipart = hits[0].ipart;
hits[0].pCollider->Action(&ai);
arce.pt = hits[0].pt;
arce.n = hits[0].n;
arce.v = m_vel;
arce.collMass = m_mass;
arce.pCollider = this;
arce.partid[0] = hits[0].partid;
arce.partid[1] = 0;
arce.idmat[0] = hits[0].surface_idx;
arce.idmat[1] = m_surface_idx;
hits[0].pCollider->Action(&arce);
}
}
vn = hits[0].n*vel0; vtang = vel0-hits[0].n*vn;
if (vn>-m_minBounceVel || m_dimLying<m_dim*0.3f)
e = 0;
m_vel = vel0-hits[0].n*(vn*(1-k)*(1+e));
if (sqr(m_dim)<(hits[0].pt-pos0).len2())
m_pos = hits[0].pt - heading0*m_dim;
else
m_pos = pos0;
if (m_vel.len2()<sqr(m_pWorld->m_vars.minSeparationSpeed) || m_flags & particle_single_contact) {
m_vel.zero(); m_wspin.zero(); m_qspin.SetIdentity();
if (m_dim!=m_dimLying)
m_pos = hits[0].pt+hits[0].n*m_dimLying;
m_bSliding = 1; m_slide_normal = hits[0].n;
if (!(flags & particle_constant_orientation))
m_qrot = GetRotationV0V1(m_qrot*m_normal,hits[0].n)*m_qrot;
//m_qrot = quaternionf(m_normal,hits[0].n);
} else {
vel_next = m_vel + (gravity + m_heading*m_accThrust - m_vel*kAirResistance +
(m_heading^(m_heading^m_dirdown)).normalize()*(m_kAccLift*m_vel.len()))*time_interval;
if (vel_next*hits[0].n<m_pWorld->m_vars.minSeparationSpeed) {
if (m_dim!=m_dimLying)
m_pos = hits[0].pt+hits[0].n*m_dimLying;
m_bSliding = 1; m_slide_normal = hits[0].n;
if (m_flags & particle_no_roll && !(m_flags & particle_constant_orientation))
m_qrot = GetRotationV0V1(m_qrot*m_normal,hits[0].n)*m_qrot;
//m_qrot = quaternionf(m_normal,hits[0].n);
}
if (m_flags & particle_no_roll) m_wspin.zero();
else m_wspin = (hits[0].n^vtang)*m_rdim;
}
if (((CPhysicalEntity*)hits[0].pCollider)->m_iSimClass>1) {
hits[0].pCollider->GetStatus(&sd);
m_vel += hits[0].n*(sd.v*hits[0].n);
}
m_bForceAwake = ((CPhysicalEntity*)hits[0].pCollider)->m_iSimClass<=2 || m_timeForceAwake>40.0f ? -1:1;
}
i = (m_bForceAwake ^ m_bForceAwake>>31) & 1;
m_timeForceAwake = m_timeForceAwake*i + time_interval*i;
m_sleepTime = 0;
m_BBox[0] = m_pos-vectorf(m_dim,m_dim,m_dim);
m_BBox[1] = m_pos+vectorf(m_dim,m_dim,m_dim);
if (m_flags & particle_traceable)
m_pWorld->RepositionEntity(this, 1);
} else
m_sleepTime += time_interval;
return 1;
}
int CParticleEntity::RayTrace(CRayGeom *pRay,geom_contact *&pcontacts)
{
static geom_contact g_ParticleContact;
prim_inters inters;
box abox;
abox.Basis.SetRow(2,m_qrot*m_normal);
abox.Basis.SetRow(0,m_qrot*GetOrthogonal(m_normal).normalized());
abox.Basis.SetRow(1,abox.Basis.GetRow(2)^abox.Basis.GetRow(0));
abox.size(m_dim,m_dim,m_dimLying);
abox.center = m_pos;
if (box_ray_intersection(&abox,&pRay->m_ray,&inters)) {
pcontacts = &g_ParticleContact;
pcontacts->pt = inters.pt[0];
pcontacts->t = (inters.pt[0]-pRay->m_ray.origin)*pRay->m_dirn;
pcontacts->id[0] = m_surface_idx;
pcontacts->iNode[0] = 0;
pcontacts->n = inters.n;
return 1;
}
return 0;
}
void CParticleEntity::DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags)
{
CPhysicalEntity::DrawHelperInformation(DrawLineFunc,flags);
if (flags & pe_helper_geometry) {
box abox;
abox.Basis.SetRow(2,m_qrot*m_normal);
abox.Basis.SetRow(0,m_qrot*GetOrthogonal(m_normal));
abox.Basis.SetRow(1,abox.Basis.GetRow(2)^abox.Basis.GetRow(0));
abox.center = m_pos;
int i,j;
vectorf pt[8];
for(i=0;i<8;i++)
pt[i] = vectorf(m_dim*((i<<1&2)-1),m_dim*((i&2)-1),m_dimLying*((i>>1&2)-1))*abox.Basis+abox.center;
for(i=0;i<8;i++) for(j=0;j<3;j++) if (i&1<<j)
DrawLineFunc(pt[i],pt[i^1<<j]);
}
}
void CParticleEntity::GetMemoryStatistics(ICrySizer *pSizer)
{
CPhysicalEntity::GetMemoryStatistics(pSizer);
if (GetType()==PE_PARTICLE)
pSizer->AddObject(this, sizeof(CParticleEntity));
}

66
CryPhysics/particleentity.h Normal file
View File

@@ -0,0 +1,66 @@
//////////////////////////////////////////////////////////////////////
//
// Particle Entity header
//
// File: particleentity.h
// Description : CParticleEntity class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef particleentity_h
#define particleentity_h
#pragma once
class CParticleEntity : public CPhysicalEntity {
public:
CParticleEntity(CPhysicalWorld *pworld);
virtual ~CParticleEntity();
virtual pe_type GetType() { return PE_PARTICLE; }
virtual int SetParams(pe_params*);
virtual int GetParams(pe_params*);
virtual int GetStatus(pe_status*);
virtual int Action(pe_action*);
virtual int Awake(int bAwake=1,int iSource=0);
virtual int IsAwake(int ipart=-1);
virtual int RayTrace(CRayGeom *pRay, geom_contact *&pcontacts);
virtual void ComputeBBox() { vectorf sz(m_dim,m_dim,m_dim); m_BBox[0]=m_pos-sz; m_BBox[1]=m_pos+sz; }
enum snapver { SNAPSHOT_VERSION = 3 };
virtual int GetStateSnapshot(class CStream &stm,float time_back=0,int flags=0);
virtual int SetStateFromSnapshot(class CStream &stm, int flags);
virtual void StartStep(float time_interval);
virtual float GetMaxTimeStep(float time_interval);
virtual int Step(float time_interval);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
float m_mass,m_dim,m_rdim,m_dimLying;
float m_kAirResistance,m_kWaterResistance, m_accThrust, m_kAccLift;
vectorf m_gravity,m_waterGravity,m_dirdown,m_normal;
vectorf m_heading,m_vel,m_wspin;
quaternionf m_qspin;
float m_minBounceVel;
int m_surface_idx;
CPhysicalEntity *m_pColliderToIgnore;
int m_iPierceability;
int m_bSliding;
vectorf m_slide_normal;
float m_timeSurplus;
plane m_waterPlane;
float m_waterDensity;
int m_bForceAwake;
float m_timeStepPerformed,m_timeStepFull;
float m_timeForceAwake;
float m_sleepTime;
coll_history_item m_CollHistory[4];
int m_iCollHistory;
};
#endif

620
CryPhysics/physicalentity.cpp Normal file
View File

@@ -0,0 +1,620 @@
//////////////////////////////////////////////////////////////////////
//
// Physical Entity
//
// File: physicalentity.cpp
// Description : PhysicalEntity class implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "bvtree.h"
#include "geometry.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "geoman.h"
#include "physicalworld.h"
RigidBody g_StaticRigidBody;
CPhysicalEntity g_StaticPhysicalEntity(0);
CPhysicalEntity::CPhysicalEntity(CPhysicalWorld *pworld)
{
m_pos.zero(); m_qrot.SetIdentity();
m_BBox[0].zero(); m_BBox[1].zero();
m_iSimClass = 0; m_iPrevSimClass = -1;
m_nGridThunks = m_nGridThunksAlloc = 0;
m_pGridThunks = 0;
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -2;
m_prev = m_next = 0; m_bProcessed = 0;
m_nRefCount = 0;//1; 0 means that initially no other physical entity references this one
m_nParts = 0;
m_nPartsAlloc = 1;
m_parts = &m_defpart;
m_iLastIdx = 0;
m_pWorld = pworld;
m_pOuterEntity = 0;
m_pBoundingGeometry = 0;
m_bProcessed_aux = 0;
m_nColliders = m_nCollidersAlloc = 0;
m_pColliders = 0;
m_iGroup = -1;
m_bMoved = 0;
m_id = -1;
m_pForeignData = 0;
m_iForeignData = m_iForeignFlags = 0;
m_timeIdle = m_maxTimeIdle = 0;
m_bPermanent = 1;
m_pEntBuddy = 0;
m_flags = pef_pushable_by_players|pef_traceable;
}
CPhysicalEntity::~CPhysicalEntity()
{
for(int i=0;i<m_nParts;i++) if (m_parts[i].pPhysGeom) {
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeom);
if (m_parts[i].pPhysGeomProxy!=m_parts[i].pPhysGeom)
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeomProxy);
}
if (m_pGridThunks) delete[] m_pGridThunks;
if (m_parts!=&m_defpart) delete[] m_parts;
if (m_pColliders) delete[] m_pColliders;
}
void CPhysicalEntity::ComputeBBox()
{
if (m_nParts==0)
m_BBox[0]=m_BBox[1]=m_pos;
else {
IGeometry *pGeom[3];
matrix3x3f R;
int i,j; box abox; vectorf sz,pos;
m_BBox[0]=vectorf(MAX); m_BBox[1]=vectorf(MIN);
for(i=0;i<m_nParts;i++) {
pGeom[0] = m_parts[i].pPhysGeomProxy->pGeom;
pGeom[1]=pGeom[2] = m_parts[i].pPhysGeom->pGeom;
j=0; do {
pGeom[j]->GetBBox(&abox);
//(m_qrot*m_parts[i].q).getmatrix(R); //Q2M_IVO
R = matrix3x3f(m_qrot*m_parts[i].q);
abox.Basis *= R.T();
sz = (abox.size*abox.Basis.Fabs())*m_parts[i].scale;
pos = m_pos+m_qrot*(m_parts[i].pos+m_parts[i].q*abox.center*m_parts[i].scale);
m_parts[i].BBox[0] = pos-sz;
m_parts[i].BBox[1] = pos+sz;
m_BBox[0].x = min_safe(m_BBox[0].x, m_parts[i].BBox[0].x);
m_BBox[0].y = min_safe(m_BBox[0].y, m_parts[i].BBox[0].y);
m_BBox[0].z = min_safe(m_BBox[0].z, m_parts[i].BBox[0].z);
m_BBox[1].x = max_safe(m_BBox[1].x, m_parts[i].BBox[1].x);
m_BBox[1].y = max_safe(m_BBox[1].y, m_parts[i].BBox[1].y);
m_BBox[1].z = max_safe(m_BBox[1].z, m_parts[i].BBox[1].z);
j++;
} while(pGeom[j]!=pGeom[j-1]);
}
}
if (m_pEntBuddy) {
m_pEntBuddy->m_BBox[0] = m_BBox[0];
m_pEntBuddy->m_BBox[1] = m_BBox[1];
}
}
int CPhysicalEntity::SetParams(pe_params *_params)
{
if (_params->type==pe_params_pos::type_id) {
pe_params_pos *params = (pe_params_pos*)_params;
get_xqs_from_matrices(params->pMtx3x3,params->pMtx3x3T,params->pMtx4x4,params->pMtx4x4T, params->pos,params->q,params->scale);
ENTITY_VALIDATE("CPhysicalEntity:SetParams(pe_params_pos)",params);
int bPosChanged = 0;
if (!is_unused(params->pos)) { m_pos = params->pos; bPosChanged=1; }
if (!is_unused(params->q)) { m_qrot = params->q; bPosChanged=1; }
if (!is_unused(params->iSimClass) && m_iSimClass>=0 && m_iSimClass<7) {
m_iSimClass = params->iSimClass;
m_pWorld->RepositionEntity(this,2);
}
if (!is_unused(params->scale)) {
for(int i=0;i<m_nParts;i++) {
m_parts[i].pos *= params->scale/m_parts[i].scale;
m_parts[i].scale = params->scale;
} bPosChanged=1;
}
if (params->bRecalcBounds && bPosChanged) {
CPhysicalEntity **pentlist;
// make triggers aware of the object's movements
if (!(m_flags & pef_never_affect_triggers))
m_pWorld->GetEntitiesAround(m_BBox[0],m_BBox[1],pentlist,ent_triggers,this);
ComputeBBox();
m_pWorld->RepositionEntity(this);
if (!(m_flags & pef_never_affect_triggers))
m_pWorld->GetEntitiesAround(m_BBox[0],m_BBox[1],pentlist,ent_triggers,this);
}
return 1;
}
if (_params->type==pe_params_bbox::type_id) {
pe_params_bbox *params = (pe_params_bbox*)_params;
ENTITY_VALIDATE("CPhysicalEntity::SetParams(pe_params_bbox)",params);
if (!is_unused(params->BBox[0])) m_BBox[0] = params->BBox[0];
if (!is_unused(params->BBox[1])) m_BBox[1] = params->BBox[1];
if (m_pEntBuddy) {
m_pEntBuddy->m_BBox[0] = m_BBox[0];
m_pEntBuddy->m_BBox[1] = m_BBox[1];
}
m_pWorld->RepositionEntity(this,1);
return 1;
}
if (_params->type==pe_params_part::type_id) {
pe_params_part *params = (pe_params_part*)_params;
int i = params->ipart;
if (i<0)
for(i=0;i<m_nParts && m_parts[i].id!=params->partid;i++);
if (i>=m_nParts)
return 0;
get_xqs_from_matrices(params->pMtx3x3,params->pMtx3x3T,params->pMtx4x4,params->pMtx4x4T, params->pos,params->q,params->scale);
ENTITY_VALIDATE("CPhysicalEntity:SetParams(pe_params_part)",params);
if (!is_unused(params->pos.x)) m_parts[i].pos = params->pos;
if (!is_unused(params->q)) m_parts[i].q = params->q;
if (!is_unused(params->scale)) m_parts[i].scale = params->scale;
if (!is_unused(params->mass)) m_parts[i].mass = params->mass;
if (!is_unused(params->density)) m_parts[i].mass = m_parts[i].pPhysGeom->V*cube(m_parts[i].scale)*params->density;
if (!is_unused(params->pPhysGeom) && params->pPhysGeom!=m_parts[i].pPhysGeom) {
if (m_parts[i].pPhysGeom==m_parts[i].pPhysGeomProxy)
m_parts[i].pPhysGeomProxy = params->pPhysGeom;
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeom);
m_parts[i].pPhysGeom = params->pPhysGeom;
m_pWorld->GetGeomManager()->AddRefGeometry(params->pPhysGeom);
}
if (!is_unused(params->pPhysGeomProxy) && params->pPhysGeomProxy!=m_parts[i].pPhysGeomProxy) {
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeomProxy);
m_parts[i].pPhysGeomProxy = params->pPhysGeomProxy;
m_pWorld->GetGeomManager()->AddRefGeometry(params->pPhysGeomProxy);
}
m_parts[i].flags = m_parts[i].flags & params->flagsAND | params->flagsOR;
m_parts[i].flagsCollider = m_parts[i].flagsCollider & params->flagsColliderAND | params->flagsColliderOR;
if (params->bRecalcBBox)
ComputeBBox();
return i+1;
}
if (_params->type==pe_params_outer_entity::type_id) {
m_pOuterEntity = (CPhysicalEntity*)((pe_params_outer_entity*)_params)->pOuterEntity;
m_pBoundingGeometry = (CGeometry*)((pe_params_outer_entity*)_params)->pBoundingGeometry;
return 1;
}
if (_params->type==pe_params_foreign_data::type_id) {
pe_params_foreign_data *params = (pe_params_foreign_data*)_params;
if (!is_unused(params->iForeignData)) m_iForeignData = params->iForeignData;
if (!is_unused(params->pForeignData)) m_pForeignData = params->pForeignData;
if (!is_unused(params->iForeignFlags)) m_iForeignFlags = params->iForeignFlags;
if (m_pEntBuddy) {
m_pEntBuddy->m_pForeignData = m_pForeignData;
m_pEntBuddy->m_iForeignData = m_iForeignData;
m_pEntBuddy->m_iForeignFlags = m_iForeignFlags;
}
return 1;
}
if (_params->type==pe_params_flags::type_id) {
pe_params_flags *params = (pe_params_flags*)_params;
int flags = m_flags;
if (!is_unused(params->flags)) m_flags = params->flags;
if (!is_unused(params->flagsAND)) m_flags &= params->flagsAND;
if (!is_unused(params->flagsOR)) m_flags |= params->flagsOR;
if (m_flags&pef_traceable && m_ig[0].x==-3) {
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -2;
if (m_pos.len2()>0)
m_pWorld->RepositionEntity(this,1);
}
if (!(m_flags&pef_traceable) && m_ig[0].x!=-3) {
m_pWorld->DetachEntityGridThunks(this);
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -3;
}
return 1;
}
return 0;
}
int CPhysicalEntity::GetParams(pe_params *_params)
{
if (_params->type==pe_params_bbox::type_id) {
pe_params_bbox *params = (pe_params_bbox*)_params;
params->BBox[0] = m_BBox[0];
params->BBox[1] = m_BBox[1];
return 1;
}
if (_params->type==pe_params_outer_entity::type_id) {
((pe_params_outer_entity*)_params)->pOuterEntity = m_pOuterEntity;
((pe_params_outer_entity*)_params)->pBoundingGeometry = m_pBoundingGeometry;
return 1;
}
if (_params->type==pe_params_foreign_data::type_id) {
pe_params_foreign_data *params = (pe_params_foreign_data*)_params;
params->iForeignData = m_iForeignData;
params->pForeignData = m_pForeignData;
params->iForeignFlags = m_iForeignFlags;
return 1;
}
if (_params->type==pe_params_part::type_id) {
pe_params_part *params = (pe_params_part*)_params;
int i;
if (params->ipart==-1 && params->partid>=0) {
for(i=0;i<m_nParts && m_parts[i].id!=params->partid;i++);
if (i==m_nParts) return 0;
} else
i = params->ipart;
params->partid = m_parts[params->ipart = i].id;
params->pos = m_parts[i].pos;
params->q = m_parts[i].q;
params->scale = m_parts[i].scale;
if (params->pMtx3x3) //resq.getmatrix((matrix3x3f&)*status->pMtx3x3) *= resscale; //Q2M_IVO
(matrix3x3f&)*params->pMtx3x3 = (matrix3x3f(m_parts[i].q)*m_parts[i].scale);
if (params->pMtx3x3T) //resq.getmatrix((matrix3x3Tf&)*params->pMtx3x3T) *= resscale; //Q2M_IVO
(matrix3x3Tf&)*params->pMtx3x3T=(matrix3x3f(m_parts[i].q)*m_parts[i].scale);
if (params->pMtx4x4) //(resq.getmatrix((matrix3x3in4x4f&)*params->pMtx4x4)*=resscale).SetColumn(3,respos); //Q2M_IVO
((matrix3x3in4x4f&)*params->pMtx4x4 = (matrix3x3f(m_parts[i].q)*m_parts[i].scale)).SetColumn(3,m_parts[i].pos);
if (params->pMtx4x4T) //(resq.getmatrix((matrix3x3in4x4Tf&)*params->pMtx4x4T)*=resscale).SetColumn(3,respos); //Q2M_IVO
((matrix3x3in4x4Tf&)*params->pMtx4x4T = (matrix3x3f(m_parts[i].q)*m_parts[i].scale)).SetColumn(3,m_parts[i].pos);
params->flagsOR=params->flagsAND = m_parts[i].flags;
params->flagsColliderOR=params->flagsColliderAND = m_parts[i].flagsCollider;
params->mass = m_parts[i].mass;
params->density = m_parts[i].pPhysGeomProxy->V>0 ?
m_parts[i].mass/(m_parts[i].pPhysGeomProxy->V*cube(m_parts[i].scale)) : 0;
params->pPhysGeom = m_parts[i].pPhysGeom;
params->pPhysGeomProxy = m_parts[i].pPhysGeomProxy;
}
if (_params->type==pe_params_flags::type_id) {
((pe_params_flags*)_params)->flags = m_flags;
return 1;
}
return 0;
}
int CPhysicalEntity::GetStatus(pe_status *_status)
{
if (_status->type==pe_status_pos::type_id) {
pe_status_pos *status = (pe_status_pos*)_status;
vectorf respos;
quaternionf resq;
float resscale;
int i;
if (status->ipart==-1 && status->partid>=0) {
for(i=0;i<m_nParts && m_parts[i].id!=status->partid;i++);
if (i==m_nParts) return 0;
} else
i = status->ipart;
if (i==-1) {
respos=m_pos; resq=m_qrot; resscale=1.0f;
for(i=0,status->flagsOR=0,status->flagsAND=-1;i<m_nParts;i++)
status->flagsOR|=m_parts[i].flags, status->flagsAND&=m_parts[i].flags;
if (m_nParts>0) {
status->pGeom = m_parts[0].pPhysGeom->pGeom;
status->pGeomProxy = m_parts[0].pPhysGeomProxy->pGeom;
} else
status->pGeom = status->pGeomProxy = 0;
} else {
if (status->flags & status_local) {
respos = m_parts[i].pos; resq = m_parts[i].q;
} else {
respos = m_pos+m_qrot*m_parts[i].pos;
resq = m_qrot*m_parts[i].q;
}
resscale = m_parts[i].scale;
status->flagsOR = status->flagsAND = m_parts[i].flags;
status->pGeom = m_parts[i].pPhysGeom->pGeom;
status->pGeomProxy = m_parts[i].pPhysGeomProxy->pGeom;
}
status->pos = respos;
status->q = resq;
status->scale = resscale;
status->iSimClass = m_iSimClass;
if (status->pMtx3x3) //resq.getmatrix((matrix3x3f&)*status->pMtx3x3) *= resscale; //Q2M_IVO
(matrix3x3f&)*status->pMtx3x3 = (matrix3x3f(resq)*resscale);
if (status->pMtx3x3T) //resq.getmatrix((matrix3x3Tf&)*status->pMtx3x3T) *= resscale; //Q2M_IVO
(matrix3x3Tf&)*status->pMtx3x3T=(matrix3x3f(resq)*resscale);
if (status->pMtx4x4) //(resq.getmatrix((matrix3x3in4x4f&)*status->pMtx4x4)*=resscale).SetColumn(3,respos); //Q2M_IVO
((matrix3x3in4x4f&)*status->pMtx4x4 = (matrix3x3f(resq)*resscale)).SetColumn(3,respos);
if (status->pMtx4x4T) //(resq.getmatrix((matrix3x3in4x4Tf&)*status->pMtx4x4T)*=resscale).SetColumn(3,respos); //Q2M_IVO
((matrix3x3in4x4Tf&)*status->pMtx4x4T = (matrix3x3f(resq)*resscale)).SetColumn(3,respos);
status->BBox[0] = m_BBox[0]-m_pos;
status->BBox[1] = m_BBox[1]-m_pos;
return 1;
}
if (_status->type==pe_status_id::type_id) {
pe_status_id *status = (pe_status_id*)_status;
int ipart = status->ipart;
if (ipart<0)
for(ipart=0;ipart<m_nParts-1 && m_parts[ipart].id!=status->partid;ipart++);
if (ipart>=m_nParts)
return 0;
phys_geometry *pgeom = status->bUseProxy ? m_parts[ipart].pPhysGeomProxy : m_parts[ipart].pPhysGeom;
if ((unsigned int)status->iPrim >= (unsigned int)pgeom->pGeom->GetPrimitiveCount() ||
pgeom->pGeom->GetType()==GEOM_TRIMESH && status->iFeature>2)
return 0;
status->id = pgeom->pGeom->GetPrimitiveId(status->iPrim, status->iFeature);
status->id = status->id&~(status->id>>31) | m_parts[ipart].surface_idx&status->id>>31;
return 1;
}
if (_status->type==pe_status_nparts::type_id)
return m_nParts;
if (_status->type==pe_status_awake::type_id)
return IsAwake();
if (_status->type==pe_status_contains_point::type_id)
return IsPointInside(((pe_status_contains_point*)_status)->pt);
if (_status->type==pe_status_caps::type_id) {
pe_status_caps *status = (pe_status_caps*)_status;
status->bCanAlterOrientation = 0;
return 1;
}
return 0;
};
int CPhysicalEntity::Action(pe_action *_action)
{
if (_action->type==pe_action_awake::type_id && m_iSimClass>=0 && m_iSimClass<7) {
Awake(((pe_action_awake*)_action)->bAwake,1);
return 1;
}
if (_action->type==pe_action_remove_all_parts::type_id) {
for(int i=m_nParts-1;i>=0;i--)
RemoveGeometry(m_parts[i].id);
return 1;
}
return 0;
}
int CPhysicalEntity::AddGeometry(phys_geometry *pgeom, pe_geomparams* params, int id)
{
if (!pgeom)
return -1;
box abox; pgeom->pGeom->GetBBox(&abox);
float mindim=min(min(abox.size.x,abox.size.y),abox.size.z), mindims=mindim*params->scale;
float maxdim=max(max(abox.size.x,abox.size.y),abox.size.z), maxdims=maxdim*params->scale;
if (id>=0) {
int i; for(i=0;i<m_nParts && m_parts[i].id!=id;i++);
if (i<m_nParts) {
if (params->flags & geom_proxy) {
if (pgeom) {
if (m_parts[i].pPhysGeomProxy!=pgeom)
m_pWorld->GetGeomManager()->AddRefGeometry(pgeom);
m_parts[i].pPhysGeomProxy = pgeom;
if (mindims<0.15f)
m_parts[i].flags |= geom_has_thin_parts;
}
} else {
m_parts[i].pos = params->pos;
m_parts[i].q = params->q;
}
return id;
}
}
get_xqs_from_matrices(params->pMtx3x3,params->pMtx3x3T,params->pMtx4x4,params->pMtx4x4T, params->pos,params->q,params->scale);
ENTITY_VALIDATE_ERRCODE("CPhysicalEntity:AddGeometry",params,-1);
if (m_nParts==m_nPartsAlloc) {
geom *pparts = m_parts;
memcpy(m_parts = new geom[m_nPartsAlloc=m_nPartsAlloc+4&~3], pparts, sizeof(geom)*m_nParts);
if (pparts!=&m_defpart) delete[] pparts;
}
m_parts[m_nParts].id = id<0 ? m_iLastIdx++:id;
m_parts[m_nParts].pPhysGeom = m_parts[m_nParts].pPhysGeomProxy = pgeom;
m_pWorld->GetGeomManager()->AddRefGeometry(pgeom);
m_parts[m_nParts].surface_idx = pgeom->surface_idx;
if (!is_unused(params->surface_idx)) m_parts[m_nParts].surface_idx = params->surface_idx;
m_parts[m_nParts].flags = params->flags & ~geom_proxy;
m_parts[m_nParts].flagsCollider = params->flagsCollider;
m_parts[m_nParts].pos = params->pos;
m_parts[m_nParts].q = params->q;
m_parts[m_nParts].scale = params->scale;
if (is_unused(params->minContactDist)) {
m_parts[m_nParts].minContactDist = max(maxdims*0.03f,mindims*(mindims<maxdims*0.3f ? 0.4f:0.1f));
if (mindims<0.15f)
m_parts[m_nParts].flags |= geom_has_thin_parts;
} else
m_parts[m_nParts].minContactDist = params->minContactDist;
m_parts[m_nParts].maxdim = maxdim;
m_nParts++;
if (params->bRecalcBBox) {
ComputeBBox();
m_pWorld->RepositionEntity(this,1);
}
return m_parts[m_nParts-1].id;
}
void CPhysicalEntity::RemoveGeometry(int id)
{
for(int i=0;i<m_nParts;i++) if (m_parts[i].id==id) {
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeom);
if (m_parts[i].pPhysGeomProxy!=m_parts[i].pPhysGeom)
m_pWorld->GetGeomManager()->UnregisterGeometry(m_parts[i].pPhysGeomProxy);
for(;i<m_nParts-1;i++) m_parts[i]=m_parts[i+1];
m_nParts--;
ComputeBBox();
m_pWorld->RepositionEntity(this,1);
return;
}
}
RigidBody *CPhysicalEntity::GetRigidBody(int ipart)
{
g_StaticRigidBody.P.zero(); g_StaticRigidBody.v.zero();
g_StaticRigidBody.L.zero(); g_StaticRigidBody.w.zero();
return &g_StaticRigidBody;
}
int CPhysicalEntity::IsPointInside(vectorf pt)
{
pt = (pt-m_pos)*m_qrot;
if (m_pBoundingGeometry)
return m_pBoundingGeometry->PointInsideStatus(pt);
for(int i=0;i<m_nParts;i++) if ((m_parts[i].flags & geom_collides) &&
m_parts[i].pPhysGeom->pGeom->PointInsideStatus((pt-m_parts[i].pos)*m_parts[i].q))
return 1;
return 0;
}
void CPhysicalEntity::AlertNeighbourhoodND()
{
int i;
if (m_iSimClass>3)
return;
for(i=0;i<m_nColliders;i++) if (m_pColliders[i]!=this) {
m_pColliders[i]->RemoveCollider(this);
m_pColliders[i]->Awake();
m_pColliders[i]->Release();
m_nRefCount--;
}
m_nColliders = 0;
if (m_nRefCount>0 || m_flags&pef_always_notify_on_deletion) {
CPhysicalEntity **pentlist;
vectorf inflator = (m_BBox[1]-m_BBox[0])*1E-3f+vectorf(4,4,4)*m_pWorld->m_vars.maxContactGap;
for(i=m_pWorld->GetEntitiesAround(m_BBox[0]-inflator,m_BBox[1]+inflator, pentlist,
ent_sleeping_rigid|ent_rigid|ent_living|ent_independent|ent_triggers)-1; i>=0; i--)
pentlist[i]->Awake();
}
}
int CPhysicalEntity::RemoveCollider(CPhysicalEntity *pCollider, bool bRemoveAlways)
{
if (m_pColliders && m_iSimClass>0) {
int i,islot; for(i=0;i<m_nColliders && m_pColliders[i]!=pCollider;i++);
if (i<m_nColliders) {
for(islot=i;i<m_nColliders-1;i++) m_pColliders[i] = m_pColliders[i+1];
if (pCollider!=this)
pCollider->Release();
m_nColliders--; return islot;
}
}
return -1;
}
int CPhysicalEntity::AddCollider(CPhysicalEntity *pCollider)
{
if (m_iSimClass==0)
return 1;
int i,j;
for(i=0;i<m_nColliders && m_pColliders[i]!=pCollider;i++);
if (i==m_nColliders) {
if (m_nColliders==m_nCollidersAlloc) {
CPhysicalEntity **pColliders = m_pColliders;
memcpy(m_pColliders = new (CPhysicalEntity*[m_nCollidersAlloc+=8]), pColliders, sizeof(CPhysicalEntity*)*m_nColliders);
if (pColliders) delete[] pColliders;
}
for(i=0;i<m_nColliders && pCollider->GetMassInv()>m_pColliders[i]->GetMassInv();i++);
for(j=m_nColliders-1; j>=i; j--) m_pColliders[j+1] = m_pColliders[j];
m_pColliders[i] = pCollider; m_nColliders++;
if (pCollider!=this)
pCollider->AddRef();
}
return i;
}
void CPhysicalEntity::DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags)
{
if (flags & pe_helper_bbox) {
int i,j;
vectorf sz,center,pt[8];
center = (m_BBox[1]+m_BBox[0])*0.5f;
sz = (m_BBox[1]-m_BBox[0])*0.5f;
for(i=0;i<8;i++)
pt[i] = vectorf(sz.x*((i<<1&2)-1),sz.y*((i&2)-1),sz.z*((i>>1&2)-1))+center;
for(i=0;i<8;i++) for(j=0;j<3;j++) if (i&1<<j)
DrawLineFunc(pt[i],pt[i^1<<j]);
}
if (flags & pe_helper_geometry) {
int iLevel = flags>>16;
geom_world_data gwd;
for(int i=0;i<m_nParts;i++) {
//(m_qrot*m_parts[i].q).getmatrix(gwd.R); //Q2M_IVO
gwd.R = matrix3x3f(m_qrot*m_parts[i].q);
gwd.offset = m_pos + m_qrot*m_parts[i].pos;
gwd.scale = m_parts[i].scale;
m_parts[i].pPhysGeom->pGeom->DrawWireframe(DrawLineFunc,&gwd, iLevel);
if (m_parts[i].pPhysGeomProxy!=m_parts[i].pPhysGeom)
m_parts[i].pPhysGeomProxy->pGeom->DrawWireframe(DrawLineFunc,&gwd, iLevel);
}
}
}
void CPhysicalEntity::GetMemoryStatistics(ICrySizer *pSizer)
{
if (GetType()==PE_STATIC)
pSizer->AddObject(this, sizeof(CPhysicalEntity));
if (m_parts!=&m_defpart)
pSizer->AddObject(m_parts, m_nParts*sizeof(m_parts[0]));
pSizer->AddObject(m_pGridThunks, m_nGridThunksAlloc*sizeof(m_pGridThunks[0]));
pSizer->AddObject(m_pColliders, m_nCollidersAlloc*sizeof(m_pColliders[0]));
}
int CPhysicalEntity::GetStateSnapshotTxt(char *txtbuf,int szbuf, float time_back)
{
CStream stm;
GetStateSnapshot(stm,time_back);
int size=bin2ascii(stm.GetPtr(),(stm.GetSize()-1>>3)+1,(unsigned char*)txtbuf);
/*
// debugging
static char test[1024*16];
int testsize=ascii2bin((const unsigned char*)txtbuf,size,(unsigned char*)test);
if(memcmp(test,stm.GetPtr(),testsize)!=0)
{
char str[256];
sprintf(str,"%d %d\n",size,testsize);
OutputDebugString(str);
}
*/
return size;
}
void CPhysicalEntity::SetStateFromSnapshotTxt(const char *txtbuf,int szbuf)
{
CStream stm;
stm.SetSize(ascii2bin((const unsigned char*)txtbuf,szbuf,stm.GetPtr())*8);
SetStateFromSnapshot(stm);
}

131
CryPhysics/physicalentity.h Normal file
View File

@@ -0,0 +1,131 @@
//////////////////////////////////////////////////////////////////////
//
// Physical Entity header
//
// File: physicalentity.h
// Description : PhysicalEntity class declarations
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef physicalentity_h
#define physicalentity_h
#pragma once
enum phentity_flags_int {
pef_use_geom_callbacks = 0x2000000
};
struct geom {
phys_geometry *pPhysGeom,*pPhysGeomProxy;
int id;
vectorf pos;
quaternionf q;
float scale;
float mass;
int surface_idx;
unsigned int flags,flagsCollider;
float maxdim;
float minContactDist;
vectorf BBox[2];
};
class CPhysicalWorld;
class CRayGeom;
class CPhysicalEntity : public CPhysicalPlaceholder {
public:
CPhysicalEntity(CPhysicalWorld *pworld);
virtual ~CPhysicalEntity();
virtual pe_type GetType() { return PE_STATIC; }
int AddRef() { return ++m_nRefCount; }
int Release() { return --m_nRefCount; }
virtual int SetParams(pe_params*);
virtual int GetParams(pe_params*);
virtual int GetStatus(pe_status*);
virtual int Action(pe_action*);
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual void *GetForeignData(int itype=0) { return itype==m_iForeignData ? m_pForeignData:0; }
virtual int GetiForeignData() { return m_iForeignData; }
virtual IPhysicalWorld *GetWorld() { return (IPhysicalWorld*)m_pWorld; }
virtual CPhysicalEntity *GetEntity() { return this; }
virtual CPhysicalEntity *GetEntityFast() { return this; }
virtual void StartStep(float time_interval) {}
virtual float GetMaxTimeStep(float time_interval) { return time_interval; }
virtual float GetLastTimeStep(float time_interval) { return time_interval; }
virtual int Step(float time_interval) { return 1; }
virtual void StepBack(float time_interval) {}
virtual int GetContactCount(int nMaxPlaneContacts) { return 0; }
virtual int RegisterContacts(float time_interval,int nMaxPlaneContacts) { return 0; }
virtual int Update(float time_interval, float damping) { return 1; }
virtual float CalcEnergy(float time_interval) { return 0; }
virtual float GetDamping(float time_interval) { return 1.0f; }
virtual int AddCollider(CPhysicalEntity *pCollider);
virtual int RemoveCollider(CPhysicalEntity *pCollider, bool bAlwaysRemove=true);
virtual int RemoveContactPoint(CPhysicalEntity *pCollider, const vectorf &pt, float mindist2) { return -1; }
virtual int HasContactsWith(CPhysicalEntity *pent) { return 0; }
virtual int HasCollisionContactsWith(CPhysicalEntity *pent) { return 0; }
virtual void AlertNeighbourhoodND();
virtual int Awake(int bAwake=1,int iSource=0) { return 0; }
virtual int IsAwake(int ipart=-1) { return 0; }
int GetColliders(CPhysicalEntity **&pentlist) { pentlist=m_pColliders; return m_nColliders; }
virtual int RayTrace(CRayGeom *pRay, geom_contact *&pcontacts) { return 0; }
virtual void ApplyVolumetricPressure(const vectorf &epicenter, float kr, float rmin) {}
virtual RigidBody *GetRigidBody(int ipart=-1);
virtual void GetContactMatrix(const vectorf &pt, int ipart, matrix3x3f &K) {}
virtual void GetSpatialContactMatrix(const vectorf &pt, int ipart, float Ibuf[][6]) {}
virtual float GetMassInv() { return 0; }
virtual int IsPointInside(vectorf pt);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual int GetStateSnapshot(class CStream &stm, float time_back=0, int flags=0) { return 0; }
virtual int SetStateFromSnapshot(class CStream &stm, int flags=0) { return 0; }
virtual int PostSetStateFromSnapshot() { return 1; }
virtual unsigned int GetStateChecksum() { return 0; }
virtual int GetStateSnapshotTxt(char *txtbuf,int szbuf, float time_back=0);
virtual void SetStateFromSnapshotTxt(const char *txtbuf,int szbuf);
virtual void ComputeBBox();
int m_nRefCount;
unsigned int m_flags;
CPhysicalEntity *m_next,*m_prev;
CPhysicalWorld *m_pWorld;
int m_iPrevSimClass;
int m_iGroup,m_bMoved;
CPhysicalEntity *m_next_coll,*m_next_coll1;
vectorf m_pos;
quaternionf m_qrot;
CPhysicalEntity **m_pColliders;
int m_nColliders,m_nCollidersAlloc;
CPhysicalEntity *m_next_aux,*m_prev_aux;
CPhysicalEntity *m_pOuterEntity;
CGeometry *m_pBoundingGeometry;
int m_bProcessed_aux;
float m_timeIdle,m_maxTimeIdle;
int m_bPermanent;
geom *m_parts,m_defpart;
int m_nParts,m_nPartsAlloc;
int m_iLastIdx;
};
extern RigidBody g_StaticRigidBody;
extern CPhysicalEntity g_StaticPhysicalEntity;
#endif

190
CryPhysics/physicalplaceholder.cpp Normal file
View File

@@ -0,0 +1,190 @@
//////////////////////////////////////////////////////////////////////
//
// Physical Placeholder
//
// File: physicalplaceholder.cpp
// Description : PhysicalPlaceholder class implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "bvtree.h"
#include "geometry.h"
#include "geoman.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "physicalworld.h"
IPhysicalWorld *CPhysicalPlaceholder::GetWorld()
{
CPhysicalWorld *pWorld;
if (g_nPhysWorlds==1)
pWorld = g_pPhysWorlds[0];
else
for(int i=0;i<g_nPhysWorlds && !(pWorld=g_pPhysWorlds[i])->IsPlaceholder(this);i++);
return pWorld;
}
CPhysicalEntity *CPhysicalPlaceholder::GetEntity()
{
CPhysicalEntity *pEntBuddy;
if (!m_pEntBuddy) {
CPhysicalWorld *pWorld;
if (g_nPhysWorlds==1)
pWorld = g_pPhysWorlds[0];
else
for(int i=0;i<g_nPhysWorlds && !(pWorld=g_pPhysWorlds[i])->IsPlaceholder(this);i++);
if (pWorld->m_pPhysicsStreamer) {
pWorld->SetCurrentEntityHost(this);
pWorld->m_pPhysicsStreamer->CreatePhysicalEntity(m_pForeignData,m_iForeignData,m_iForeignFlags);
pWorld->SetCurrentEntityHost(0);
if (!m_pEntBuddy) {
pWorld->m_pLog->Log("\002Error: physical entity on-demand creation failed (type: %d, id: %d)",m_iForeignData,m_id);
pEntBuddy = pWorld->m_pHeightfield;
} else
pEntBuddy = (CPhysicalEntity*)m_pEntBuddy;
}
} else
pEntBuddy = (CPhysicalEntity*)m_pEntBuddy;
pEntBuddy->m_timeIdle = 0;
return pEntBuddy;
}
pe_type CPhysicalPlaceholder::GetType()
{
switch (m_iSimClass) {
case 0: return PE_STATIC;
case 1:case 2: return PE_RIGID;
case 3: return PE_LIVING;
case 4: return PE_PARTICLE;
default: return PE_NONE;
}
}
int CPhysicalPlaceholder::SetParams(pe_params *_params)
{
if (_params->type==pe_params_bbox::type_id) {
pe_params_bbox *params = (pe_params_bbox*)_params;
if (!is_unused(params->BBox[0])) m_BBox[0] = params->BBox[0];
if (!is_unused(params->BBox[1])) m_BBox[1] = params->BBox[1];
if (m_pEntBuddy) {
m_pEntBuddy->m_BBox[0] = m_BBox[0];
m_pEntBuddy->m_BBox[1] = m_BBox[1];
}
((CPhysicalWorld*)GetWorld())->RepositionEntity(this,1);
return 1;
}
if (_params->type==pe_params_pos::type_id) {
pe_params_pos *params = (pe_params_pos*)_params;
if (!is_unused(params->pos) | !is_unused(params->q) | !is_unused(params->scale) |
(intptr_t)params->pMtx3x3 | (intptr_t)params->pMtx3x3T | (intptr_t)params->pMtx4x4 | (intptr_t)params->pMtx4x4T)
return GetEntity()->SetParams(params);
if (!is_unused(params->iSimClass))
m_iSimClass = params->iSimClass;
return 1;
}
if (_params->type==pe_params_foreign_data::type_id) {
pe_params_foreign_data *params = (pe_params_foreign_data*)_params;
if (!is_unused(params->iForeignData)) m_iForeignData = params->iForeignData;
if (!is_unused(params->pForeignData)) m_pForeignData = params->pForeignData;
if (!is_unused(params->iForeignFlags)) m_iForeignFlags = params->iForeignFlags;
if (m_pEntBuddy) {
m_pEntBuddy->m_pForeignData = m_pForeignData;
m_pEntBuddy->m_iForeignData = m_iForeignData;
m_pEntBuddy->m_iForeignFlags = m_iForeignFlags;
}
return 1;
}
if (m_pEntBuddy)
return m_pEntBuddy->SetParams(_params);
return 0;//GetEntity()->SetParams(_params);
}
int CPhysicalPlaceholder::GetParams(pe_params *_params)
{
if (_params->type==pe_params_bbox::type_id) {
pe_params_bbox *params = (pe_params_bbox*)_params;
params->BBox[0] = m_BBox[0];
params->BBox[1] = m_BBox[1];
return 1;
}
if (_params->type==pe_params_foreign_data::type_id) {
pe_params_foreign_data *params = (pe_params_foreign_data*)_params;
params->iForeignData = m_iForeignData;
params->pForeignData = m_pForeignData;
params->iForeignFlags = m_iForeignFlags;
return 1;
}
return GetEntity()->GetParams(_params);
}
int CPhysicalPlaceholder::GetStatus(pe_status* _status)
{
if (_status->type==pe_status_placeholder::type_id) {
((pe_status_placeholder*)_status)->pFullEntity = m_pEntBuddy;
return 1;
}
if (_status->type==pe_status_awake::type_id)
return 0;
return GetEntity()->GetStatus(_status);
}
int CPhysicalPlaceholder::Action(pe_action* action) {
if (action->type==pe_action_awake::type_id && ((pe_action_awake*)action)->bAwake==0 && !m_pEntBuddy) {
if (m_iSimClass==2)
m_iSimClass = 1;
return 1;
}
return GetEntity()->Action(action);
}
int CPhysicalPlaceholder::AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id) {
return GetEntity()->AddGeometry(pgeom,params,id);
}
void CPhysicalPlaceholder::RemoveGeometry(int id) {
return GetEntity()->RemoveGeometry(id);
}
int CPhysicalPlaceholder::GetStateSnapshot(class CStream &stm, float time_back, int flags) {
return GetEntity()->GetStateSnapshot(stm,time_back,flags);
}
int CPhysicalPlaceholder::SetStateFromSnapshot(class CStream &stm, int flags) {
return GetEntity()->SetStateFromSnapshot(stm,flags);
}
int CPhysicalPlaceholder::PostSetStateFromSnapshot() {
return GetEntity()->PostSetStateFromSnapshot();
}
int CPhysicalPlaceholder::GetStateSnapshotTxt(char *txtbuf,int szbuf, float time_back) {
return GetEntity()->GetStateSnapshotTxt(txtbuf,szbuf,time_back);
}
void CPhysicalPlaceholder::SetStateFromSnapshotTxt(const char *txtbuf,int szbuf) {
GetEntity()->SetStateFromSnapshotTxt(txtbuf,szbuf);
}
unsigned int CPhysicalPlaceholder::GetStateChecksum() {
return GetEntity()->GetStateChecksum();
}
int CPhysicalPlaceholder::Step(float time_interval) {
return GetEntity()->Step(time_interval);
}
void CPhysicalPlaceholder::StartStep(float time_interval) {
GetEntity()->StartStep(time_interval);
}
void CPhysicalPlaceholder::StepBack(float time_interval) {
return GetEntity()->StepBack(time_interval);
}

76
CryPhysics/physicalplaceholder.h Normal file
View File

@@ -0,0 +1,76 @@
//////////////////////////////////////////////////////////////////////
//
// Physical Placeholder header
//
// File: physicalplaceholder.h
// Description : PhysicalPlaceholder class declarations
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef physicalplaceholder_h
#define physicalplaceholder_h
#pragma once
struct pe_gridthunk {
pe_gridthunk *next;
pe_gridthunk *prev;
class CPhysicalPlaceholder *pent;
};
class CPhysicalEntity;
class CPhysicalPlaceholder : public IPhysicalEntity {
public:
vectorf m_BBox[2];
void *m_pForeignData;
int m_iForeignData : 16;
int m_iForeignFlags : 16;
struct vec2dpacked {
int x : 16;
int y : 16;
};
vec2dpacked m_ig[2];
pe_gridthunk *m_pGridThunks;
int m_nGridThunks : 16;
int m_nGridThunksAlloc : 16;
CPhysicalPlaceholder *m_pEntBuddy;
unsigned int m_bProcessed : 1;
int m_id : 23;
int m_iSimClass : 8;
virtual CPhysicalEntity *GetEntity();
virtual CPhysicalEntity *GetEntityFast() { return (CPhysicalEntity*)m_pEntBuddy; }
virtual pe_type GetType();
virtual int SetParams(pe_params* params);
virtual int GetParams(pe_params* params);
virtual int GetStatus(pe_status* status);
virtual int Action(pe_action* action);
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual void *GetForeignData(int itype=0) { return m_iForeignData==itype ? m_pForeignData : 0; }
virtual int GetiForeignData() { return m_iForeignData; }
virtual int GetStateSnapshot(class CStream &stm, float time_back=0, int flags=0);
virtual int SetStateFromSnapshot(class CStream &stm, int flags=0);
virtual int PostSetStateFromSnapshot();
virtual int GetStateSnapshotTxt(char *txtbuf,int szbuf, float time_back=0);
virtual void SetStateFromSnapshotTxt(const char *txtbuf,int szbuf);
virtual unsigned int GetStateChecksum();
virtual void StartStep(float time_interval);
virtual int Step(float time_interval);
virtual void StepBack(float time_interval);
virtual IPhysicalWorld *GetWorld();
virtual void GetMemoryStatistics(ICrySizer *pSizer) {};
};
#endif

1776
CryPhysics/physicalworld.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

197
CryPhysics/physicalworld.h Normal file
View File

@@ -0,0 +1,197 @@
//////////////////////////////////////////////////////////////////////
//
// Physical Entity header
//
// File: physicalentity.h
// Description : PhysicalEntity and PhysicalWorld classes declarations
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef physicalworld_h
#define physicalworld_h
#pragma once
const int NSURFACETYPES = 2048;
const int PLACEHOLDER_CHUNK_SZLG2 = 8;
const int PLACEHOLDER_CHUNK_SZ = 1<<PLACEHOLDER_CHUNK_SZLG2;
class CPhysicalPlaceholder;
class CPhysicalEntity;
struct pe_gridthunk;
enum { pef_step_requested = 0x10000000 };
class CPhysicalWorld : public IPhysicalWorld, public IPhysUtils, public CGeomManager {
public:
CPhysicalWorld(ILog *pLog);
~CPhysicalWorld();
virtual void Init();
virtual void Shutdown(int bDeleteEntities = 1);
virtual void Release() { delete this; }
virtual IGeomManager* GetGeomManager() { return this; }
virtual IPhysUtils* GetPhysUtils() { return this; }
virtual void SetupEntityGrid(int axisz, vectorf org, int nx,int ny, float stepx,float stepy);
virtual void SetHeightfieldData(const heightfield *phf);
virtual int GetHeightfieldData(heightfield *phf);
virtual int SetSurfaceParameters(int surface_idx, float bounciness,float friction, unsigned int flags=0);
virtual int GetSurfaceParameters(int surface_idx, float &bounciness,float &friction, unsigned int &flags);
virtual PhysicsVars *GetPhysVars() { return &m_vars; }
virtual IPhysicalEntity* CreatePhysicalEntity(pe_type type, pe_params* params=0, void *pForeignData=0,int iForeignData=0, int id=-1)
{ return CreatePhysicalEntity(type,0.0f,params,pForeignData,iForeignData,id); }
virtual IPhysicalEntity* CreatePhysicalEntity(pe_type type, float lifeTime, pe_params* params=0, void *pForeignData=0,int iForeignData=0,
int id=-1,IPhysicalEntity *pHostPlaceholder=0);
virtual IPhysicalEntity *CreatePhysicalPlaceholder(pe_type type, pe_params* params=0, void *pForeignData=0,int iForeignData=0, int id=-1);
virtual int DestroyPhysicalEntity(IPhysicalEntity *pent, int mode=0);
virtual int SetPhysicalEntityId(IPhysicalEntity *pent, int id, int bReplace=1);
virtual int GetPhysicalEntityId(IPhysicalEntity *pent);
virtual IPhysicalEntity* GetPhysicalEntityById(int id);
int IsPlaceholder(CPhysicalPlaceholder *pent) {
if (!pent) return 0;
int iChunk; for(iChunk=0; iChunk<m_nPlaceholderChunks && (unsigned int)(pent-m_pPlaceholders[iChunk])>=(unsigned int)PLACEHOLDER_CHUNK_SZ; iChunk++);
return iChunk<m_nPlaceholderChunks ? (iChunk<<PLACEHOLDER_CHUNK_SZLG2 | pent-m_pPlaceholders[iChunk])+1 : 0;
}
void SetCurrentEntityHost(CPhysicalPlaceholder *pHost) { m_pCurEntityHost=pHost; }
virtual void TimeStep(float time_interval, int flags=ent_all|ent_deleted);
virtual float GetPhysicsTime() { return m_timePhysics; }
virtual int GetiPhysicsTime() { return m_iTimePhysics; }
virtual void SetPhysicsTime(float time) {
m_timePhysics = time;
if (m_vars.timeGranularity>0)
m_iTimePhysics = (int)(m_timePhysics/m_vars.timeGranularity+0.5f);
}
virtual void SetiPhysicsTime(int itime) { m_timePhysics = (m_iTimePhysics=itime)*m_vars.timeGranularity; }
virtual void SetSnapshotTime(float time_snapshot,int iType=0) {
m_timeSnapshot[iType] = time_snapshot;
if (m_vars.timeGranularity>0)
m_iTimeSnapshot[iType] = (int)(time_snapshot/m_vars.timeGranularity+0.5f);
}
virtual void SetiSnapshotTime(int itime_snapshot,int iType=0) {
m_iTimeSnapshot[iType] = itime_snapshot; m_timeSnapshot[iType] = itime_snapshot*m_vars.timeGranularity;
}
virtual int RayWorldIntersection(vectorf org,vectorf dir, int objtypes, unsigned int flags, ray_hit *hits,int nmaxhits,
IPhysicalEntity *pSkipEnt=0,IPhysicalEntity *pSkipEntAux=0);
virtual void SimulateExplosion(vectorf epicenter,vectorf epicenterImp, float rmin,float rmax, float r,float impulsive_pressure_at_r,
int nOccRes=0,int nGrow=0,float rmin_occ=0.1f, IPhysicalEntity **pSkipEnts=0,int nSkipEnts=0,
int iTypes=ent_rigid|ent_sleeping_rigid|ent_living|ent_independent);
virtual float IsAffectedByExplosion(IPhysicalEntity *pent);
virtual void ResetDynamicEntities();
virtual void DestroyDynamicEntities();
virtual void PurgeDeletedEntities();
virtual void DrawPhysicsHelperInformation(void (*DrawLineFunc)(float*,float*));
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual int CollideEntityWithBeam(IPhysicalEntity *_pent, vectorf org,vectorf dir,float r, ray_hit *phit);
virtual int RayTraceEntity(IPhysicalEntity *pient, vectorf origin,vectorf dir, ray_hit *pHit, pe_params_pos *pp=0);
virtual int GetEntitiesInBox(vectorf ptmin,vectorf ptmax, IPhysicalEntity **&pList, int objtypes) {
return GetEntitiesAround(ptmin,ptmax, (CPhysicalEntity**&)pList, objtypes);
}
int GetEntitiesAround(const vectorf &ptmin,const vectorf &ptmax, CPhysicalEntity **&pList, int objtypes, CPhysicalEntity *pPetitioner=0);
void RepositionEntity(CPhysicalPlaceholder *pobj, int flags=3);
void DetachEntityGridThunks(CPhysicalPlaceholder *pobj);
void ScheduleForStep(CPhysicalEntity *pent);
CPhysicalEntity *CheckColliderListsIntegrity();
virtual int BreakPolygon(vector2df *ptSrc,int nPt, int nCellx,int nCelly, int maxPatchTris, vector2df *&ptout,int *&nPtOut,
float jointhresh=0.5f,int seed=-1)
{ return ::BreakPolygon(ptSrc,nPt, nCellx,nCelly, maxPatchTris, ptout,nPtOut, jointhresh,seed); };
virtual int CoverPolygonWithCircles(strided_pointer<vector2df> pt,int npt,bool bConsecutive, const vector2df &center,
vector2df *&centers,float *&radii, float minCircleRadius)
{ return ::CoverPolygonWithCircles(pt,npt,bConsecutive, center, centers,radii, minCircleRadius); }
virtual void DeletePointer(void *pdata) { if (pdata) delete[] pdata; }
virtual void SetPhysicsStreamer(IPhysicsStreamer *pStreamer) { m_pPhysicsStreamer=pStreamer; }
virtual void SetPhysicsEventClient(IPhysicsEventClient *pEventClient) { m_pEventClient=pEventClient; }
virtual float GetLastEntityUpdateTime(IPhysicalEntity *pent) { return m_updateTimes[((CPhysicalPlaceholder*)pent)->m_iSimClass & 7]; }
void AddEntityProfileInfo(CPhysicalEntity *pent,int nTicks);
virtual int GetEntityProfileInfo(phys_profile_info *&pList);
virtual int SerializeWorld(const char *fname, int bSave);
virtual int SerializeGeometries(const char *fname, int bSave);
PhysicsVars m_vars;
ILog *m_pLog;
IPhysicsStreamer *m_pPhysicsStreamer;
IPhysicsEventClient *m_pEventClient;
CPhysicalEntity *m_pTypedEnts[8],*m_pTypedEntsPerm[8];
CPhysicalEntity **m_pTmpEntList,**m_pTmpEntList1;
float *m_pGroupMass,*m_pMassList;
int *m_pGroupIds,*m_pGroupNums;
grid m_entgrid;
int m_iEntAxisz;
pe_gridthunk **m_pEntGrid;
int m_nEnts,m_nEntsAlloc;
int m_nDynamicEntitiesDeleted;
CPhysicalPlaceholder **m_pEntsById;
int m_nIdsAlloc, m_iNextId;
int m_bGridThunksChanged;
int m_bUpdateOnlyFlagged;
int m_nPlaceholders,m_nPlaceholderChunks,m_iLastPlaceholder;
CPhysicalPlaceholder **m_pPlaceholders;
int *m_pPlaceholderMap;
CPhysicalPlaceholder *m_pCurEntityHost;
CPhysicalEntity *m_pEntBeingDeleted;
int *m_pGridStat[6],*m_pGridDyn[6];
int m_nOccRes;
vectorf m_lastEpicenter;
float m_lastRmax;
CPhysicalEntity **m_pExplVictims;
float *m_pExplVictimsFrac;
int m_nExplVictims,m_nExplVictimsAlloc;
CPhysicalEntity *m_pHeightfield;
matrix3x3f m_HeightfieldBasis;
vectorf m_HeightfieldOrigin;
float m_timePhysics,m_timeSurplus,m_timeSnapshot[4];
int m_iTimePhysics,m_iTimeSnapshot[4];
float m_updateTimes[8];
int m_iSubstep,m_bWorldStep,m_iCurGroup;
CPhysicalEntity *m_pAuxStepEnt;
phys_profile_info m_pEntProfileData[16];
int m_nProfiledEnts;
float m_BouncinessTable[NSURFACETYPES];
float m_FrictionTable[NSURFACETYPES];
float m_DynFrictionTable[NSURFACETYPES];
unsigned int m_SurfaceFlagsTable[NSURFACETYPES];
};
extern int g_nPhysWorlds;
extern CPhysicalWorld *g_pPhysWorlds[];
#ifdef ENTITY_PROFILER_ENABLED
#define PHYS_ENTITY_PROFILER CPhysEntityProfiler ent_profiler(this);
#else
#define PHYS_ENTITY_PROFILER
#endif
struct CPhysEntityProfiler {
int64 m_iStartTime;
CPhysicalEntity *m_pEntity;
CPhysEntityProfiler(CPhysicalEntity *pent) {
m_pEntity = pent;
m_iStartTime = GetTicks();
}
~CPhysEntityProfiler() {
if (m_pEntity->m_pWorld->m_vars.bProfileEntities)
m_pEntity->m_pWorld->AddEntityProfileInfo(m_pEntity,GetTicks()-m_iStartTime);
}
};
#endif

379
CryPhysics/polynomial.h Normal file
View File

@@ -0,0 +1,379 @@
#ifndef polynomial_h
#define polynomial_h
#pragma once
template<class ftype,int degree> class polynomial_tpl {
public:
explicit polynomial_tpl() { denom=(ftype)1; };
explicit polynomial_tpl(ftype op) { zero(); data[degree]=op; }
polynomial_tpl& zero() {
for(int i=0;i<=degree;i++) data[i]=0;
denom=(ftype)1; return *this;
}
polynomial_tpl(const polynomial_tpl<ftype,degree>& src) { *this=src; }
polynomial_tpl& operator=(const polynomial_tpl<ftype,degree> &src) {
denom=src.denom; for(int i=0;i<=degree;i++) data[i]=src.data[i];
return *this;
}
template<int degree1> polynomial_tpl& operator=(const polynomial_tpl<ftype,degree1> &src) {
int i; denom = src.denom;
for(i=0;i<=min(degree,degree1);i++) data[i] = src.data[i];
for(;i<degree;i++) data[i]=0;
return *this;
}
polynomial_tpl& set(ftype *pdata) {
for(int i=0;i<=degree;i++) data[degree-i]=pdata[i];
return *this;
}
ftype &operator[](int idx) { return data[idx]; }
void calc_deriviative(polynomial_tpl<ftype,degree> &deriv,int curdegree=degree) const;
polynomial_tpl& fixsign() {
ftype sg = sgnnz(denom); denom *= sg;
for(int i=0;i<=degree;i++) data[i]*=sg;
return *this;
}
int findroots(ftype start,ftype end, ftype *proots, int nIters=20,int curdegree=degree) const;
int nroots(ftype start,ftype end) const;
ftype eval(ftype x) const {
ftype res=0;
for(int i=degree;i>=0;i--) res = res*x+data[i];
return res;
}
ftype eval(ftype x,int subdegree) const {
ftype res = data[subdegree];
for(int i=subdegree-1;i>=0;i--) res = res*x+data[i];
return res;
}
polynomial_tpl& operator+=(ftype op) { data[0] += op*denom; return *this; }
polynomial_tpl& operator-=(ftype op) { data[0] -= op*denom; return *this; }
polynomial_tpl operator*(ftype op) const {
polynomial_tpl<ftype,degree> res; res.denom = denom;
for(int i=0;i<=degree;i++) res.data[i] = data[i]*op;
return res;
}
polynomial_tpl& operator*=(ftype op) {
for(int i=0;i<=degree;i++) data[i]*=op;
return *this;
}
polynomial_tpl operator/(ftype op) const {
polynomial_tpl<ftype,degree> res = *this;
res.denom = denom*op;
return res;
}
polynomial_tpl& operator/=(ftype op) { denom *= op; return *this; }
polynomial_tpl<ftype,degree*2> sqr() const { return *this**this; }
ftype denom;
ftype data[degree+1];
};
template <class ftype>
struct tagPolyE
{
inline static ftype polye() {return (ftype)1E-10;}
};
inline float tagPolyE<float>::polye() {return 1e-6f;}
template <class ftype> inline ftype polye() { return tagPolyE<ftype>::polye(); }
#define degmax(degree1,degree2) degree1-(degree1-degree2&(degree1-degree2)>>31)
template<class ftype,int degree> polynomial_tpl<ftype,degree> operator+(const polynomial_tpl<ftype,degree> &pn, ftype op) {
polynomial_tpl<ftype,degree> res = pn;
res.data[0] += op*res.denom;
return res;
}
template<class ftype,int degree> polynomial_tpl<ftype,degree> operator-(const polynomial_tpl<ftype,degree> &pn, ftype op) {
polynomial_tpl<ftype,degree> res = pn;
res.data[0] -= op*res.denom;
return res;
}
template<class ftype,int degree> polynomial_tpl<ftype,degree> operator+(ftype op, const polynomial_tpl<ftype,degree> &pn) {
polynomial_tpl<ftype,degree> res = pn;
res.data[0] += op*res.denom;
return res;
}
template<class ftype,int degree> polynomial_tpl<ftype,degree> operator-(ftype op, const polynomial_tpl<ftype,degree> &pn) {
polynomial_tpl<ftype,degree> res = pn;
res.data[0] -= op*res.denom;
for(int i=0;i<=degree;i++) res.data[i]=-res.data[i];
return res;
}
template<class ftype,int degree>
polynomial_tpl<ftype,degree*2> psqr(const polynomial_tpl<ftype,degree> &op) { return op*op; }
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degmax(degree1,degree2)> operator+(const polynomial_tpl<ftype,degree1> &op1, const polynomial_tpl<ftype,degree2> &op2) {
polynomial_tpl<ftype,degmax(degree1,degree2)> res; int i;
for(i=0;i<=min(degree1,degree2);i++) res.data[i] = op1.data[i]*op2.denom+op2.data[i]*op1.denom;
for(;i<=degree1;i++) res.data[i] = op1.data[i]*op2.denom;
for(;i<=degree2;i++) res.data[i] = op2.data[i]*op1.denom;
res.denom = op1.denom*op2.denom;
return res;
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degmax(degree1,degree2)> operator-(const polynomial_tpl<ftype,degree1> &op1, const polynomial_tpl<ftype,degree2> &op2) {
polynomial_tpl<ftype,degmax(degree1,degree2)> res; int i;
for(i=0;i<=min(degree1,degree2);i++) res.data[i] = op1.data[i]*op2.denom-op2.data[i]*op1.denom;
for(;i<=degree1;i++) res.data[i] = op1.data[i]*op2.denom;
for(;i<=degree2;i++) res.data[i] = op2.data[i]*op1.denom;
res.denom = op1.denom*op2.denom;
return res;
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degree1>& operator+=(polynomial_tpl<ftype,degree1> &op1, const polynomial_tpl<ftype,degree2> &op2) {
for(int i=0;i<min(degree1,degree2);i++) op1.data[i] = op1.data[i]*op2.denom + op2.data[i]*op1.denom;
op1.denom *= op2.denom;
return op1;
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degree1>& operator-=(polynomial_tpl<ftype,degree1> &op1, const polynomial_tpl<ftype,degree2> &op2) {
for(int i=0;i<min(degree1,degree2);i++) op1.data[i] = op1.data[i]*op2.denom - op2.data[i]*op1.denom;
op1.denom *= op2.denom;
return op1;
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degree1+degree2> operator*(const polynomial_tpl<ftype,degree1> &op1, const polynomial_tpl<ftype,degree2> &op2)
{
polynomial_tpl<ftype,degree1+degree2> res; res.zero();
int j;
switch (degree1) {
case 8: for(j=0;j<=degree2;j++) res.data[8+j] += op1.data[8]*op2.data[j];
case 7: for(j=0;j<=degree2;j++) res.data[7+j] += op1.data[7]*op2.data[j];
case 6: for(j=0;j<=degree2;j++) res.data[6+j] += op1.data[6]*op2.data[j];
case 5: for(j=0;j<=degree2;j++) res.data[5+j] += op1.data[5]*op2.data[j];
case 4: for(j=0;j<=degree2;j++) res.data[4+j] += op1.data[4]*op2.data[j];
case 3: for(j=0;j<=degree2;j++) res.data[3+j] += op1.data[3]*op2.data[j];
case 2: for(j=0;j<=degree2;j++) res.data[2+j] += op1.data[2]*op2.data[j];
case 1: for(j=0;j<=degree2;j++) res.data[1+j] += op1.data[1]*op2.data[j];
case 0: for(j=0;j<=degree2;j++) res.data[0+j] += op1.data[0]*op2.data[j];
}
res.denom = op1.denom*op2.denom;
return res;
}
template <class ftype>
void polynomial_divide(const polynomial_tpl<ftype,8> &num, const polynomial_tpl<ftype,8> &den, polynomial_tpl<ftype,8> &quot,
polynomial_tpl<ftype,8> &rem, int degree1,int degree2)
{
int i,j,k,l; ftype maxel;
for(i=0;i<=degree1;i++) rem.data[i] = num.data[i];
for(i=0;i<=degree1-degree2;i++) quot.data[i] = 0;
for(i=1,maxel=fabs_tpl(num.data[0]); i<=degree1; i++) maxel = max(maxel,num.data[i]);
for(maxel*=polye<ftype>(); degree1>=0 && fabs_tpl(num.data[degree1])<maxel; degree1--);
for(i=1,maxel=fabs_tpl(den.data[0]); i<=degree1; i++) maxel = max(maxel,den.data[i]);
for(maxel*=polye<ftype>(); degree2>=0 && fabs_tpl(den.data[degree2])<maxel; degree2--);
rem.denom = num.denom;
quot.denom = (ftype)1;
if (degree1<0 || degree2<0)
return;
for(k=degree1-degree2,l=degree1; l>=degree2; l--,k--) {
quot.data[k] = rem.data[l]*den.denom; quot.denom *= den.data[degree2];
for(i=degree1-degree2; i>k; i--) quot.data[i] *= den.data[degree2];
for(i=degree2-1,j=l-1; i>=0; i--,j--)
rem.data[j] = rem.data[j]*den.data[degree2] - den.data[i]*rem.data[l];
for(; j>=0; j--) rem.data[j] *= den.data[degree2];
rem.denom *= den.data[degree2];
}
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degree1-degree2> operator/(const polynomial_tpl<ftype,degree1> &num, const polynomial_tpl<ftype,degree2> &den) {
polynomial_tpl<ftype,degree1-degree2> quot;
polynomial_tpl<ftype,degree1> rem;
polynomial_divide((polynomial_tpl<ftype,8>&)num,(polynomial_tpl<ftype,8>&)den, (polynomial_tpl<ftype,8>&)quot,
(polynomial_tpl<ftype,8>&)rem, degree1,degree2);
return quot;
}
template <class ftype,int degree1,int degree2>
polynomial_tpl<ftype,degree2-1> operator%(const polynomial_tpl<ftype,degree1> &num, const polynomial_tpl<ftype,degree2> &den) {
polynomial_tpl<ftype,degree1-degree2> quot;
polynomial_tpl<ftype,degree1> rem;
polynomial_divide((polynomial_tpl<ftype,8>&)num,(polynomial_tpl<ftype,8>&)den, (polynomial_tpl<ftype,8>&)quot,
(polynomial_tpl<ftype,8>&)rem, degree1,degree2);
return (polynomial_tpl<ftype,degree2-1>&)rem;
}
template <class ftype,int degree>
void polynomial_tpl<ftype,degree>::calc_deriviative(polynomial_tpl<ftype,degree> &deriv, int curdegree) const {
for(int i=0; i<curdegree; i++)
deriv.data[i] = data[i+1]*(i+1);
deriv.denom = denom;
}
template <class ftype,int degree>
int polynomial_tpl<ftype,degree>::nroots(ftype start,ftype end) const
{
polynomial_tpl<ftype,degree> f[degree+1];
int i,j,sg_a,sg_b;
ftype val,prevval;
calc_deriviative(f[0]);
polynomial_divide((polynomial_tpl<ftype,8>&)*this,(polynomial_tpl<ftype,8>&)f[0], (polynomial_tpl<ftype,8>&)f[degree],
(polynomial_tpl<ftype,8>&)f[1], degree,degree-1);
f[1].denom = -f[1].denom;
for(i=2;i<degree;i++) {
polynomial_divide((polynomial_tpl<ftype,8>&)f[i-2],(polynomial_tpl<ftype,8>&)f[i-1], (polynomial_tpl<ftype,8>&)f[degree],
(polynomial_tpl<ftype,8>&)f[i], degree+1-i,degree-i);
f[i].denom = -f[i].denom;
if (fabs_tpl(f[i].denom)>(ftype)1E10) {
for(j=0;j<=degree-1-i;j++) f[i].data[j] *= (ftype)1E-10;
f[i].denom *= (ftype)1E-10;
}
}
prevval = eval(start)*denom;
for(i=sg_a=0; i<degree; i++,prevval=val) {
val = f[i].eval(start,degree-1-i)*f[i].denom;
sg_a += isneg(val*prevval);
}
prevval = eval(end)*denom;
for(i=sg_b=0; i<degree; i++,prevval=val) {
val = f[i].eval(end,degree-1-i)*f[i].denom;
sg_b += isneg(val*prevval);
}
return fabs_tpl(sg_a-sg_b);
}
template<class ftype> inline ftype cubert_tpl(ftype x) { return fabs_tpl(x)>1E-20 ? exp_tpl(log_tpl(fabs_tpl(x))*(ftype)(1.0/3))*sgnnz(x) : x; }
template<class ftype> inline ftype pow_tpl(ftype x,ftype pow) { return fabs_tpl(x)>1E-20 ? exp_tpl(log_tpl(fabs_tpl(x))*pow)*sgnnz(x) : x; }
template<class ftype> inline void swap(ftype *ptr,int i,int j) { ftype t=ptr[i]; ptr[i]=ptr[j]; ptr[j]=t; }
template <class ftype,int maxdegree>
int polynomial_tpl<ftype,maxdegree>::findroots(ftype start,ftype end, ftype *proots,int nIters,int degree) const
{
int i,j,nRoots=0; ftype maxel;
for(i=1,maxel=fabs_tpl(data[0]); i<=degree; i++) maxel = max(maxel,data[i]);
for(maxel*=polye<ftype>(); degree>0 && fabs_tpl(data[degree])<maxel; degree--);
if (degree==1) {
proots[0] = data[0]/data[1];
nRoots = 1;
} else if (degree==2) {
ftype a,b,c,d,bound[2],sg;
a=data[2]; b=data[1]; c=data[0]; d=sgnnz(a);
a*=d; b*=d; c*=d; d=b*b-a*c*4;
bound[0]=start*a*2+b; bound[1]=end*a*2+b; sg=sgnnz(bound[0]*bound[1]);
bound[0]*=bound[0]; bound[1]*=bound[1];
bound[isneg(fabs_tpl(end)-fabs_tpl(start))] *= sg;
if (isnonneg(d) & inrange(d,bound[0],bound[1])) {
d = sqrt_tpl(d); a = (ftype)0.5/a;
proots[nRoots] = (-b-d)*a; nRoots += inrange(proots[nRoots], start,end);
proots[nRoots] = (-b+d)*a; nRoots += inrange(proots[nRoots], start,end);
}
} else if (degree==3) {
real t,a,b,c,a3,p,q,Q,Qr,Ar,Ai,phi;
t = (ftype)1.0/data[3]; a = data[2]*t; b = data[1]*t; c = data[0]*t; a3 = a*(ftype)(1.0/3);
p = b-a*a3; q = (a3*b-c)*(ftype)0.5-cube(a3);
Q = cube(p*(ftype)(1.0/3)) + q*q;
Qr = sqrt_tpl(fabs_tpl(Q));
if (Q>0) {
proots[0] = cubert_tpl(q+Qr)+cubert_tpl(q-Qr) - a3;
nRoots = 1;
} else {
phi = atan2_tpl(Qr,q)*(ftype)(1.0/3);
t = pow_tpl(Qr*Qr+q*q,(ftype)(1.0/6));
Ar = t*cos_tpl(phi); Ai = t*sin_tpl(phi);
proots[0] = 2*Ar - a3;
proots[1] = -Ar + Ai*sqrt3 - a3;
proots[2] = -Ar - Ai*sqrt3 - a3;
i = idxmax3(proots); swap(proots,i,2);
i = isneg(proots[0]-proots[1]); swap(proots,i,1);
nRoots = 3;
}
} else if (degree==4) {
ftype t,a3,a2,a1,a0,y,R,D,E,subroots[3];
const ftype e = 1E-9;
t=(ftype)1.0/data[4]; a3=data[3]*t; a2=data[2]*t; a1=data[1]*t; a0=data[0]*t;
polynomial_tpl<ftype,3> p3aux; ftype kp3aux[]={ 1, -a2, a1*a3-4*a0, 4*a2*a0-a1*a1-a3*a3*a0 }; p3aux.set(kp3aux);
p3aux.findroots(-1E10,1E10,subroots); y=subroots[0];
R = a3*a3*(ftype)0.25-a2+y;
if (R>-e) {
if (R<e) {
D = E = a3*a3*(ftype)(3.0/4)-2*a2; t = y*y-4*a0;
if (t<-e)
return 0;
t = 2*sqrt_tpl(max((ftype)0,t));
} else {
R = sqrt_tpl(max((ftype)0,R));
D = E = a3*a3*(ftype)(3.0/4)-R*R-2*a2;
t = (4*a3*a2-8*a1-a3*a3*a3)/R*(ftype)0.25;
}
if (D+t>-e) {
D = sqrt_tpl(max((ftype)0,D+t));
proots[nRoots++] = a3*(ftype)-0.25+(R-D)*(ftype)0.5;
proots[nRoots++] = a3*(ftype)-0.25+(R+D)*(ftype)0.5;
}
if (E-t>-e) {
E = sqrt_tpl(max((ftype)0,E-t));
proots[nRoots++] = a3*(ftype)-0.25-(R+E)*(ftype)0.5;
proots[nRoots++] = a3*(ftype)-0.25-(R-E)*(ftype)0.5;
}
if (nRoots==4) {
i = idxmax3(proots); if (proots[3]<proots[i]) swap(proots,i,3);
i = idxmax3(proots); swap(proots,i,2);
i = isneg(proots[0]-proots[1]); swap(proots,i,1);
}
}
} else if (degree>4) {
ftype roots[maxdegree+1],prevroot,val,prevval[2],curval,bound[2],middle;
polynomial_tpl<ftype,maxdegree> deriv;
int nExtremes,iter,iBound;
calc_deriviative(deriv);
// find a subset of deriviative extremes between start and end
for(nExtremes=deriv.findroots(start,end,roots+1,nIters,degree-1)+1; nExtremes>1 && roots[nExtremes-1]>end; nExtremes--);
for(i=1;i<nExtremes && roots[i]<start;i++);
roots[i-1] = start; roots[nExtremes++] = end;
for(prevroot=start,prevval[0]=eval(start,degree),nRoots=0; i<nExtremes; prevval[0]=val,prevroot=roots[i++]) {
val = eval(roots[i],degree);
if (val*prevval[0]<0) {
// we have exactly one root between prevroot and roots[i]
bound[0]=prevroot; bound[1]=roots[i]; iter=0;
do {
middle = (bound[0]+bound[1])*(ftype)0.5;
curval = eval(middle,degree);
iBound = isneg(prevval[0]*curval);
bound[iBound] = middle;
prevval[iBound] = curval;
} while(++iter<nIters);
proots[nRoots++] = middle;
}
}
}
for(i=0; i<nRoots && proots[i]<start;i++);
for(; nRoots>i && proots[nRoots-1]>end;nRoots--);
for(j=i;j<nRoots;j++) proots[j-i] = proots[j];
return nRoots-i;
}
typedef polynomial_tpl<real,2> P2;
typedef polynomial_tpl<real,1> P1;
typedef polynomial_tpl<float,2> P2f;
typedef polynomial_tpl<float,1> P1f;
#endif

506
CryPhysics/qhull.cpp Normal file
View File

@@ -0,0 +1,506 @@
//////////////////////////////////////////////////////////////////////
//
// Quick Hull
//
// File: qhull.cpp
// Description : Quick Hull algorithm implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "utils.h"
///////////////////////////// Qhull 3d ///////////////////////////////
struct ptitem {
ptitem *next,*prev;
};
struct qhtritem {
qhtritem *next,*prev;
ptitem *ptassoc;
vectorf n;
float d;
int idx[3];
qhtritem *buddy[3];
int deleted;
};
inline void relocate_ptritem(qhtritem *&ptr,intptr_t diff) {
ptr = (qhtritem*)((intptr_t)ptr+diff & ~-iszero((intptr_t)ptr)); // offset the pointer, but leave out 0s
}
inline void relocate_tritem(qhtritem *ptr,int diff) {
relocate_ptritem(ptr->next,diff); relocate_ptritem(ptr->prev,diff);
relocate_ptritem(ptr->buddy[0],diff); relocate_ptritem(ptr->buddy[1],diff); relocate_ptritem(ptr->buddy[2],diff);
}
template<class item> void delete_item_from_list(item *p) {
if (p->prev) p->prev->next = p->next;
if (p->next) p->next->prev = p->prev;
p->prev = p->next = 0;
}
template<class item> void add_item_to_list(item *&pin, item *pnew) {
if (!pin) pin=pnew->next=pnew->prev=pnew;
else {
pnew->next = pin->next; pnew->prev = pin;
pin->next->prev = pnew; pin->next = pnew;
}
}
template<class item> void merge_lists(item *&plist, item *pnew) {
if (!pnew) return;
if (!plist) plist = pnew;
else {
plist->next->prev = pnew->prev;
pnew->prev->next = plist->next;
plist->next = pnew;
pnew->prev = plist;
}
}
void associate_ptlist_with_trilist(ptitem *ptlist, qhtritem *trilist, ptitem *pt0,strided_pointer<vectorf> pvtx, float e) {
if (!ptlist) return;
ptitem *pt=ptlist,*ptnext,*ptlast=ptlist->prev;
qhtritem *tr; int i;
do {
ptnext = pt->next;
delete_item_from_list(pt);
tr=trilist; i=pt-pt0; do {
if (pvtx[i]*tr->n > tr->d+e) {
add_item_to_list(tr->ptassoc, pt);
break;
}
tr=tr->next;
} while(tr!=trilist);
if (pt==ptlast) break;
pt = ptnext;
} while(true);
}
static inline void swap(int *v,void **p, int i1,int i2)
{
int ti=v[i1]; v[i1]=v[i2]; v[i2]=ti;
void *tp=p[i1]; p[i1]=p[i2]; p[i2]=tp;
}
static void qsort(int *v,void **p, int left,int right)
{
if (left>=right) return;
int i,last;
swap(v,p, left, left+right>>1);
for(last=left,i=left+1; i<=right; i++)
if (v[i] < v[left])
swap(v,p, ++last, i);
swap(v,p, left,last);
qsort(v,p, left,last-1);
qsort(v,p, last+1,right);
}
static int bin_search(int *v,int n,int idx)
{
int left=0,right=n,m;
do {
m = left+right>>1;
if (v[m]==idx) return m;
if (v[m]<idx) left=m;
else right=m;
} while (left<right-1);
return left;
}
int __qhullcalled=0;
int qhull(strided_pointer<vectorf> pts, int npts, index_t*& pTris)
{
static ptitem ptbuf[1024];
static qhtritem trbuf[1024];
static qhtritem *tmparr_ptr_buf[512];
static int tmparr_idx_buf[512];
int iter=0,maxiter=0;
__qhullcalled++;
ptitem *pt,*ptmax,*ptdeleted,*ptlist = npts>sizeof(ptbuf)/sizeof(ptbuf[0]) ? new ptitem[npts] : ptbuf;
qhtritem *tr,*trnext,*trend,*trnew,*trdata=trbuf,*trstart=0,*trlast,*trbest;
int i,j,k,ti,trdatasz=sizeof(trbuf)/sizeof(trbuf[0]), bidx[6],n,next_iter,delbuds;
qhtritem **tmparr_ptr=tmparr_ptr_buf;
int *tmparr_idx=tmparr_idx_buf, tmparr_sz=512;
float dist,maxdist,e;
if (npts<=4) {
pTris = new index_t[6];
pTris[0]=0; pTris[1]=min(1,npts-1); pTris[2]=min(2,npts-1);
pTris[3]=0; pTris[4]=min(1,npts-1); pTris[5]=min(4,npts-1);
return 2;
}
vectorf pmin(MAX),pmax(MIN);
// select points for initial tetrahedron
// first, find 6 points corresponding to min and max coordinates
for(i=1;i<npts;i++) {
if (pts[i].x>pmax.x) { pmax.x=pts[i].x; bidx[0]=i; }
if (pts[i].x<pmin.x) { pmin.x=pts[i].x; bidx[1]=i; }
if (pts[i].y>pmax.y) { pmax.y=pts[i].y; bidx[2]=i; }
if (pts[i].y<pmin.y) { pmin.y=pts[i].y; bidx[3]=i; }
if (pts[i].z>pmax.z) { pmax.z=pts[i].z; bidx[4]=i; }
if (pts[i].z<pmin.z) { pmin.z=pts[i].z; bidx[5]=i; }
}
e = max(max(pmax.x-pmin.x,pmax.y-pmin.y),pmax.z-pmin.z)*0.01f;
// select 4 unique points from the 6 found above
for(k=0;k<2;k++) {
for(i=0;i<3;i++) for(j=i+1;j<4;j++) if (bidx[i]==bidx[j]) {
ti=bidx[4+k]; bidx[4+k]=bidx[j]; bidx[j]=ti;
goto nextk;
} break;
nextk:;
}
if (k==2) { // if 4 unique points cannot be found, select 4 arbitrary points
bidx[0]=0; bidx[1]=npts/3; bidx[2]=npts*2/3; bidx[3]=npts-1;
}
vectorf cp1 = pts[bidx[1]]-pts[bidx[0]] ^ pts[bidx[2]]-pts[bidx[0]];
if (sqr(cp1*(pts[bidx[3]]-pts[bidx[0]])) < cube(e)) { // coplanar points, find another 4th poind
for(i=0,maxdist=0; i<npts; i++) {
dist = fabs_tpl((pts[i]-pts[bidx[0]])*cp1);
if (dist > maxdist) {
maxdist=dist; bidx[3]=i;
}
}
}
// build a double linked list from all points
for(i=0;i<npts;i++) {
ptlist[i].prev = ptlist+i-1;
ptlist[i].next = ptlist+i+1;
}
ptlist[0].prev = ptlist+npts-1;
ptlist[npts-1].next = ptlist;
// remove selected points from the list
for(i=0;i<4;i++) delete_item_from_list(ptlist+bidx[i]);
// assign 3 points to each of 4 initial triangles
for(i=0;i<4;i++) {
for(j=k=0; j<4; j++)
if (j!=i) trbuf[i].idx[k++]=bidx[j]; // skip i.th point in i.th triangle
trbuf[i].n = pts[trbuf[i].idx[1]]-pts[trbuf[i].idx[0]] ^ pts[trbuf[i].idx[2]]-pts[trbuf[i].idx[0]];
trbuf[i].d = trbuf[i].n*pts[trbuf[i].idx[0]];
if (pts[bidx[i]]*trbuf[i].n > trbuf[i].d) { // flip the orientation so that ccw normal points outwards
ti=trbuf[i].idx[0]; trbuf[i].idx[0]=trbuf[i].idx[2]; trbuf[i].idx[2]=ti;
trbuf[i].n = -trbuf[i].n; trbuf[i].d = -trbuf[i].d;
}
trbuf[i].ptassoc = 0;
trbuf[i].deleted = 0;
add_item_to_list(trstart, trbuf+i);
}
// fill buddy links for each triangle
for(i=0;i<4;i++) for(j=0;j<4;j++) if (j!=i) for(k=0;k<3;k++) for(ti=0;ti<3;ti++)
if (trbuf[i].idx[k]==trbuf[j].idx[ti] && trbuf[i].idx[k==2?0:k+1]==trbuf[j].idx[ti==0?2:ti-1]) {
trbuf[i].buddy[k] = trbuf+j; break;
}
trend = trstart+4;
for(i=0;i<4;i++) if (trbuf[i].n.len2()<1E-6f) {
#ifdef _DEBUG
//OutputDebugString("WARNING: convex hull not computed because of degenerate initial triangles\n");
#endif
n=0; goto endqhull; // some degenerate case, don't party with it
}
// associate points with one of the initial triangles
for(i=0;i<npts;i++) if (ptlist[i].next) break;
associate_ptlist_with_trilist(ptlist+i, trstart, ptlist,pts, e);
#define DELETE_TRI(ptri) { \
merge_lists(ptdeleted, (ptri)->ptassoc); \
if ((ptri)==trstart) trstart=(ptri)->next; \
if ((ptri)==trnext) trnext=(ptri)->next; \
delete_item_from_list(ptri); (ptri)->deleted=1; \
}
// main loop
iter=0;maxiter=npts*npts*2;
tr=trstart; do {
trnext = tr->next;
pt=tr->ptassoc; if (pt) {
// find the fartherst of the associated with the triangle points
maxdist=-1E10; do {
if ((dist = pts[(int)(pt-ptlist)]*tr->n) > maxdist)
{ maxdist = dist; ptmax = pt; }
pt = pt->next;
} while (pt!=tr->ptassoc);
ptdeleted = 0;
if (tr->ptassoc==ptmax) tr->ptassoc=ptmax->next;
delete_item_from_list(ptmax);
if (tr->ptassoc==ptmax) tr->ptassoc=0;
// find the triangle that the point can see "most confidently"
tr=trstart; trlast=tr->prev; ti=ptmax-ptlist; maxdist=-1E10;
do {
trnext = tr->next;
if (pts[ti]*tr->n-tr->d > maxdist) {
maxdist = pts[ti]*tr->n-tr->d;
trbest = tr;
}
if (tr==trlast) break;
tr = trnext;
} while (true);
// "flood fill" triangles that the point can see around that one
DELETE_TRI(trbest)
tr = trbest->next=trbest->prev = trbest;
do {
if (!tr->buddy[0]->deleted && pts[ti]*tr->buddy[0]->n > tr->buddy[0]->d) {
DELETE_TRI(tr->buddy[0])
add_item_to_list(tr, tr->buddy[0]);
}
if (!tr->buddy[1]->deleted && pts[ti]*tr->buddy[1]->n > tr->buddy[1]->d) {
DELETE_TRI(tr->buddy[1])
add_item_to_list(tr, tr->buddy[1]);
}
if (!tr->buddy[2]->deleted && pts[ti]*tr->buddy[2]->n > tr->buddy[2]->d) {
DELETE_TRI(tr->buddy[2])
add_item_to_list(tr, tr->buddy[2]);
}
tr = tr->next;
} while(tr!=trbest);
/*// delete triangles that the point can see
tr=trstart; trlast=tr->prev; ti=ptmax-ptlist;
do {
trnext = tr->next;
if (pts[ti]*tr->n > tr->d)
DELETE_TRI(tr)
if (tr==trlast) break;
tr = trnext;
} while (true);*/
// delete near-visible triangles around deleted area edges to preserve hole convexity
// do as many iterations as needed
do {
tr=trstart; trlast=tr->prev; next_iter=0;
do {
trnext = tr->next;
if (pts[ti]*tr->n>tr->d-e*0.5f) {
delbuds = tr->buddy[0]->deleted+tr->buddy[1]->deleted+tr->buddy[2]->deleted;
if (delbuds >= 2) { // delete triangles that have 2+ buddies deleted
if (tr==trlast) trlast=tr->next;
DELETE_TRI(tr); next_iter=1;
} else if (delbuds==1) { // follow triangle fan around both shared edge ends
int bi,bi0,bi1,nfantris,fandir;
qhtritem *fantris[64],*tr1;
for(bi0=0; bi0<3 && !tr->buddy[bi0]->deleted; bi0++); // bi0 - deleted buddy index
for(fandir=-1; fandir<=1; fandir+=2) { // follow fans in 2 possible directions
tr1=tr; bi1=bi0; nfantris=0;
do {
bi=bi1+fandir; if (bi>2) bi-=3; if (bi<0) bi+=3;
for(bi1=0; bi1<3 && tr1->buddy[bi]->buddy[bi1]!=tr1; bi1++);
fantris[nfantris++] = tr1; // store this triangle in a temporary fan list
tr1=tr1->buddy[bi]; bi=bi1; // go to the next fan triangle
if (pts[ti]*tr1->n <= tr1->d-e) break; // discard this fan
if (tr1->deleted) {
if (tr1!=tr->buddy[bi0]) {
// delete fan only if it ended on _another_ deleted triangle
for(--nfantris;nfantris>=0;nfantris--) {
if (fantris[nfantris]==trlast) trlast=fantris[nfantris]->next;
DELETE_TRI(fantris[nfantris])
}
next_iter=1;
} break; // fan end
}
} while(true);
}
}
}
if (tr==trlast) break;
tr = trnext;
} while (tr);
} while (next_iter && trstart);
if (!trstart || trstart->deleted) {
#ifdef _DEBUG
//OutputDebugString("WARNING: convex hull not computed because all triangles were deleted (too thing or too small objects)\n");
#endif
n=0; goto endqhull;
}
// find triangles that shared an edge with deleted triangles
trnew=0; tr=trstart; do {
for(i=0;i<3;i++) if (tr->buddy[i]->deleted) {
// create a new triangle
if (trend>=trdata+trdatasz) {
qhtritem* trdata_new = new qhtritem[trdatasz+=256];
memcpy(trdata_new, trdata, (trend-trdata)*sizeof(qhtritem));
intptr_t diff = (intptr_t)trdata_new-(intptr_t)trdata;
for(n=0;n<trdatasz-256;n++)
relocate_tritem(trdata_new+n,diff);
relocate_ptritem(trend,diff); relocate_ptritem(trstart,diff);
relocate_ptritem(trnext,diff);relocate_ptritem(tr,diff);
relocate_ptritem(trbest,diff);relocate_ptritem(trnew,diff);
if (trdata!=trbuf) delete trdata;
trdata = trdata_new;
}
trend->idx[0] = ptmax-ptlist;
trend->idx[1] = tr->idx[i==2 ? 0:i+1];
trend->idx[2] = tr->idx[i];
trend->ptassoc = 0; trend->deleted = 0;
trend->n = pts[trend->idx[1]]-pts[trend->idx[0]] ^ pts[trend->idx[2]]-pts[trend->idx[0]];
trend->d = pts[trend->idx[0]]*trend->n;
trend->buddy[1] = tr; tr->buddy[i] = trend;
trend->buddy[0]=trend->buddy[2]=0;
add_item_to_list(trnew, trend++);
}
tr=tr->next;
} while (tr!=trstart);
// sort pointers to the new triangles by their 2nd vertex index
n=trend-trnew; if (tmparr_sz<n) {
if (tmparr_idx!=tmparr_idx_buf) delete tmparr_idx;
if (tmparr_ptr!=tmparr_ptr_buf) delete tmparr_ptr;
tmparr_idx = new int[n];
tmparr_ptr = new(qhtritem*[n]);
}
for(tr=trnew,i=0;tr<trend;tr++,i++) { tmparr_idx[i]=tr->idx[2]; tmparr_ptr[i]=tr; }
qsort(tmparr_idx,(void**)tmparr_ptr, 0,trend-trnew-1);
// find 0th buddy for each new triangle (i.e. the triangle, which has its idx[2]==tr->idx[1]
for(tr=trnew;tr<trend;tr++) {
i = bin_search(tmparr_idx,n, tr->idx[1]);
tr->buddy[0] = tmparr_ptr[i];
tmparr_ptr[i]->buddy[2] = tr;
}
for(tr=trnew;tr<trend;tr++)
if (!tr->buddy[0] || !tr->buddy[2]) {
#ifdef _DEBUG
//DEBUG_BREAK;
//OutputDebugString("WARNING: failed to compute convex hull due to geometric error\n");
#endif
goto endqh;
}
// assign all points from the deleted triangles to the new triangles
associate_ptlist_with_trilist(ptdeleted,trnew, ptlist,pts, e);
// add new triangles to the list
merge_lists(trnext, trnew);
} else if (trnext==trstart)
break; // all triangles in queue have no associated vertices
tr = trnext;
} while (++iter<maxiter);
#ifdef _DEBUG
//if (iter>=maxiter)
//OutputDebugString("WARNING: failed to compute convex hull during iterations limit\n");
#endif
endqh:
// build the final triangle list
for(tr=trstart,n=1; tr->next!=trstart; tr=tr->next,n++);
pTris = new index_t[n*3];
i=0; tr=trstart; do {
pTris[i]=tr->idx[0]; pTris[i+1]=tr->idx[1]; pTris[i+2]=tr->idx[2];
tr = tr->next; i+=3;
} while(tr!=trstart);
endqhull:
if (ptlist!=ptbuf) delete[] ptlist;
if (tmparr_idx!=tmparr_idx_buf) delete[] tmparr_idx;
if (tmparr_ptr!=tmparr_ptr_buf) delete[] tmparr_ptr;
if (trdata!=trbuf) delete[] trdata;
return n;
}
///////////////////////////// Qhull 2d ///////////////////////////////
inline void delete_item(ptitem2d *pitem, ptitem2d *&pref) {
pitem->next->prev = pitem->prev;
pitem->prev->next = pitem->next;
if (pref==pitem)
pref = pitem->next!=pitem ? pitem->next : 0;
}
inline void add_item(ptitem2d *pitem, ptitem2d *&pref) {
if (pref) {
pref->next->prev = pitem; pitem->next = pref->next;
pref->next = pitem; pitem->prev = pref;
} else {
pitem->next=pitem->prev = pitem;
pref = pitem;
}
}
int qhull2d(ptitem2d *pts,int nVtx, edgeitem *edges)
{
intptr_t imask;
int i,i0,i1,nEdges;
vector2df edge;
ptitem2d *ppt,*ppt_best,*ppt_next,*plist;
edgeitem *pedge,*pstart,*pend;
for(i=i0=0;i<nVtx;i++) { // find the 1st point - the one w/ max y
imask = -isneg(pts[i0].pt.y-pts[i].pt.y);
i0 = i&imask | i0&~imask;
}
// find the 2nd point - the one farthest from the 1st one
for(i1=0,i=1; i<nVtx; i++) {
imask = -isneg((pts[i1].pt-pts[i0].pt).len2()-(pts[i].pt-pts[i0].pt).len2());
i1 = i&imask | i1&~imask;
}
// form the 1st two edges
edges[0].prev=edges[0].next=edges+1; edges[0].pvtx=pts+i0; edges[0].plist=0;
edges[1].prev=edges[1].next=edges+0; edges[1].pvtx=pts+i1; edges[1].plist=0;
edge = pts[i1].pt-pts[i0].pt;
for(i=0; i<nVtx; i++) if ((i-i0)*(i-i1))
add_item(pts+i,edges[isneg(pts[i].pt-pts[i0].pt ^ edge)].plist);
nEdges = 2;
do {
for(i=0;i<nEdges && !edges[i].plist;i++);
if (i==nEdges)
break;
// find up the farthest point associated with the edge
edge = edges[i].next->pvtx->pt-edges[i].pvtx->pt; ppt=ppt_best = edges[i].plist;
do {
imask = -(intptr_t)isneg((ppt_best->pt-edges[i].pvtx->pt^edge) - (ppt->pt-edges[i].pvtx->pt^edge));
ppt_best = (ptitem2d*)((intptr_t)ppt&imask | (intptr_t)ppt_best&~imask);
} while((ppt=ppt->next)!=edges[i].plist);
// trace contour from i cw while edges are facing the point ppt_best (pstart - 1st edge to be deleted)
for(pstart=edges+i; pstart->prev!=edges+i &&
(ppt_best->pt-pstart->prev->pvtx->pt ^ pstart->pvtx->pt-pstart->prev->pvtx->pt)>0; pstart=pstart->prev);
// trace contour from i ccw while edges are facing the point ppt_best (pend - edge after the last edge to be deleted)
for(pend=edges[i].next; pend!=edges+i &&
(ppt_best->pt-pend->pvtx->pt ^ pend->next->pvtx->pt-pend->pvtx->pt)>0; pend=pend->next);
// delete point ppt_best from the ith edge associated list
delete_item(ppt_best,edges[i].plist);
// merge point lists for edges pstart-pend
for(pedge=pstart,plist=0; pedge!=pend && !pedge->plist; pedge=pedge->next);
if (pedge!=pend) for(plist=pedge->plist,pedge->plist=0,pedge=pedge->next; pedge!=pend; pedge=pedge->next) if (pedge->plist) {
plist->next->prev = pedge->plist->prev; pedge->plist->prev->next = plist->next;
plist->next = pedge->plist; pedge->plist->prev = plist;
pedge->plist = 0;
}
// create a new edge between pstart and pend (first one will override pstart)
pstart->next=edges+nEdges; edges[nEdges].prev=pstart;
pend->prev = edges+nEdges; edges[nEdges].next=pend;
edges[nEdges].pvtx = ppt_best;
pstart->plist = edges[nEdges].plist = 0;
// associate some points from that list with one of the 2 new edges
if (plist) {
edgeitem *pedge[2]; vector2df edge[2]; float dist[2];
ppt = plist; pedge[0] = pstart; pedge[1] = edges+nEdges;
edge[0] = pedge[0]->next->pvtx->pt-pedge[0]->pvtx->pt;
edge[1] = pedge[1]->next->pvtx->pt-pedge[1]->pvtx->pt;
do {
ppt_next = ppt->next;
dist[0] = (ppt->pt-pedge[0]->pvtx->pt ^ edge[0])*edge[1].len2();
dist[1] = (ppt->pt-pedge[1]->pvtx->pt ^ edge[1])*edge[0].len2();
i = isneg(dist[0]-dist[1]);
if (dist[i]>0)
add_item(ppt,pedge[i]->plist);
} while((ppt=ppt_next)!=plist);
}
nEdges++;
} while(true);
return nEdges;
}

145
CryPhysics/quotient.h Normal file
View File

@@ -0,0 +1,145 @@
//////////////////////////////////////////////////////////////////////
//
// Quotient header
//
// File: quotient.h
// Description : quotient template class declaration and inlined implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef quotient_h
#define quotient_h
#pragma once
// warning: all comparisons assume quotent's y>=0, use fixsign() to ensure this is the case
template<class ftype> class quotient_tpl {
public:
quotient_tpl() {}
quotient_tpl(type_min) { x=-1;y=0; }
quotient_tpl(type_max) { x=1;y=0; }
explicit quotient_tpl(ftype _x,ftype _y=1) { x=_x;y=_y; }
quotient_tpl(const quotient_tpl<ftype>& src) { x=src.x;y=src.y; }
template<class ftype1> quotient_tpl(const quotient_tpl<ftype1>& src) { x=src.x;y=src.y; }
template<class ftype1,class ftype2> quotient_tpl& set(ftype1 nx,ftype2 ny) { (*this=nx)/=ny; return *this; }
quotient_tpl& operator=(const quotient_tpl<ftype>& src) { x=src.x;y=src.y; return *this; }
template<class ftype1> quotient_tpl& operator=(const quotient_tpl<ftype1>& src) { x=src.x;y=src.y; return *this; }
quotient_tpl& operator=(ftype src) { x=src;y=1; return *this; }
quotient_tpl& fixsign() { int sgny=::sgnnz(y); x*=sgny; y*=sgny; return *this; }
ftype val() { return y!=0 ? x/y : 0; }
quotient_tpl operator-() const { return quotient_tpl(-x,y); }
quotient_tpl operator*(ftype op) const { return quotient_tpl(x*op,y); }
quotient_tpl operator/(ftype op) const { return quotient_tpl(x,y*op); }
quotient_tpl operator+(ftype op) const { return quotient_tpl(x+y*op,y); }
quotient_tpl operator-(ftype op) const { return quotient_tpl(x-y*op,y); }
quotient_tpl& operator*=(ftype op) { x*=op; return *this; }
quotient_tpl& operator/=(ftype op) { y*=op; return *this; }
quotient_tpl& operator+=(ftype op) { x+=op*y; return *this; }
quotient_tpl& operator-=(ftype op) { x-=op*y; return *this; }
bool operator==(ftype op) const { return x==op*y; }
bool operator!=(ftype op) const { return x!=op*y; }
bool operator<(ftype op) const { return x-op*y<0; }
bool operator>(ftype op) const { return x-op*y>0; }
bool operator<=(ftype op) const { return x-op*y<=0; }
bool operator>=(ftype op) const { return x-op*y>=0; }
int sgn() { return ::sgn(x); }
int sgnnz() { return ::sgnnz(x); }
int isneg() { return ::isneg(x); }
int isnonneg() { return ::isnonneg(x); }
int isin01() { return ::isneg(fabs_tpl(x*2-y)-fabs_tpl(y)); }
ftype x,y;
};
template<> inline quotient_tpl<float>::quotient_tpl(type_min) { x=-1;y=1E-15f; }
template<> inline quotient_tpl<float>::quotient_tpl(type_max) { x=1;y=1E-15f; }
template<> inline quotient_tpl<double>::quotient_tpl(type_min) { x=-1;y=1E-50f; }
template<> inline quotient_tpl<double>::quotient_tpl(type_max) { x=1;y=1E-50f; }
template<class ftype1,class ftype2>
quotient_tpl<ftype1> operator*(const quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2) { return quotient_tpl<ftype1>(op1.x*op2.x,op1.y*op2.y); }
template<class ftype1,class ftype2>
quotient_tpl<ftype1> operator/(const quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2) { return quotient_tpl<ftype1>(op1.x*op2.y,op1.y*op2.x); }
template<class ftype1,class ftype2>
quotient_tpl<ftype1> operator+(const quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2)
{ return /*op1.y==op2.y ? quotient_tpl<ftype1>(op1.x+op2.x,op1.y) :*/ quotient_tpl<ftype1>(op1.x*op2.y+op2.x*op1.y, op1.y*op2.y); }
template<class ftype1,class ftype2>
quotient_tpl<ftype1> operator-(const quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2)
{ return /*op1.y==op2.y ? quotient_tpl<ftype1>(op1.x-op2.x,op1.y) :*/ quotient_tpl<ftype1>(op1.x*op2.y-op2.x*op1.y, op1.y*op2.y); }
template<class ftype1,class ftype2>
quotient_tpl<ftype1>& operator*=(quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2) { op1.x*=op2.x; op1.y*=op2.y; return op1; }
template<class ftype1,class ftype2>
quotient_tpl<ftype1>& operator/=(quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2) { op1.x*=op2.y; op1.y*=op2.x; return op1; }
template<class ftype1,class ftype2> quotient_tpl<ftype1>& operator+=(quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2)
{ /*if (op1.y==op2.y) op1.x+=op2.x; else*/ { op1.x=op1.x*op2.y+op2.x*op1.y; op1.y*=op2.y; } return op1; }
template<class ftype1,class ftype2> quotient_tpl<ftype1>& operator-=(quotient_tpl<ftype1> &op1,const quotient_tpl<ftype2> &op2)
{ /*if (op1.y==op2.y) op1.x-=op2.x; else*/ { op1.x=op1.x*op2.y-op2.x*op1.y; op1.y*=op2.y; } return op1; }
template<class ftype> quotient_tpl<ftype> operator*(ftype op, const quotient_tpl<ftype> &q) { return quotient_tpl<ftype>(q.x*op,q.y); }
template<class ftype> quotient_tpl<ftype> operator/(ftype op, const quotient_tpl<ftype> &q) { return quotient_tpl<ftype>(q.x,q.y*op); }
template<class ftype> quotient_tpl<ftype> operator+(ftype op, const quotient_tpl<ftype> &q) { return quotient_tpl<ftype>(op*q.y+q.x,q.y); }
template<class ftype> quotient_tpl<ftype> operator-(ftype op, const quotient_tpl<ftype> &q) { return quotient_tpl<ftype>(op*q.y-q.x,q.y); }
template<class ftype> bool operator==(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y==op2.x; }
template<class ftype> bool operator!=(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y!=op2.x; }
template<class ftype> bool operator<(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y-op2.x<0; }
template<class ftype> bool operator>(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y-op2.x>0; }
template<class ftype> bool operator<=(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y-op2.x<=0; }
template<class ftype> bool operator>=(ftype op1, const quotient_tpl<ftype> &op2) { return op1*op2.y-op2.x>=0; }
template<class ftype1,class ftype2> bool operator==(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y==op2.x*op1.y; }
template<class ftype1,class ftype2> bool operator!=(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y!=op2.x*op1.y; }
template<class ftype1,class ftype2> bool operator<(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y-op2.x*op1.y < 0; }
template<class ftype1,class ftype2> bool operator>(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y-op2.x*op1.y > 0; }
template<class ftype1,class ftype2> bool operator<=(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y-op2.x*op1.y <= 0; }
template<class ftype1,class ftype2> bool operator>=(const quotient_tpl<ftype1> &op1, const quotient_tpl<ftype2> &op2)
{ return op1.x*op2.y-op2.x*op1.y >= 0; }
template<class ftype> int sgn(const quotient_tpl<ftype> &op) { return sgn(op.x); }
template<class ftype> int sgnnz(const quotient_tpl<ftype> &op) { return sgnnz(op.x); }
template<class ftype> int isneg(const quotient_tpl<ftype> &op) { return isneg(op.x); }
template<class ftype> int isnonneg(const quotient_tpl<ftype> &op) { return isnonneg(op.x); }
template<class ftype> int sgn_safe(const quotient_tpl<ftype> &op) { return sgn(op.x)*sgnnz(op.y); }
template<class ftype> int sgnnz_safe(const quotient_tpl<ftype> &op) { return sgnnz(op.x)*sgnnz(op.y); }
template<class ftype> int isneg_safe(const quotient_tpl<ftype> &op) { return isneg(op.x)^isneg(op.y); }
template<class ftype> int isnonneg_safe(const quotient_tpl<ftype> &op) { return isnonneg(op.x)*isnonneg(op.y); }
template<class ftype> quotient_tpl<ftype> fabs_tpl(const quotient_tpl<ftype> op) { return quotient_tpl<ftype>(fabs_tpl(op.x),fabs_tpl(op.y)); }
template<class ftype> quotient_tpl<ftype> max(const quotient_tpl<ftype> &op1,const quotient_tpl<ftype> &op2) {
int mask1=isneg(op2.x*op1.y-op1.x*op2.y), mask2=mask1^1;
return quotient_tpl<ftype>(op1.x*mask1+op2.x*mask2, op1.y*mask1+op2.y*mask2);
}
template<class ftype> quotient_tpl<ftype> min(const quotient_tpl<ftype> &op1,const quotient_tpl<ftype> &op2) {
int mask1=isneg(op1.x*op2.y-op2.x*op1.y), mask2=mask1^1;
return quotient_tpl<ftype>(op1.x*mask1+op2.x*mask2, op1.y*mask1+op2.y*mask2);
}
template<class ftype> quotient_tpl<ftype> fake_atan2(ftype y,ftype x) {
quotient_tpl<ftype> res;
ftype src[2] = { x,y };
int ix=isneg(x),iy=isneg(y),iflip=isneg(fabs_tpl(x)-fabs_tpl(y));
res.x = src[iflip^1]*(1-iflip*2)*sgnnz(src[iflip]); res.y = fabs_tpl(src[iflip]);
res += (iy*2+(ix^iy)+(iflip^ix^iy))*2;
return res;
}
typedef quotient_tpl<float> quotientf;
typedef quotient_tpl<real> quotient;
typedef quotient_tpl<int> quotienti;
#endif

33
CryPhysics/raybv.cpp Normal file
View File

@@ -0,0 +1,33 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "raybv.h"
BVray CRayBV::g_BVray;
int CRayBV::GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp)
{
return 1; // ray should already be in the buffer
}
void CRayBV::GetNodeBV(BV *&pBV, int iNode)
{
pBV = &g_BVray;
g_BVray.iNode = 0;
g_BVray.type = ray::type;
g_BVray.aray.origin = m_pray->origin;
g_BVray.aray.dir = m_pray->dir;
}
void CRayBV::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode)
{
pBV = &g_BVray;
g_BVray.iNode = 0;
g_BVray.type = ray::type;
g_BVray.aray.origin = Rw*m_pray->origin*scalew + offsw;
g_BVray.aray.dir = Rw*m_pray->dir*scalew;
}

24
CryPhysics/raybv.h Normal file
View File

@@ -0,0 +1,24 @@
#ifndef raybv_h
#define raybv_h
#pragma once
class CRayBV : public CBVTree {
public:
CRayBV() {}
virtual int GetType() { return BVT_RAY; }
virtual float Build(CGeometry *pGeom) { m_pGeom=pGeom; return 0.0f; }
void SetRay(ray *pray) { m_pray = pray; }
virtual void GetNodeBV(BV *&pBV, int iNode=0);
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0) {}
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp);
CGeometry *m_pGeom;
ray *m_pray;
static BVray g_BVray;
};
#endif

70
CryPhysics/raygeom.cpp Normal file
View File

@@ -0,0 +1,70 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "raybv.h"
#include "raygeom.h"
void CRayGeom::GetBBox(box *pbox)
{
pbox->Basis.SetRow(2,m_dirn);
//pbox->Basis.SetRow(1,m_dirn.orthogonal().normalized());
pbox->Basis.SetRow(1,GetOrthogonal(m_dirn).normalized());
pbox->Basis.SetRow(0,pbox->Basis.GetRow(1)^m_dirn);
}
int CRayGeom::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
static short g_id = -1;
m_Tree.PrepareForIntersectionTest(pGTest);
pGTest->pGeometry = this;
pGTest->pBVtree = &m_Tree;
pGTest->primbuf = pGTest->primbuf1 = &m_ray;
pGTest->typeprim = ray::type;
pGTest->szprim = sizeof(ray);
pGTest->idbuf = &g_id;
pGTest->surfaces = 0;
pGTest->edges = 0;
pGTest->minAreaEdge = 0;
return 1;
}
int CRayGeom::PreparePrimitive(geom_world_data *pgwd, primitive *&pprim)
{
pprim = &m_ray;
return ray::type;
}
int CRayGeom::GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId)
{
return 1;
}
int CRayGeom::RegisterIntersection(primitive *pprim1,primitive *pprim2, geometry_under_test *pGTest1,geometry_under_test *pGTest2, prim_inters *pinters)
{
geom_contact *pres = pGTest1->contacts + *pGTest1->pnContacts;
(*pGTest1->pnContacts)++;
pres->ptborder = &pres->pt;
pres->nborderpt = 1;
pres->parea = 0;
pres->t = (pinters->pt[0]-m_ray.origin)*m_dirn;
pres->pt = pinters->pt[0];
pres->n = -pinters->n;
pres->dir.zero();
pres->vel = 0;
pres->iUnprojMode = 0;
pres->id[0] = -1;
pres->id[1] = pinters->id[1];
pres->iPrim[0] = 0; pres->iFeature[0] = 0x20;
pres->iPrim[1] = pGTest2 && pGTest2->typeprim==indexed_triangle::type ? ((indexed_triangle*)pprim2)->idx : 0;
pres->iFeature[1] = pinters->iFeature[0][1];
pres->iNode[0] = pinters->iNode[0];
pres->iNode[1] = pinters->iNode[1];
if (*pGTest1->pnContacts>=pGTest1->nMaxContacts || pGTest1->pParams->bStopAtFirstTri)
pGTest1->bStopIntersection = 1;
return 1;
}

37
CryPhysics/raygeom.h Normal file
View File

@@ -0,0 +1,37 @@
#ifndef raygeom_h
#define raygeom_h
#pragma once
class CRayGeom : public CGeometry {
public:
CRayGeom() { m_iCollPriority=0; m_Tree.Build(this); m_Tree.SetRay(&m_ray); m_minVtxDist=1.0f; }
CRayGeom(const ray *pray) {
m_iCollPriority=0; m_ray.origin = pray->origin; m_ray.dir = pray->dir; m_dirn = pray->dir.normalized();
m_Tree.Build(this); m_Tree.SetRay(&m_ray); m_minVtxDist=1.0f;
}
CRayGeom(const vectorf &origin, const vectorf &dir) {
m_iCollPriority=0; m_ray.origin = origin; m_ray.dir = dir; m_dirn = dir.normalized();
m_Tree.Build(this); m_Tree.SetRay(&m_ray); m_minVtxDist=1.0f;
}
CRayGeom *CreateRay(const vectorf &origin, const vectorf &dir, const vectorf *pdirn=0) {
m_ray.origin = origin; m_ray.dir = dir; m_dirn = pdirn ? *pdirn : dir.normalized(); return this;
}
virtual int GetType() { return GEOM_RAY; }
virtual int IsAPrimitive() { return 1; }
virtual void GetBBox(box *pbox);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts);
virtual int RegisterIntersection(primitive *pprim1,primitive *pprim2, geometry_under_test *pGTest1,geometry_under_test *pGTest2,
prim_inters *pinters);
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId);
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim);
virtual CBVTree *GetBVTree() { return &m_Tree; }
virtual int GetPrimitive(int iPrim, primitive *pprim) { *(ray*)pprim = m_ray; return sizeof(ray); }
ray m_ray;
vectorf m_dirn;
CRayBV m_Tree;
};
#endif

1237
CryPhysics/rigidbody.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

115
CryPhysics/rigidbody.h Normal file
View File

@@ -0,0 +1,115 @@
//////////////////////////////////////////////////////////////////////
//
// Rigid Body header
//
// File: rigidbody.cpp
// Description : RigidBody class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef rigidbody_h
#define rigidbody_h
#pragma once
struct entity_contact;
const int MAX_CONTACTS = 4096;
enum solver_events { solver_initialize, solver_end_iter, solver_end };
class IRigidBodyOwner {
public:
virtual void AddImpulseAtContact(entity_contact *pcontact, int iop, const vectorf &dP) = 0;
virtual vectorf GetVelocityAtContact(entity_contact *pcontact, int iop) = 0;
virtual int OnRegisterContact(entity_contact *pcontact, int iop) = 0;
virtual void OnSolverEvent(int iEvent) = 0;
};
class RigidBody : public IRigidBodyOwner {
public:
RigidBody();
void Create(const vectorf &center,const vectorf &Ibody0,const quaternionf &q0, float volume,float mass,
const quaternionf &qframe,const vectorf &posframe);
void Add(const vectorf &center,const vectorf Ibodyop,const quaternionf &qop, float volume,float mass);
void zero();
void Step(float dt);
void UpdateState();
void GetContactMatrix(const vectorf &r, matrix3x3f &K);
virtual void AddImpulseAtContact(entity_contact *pcontact, int iop, const vectorf &dP);
virtual vectorf GetVelocityAtContact(entity_contact *pcontact, int iop);
virtual int OnRegisterContact(entity_contact *pcontact, int iop) { return 1; }
virtual void OnSolverEvent(int iEvent) {}
vectorf pos;
quaternionf q;
vectorf P,L;
vectorf w,v;
float M,Minv; // mass, 1.0/mass (0 for static objects)
float V; // volume
matrix3x3diagf Ibody; // diagonalized inertia tensor (aligned with body's axes of inertia)
matrix3x3diagf Ibody_inv; // { 1/Ibody.ii }
quaternionf qfb; // frame->body rotation
vectorf offsfb; // frame->body offset
matrix3x3f Iinv; // I^-1(t)
vectorf Fcollision,Tcollision;
int bProcessed; // used internally
float Eunproj;
float softness[2];
IRigidBodyOwner *pOwner;
};
enum contactflags { contact_count_mask=0x3F, contact_new=0x40, contact_2b_verified=0x80, contact_2b_verified_log2=7,
contact_angular=0x100, contact_constraint_3dof=0x200, contact_constraint_2dof=0x400,
contact_constraint_1dof=0x800, contact_solve_for=0x1000,
contact_constraint=contact_constraint_3dof|contact_constraint_2dof|contact_constraint_1dof,
contact_angular_log2=8,contact_bidx=0x2000,contact_bidx_log2=13, contact_maintain_count=0x4000,
contact_wheel=0x8000, contact_use_C=0x10000, contact_inexact=0x20000 };
class CPhysicalEntity;
struct entity_contact {
vectorf pt[2];
vectorf n;
vectorf dir;
vectorf ptloc[2];
CPhysicalEntity *pent[2];
int ipart[2];
RigidBody *pbody[2];
vectorf nloc;
float friction;
int id[2];
int flags;
vectorf vrel;
vectorf vreq;
//float vsep;
float Pspare;
float penetration;
matrix3x3f K,Kinv;
matrix3x3f C;
int iNormal;
int iPrim[2];
int iFeature[2];
int bProcessed;
int iCount,*pBounceCount;
vectorf r0,r;
vectorf dP,P;
float dPn;
};
extern bool g_bUsePreCG;
extern int g_nContacts,g_nBodies;
void InitContactSolver(float time_interval);
void RegisterContact(entity_contact *pcontact);
void InvokeContactSolver(float time_interval, SolverSettings *pss);
char *AllocSolverTmpBuf(int size);
#endif

2797
CryPhysics/rigidentity.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

180
CryPhysics/rigidentity.h Normal file
View File

@@ -0,0 +1,180 @@
//////////////////////////////////////////////////////////////////////
//
// Rigid Entity header
//
// File: rigidentity.h
// Description : RigidEntity class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef rigidentity_h
#define rigidentity_h
#pragma once
typedef uint64 masktype;
#define getmask(i) ((uint64)1<<(i))
const int NMASKBITS = 64;
enum rentity_flags_int {
ref_contact_overflow = 0x4000000
};
//typedef unsigned int masktype;
//#define getmask(i) (1u<<(i))
//const int NMASKBITS = 32;
enum constr_info_flags { constraint_limited_1axis=1, constraint_limited_2axes=2, constraint_rope=4 };
struct constraint_info {
quaternionf qframe_rel[2];
float limits[2];
unsigned int flags;
CPhysicalEntity *pConstraintEnt;
int bActive;
};
struct checksum_item {
int iPhysTime;
unsigned int checksum;
};
const int NCHECKSUMS = 32;
class CRigidEntity : public CPhysicalEntity {
public:
CRigidEntity(CPhysicalWorld *pworld);
virtual ~CRigidEntity();
virtual pe_type GetType() { return PE_RIGID; }
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual int SetParams(pe_params *_params);
virtual int GetParams(pe_params *_params);
virtual int GetStatus(pe_status*);
virtual int Action(pe_action*);
virtual int AddCollider(CPhysicalEntity *pCollider);
virtual int RemoveCollider(CPhysicalEntity *pCollider, bool bRemoveAlways=true);
virtual int RemoveContactPoint(CPhysicalEntity *pCollider, const vectorf &pt, float mindist2);
virtual int HasContactsWith(CPhysicalEntity *pent);
virtual int HasCollisionContactsWith(CPhysicalEntity *pent);
virtual int Awake(int bAwake=1,int iSource=0);
virtual int IsAwake(int ipart=-1) { return m_bAwake; }
virtual void AlertNeighbourhoodND();
virtual RigidBody *GetRigidBody(int ipart=-1) { return &m_body; }
virtual void GetContactMatrix(const vectorf &pt, int ipart, matrix3x3f &K) { m_body.GetContactMatrix(pt-m_body.pos,K); }
virtual float GetMassInv() { return m_body.Minv; }
enum snapver { SNAPSHOT_VERSION = 9 };
virtual int GetSnapshotVersion() { return SNAPSHOT_VERSION; }
virtual int GetStateSnapshot(class CStream &stm, float time_back=0, int flags=0);
virtual int SetStateFromSnapshot(class CStream &stm, int flags=0);
virtual int PostSetStateFromSnapshot();
virtual unsigned int GetStateChecksum();
int WriteContacts(CStream &stm,int flags);
int ReadContacts(CStream &stm,int flags);
virtual void StartStep(float time_interval);
virtual float GetMaxTimeStep(float time_interval);
virtual float GetLastTimeStep(float time_interval) { return m_lastTimeStep; }
virtual int Step(float time_interval);
virtual void StepBack(float time_interval);
virtual int GetContactCount(int nMaxPlaneContacts);
virtual int RegisterContacts(float time_interval,int nMaxPlaneContacts);
virtual int Update(float time_interval, float damping);
virtual float CalcEnergy(float time_interval);
virtual float GetDamping(float time_interval);
virtual void CheckAdditionalGeometry(float time_interval, masktype &contact_mask) {}
virtual void AddAdditionalImpulses(float time_interval) {}
virtual void RecomputeMassDistribution(int ipart=-1,int bMassChanged=1);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
int RegisterConstraint(const vectorf &pt0,const vectorf &pt1, int ipart0, CPhysicalEntity *pBuddy,int ipart1, int flags);
int RemoveConstraint(int iConstraint);
int RegisterContactPoint(masktype &contact_mask, int idx, const vectorf &pt, const geom_contact *pcontacts, int iPrim0,int iFeature0,
int iPrim1,int iFeature1, int flags=contact_new, float penetration=0);
int CheckForNewContacts(geom_world_data *pgwd0,intersection_params *pip, int &itmax, int iStartPart=0,int nParts=-1);
virtual int GetPotentialColliders(CPhysicalEntity **&pentlist);
virtual int CheckSelfCollision(int ipart0,int ipart1) { return 0; }
void UpdatePenaltyContacts(float time_interval);
int UpdatePenaltyContact(int i, float time_interval);
int VerifyExistingContacts(float maxdist);
void UpdateConstraints();
void UpdateContactsAfterStepBack(float time_interval);
void ApplyBuoyancy(float time_interval,const vectorf &gravity);
void ArchiveContact(int idx);
int CompactContactBlock(masktype &contact_mask, float maxPlaneDist, int nMaxContacts,int &nContacts, vectorf &n, float &maxDist,
const vectorf& ptTest = vectorf(zero), const vectorf& dirTest = vectorf(zero));
int IsFast(float time_interval);
masktype *m_pColliderContacts,*m_pColliderConstraints;
entity_contact *m_pContacts,*m_pConstraints;
constraint_info *m_pConstraintInfos;
int m_nContactsAlloc,m_nConstraintsAlloc;
//int m_iStickyContacts[8],m_nStickyContacts;
//int m_iSlidingContacts[8],m_nSlidingContacts;
int m_bProhibitUnproj;
int m_bProhibitUnprojection,m_bEnforceContacts;
vectorf m_prevUnprojDir;
int m_bCollisionCulling;
int m_bJustLoaded;
int m_bStable;
int m_bHadSeverePenetration;
unsigned int m_nRestMask;
int m_nPrevColliders;
int m_bSteppedBack,m_nStepBackCount;
float m_velFastDir,m_sizeFastDir;
int m_bCanSweep;
float m_timeStepFull;
float m_timeStepPerformed;
float m_lastTimeStep;
float m_minAwakeTime;
vectorf m_gravity,m_gravityFreefall;
float m_Emin;
float m_maxAllowedStep;
vectorf m_vhist[4],m_whist[4],m_Lhist[4];
int m_iDynHist;
int m_bAwake;
int m_nSleepFrames;
float m_damping,m_dampingFreefall;
float m_dampingEx;
float m_maxw;
float m_softness[4];
RigidBody m_body;
vectorf m_Pext[2],m_Lext[2];
vectorf m_prevPos,m_prevv,m_prevw;
quaternionf m_prevq;
float m_E0,m_Estep;
float m_impulseTime;
float m_timeContactOverflow;
coll_history_item m_CollHistory[8];
int m_iCollHistory;
plane m_waterPlane;
float m_waterDensity;
float m_waterDamping;
float m_waterResistance;
vectorf m_waterFlow;
float m_EminWater;
int m_bFloating;
float m_submergedFraction;
float m_maxWaterResistance2;
checksum_item m_checksums[NCHECKSUMS];
int m_iLastChecksum;
};
extern CPhysicalEntity *g_CurColliders[128];
extern int g_CurCollParts[128][2];
#endif

801
CryPhysics/ropeentity.cpp Normal file
View File

@@ -0,0 +1,801 @@
//////////////////////////////////////////////////////////////////////
//
// Rope Entity
//
// File: ropeentity.cpp
// Description : CRopeEntity class implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "bvtree.h"
#include "geometry.h"
#include "overlapchecks.h"
#include "raybv.h"
#include "raygeom.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "geoman.h"
#include "physicalworld.h"
#include "ropeentity.h"
CRopeEntity::CRopeEntity(CPhysicalWorld *pWorld) : CPhysicalEntity(pWorld)
{
m_nSegs = -1;
m_segs = 0;
m_length = 0;
m_pTiedTo[0] = m_pTiedTo[1] = 0;
m_iTiedPart[0] = m_iTiedPart[1] = 0;
m_idConstraint = 0;
m_iConstraintClient = 0;
m_gravity = pWorld->m_vars.gravity;
m_bAwake = m_nSlowFrames = 0;
m_damping = 0.5f;
m_iSimClass = 4;
m_Emin = sqr(0.003f);
m_maxAllowedStep = 0.01f;
m_mass = 1.0f;
m_collDist = 0.01f;
m_flags = 0;
m_surface_idx = 0;
m_ig[0].x=m_ig[1].x=m_ig[0].y=m_ig[1].y = -3;
m_defpart.id = 0;
m_friction = 0.2f;
}
CRopeEntity::~CRopeEntity()
{
if (m_segs)
delete[] m_segs;
}
void CRopeEntity::AlertNeighbourhoodND()
{
pe_params_rope pr;
pr.pEntTiedTo[0] = pr.pEntTiedTo[1] = 0;
SetParams(&pr);
if (m_segs)
delete[] m_segs;
m_segs = 0; m_nSegs = 0;
}
int CRopeEntity::Awake(int bAwake,int iSource)
{
m_bAwake=bAwake; m_nSlowFrames=0;
int i;
if (m_nSegs>0) {
for(i=0;i<=m_nSegs;i++) if (m_segs[i].pContactEnt && m_segs[i].pContactEnt->m_iSimClass==7)
m_segs[i].pContactEnt = 0;
for(i=0;i<2;i++) if (m_pTiedTo[i] && m_pTiedTo[i]->m_iSimClass==7)
m_pTiedTo[i] = 0;
}
return 1;
}
int CRopeEntity::SetParams(pe_params *_params)
{
int res;
vectorf prevpos = m_pos;
if (res = CPhysicalEntity::SetParams(_params)) {
if (_params->type==pe_params_pos::type_id) {
//matrix3x3f R; m_qrot.getmatrix(R); //Q2M_IVO
matrix3x3f R = matrix3x3f(m_qrot);
for(int i=0;i<=m_nSegs;i++) {
m_segs[i].pt = R*(m_segs[i].pt-prevpos)+m_pos;
m_segs[i].vel = R*m_segs[i].vel;
m_segs[i].dir = R*m_segs[i].dir;
}
m_qrot.SetIdentity();
}
return res;
}
if (_params->type==pe_simulation_params::type_id) {
pe_simulation_params *params = (pe_simulation_params*)_params;
if (!is_unused(params->gravity)) m_gravity = params->gravity;
if (!is_unused(params->damping)) m_damping = params->damping;
if (!is_unused(params->maxTimeStep)) m_maxAllowedStep = params->maxTimeStep;
if (!is_unused(params->minEnergy)) m_Emin = params->minEnergy;
return 1;
}
if (_params->type==pe_params_rope::type_id) {
pe_params_rope *params = (pe_params_rope*)_params;
int i;
if (!is_unused(params->length)) m_length = params->length;
if (!is_unused(params->mass)) m_mass = params->mass;
if (!is_unused(params->collDist)) m_collDist = params->collDist;
if (!is_unused(params->surface_idx)) m_surface_idx = params->surface_idx;
if (!is_unused(params->friction)) m_friction = params->friction;
if (!is_unused(params->nSegments)) {
if (params->nSegments>m_nSegs) {
if (m_segs) delete[] m_segs;
m_segs = new rope_segment[params->nSegments+1];
memset(m_segs, 0, (params->nSegments+1)*sizeof(rope_segment));
float seglen = m_length/params->nSegments;
for(i=0;i<=params->nSegments;i++)
(m_segs[i].pt = m_pos).z -= seglen*i;
}
m_nSegs = params->nSegments;
}
if (!is_unused(params->pPoints) && params->pPoints)
for(i=0;i<=m_nSegs;i++) m_segs[i].pt = params->pPoints[i];
if (!is_unused(params->pVelocities) && params->pVelocities)
for(i=0;i<=m_nSegs;i++) m_segs[i].vel = params->pVelocities[i];
if (m_idConstraint && m_pTiedTo[m_iConstraintClient]) {
pe_action_remove_constraint arc;
arc.idConstraint = m_idConstraint;
m_pTiedTo[m_iConstraintClient]->Action(&arc);
m_idConstraint = 0;
}
for(i=0;i<2;i++) if (m_pTiedTo[i]) {
if (m_pTiedTo[i]->m_iSimClass==7)
m_pTiedTo[i] = 0;
else
m_pTiedTo[i]->Awake();
}
for(i=0;i<2;i++) if (!is_unused(params->pEntTiedTo[i])) {
if (m_pTiedTo[i]) m_pTiedTo[i]->Release();
m_pTiedTo[i] = params->pEntTiedTo[i]==WORLD_ENTITY ? &g_StaticPhysicalEntity :
(params->pEntTiedTo[i] ? ((CPhysicalPlaceholder*)params->pEntTiedTo[i])->GetEntity() : 0);
if (!is_unused(params->idPartTiedTo[i])) {
for(m_iTiedPart[i]=0; m_iTiedPart[i]<m_pTiedTo[i]->m_nParts &&
params->idPartTiedTo[i]!=m_pTiedTo[i]->m_parts[m_iTiedPart[i]].id; m_iTiedPart[i]++);
if (m_iTiedPart[i]>=m_pTiedTo[i]->m_nParts)
m_iTiedPart[i] = 0;
}
if (m_pTiedTo[i]) {
RigidBody *pbody = m_pTiedTo[i]->GetRigidBody(m_iTiedPart[i]);
if (!is_unused(params->ptTiedTo[i]))
m_ptTiedLoc[i] = (params->ptTiedTo[i]-pbody->pos)*pbody->q;
else if (!is_unused(params->pEntTiedTo[i]) || !is_unused(params->idPartTiedTo[i]))
m_ptTiedLoc[i] = (m_segs[m_nSegs*i].pt-pbody->pos)*pbody->q;
m_pTiedTo[i]->AddRef();
}
}
m_bAwake = 1;
m_nSlowFrames = 0;
if (m_pTiedTo[0] && (m_pTiedTo[0]==this || m_pTiedTo[0]->m_iSimClass==3 || m_pTiedTo[0]->GetType()==PE_ROPE || m_pTiedTo[0]->GetType()==PE_SOFT) ||
m_pTiedTo[1] && (m_pTiedTo[1]==this || m_pTiedTo[1]->m_iSimClass==3 || m_pTiedTo[1]->GetType()==PE_ROPE || m_pTiedTo[1]->GetType()==PE_SOFT))
{ // rope cannot be attached to such objects
if (m_pTiedTo[0]) m_pTiedTo[0]->Release();
if (m_pTiedTo[1]) m_pTiedTo[1]->Release();
m_pTiedTo[0] = 0; m_pTiedTo[1] = 0;
return 0;
}
if (m_pTiedTo[0] && m_pTiedTo[1]) {
pe_action_add_constraint aac;
m_iConstraintClient = isneg(m_pTiedTo[0]->GetMassInv()-m_pTiedTo[1]->GetMassInv());
for(i=0;i<2;i++) {
RigidBody *pbody = m_pTiedTo[m_iConstraintClient^i]->GetRigidBody(m_iTiedPart[m_iConstraintClient^i]);
aac.pt[i] = pbody->pos + pbody->q*m_ptTiedLoc[m_iConstraintClient^i];
aac.partid[i] = m_pTiedTo[m_iConstraintClient^i]->m_parts[m_iTiedPart[m_iConstraintClient^i]].id;
}
aac.pBuddy = m_pTiedTo[m_iConstraintClient^1];
aac.pConstraintEntity = this;
m_idConstraint = m_pTiedTo[m_iConstraintClient]->Action(&aac);
}
return 1;
}
return 0;
}
int CRopeEntity::GetParams(pe_params *_params)
{
int res;
if (res = CPhysicalEntity::GetParams(_params))
return res;
if (_params->type==pe_simulation_params::type_id) {
pe_simulation_params *params = (pe_simulation_params*)_params;
params->gravity = m_gravity;
params->damping = m_damping;
params->maxTimeStep = m_maxAllowedStep;
params->minEnergy = m_Emin;
return 1;
}
if (_params->type==pe_params_rope::type_id) {
pe_params_rope *params = (pe_params_rope*)_params;
params->length = m_length;
params->mass = m_mass;
params->nSegments = m_nSegs;
params->collDist = m_collDist;
params->surface_idx = m_surface_idx;
params->friction = m_friction;
params->pPoints = &m_segs[0].pt;
params->pVelocities = &m_segs[0].vel;
params->iStride = sizeof(rope_segment);
for(int i=0;i<2;i++) if (params->pEntTiedTo[i] = m_pTiedTo[i]) {
if (m_pTiedTo[i]==&g_StaticPhysicalEntity)
params->pEntTiedTo[i] = WORLD_ENTITY;
params->idPartTiedTo[i] = m_pTiedTo[i]->m_parts[m_iTiedPart[i]].id;
RigidBody *pbody = m_pTiedTo[i]->GetRigidBody(m_iTiedPart[i]);
params->ptTiedTo[i] = pbody->pos + pbody->q*m_ptTiedLoc[i];
}
return 1;
}
return 0;
}
int CRopeEntity::GetStatus(pe_status *_status)
{
int res;
if (res = CPhysicalEntity::GetStatus(_status)) {
if (_status->type==pe_status_caps::type_id) {
pe_status_caps *status = (pe_status_caps*)_status;
status->bCanAlterOrientation = 1;
}
return res;
}
if (_status->type==pe_status_rope::type_id) {
pe_status_rope *status = (pe_status_rope*)_status;
status->nSegments = m_nSegs;
int i;
if (!is_unused(status->pPoints) && status->pPoints)
for(i=0;i<=m_nSegs;i++) status->pPoints[i] = m_segs[i].pt;
if (!is_unused(status->pVelocities) && status->pVelocities)
for(i=0;i<=m_nSegs;i++) status->pVelocities[i] = m_segs[i].vel;
return 1;
}
return 0;
}
int CRopeEntity::Action(pe_action *_action)
{
int res,i,j;
if (res = CPhysicalEntity::Action(_action))
return res;
if (_action->type==pe_action_impulse::type_id) {
pe_action_impulse *action = (pe_action_impulse*)_action;
if (!is_unused(action->ipart)) i = action->ipart;
else if (!is_unused(action->partid)) i = action->partid;
else if (!is_unused(action->point)) {
for(j=1,i=0;j<=m_nSegs;j++) if ((m_segs[j].pt-action->point).len2()<(m_segs[i].pt-action->point).len2())
i=j;
if ((m_segs[i].pt-action->point).len2()>sqr(m_collDist*3) &&
(i==0 || (m_segs[i].pt-m_segs[i-1].pt^action->point-m_segs[i-1].pt).len2() > sqr(m_collDist)*(m_segs[i].pt-m_segs[i-1].pt).len2()))
return 0;
} else
return 0;
if ((unsigned int)i>(unsigned int)m_nSegs)
return 0;
m_segs[i].vel_ext += action->impulse*((m_nSegs+1)/m_mass);
m_bAwake = 1; m_nSlowFrames = 0;
return 1;
}
if (_action->type==pe_action_reset::type_id) {
for(i=0;i<=m_nSegs;i++) {
m_segs[i].vel.zero();
if (m_segs[i].pContactEnt) {
m_segs[i].pContactEnt->Release();
m_segs[i].pContactEnt = 0;
}
}
return 1;
}
return 0;
}
void CRopeEntity::StartStep(float time_interval)
{
m_timeStepPerformed = 0;
m_timeStepFull = time_interval;
}
float CRopeEntity::GetMaxTimeStep(float time_interval)
{
if (m_timeStepPerformed > m_timeStepFull-0.001f)
return time_interval;
return min(min(m_timeStepFull-m_timeStepPerformed,m_maxAllowedStep),time_interval);
}
int __ropeframe = 0;
int CRopeEntity::Step(float time_interval)
{
if (m_nSegs<=0 || !m_bAwake)
return 1;
int i;
for(i=0;i<=m_nSegs;i++) if (m_segs[i].pContactEnt && m_segs[i].pContactEnt->m_iSimClass==7)
m_segs[i].pContactEnt = 0;
for(i=0;i<2;i++) if (m_pTiedTo[i] && m_pTiedTo[i]->m_iSimClass==7)
m_pTiedTo[i] = 0;
if (m_timeStepPerformed > m_timeStepFull-0.001f)
return 1;
m_timeStepPerformed += time_interval;
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
PHYS_ENTITY_PROFILER
__ropeframe++;
float seglen=m_length/m_nSegs,seglen2=sqr(seglen), rseglen=m_nSegs/m_length,rseglen2=sqr(rseglen),Ebefore;
int iDir,iStart,iEnd,iter,bStretched,bHasContacts=0;
vectorf dir,ptend[2],sz;
float len2,diff,a,b,r2,r2new,pAp,vmax,k,E,damping=max(0.0f,1.0f-m_damping*time_interval);
for(i=0;i<=m_nSegs;i++) {
m_segs[i].pt += m_segs[i].vel*time_interval;
m_segs[i].vel += m_gravity*time_interval;
}
for(i=0;i<2;i++) if (m_pTiedTo[i]) {
RigidBody *pbody = m_pTiedTo[i]->GetRigidBody(m_iTiedPart[i]);
m_segs[m_nSegs*i].pt = ptend[i] = pbody->pos + pbody->q*m_ptTiedLoc[i];
m_segs[m_nSegs*i].vel = pbody->v + (pbody->w^ptend[i]-pbody->pos);
}
if (m_pTiedTo[0] && m_pTiedTo[1] && (ptend[1]-ptend[0]).len2()>sqr(m_length*0.99f)) {
float newlen=(ptend[1]-ptend[0]).len(), newseglen=newlen/m_nSegs;
dir = (ptend[1]-ptend[0]).normalized();
for(i=1;i<m_nSegs;i++) {
m_segs[i].dir = dir;
m_segs[i].pt = ptend[0] + m_segs[i].dir*(newseglen*i);
}
m_bAwake = m_pTiedTo[0]->GetStatus(&pe_status_awake()) | m_pTiedTo[1]->GetStatus(&pe_status_awake());
m_BBox[0].x = min(m_segs[0].pt.x,m_segs[m_nSegs].pt.x); m_BBox[1].x = max(m_segs[0].pt.x,m_segs[m_nSegs].pt.x);
m_BBox[0].y = min(m_segs[0].pt.y,m_segs[m_nSegs].pt.y); m_BBox[1].y = max(m_segs[0].pt.y,m_segs[m_nSegs].pt.y);
m_BBox[0].z = min(m_segs[0].pt.z,m_segs[m_nSegs].pt.z); m_BBox[1].z = max(m_segs[0].pt.z,m_segs[m_nSegs].pt.z);
a = max(max(m_BBox[1].x-m_BBox[0].x,m_BBox[1].y-m_BBox[0].y),m_BBox[1].z-m_BBox[0].z)*0.01f+m_collDist*2;
m_BBox[0] -= vectorf(a,a,a); m_BBox[1] += vectorf(a,a,a);
if (m_flags & pef_traceable)
m_pWorld->RepositionEntity(this,1);
return 1;
}
// first, ensure that all vertices have distance between them equal to seglen
iter=300;
iDir = m_pTiedTo[1] && !m_pTiedTo[0] ? 1:-1;
do {
iDir = -iDir;
iStart = m_nSegs & iDir>>31;
iEnd = m_nSegs & -iDir>>31;
for(i=iStart;i!=iEnd;i+=iDir) {
dir = m_segs[i+iDir].pt-m_segs[i].pt; len2 = dir.len2();
diff = fabs_tpl(len2-seglen2);
if (bStretched = (diff>seglen2*0.01f)) {
if (diff<seglen2*0.2f) { // use 3 terms of 1/sqrt(x) Taylor series expansion
k = len2*rseglen2; k = 1.875f-(1.25f-0.375f*k)*k;
} else
k = seglen/sqrt_tpl(len2);
m_segs[i+iDir].pt = m_segs[i].pt + dir*k;
iter--;
} else k = 1.0f;
m_segs[i+(iDir>>31)].dir = dir*(k*rseglen);
}
if (m_pTiedTo[0]) m_segs[0].pt = ptend[0];
if (m_pTiedTo[1]) m_segs[m_nSegs].pt = ptend[1];
} while(m_pTiedTo[iDir+1>>1] && bStretched && iter>0);
m_BBox[0] = m_BBox[1] = m_segs[0].pt;
for(i=0;i<=m_nSegs;i++) {
(m_segs[i].vel += m_segs[i].vel_ext) *= damping;
m_segs[i].vel_ext.zero();
m_segs[i].bRecalcDir = 0;
m_BBox[0].x = min(m_BBox[0].x, m_segs[i].pt.x); m_BBox[1].x = max(m_BBox[1].x, m_segs[i].pt.x);
m_BBox[0].y = min(m_BBox[0].y, m_segs[i].pt.y); m_BBox[1].y = max(m_BBox[1].y, m_segs[i].pt.y);
m_BBox[0].z = min(m_BBox[0].z, m_segs[i].pt.z); m_BBox[1].z = max(m_BBox[1].z, m_segs[i].pt.z);
}
a = max(max(m_BBox[1].x-m_BBox[0].x,m_BBox[1].y-m_BBox[0].y),m_BBox[1].z-m_BBox[0].z)*0.01f+m_collDist*2;
m_BBox[0] -= vectorf(a,a,a); m_BBox[1] += vectorf(a,a,a);
if (m_flags & rope_collides) {
CPhysicalEntity **pentlist;
int j,iseg,nEnts,nCheckParts,nLocChecks,iend;
box boxrope,boxpart;
vectorf center,ptres[2],rotax,n;
float angle,dist2,t,cost,sint;
struct check_part {
vectorf offset;
matrix3x3f R;
float scale;
box bbox;
CPhysicalEntity *pent;
int ipart;
};
check_part checkParts[20];
CRayGeom aray; aray.m_iCollPriority = 10;
intersection_params ip;
geom_contact *pcontact;
CPhysicalEntity *pent;
geom_world_data gwd;
g_Overlapper.Init();
boxrope.size = (m_BBox[1]-m_BBox[0])*0.5f;
center = (m_BBox[0]+m_BBox[1])*0.5f;
ip.bStopAtFirstTri = true;
ip.iUnprojectionMode = 1;
nEnts = m_pWorld->GetEntitiesAround(m_BBox[0],m_BBox[1], pentlist,
(m_flags & rope_collides_with_terrain ? ent_terrain:0) |
ent_static|ent_sleeping_rigid|ent_rigid|ent_living|ent_sort_by_mass|ent_ignore_noncolliding|ent_triggers, this);
for(i=j=nCheckParts=0;i<nEnts;i++) if (pentlist[i]!=this)
for(j=0;j<pentlist[i]->m_nParts;j++) {
checkParts[nCheckParts].offset = pentlist[i]->m_pos+pentlist[i]->m_qrot*pentlist[i]->m_parts[j].pos;
//(pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q).getmatrix(boxrope.Basis); //Q2M_IVO
boxrope.Basis = matrix3x3f(pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q);
boxrope.center = (center-checkParts[nCheckParts].offset)*boxrope.Basis;
if (pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom->GetType()!=GEOM_HEIGHTFIELD) {
pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom->GetBBox(&checkParts[nCheckParts].bbox);
checkParts[nCheckParts].bbox.center *= pentlist[i]->m_parts[j].scale;
checkParts[nCheckParts].bbox.size *= pentlist[i]->m_parts[j].scale;
} else
checkParts[nCheckParts].bbox = boxrope;
boxrope.bOriented++;
if (box_box_overlap_check(&boxrope,&checkParts[nCheckParts].bbox)) {
checkParts[nCheckParts].pent = pentlist[i];
checkParts[nCheckParts].ipart = j;
checkParts[nCheckParts].R = boxrope.Basis;
checkParts[nCheckParts].scale = pentlist[i]->m_parts[j].scale;
pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom->PrepareForRayTest(
pentlist[i]->m_parts[j].scale==1.0f ? seglen:seglen/pentlist[i]->m_parts[j].scale);
if (++nCheckParts==sizeof(checkParts)/sizeof(checkParts[0]))
goto enoughgeoms;
}
} enoughgeoms:
if (m_pTiedTo[0] && m_pTiedTo[1])
iDir = isneg(m_pTiedTo[1]->GetRigidBody(m_iTiedPart[1])->Minv-m_pTiedTo[0]->GetRigidBody(m_iTiedPart[0])->Minv);
else
iDir = iszero((intptr_t)m_pTiedTo[0]);
iDir = 1-iDir*2;
iStart = m_nSegs & iDir>>31;
iEnd = m_nSegs & -iDir>>31;
for(i=iStart;i!=iEnd;i+=iDir) {
iseg = i+(iDir>>31);
if (pent=m_segs[iseg].pContactEnt) {
//(pent->m_qrot*pent->m_parts[m_segs[iseg].iContactPart].q).getmatrix(gwd.R); //Q2M_IVO
gwd.R = matrix3x3f(pent->m_qrot*pent->m_parts[m_segs[iseg].iContactPart].q);
gwd.offset = pent->m_pos + pent->m_qrot*pent->m_parts[m_segs[iseg].iContactPart].pos;
gwd.scale = pent->m_parts[m_segs[iseg].iContactPart].scale;
if (!m_segs[iseg].bRecheckContact &&
pent->m_parts[m_segs[iseg].iContactPart].pPhysGeomProxy->pGeom->FindClosestPoint(&gwd, m_segs[iseg].iPrim,m_segs[iseg].iFeature,
m_segs[i+iDir].pt,m_segs[i].pt, ptres)>=0 &&
(dist2=(n=ptres[1]-ptres[0]).len2())<sqr(m_collDist*2) && // drop contact when gap becomes too big
sqr_signed(m_segs[iseg].vreq=n*m_segs[iseg].ncontact)>n.len2()*sqr(0.75f)) // drop contact if normal changes abruptly
{
n.normalize();
m_segs[i].tcontact = (ptres[1]-m_segs[iseg].pt).len()*rseglen;
if (dist2<sqr(m_collDist*0.85f)) { // reinforce the distance
t = (iDir>>31&1) + m_segs[i].tcontact*iDir; // 1.0-t for iDir==-1
j = isneg(t-0.1f); t += j; j = -j & iDir;
if ((unsigned int)i-j<=(unsigned int)m_nSegs) {
rotax = (m_segs[i+iDir-j].pt-m_segs[i-j].pt^n).normalized();
angle = (m_collDist-sqrt_tpl(dist2))/(t*seglen);
m_segs[i+iDir-j].pt = m_segs[i+iDir-j].pt.rotated(m_segs[i-j].pt, rotax, cos_tpl(angle),sin_tpl(angle));
m_segs[i+iDir-j].bRecalcDir = m_segs[max(0,i+iDir-j-1)].bRecalcDir = 1;
}
m_segs[iseg].vreq = 0;
} else
m_segs[iseg].vreq = (m_collDist-m_segs[iseg].vreq)*10.0f;
m_segs[i].ncontact = n;
RigidBody *pbody = m_segs[iseg].pContactEnt->GetRigidBody(m_segs[iseg].iContactPart);
m_segs[iseg].vcontact = pbody->v+(pbody->w^ptres[0]-pbody->pos);
bHasContacts = 1;
} else {
m_segs[iseg].pContactEnt->Release();
m_segs[iseg].pContactEnt = 0;
}
}
if (!m_segs[iseg].pContactEnt) for(j=nLocChecks=0;j<nCheckParts && nLocChecks<6;j++) {
aray.m_ray.origin = (m_segs[i].pt-checkParts[j].offset)*checkParts[j].R;
aray.m_ray.dir = (m_segs[i+iDir].pt-checkParts[j].offset)*checkParts[j].R-aray.m_ray.origin;
if (box_ray_overlap_check(&checkParts[j].bbox,&aray.m_ray)) {
ip.centerOfRotation = aray.m_ray.origin;
gwd.offset.zero(); gwd.R.SetIdentity(); gwd.scale=checkParts[j].scale;
if (checkParts[j].pent->m_parts[checkParts[j].ipart].pPhysGeomProxy->pGeom->Intersect(&aray,&gwd,0,&ip,pcontact) &&
pcontact->iUnprojMode==1)
{
if (pcontact->t>(real)0.5) {
pcontact->t=(real)0.5; m_segs[iseg].bRecheckContact=1;
} else
m_segs[iseg].bRecheckContact=0;
if (m_segs[iseg].pContactEnt)
m_segs[iseg].pContactEnt->Release();
m_segs[iseg].pContactEnt = 0;
diff = m_segs[iseg].tcontact = (pcontact->pt-aray.m_ray.origin).len();
m_segs[iseg].tcontact = m_segs[iseg].tcontact*iDir-seglen*(iDir>>31); // flip tcontact when iDir is -1
iend = -iseg>>31 & 1;
if ((unsigned int)(iseg-1)>=(unsigned int)(m_nSegs-2) && m_pTiedTo[iend] && isneg(m_segs[iseg].tcontact*rseglen-0.5f)^iend)
continue; // ignore collisions too close to tied ends
(m_segs[iseg].pContactEnt = checkParts[j].pent)->AddRef();
m_segs[iseg].iContactPart = checkParts[j].ipart;
rotax = checkParts[j].R*-pcontact->dir;
if (m_collDist > diff*0.25f) {
if ((unsigned int)(i-iDir)>(unsigned int)m_nSegs)
continue;
// the contact is too close to the segment start (requires >15 deg. gap unproj), rotate start point to get the safe gap
float tgap = m_collDist/seglen;
cost = cos_tpl(tgap); sint = sin_tpl(tgap);
m_segs[i].pt = m_segs[i].pt.rotated(m_segs[i-iDir].pt, rotax, cost,sint);
m_segs[i+iDir].pt = m_segs[i+iDir].pt.rotated(m_segs[i-iDir].pt, rotax, cost,sint);
m_segs[i].bRecalcDir = m_segs[max(0,i-1)].bRecalcDir = 1;
} else
pcontact->t += m_collDist/diff;
cost = cos_tpl(pcontact->t); sint = sin_tpl(pcontact->t);
m_segs[iseg].ncontact = checkParts[j].R*(pcontact->n.normalized().rotated(pcontact->dir,cost,-sint));
m_segs[iseg].tcontact *= rseglen;
m_segs[i+iDir].pt = m_segs[i+iDir].pt.rotated(m_segs[i].pt, rotax, cost,sint);
aray.m_ray.dir = (m_segs[i+iDir].pt-checkParts[j].offset)*checkParts[j].R-aray.m_ray.origin;
m_segs[iseg].iPrim = pcontact->iPrim[0];
m_segs[iseg].iFeature = pcontact->iFeature[0];
/*m_segs[iseg].friction[0] = 0.5f*max(0.0f, m_pWorld->m_FrictionTable[m_surface_idx&NSURFACETYPES-1] +
m_pWorld->m_FrictionTable[pcontact->id[0]&NSURFACETYPES-1]);
m_segs[iseg].friction[1] = 0.5f*max(0.0f, m_pWorld->m_DynFrictionTable[m_surface_idx&NSURFACETYPES-1] +
m_pWorld->m_DynFrictionTable[pcontact->id[0]&NSURFACETYPES-1]);*/
m_segs[iseg].friction[0] = m_segs[iseg].friction[1] = m_friction;
RigidBody *pbody = m_segs[iseg].pContactEnt->GetRigidBody(m_segs[iseg].iContactPart);
m_segs[iseg].vcontact = pbody->v+(pbody->w^pcontact->pt-pbody->pos);
m_segs[i+iDir].bRecalcDir = m_segs[max(0,i+iDir-1)].bRecalcDir = 1;
m_segs[iseg].vreq = 0;
bHasContacts = 1;
//break;
}
//nLocChecks++;
}
}
}
for(i=0;i<m_nSegs;i++) if (m_segs[i].bRecalcDir)
m_segs[i].dir = (m_segs[i+1].pt-m_segs[i].pt).normalized();
}
for(i=0,Ebefore=0;i<=m_nSegs;i++)
Ebefore += m_segs[i].vel.len2();
if (bHasContacts) {
int iter,bBounced;
float vrel,dPtang;
vectorf dp;
for(i=0;i<m_nSegs;i++)
m_segs[i].kdP = 0.5f;
if (m_pTiedTo[0]) m_segs[0].kdP=0.0f;
if (m_pTiedTo[1]) m_segs[m_nSegs-1].kdP=1.0f;
// solve for velocities using relaxation solver with friction
iter = 200;
do {
for(i=bBounced=0;i<m_nSegs;i++) {
if (fabsf(vrel=(m_segs[i+1].vel-m_segs[i].vel)*m_segs[i].dir) > seglen*0.005f) {
m_segs[i].vel += m_segs[i].dir*(vrel*m_segs[i].kdP);
m_segs[i+1].vel -= m_segs[i].dir*(vrel*(1.0f-m_segs[i].kdP));
bBounced++;
}
if (m_segs[i].pContactEnt) {
dp = m_segs[i].vel*(1.0f-m_segs[i].tcontact)+m_segs[i+1].vel*m_segs[i].tcontact-m_segs[i].vcontact;
if ((vrel=dp*m_segs[i].ncontact-m_segs[i].vreq) < -seglen*0.005f) {
if (m_segs[i].friction[0]>0.01f) {
m_segs[i].dP -= dPtang=sqrt_tpl(max(0.0f,dp.len2()-sqr(vrel)));
if (m_segs[i].dP-vrel*m_segs[i].friction[0] < 0) { // friction cannot stop sliding
dp += (dp-m_segs[i].ncontact*vrel)*((m_segs[i].dP-vrel*m_segs[i].friction[1])/dPtang); // remove part of dp that friction cannot stop
dp *= vrel/(m_segs[i].ncontact*dp); // apply impulse along dp so that it stops normal component
m_segs[i].dP = 0;
} else
m_segs[i].dP -= vrel*m_segs[i].friction[0];
} else
dp = m_segs[i].ncontact*vrel;
m_segs[i].vel -= dp*((1.0f-m_segs[i].tcontact)*m_segs[i].kdP);
m_segs[i+1].vel -= dp*(m_segs[i].tcontact*(1.0f-m_segs[i].kdP));
bBounced++;
}
}
}
} while (bBounced && --iter);
} else {
// solve for velocities using conjugate gradient
for(i=0,r2=0;i<m_nSegs;i++) {
r2 += sqr(m_segs[i].dP = m_segs[i].r = (m_segs[i+1].vel-m_segs[i].vel)*m_segs[i].dir);
m_segs[i].cosnext = m_segs[i].dir*m_segs[i+1].dir;
m_segs[i].kdP = 2.0f;
m_segs[i].P = 0;
}
if (m_pTiedTo[0]) m_segs[0].kdP=1.0f;
if (m_pTiedTo[1]) m_segs[m_nSegs-1].kdP=1.0f;
iter = m_nSegs*3;
do {
m_segs[0].dv = m_segs[0].dP*m_segs[0].kdP;
m_segs[1].dv = -m_segs[0].cosnext*m_segs[0].dP;
for(i=1;i<m_nSegs;i++) {
m_segs[i-1].dv -= m_segs[i-1].cosnext*m_segs[i].dP;
m_segs[i].dv += m_segs[i].dP*m_segs[i].kdP;
m_segs[i+1].dv = -m_segs[i].cosnext*m_segs[i].dP;
}
for(i=0,pAp=0;i<m_nSegs;i++)
pAp += m_segs[i].dv*m_segs[i].dP;
if (fabs_tpl(pAp)<1E-10) break;
a = r2/pAp;
for(i=0,r2new=0;i<m_nSegs;i++) {
r2new += sqr(m_segs[i].r -= m_segs[i].dv*a);
m_segs[i].P += m_segs[i].dP*a;
}
if (r2new>r2*500)
break;
b = r2new/r2; r2 = r2new;
for(i=0,vmax=0;i<m_nSegs;i++) {
(m_segs[i].dP*=b) += m_segs[i].r;
vmax = max(vmax,sqr(m_segs[i].r));
}
} while(--iter && vmax>sqr(0.003f));
if (!m_pTiedTo[0]) m_segs[0].vel += m_segs[0].dir*m_segs[0].P;
if (!m_pTiedTo[1]) m_segs[m_nSegs].vel -= m_segs[m_nSegs-1].dir*m_segs[m_nSegs-1].P;
m_segs[1].vel -= m_segs[0].dir*m_segs[0].P;
m_segs[m_nSegs-1].vel += m_segs[m_nSegs-1].dir*m_segs[m_nSegs-1].P;
for(i=1;i<m_nSegs-1;i++) {
m_segs[i].vel += m_segs[i].dir*m_segs[i].P;
m_segs[i+1].vel -= m_segs[i].dir*m_segs[i].P;
}
}
for(i=0,E=0;i<=m_nSegs;i++) E += m_segs[i].vel.len2();
if (E>Ebefore && E>m_Emin) {
k = sqrt_tpl(Ebefore/E);
for(i=0;i<=m_nSegs;i++) m_segs[i].vel*=k;
E = Ebefore;
}
i = -isneg(E-m_Emin*(m_nSegs+1));
m_nSlowFrames = (m_nSlowFrames&i)-i;
if (!(m_bAwake = isneg(m_nSlowFrames-4)))
for(i=iszero((intptr_t)m_pTiedTo[0])^1; i<m_nSegs+iszero((intptr_t)m_pTiedTo[1]); i++)
m_segs[i].vel.zero();
m_pos = m_segs[0].pt;
if (m_flags & pef_traceable)
m_pWorld->RepositionEntity(this,1);
return isneg(m_timeStepFull-m_timeStepPerformed-0.001f);
}
int CRopeEntity::RayTrace(CRayGeom *pRay,geom_contact *&pcontacts)
{
static geom_contact g_RopeContact;
vectorf dp,dir,l,pt;
float t,t1,llen2,raylen=pRay->m_ray.dir*pRay->m_dirn;
int i;
for(i=0;i<m_nSegs;i++) {
dp = pRay->m_ray.origin-m_segs[i].pt;
if ((dp^pRay->m_dirn).len2()<sqr(m_collDist) && inrange((m_segs[i].pt-pRay->m_ray.origin)*pRay->m_dirn, 0.0f,raylen)) {
pt = m_segs[i].pt; break;
}
dir = m_segs[i+1].pt-m_segs[i].pt;
l = dir^pRay->m_dirn; llen2 = l.len2();
t = (dp^pRay->m_dirn)*l; t1 = (dp^dir)*l;
if (sqr(dp*l)<sqr(m_collDist)*llen2 && inrange(t, 0.0f,llen2) && inrange(t1, 0.0f,llen2*raylen)) {
pt = m_segs[i].pt+dir*(t/llen2); break;
}
}
if (i<m_nSegs) {
pcontacts = &g_RopeContact;
pcontacts->pt = pt;
pcontacts->t = (pt-pRay->m_ray.origin)*pRay->m_dirn;
pcontacts->id[0] = m_surface_idx;
pcontacts->iNode[0] = 0;
pcontacts->n = -pRay->m_dirn;
return 1;
}
return 0;
}
int CRopeEntity::GetStateSnapshot(CStream &stm,float time_back,int flags)
{
stm.WriteNumberInBits(SNAPSHOT_VERSION,4);
WritePacked(stm,m_nSegs);
if (m_nSegs>0) for(int i=0; i<=m_nSegs; i++) {
stm.Write(m_segs[i].pt);
if (m_segs[i].vel.len2()>0) {
stm.Write(true);
stm.Write(m_segs[i].vel);
} else stm.Write(false);
}
stm.Write(m_bAwake!=0);
return 1;
}
int CRopeEntity::SetStateFromSnapshot(CStream &stm, int flags)
{
int i,ver=0;
bool bnz;
stm.ReadNumberInBits(ver,4);
if (ver!=SNAPSHOT_VERSION)
return 0;
ReadPacked(stm,i);
if (i!=m_nSegs)
return 0;
if (!(flags & ssf_no_update)) {
if (m_nSegs>0) for(i=0; i<=m_nSegs; i++) {
stm.Read(m_segs[i].pt);
stm.Read(bnz); if (bnz)
stm.Read(m_segs[i].vel);
else
m_segs[i].vel.zero();
if (m_segs[i].pContactEnt) {
m_segs[i].pContactEnt->Release();
m_segs[i].pContactEnt = 0;
}
}
stm.Read(bnz);
m_bAwake = bnz ? 1:0;
for(i=0;i<m_nSegs;i++)
m_segs[i].dir = (m_segs[i+1].pt-m_segs[i].pt).normalized();
m_pos = m_segs[0].pt;
if (m_flags & pef_traceable)
m_pWorld->RepositionEntity(this,1);
} else {
for(i=0;i<=m_nSegs;i++) {
stm.Seek(stm.GetReadPos()+sizeof(vectorf)*8);
stm.Read(bnz); if (bnz)
stm.Seek(stm.GetReadPos()+sizeof(vectorf)*8);
}
stm.Read(bnz);
}
return 1;
}
void CRopeEntity::DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags)
{
CPhysicalEntity::DrawHelperInformation(DrawLineFunc,flags);
int i;
if (flags & pe_helper_geometry) for(i=0;i<m_nSegs;i++)
DrawLineFunc(m_segs[i].pt,m_segs[i+1].pt);
if (flags & pe_helper_collisions) for(i=0;i<m_nSegs;i++) if (m_segs[i].pContactEnt) {
vectorf pt = m_segs[i].pt+(m_segs[i+1].pt-m_segs[i].pt)*m_segs[i].tcontact;
DrawLineFunc(pt,pt+m_segs[i].ncontact*m_pWorld->m_vars.maxContactGap*30);
}
}
void CRopeEntity::GetMemoryStatistics(ICrySizer *pSizer)
{
CPhysicalEntity::GetMemoryStatistics(pSizer);
if (GetType()==PE_ROPE)
pSizer->AddObject(this, sizeof(CRopeEntity));
pSizer->AddObject(m_segs, m_nSegs*sizeof(m_segs[0]));
}

76
CryPhysics/ropeentity.h Normal file
View File

@@ -0,0 +1,76 @@
#ifndef ropeentity_h
#define ropeentity_h
struct rope_segment {
~rope_segment() { if (pContactEnt) pContactEnt->Release(); }
vectorf pt;
vectorf vel;
vectorf dir;
vectorf vel_ext;
CPhysicalEntity *pContactEnt;
int iContactPart;
int bRecheckContact;
float tcontact;
vectorf ncontact;
vectorf vcontact;
float vreq;
float friction[2];
int iPrim,iFeature;
int bRecalcDir;
float r,P;
float dP,dv;
float cosnext,kdP;
};
class CRopeEntity : public CPhysicalEntity {
public:
CRopeEntity(CPhysicalWorld *pworld);
virtual ~CRopeEntity();
virtual pe_type GetType() { return PE_ROPE; }
virtual int SetParams(pe_params*);
virtual int GetParams(pe_params*);
virtual int GetStatus(pe_status*);
virtual int Action(pe_action*);
virtual void StartStep(float time_interval);
virtual float GetMaxTimeStep(float time_interval);
virtual int Step(float time_interval);
virtual int Awake(int bAwake=1,int iSource=0);
virtual int IsAwake(int ipart=-1) { return m_bAwake; }
virtual void AlertNeighbourhoodND();
virtual int RayTrace(CRayGeom *pRay, geom_contact *&pcontacts);
enum snapver { SNAPSHOT_VERSION = 8 };
virtual int GetStateSnapshot(CStream &stm, float time_back=0,int flags=0);
virtual int SetStateFromSnapshot(CStream &stm, int flags);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
vectorf m_gravity;
float m_damping;
float m_maxAllowedStep;
float m_Emin;
int m_bAwake;
float m_timeStepPerformed,m_timeStepFull;
int m_nSlowFrames;
float m_length;
int m_nSegs;
float m_mass;
float m_collDist;
int m_surface_idx;
float m_friction;
rope_segment *m_segs;
CPhysicalEntity *m_pTiedTo[2];
vectorf m_ptTiedLoc[2];
int m_iTiedPart[2];
int m_idConstraint;
int m_iConstraintClient;
};
#endif

533
CryPhysics/rotunprojectionchecks.cpp Normal file
View File

@@ -0,0 +1,533 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "unprojectionchecks.h"
#define UPDATE_IDBEST(tbest,newid) \
(idbest&=~-bBest) |= (newid)&-bBest; \
(tbest.x*=bBest^1) += tsin.x*bBest; \
(tbest.y*=bBest^1) += tsin.y*bBest
int tri_tri_rot_unprojection(unprojection_mode *pmode, const triangle *ptri1,int iFeature1, const triangle *ptri2,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
const triangle *ptri[2] = { ptri1,ptri2 };
vectorr pt,ptz[2][3],ptx[2][3],pty[2][3],rotax,pt0,pt1,edge0,edge1,n0z,n0x,n0y,ptz0,ptx0,pty0,n,pt0_rot,
pt1_rot,edge0_rot,ptz1,ptx1,pty1,dir0,dir1,norm[2];
quotient tsin,tcos,tmin(pmode->tmax,1),t0,t1;
real kcos,ksin,k,a,b,c,d,sg0,sg1;
int itri,i,j,j1,bContact,bBest,idbest=-1,imask=iFeature2<<8 | iFeature1;
// compute components for vec.rot(t) = vecz + vecx*cos(t) + vecy*sin(t)
for(itri=0,rotax=pmode->dir;itri<2;itri++,rotax*=-1) for(i=0;i<3;i++) {
pt = ptri[itri]->pt[i]-pmode->center;
ptz[itri][i] = rotax*(pt*rotax); ptx[itri][i] = pt-ptz[itri][i]; pty[itri][i] = rotax^ptx[itri][i];
}
// vertex-face contacts, (p[i].rot(t)-origin)*n = 0; -t for the 2nd triangle
for(itri=0;itri<2;itri++) for(i=0;i<3;i++) if ((0x40<<(itri<<3) | (0x80|i)<<((itri^1)<<3))!=imask) {
kcos = ptx[itri^1][i]*ptri[itri]->n; ksin = pty[itri^1][i]*ptri[itri]->n;
k = (ptz[itri^1][i]-ptri[itri]->pt[0]+pmode->center)*ptri[itri]->n;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) (=eliminate foreign root)
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt = ptx[itri^1][i]*tcos.x + pty[itri^1][i]*tsin.x + (ptz[itri^1][i]+pmode->center)*tsin.y;
bContact = // point is inside itri
isneg((pt-ptri[itri]->pt[0]*tsin.y ^ ptri[itri]->pt[1]-ptri[itri]->pt[0])*ptri[itri]->n) &
isneg((pt-ptri[itri]->pt[1]*tsin.y ^ ptri[itri]->pt[2]-ptri[itri]->pt[1])*ptri[itri]->n) &
isneg((pt-ptri[itri]->pt[2]*tsin.y ^ ptri[itri]->pt[0]-ptri[itri]->pt[2])*ptri[itri]->n);
// additionally check that triangles don't intersect during contact
pt = ptx[itri^1][inc_mod3[i]]*tcos.x + pty[itri^1][inc_mod3[i]]*tsin.x + (ptz[itri^1][inc_mod3[i]] +
pmode->center - ptri[itri]->pt[0])*tsin.y;
sg0 = pt*ptri[itri]->n + pmode->minPtDist*tsin.y;
pt = ptx[itri^1][dec_mod3[i]]*tcos.x + pty[itri^1][dec_mod3[i]]*tsin.x + (ptz[itri^1][dec_mod3[i]] +
pmode->center - ptri[itri]->pt[0])*tsin.y;
sg1 = pt*ptri[itri]->n + pmode->minPtDist*tsin.y;
bContact &= isnonneg(sg0*sg1);
bBest = bContact & isneg(tsin-tmin);
UPDATE_IDBEST(tmin,itri<<2 | i);
}
}
} else { // check if triangles are already separated in their initial state
sg0 = (ptri[itri^1]->pt[inc_mod3[i]]-ptri[itri]->pt[0])*ptri[itri]->n + pmode->minPtDist;
sg1 = (ptri[itri^1]->pt[dec_mod3[i]]-ptri[itri]->pt[0])*ptri[itri]->n + pmode->minPtDist;
bBest = isnonneg(sg0) & isnonneg(sg1); tsin.set(0,1);
UPDATE_IDBEST(tmin,itri<<2 | i);
}
rotax = pmode->dir;
n0z = rotax*(ptri[0]->n*rotax); n0x = ptri[0]->n-n0z; n0y = rotax^n0x;
// edge-edge contacts
// (p0.rot(t)-p1)*(l0.rot(t)^l1) = 0
// p0.rot(t)*(l0.rot(t)^l1) - p1*(l0.rot(t)^l1) = 0;
// l1*(p0^l0).rot(t) + l0.rot(t)*(p1^l1) = 0;
// l1*(p0^l0).rot(t) + l0*(p1^l1).rot(-t) = 0; - this one's just to get symmetrical form
// note that p.rot(t)^l.rot(t)==(p^l).rot(t) only if p is relative to the center of rotation
for(i=0;i<3;i++) {
pt0 = ptri[0]->pt[i]-pmode->center;
edge0 = ptri[0]->pt[inc_mod3[i]]-ptri[0]->pt[i];
pt = pt0^edge0; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
for(j=0;j<3;j++) if ((0xA0|i | (0xA0|j)<<8)!=imask) {
pt1 = ptri[1]->pt[j]-pmode->center;
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j];
pt = pt1^edge1; ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
kcos = edge1*ptx0+edge0*ptx1; ksin = edge1*pty0-edge0*pty1; k = edge1*ptz0+edge0*ptz1;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j1=0;j1<2;j1++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) (=eliminate foreign root)
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt0_rot = ptx[0][i]*tcos.x + pty[0][i]*tsin.x + ptz[0][i]*tsin.y;
pt1_rot = ptx[0][inc_mod3[i]]*tcos.x + pty[0][inc_mod3[i]]*tsin.x + ptz[0][inc_mod3[i]]*tsin.y;
edge0_rot = pt1_rot-pt0_rot;
n = edge0_rot^edge1; pt = pt1*tsin.y-pt0_rot;
t0.set((pt^edge1)*n, n.len2());
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
bContact = isneg(fabs_tpl(t0.x*2-t0.y)-t0.y) & isneg(fabs_tpl(t1.x*2-t1.y)-t1.y);
// additionally check that triangles don't intersect during contact
dir0 = (n0z*tsin.y+n0x*tcos.x+n0y*tsin.x)^edge0_rot;
dir1 = ptri[1]->n^edge1;
bContact &= isneg((dir0*n)*(dir1*n));
bBest = bContact & isneg(tsin-tmin);
UPDATE_IDBEST(tmin,i<<2 | j | 0x80);
}
}
} else { // check if triangles are already separated in their initial state
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j]; n = edge0^edge1;
dir0 = ptri[0]->n^edge0; dir1 = ptri[1]->n^edge1;
bBest = isneg((dir0*n)*(dir1*n)); tsin.set(0,1);
// additionally check that edges intersection point will not move inside the 2nd triangle during unprojection
bBest &= isneg(dir1*(pmode->dir ^ (ptri[0]->pt[i]-pmode->center)*n.len2() + edge0*((ptri[1]->pt[j]-ptri[0]->pt[i]^edge1)*n)));
UPDATE_IDBEST(tmin,i<<2 | j | 0x80);
}
}
if (idbest==-1)
return 0;
if (idbest & 0x80) {
i = idbest>>2 & 3; j = idbest & 3;
pcontact->t = tmin.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pt0_rot = ptx[0][i]*pcontact->taux + pty[0][i]*pcontact->t + ptz[0][i] + pmode->center;
pt1_rot = ptx[0][inc_mod3[i]]*pcontact->taux + pty[0][inc_mod3[i]]*pcontact->t + ptz[0][inc_mod3[i]] + pmode->center;
edge0_rot = pt1_rot-pt0_rot;
edge1 = ptri[1]->pt[inc_mod3[j]]-ptri[1]->pt[j];
pt = ptri[1]->pt[j]-pt0_rot; n = edge0_rot ^ edge1;
t0.set((pt^edge1)*n, n.len2());
pcontact->pt = pt0_rot + edge0_rot*t0.val();
pcontact->n = n*sgnnz((ptri2->n^edge1)*n);
pcontact->iFeature[0] = 0xA0 | i;
pcontact->iFeature[1] = 0xA0 | j;
} else {
itri = idbest>>2; i = idbest & 3;
pcontact->t = tmin.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pcontact->pt = ptz[itri^1][i] + ptx[itri^1][i]*max(pcontact->taux,(real)(itri^1)) + pty[itri^1][i]*(pcontact->t*itri) + pmode->center;
norm[0] = n0z+n0x*pcontact->taux+n0y*pcontact->t; norm[1] = -ptri[1]->n;
pcontact->n = norm[itri];
pcontact->iFeature[itri] = 0x40;
pcontact->iFeature[itri^1] = 0x80 | i;
}
return 1;
}
int ray_tri_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
vectorr pt,ptz[2],ptx[2],pty[2],rotax=pmode->dir,edge1,ptz0,ptx0,pty0,pt1,pt0_rot,pt1_rot,edge0_rot,ptz1,ptx1,pty1,n;
quotient tsin,tcos,tmin(pmode->tmax,1),t0,t1;
real kcos,ksin,k,a,b,c,d;
int i,j,bContact,bBest,idbest=-1,imask=iFeature2<<8 | iFeature1;
pt = pray->origin-pmode->center;
ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
pt += pray->dir;
ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
// ray end - triangle face
for(i=1;i>=0;i--) {
kcos = ptx[i]*ptri->n; ksin = pty[i]*ptri->n;
k = (ptz[i]-ptri->pt[0]+pmode->center)*ptri->n;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt = ptx[i]*tcos.x + pty[i]*tsin.x + (ptz[i]+pmode->center)*tsin.y;
bContact = // point is inside itri
isneg((pt-ptri->pt[0]*tsin.y ^ ptri->pt[1]-ptri->pt[0])*ptri->n) &
isneg((pt-ptri->pt[1]*tsin.y ^ ptri->pt[2]-ptri->pt[1])*ptri->n) &
isneg((pt-ptri->pt[2]*tsin.y ^ ptri->pt[0]-ptri->pt[2])*ptri->n);
bBest = bContact & isneg(tsin-tmin);
UPDATE_IDBEST(tmin,i);
}
}
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
break;
}
if (idbest>=0) {
i = idbest & 1;
pcontact->t = tmin.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
pcontact->n = -ptri->n;
pcontact->iFeature[0] = 0x80 | i;
pcontact->iFeature[1] = 0x40;
return 1;
}
// ray - triangle edge
pt = pray->origin-pmode->center^pray->dir; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
for(i=0;i<3;i++) if ((0xA0 | (0xA0|i)<<8)!=imask) {
pt1 = ptri->pt[i]-pmode->center; edge1 = ptri->pt[inc_mod3[i]]-ptri->pt[i];
pt = pt1^edge1; ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
kcos = edge1*ptx0+pray->dir*ptx1; ksin = edge1*pty0-pray->dir*pty1; k = edge1*ptz0+pray->dir*ptz1;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt0_rot = ptx[0]*tcos.x + pty[0]*tsin.x + ptz[0]*tsin.y;
pt1_rot = ptx[1]*tcos.x + pty[1]*tsin.x + ptz[1]*tsin.y;
edge0_rot = pt1_rot-pt0_rot;
n = edge0_rot^edge1; pt = pt1*tsin.y-pt0_rot;
t0.set((pt^edge1)*n, n.len2());
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
bContact = isneg(fabs_tpl(t0.x*2-t0.y)-t0.y) & isneg(fabs_tpl(t1.x*2-t1.y)-t1.y);
bBest = bContact & isneg(tsin-tmin);
UPDATE_IDBEST(tmin,i | 0x80);
}
}
}
if (idbest<0)
return 0;
i = idbest & 3;
pcontact->t = tmin.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pt0_rot = ptx[0]*pcontact->taux + pty[0]*pcontact->t + ptz[0] + pmode->center;
pt1_rot = ptx[1]*pcontact->taux + pty[1]*pcontact->t + ptz[1] + pmode->center;
edge0_rot = pt1_rot-pt0_rot;
edge1 = ptri->pt[inc_mod3[i]]-ptri->pt[i];
pt = ptri->pt[i]-pt0_rot; n = edge0_rot ^ edge1;
t0.set((pt^edge1)*n, n.len2());
pcontact->pt = pt0_rot + edge0_rot*t0.val();
pcontact->n = n*sgnnz((ptri->n^edge1)*n);
pcontact->iFeature[0] = 0xA0;
pcontact->iFeature[1] = 0xA0 | i;
return 1;
}
int tri_ray_rot_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1, const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
int res = ray_tri_rot_unprojection(pmode,pray,iFeature2,ptri,iFeature1,pcontact,parea);
if (res) {
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
}
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
return res;
}
int ray_cyl_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const cylinder *pcyl,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
vectorr center,ccap,v,vx,vy,vz,l,lx,ly,lz,vsin,vcos,pt,ptz[2],ptx[2],pty[2], rotax=pmode->dir, axis=pcyl->axis;
real a,b,c,d,k,ksin,kcos, hh=pcyl->hh, r2=sqr(pcyl->r), len2=pray->dir.len2(), roots[4];
quotient tmax(0,1),tsin,tcos,t;
int i,j,icap,idbest=-1,bContact,bBest;
polynomial_tpl<real,4> pn;
center = pcyl->center-pmode->center;
pt = pray->origin-pmode->center;
ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
pt += pray->dir;
ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
for(i=1; i>=0; i--) {
// ray end - cylinder cap
kcos = ptx[i]*axis; ksin = pty[i]*axis;
for(icap=-1; icap<=1; icap+=2) {
k = (ptz[i]-center-axis*(hh*icap))*axis;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) = remove phantom roots
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt = ptx[i]*tcos.x + pty[i]*tsin.x + ptz[i]*tsin.y;
bContact = isneg((pt-(center+axis*(hh*icap))*tsin.y).len2() - r2*sqr(tsin.y));
bBest = bContact & isneg(tmax-tsin);
UPDATE_IDBEST(tmax,icap+1|i);
}
}
}
// ray end - cylinder side
v = ptz[i]-center ^ axis;
vcos = ptx[i]^axis; vsin = pty[i]^axis;
pn = psqr(P2(sqr(vsin)-sqr(vcos)) + P1((v*vsin)*2) + sqr(v)+sqr(vcos)-r2) - psqr(P1(vcos*vsin)+v*vcos)*((real)1-P2(1))*4;
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
tsin.set(roots[j],1);
pt = ptz[i]+ptx[i]*sqrt_tpl(1-sqr(tsin.x))+pty[i]*tsin.x-center;
bContact = isneg(fabs_tpl(pt*axis)-hh) & inrange((pt^axis).len2(), r2*sqr((real)0.99),r2*sqr((real)1.01));
bBest = isneg(tmax-tsin) & bContact;
UPDATE_IDBEST(tmax,0x10|i);
}
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
break;
}
// ray - cylinder side
lz = rotax*(pray->dir*rotax); lx = pray->dir-lz; ly = rotax^lx;
v = pray->origin-pmode->center^pray->dir;
vz = rotax*(v*rotax); vx = v-vz; vy = rotax^vx;
v = axis^center;
kcos = axis*vx-v*lx; ksin = axis*vy-v*ly; k = axis*vz-v*lz;
vcos = lx^axis; vsin = ly^axis; v = lz^axis;
pn = psqr(P2(sqr(ksin)-sqr(kcos)-r2*(sqr(vsin)-sqr(vcos)))+P1(((ksin*k)-r2*(vsin*v))*2)+sqr(kcos)+sqr(k)-r2*(sqr(vcos)+sqr(v))) -
psqr(P1((kcos*ksin-r2*(vcos*vsin))*2)+(kcos*k-r2*(vcos*v))*2);
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
tsin.set(roots[j],1); tcos.x = sqrt_tpl(1-sqr(tsin.x));
pt = ptz[0] + ptx[0]*tcos.x + pty[0]*tsin.x - center;
l = lz + lx*tcos.x + ly*tsin.x; v = l^axis; k = v.len2();
bContact = inrange(sqr(pt*v), r2*k*sqr((real)0.99),r2*k*sqr((real)1.01)); // remove phantom roots
t.set((-pt^axis)*v, k);
(pt*=t.y) += l*t.x;
bContact &= inrange(sqr(t.x), (real)0,len2*sqr(t.y));
bContact &= isneg(fabs_tpl(pt*axis)-hh*t.y);
bBest = isneg(tmax-tsin) & bContact;
UPDATE_IDBEST(tmax,0x20);
}
// ray - cylinder cap
for(icap=-1; icap<=1; icap+=2) {
ccap = center+axis*(hh*icap);
vcos = (axis^vx)-(axis^(ccap^lx)); vsin = (axis^vy)-(axis^(ccap^ly)); v = (axis^vz)-(axis^(ccap^lz));
kcos = lx*axis; ksin = ly*axis; k = lz*axis;
pn = psqr(P2(sqr(vsin)-sqr(vcos)-r2*(sqr(ksin)-sqr(kcos)))+P1(((vsin*v)-r2*(ksin*k))*2)+sqr(vcos)+sqr(v)-r2*(sqr(kcos)+sqr(k))) -
psqr(P1((vcos*vsin-r2*(kcos*ksin))*2)+(vcos*v-r2*(kcos*k))*2);
if (pn.nroots(0,1)) for(j=pn.findroots(0,1,roots)-1;j>=0;j--) {
tsin.set(roots[j],1); tcos.x = sqrt_tpl(1-sqr(tsin.x));
pt = ptz[0] + ptx[0]*tcos.x + pty[0]*tsin.x;
l = lz + lx*tcos.x + ly*tsin.x;
t.set((ccap-pt)*axis, l*axis);
bContact = inrange(((pt-ccap)*t.y+l*t.x).len2(), r2*sqr(t.y*(real)0.99),r2*sqr(t.y*(real)1.01)); // remove phantom roots
bContact &= inrange(t.x, (real)0,t.y);
bBest = isneg(tmax-tsin) & bContact;
UPDATE_IDBEST(tmax,0x40|icap+1>>1);
}
}
if (idbest<0)
return 0;
switch (idbest & 0xF0) {
case 0x00: // ray end - cyl cap
i = idbest & 1; icap = (idbest&2)-1;
pcontact->t = tmax.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
pcontact->n = axis*-icap;
pcontact->iFeature[0] = 0x80 | i;
pcontact->iFeature[1] = 0x41 + (icap+1>>1);
break;
case 0x10: // ray end - cyl side
i = idbest & 1;
pcontact->t = tmax.x;
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pcontact->pt = ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + pmode->center;
pcontact->n = pcyl->center-pcontact->pt;
(pcontact->n -= pcyl->axis*(pcyl->axis*pcontact->n)).normalize();
pcontact->iFeature[0] = 0x80 | i;
pcontact->iFeature[1] = 0x40;
break;
case 0x20: // ray - cyl side
pcontact->t = tmax.x;
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pt = ptz[0] + ptx[0]*pcontact->taux + pty[0]*pcontact->t;
l = lz + lx*pcontact->taux + ly*pcontact->t;
t.set((pt^axis)*(l^axis),(l^axis).len2());
pcontact->pt = pt+l*t.val()+pmode->center;
pcontact->n = (l^axis).normalized();
pcontact->n *= sgnnz(pcontact->n*(pcyl->center-pcontact->pt));
pcontact->iFeature[0] = 0x20;
pcontact->iFeature[1] = 0x40;
break;
case 0x40: // ray - cyl cap
icap = ((idbest&1)<<1)-1;
pcontact->t = tmax.x;
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pt = ptz[0] + ptx[0]*pcontact->taux + pty[0]*pcontact->t;
l = lz + lx*pcontact->taux + ly*pcontact->t;
ccap = center+axis*(hh*icap);
t.set((ccap-pt)*axis,l*axis);
pcontact->pt = pt+l*t.val();
pcontact->n = pcontact->pt-ccap; pcontact->pt += pmode->center;
pcontact->n = ((axis^pcontact->n)^l).normalized();
pcontact->n *= sgnnz(pcontact->n*(pcyl->center-pcontact->pt));
pcontact->iFeature[0] = 0x20;
pcontact->iFeature[1] = 0x20|icap+1>>1;
}
return 1;
}
int cyl_ray_rot_unprojection(unprojection_mode *pmode, const cylinder *pcyl,int iFeature1, const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
int res = ray_cyl_rot_unprojection(pmode,pray,iFeature2,pcyl,iFeature1,pcontact,parea);
if (res) {
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
}
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
return res;
}
int ray_box_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const box *pbox,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
int i,j,ifeat,idir,ix,iy,bContact,bBest,idbest=-1;
vectorr pt,ptz[2],ptx[2],pty[2],ptz0,ptx0,pty0,pt1,pt0_rot,pt1_rot,edge0_rot,ptz1,ptx1,pty1,origin,dir,center,rotax,n,size;
quotient tsin,tcos,tmax(0,1),t0,t1;
real kcos,ksin,k,a,b,c,d;
origin = pbox->Basis*(pray->origin-pbox->center);
dir = pbox->Basis*pray->dir;
center = pbox->Basis*(pmode->center-pbox->center);
rotax = pbox->Basis*pmode->dir;
size = pbox->size;
pt = origin-center; ptz[0] = rotax*(pt*rotax); ptx[0] = pt-ptz[0]; pty[0] = rotax^ptx[0];
pt += dir; ptz[1] = rotax*(pt*rotax); ptx[1] = pt-ptz[1]; pty[1] = rotax^ptx[1];
// ray end - box face
for(i=1;i>=0;i--) {
for(ifeat=0;ifeat<6;ifeat++) {
idir = ifeat>>1; ix = inc_mod3[idir]; iy = dec_mod3[idir];
kcos = ptx[i][idir]; ksin = pty[i][idir];
k = ptz[i][idir]-size[idir]*((ifeat&1)*2-1)+center[idir];
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1) = remove phantom roots
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt = ptx[i]*tcos.x + pty[i]*tsin.x + (ptz[i]+pmode->center)*tsin.y;
bContact = // point is inside the face
isneg(fabs_tpl(pt[ix])-size[ix]*tsin.y) & isneg(fabs_tpl(pt[iy])-size[iy]*tsin.y);
bBest = bContact & isneg(tmax-tsin);
UPDATE_IDBEST(tmax,i|ifeat<<1);
}
}
}
if ((pray->origin-pmode->center).len2()<sqr(pmode->minPtDist))
break;
}
// ray - box edge
pt = origin-center^dir; ptz0 = rotax*(pt*rotax); ptx0 = pt-ptz0; pty0 = rotax^ptx0;
for(ifeat=0;ifeat<12;ifeat++) {
idir = ifeat>>2; ix = inc_mod3[idir]; iy = dec_mod3[idir];
pt1[idir]=0; pt1[ix]=size[ix]*((ifeat&1)*2-1); pt1[iy]=size[iy]*((ifeat&2)-1);
pt1 = pt1-center;
pt = cross_with_ort(pt1,idir); ptz1 = rotax*(pt*rotax); ptx1 = pt-ptz1; pty1 = rotax^ptx1;
kcos = ptx0[idir]+dir*ptx1; ksin = pty0[idir]-dir*pty1; k = ptz0[idir]+dir*ptz1;
a = sqr(ksin)+sqr(kcos); b = ksin*k; c = sqr(k)-sqr(kcos); d = b*b-a*c;
if (d>=0) {
d = sqrt_tpl(d); tsin.set(-b-d,a);
for(j=0;j<2;j++,tsin.x+=d*2)
if (isneg(fabs_tpl(tsin.x*2-tsin.y)-tsin.y) & // tsin in (0..1)
isneg((ksin*tsin.x+k*tsin.y)*kcos)) // tcos in (0..1)
{
tcos.set(sqrt_tpl(sqr(tsin.y)-sqr(tsin.x)),tsin.y);
pt0_rot = ptx[0]*tcos.x + pty[0]*tsin.x + ptz[0]*tsin.y;
pt1_rot = ptx[1]*tcos.x + pty[1]*tsin.x + ptz[1]*tsin.y;
edge0_rot = pt1_rot-pt0_rot;
n = cross_with_ort(edge0_rot,idir); pt = pt1*tsin.y-pt0_rot;
t0.set(cross_with_ort(pt,idir)*n, n.len2());
t1.set((pt^edge0_rot)*n, t0.y*tsin.y);
bContact = inrange(t0.x, (real)0,t0.y) & isneg(fabs_tpl(t1.x)-fabs_tpl(t1.y)*size[idir]);
bBest = bContact & isneg(tmax-tsin);
UPDATE_IDBEST(tmax,ifeat | 0x80);
}
}
}
if (idbest<0)
return 0;
if ((idbest & 0x80)==0) {
i = idbest & 1; ifeat = idbest>>1; idir = ifeat>>1;
pcontact->t = tmax.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pcontact->pt = (ptz[i] + ptx[i]*pcontact->taux + pty[i]*pcontact->t + center)*pbox->Basis + pbox->center;
pcontact->n.zero()[idir]=1-(ifeat&1)*2; pcontact->n = pcontact->n*pbox->Basis;
pcontact->iFeature[0] = 0x80 | i;
pcontact->iFeature[1] = 0x40 | ifeat;
} else {
ifeat = idbest&0x7F; idir = ifeat>>2; ix = inc_mod3[idir]; iy = dec_mod3[idir];
pcontact->t = tmax.val();
pcontact->taux = sqrt_tpl(1-sqr(pcontact->t));
pt0_rot = ptx[0]*pcontact->taux + pty[0]*pcontact->t + ptz[0] + center;
pt1_rot = ptx[1]*pcontact->taux + pty[1]*pcontact->t + ptz[1] + center;
edge0_rot = pt1_rot-pt0_rot;
pt[idir]=0; pt[ix]=size[ix]*((ifeat&1)*2-1); pt[iy]=size[iy]*((ifeat&2)-1);
n = cross_with_ort(edge0_rot,idir);
t0.set(cross_with_ort(pt-pt0_rot,idir)*n, n.len2());
pt = pt0_rot + edge0_rot*t0.val();
pcontact->n = (n*-sgnnz(n*pt))*pbox->Basis;
pcontact->pt = pt*pbox->Basis+pbox->center;
pcontact->iFeature[0] = 0xA0;
pcontact->iFeature[1] = 0x20 | ifeat;
}
return 1;
}
int box_ray_rot_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1, const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea)
{
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
int res = ray_box_rot_unprojection(pmode,pray,iFeature2,pbox,iFeature1,pcontact,parea);
if (res) {
pcontact->pt = pcontact->pt.rotated(pmode->center,pmode->dir, pcontact->taux,-pcontact->t);
pcontact->n = pcontact->n.rotated(pmode->dir, pcontact->taux,-pcontact->t).Flip();
int iFeature = pcontact->iFeature[0]; pcontact->iFeature[0]=pcontact->iFeature[1]; pcontact->iFeature[1]=iFeature;
}
(const_cast<unprojection_mode*>(pmode))->dir.Flip();
return res;
}

114
CryPhysics/singleboxtree.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "bvtree.h"
#include "geometry.h"
#include "singleboxtree.h"
float CSingleBoxTree::Build(CGeometry *pGeom)
{
m_pGeom = pGeom;
return m_Box.size.volume()*8;
}
void CSingleBoxTree::SetBox(box *pbox)
{
m_Box.Basis = pbox->Basis;
m_Box.bOriented = m_Box.Basis.IsIdentity()^1;
m_Box.center = pbox->center;
m_Box.size = pbox->size;
}
void CSingleBoxTree::GetNodeBV(BV *&pBV,int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = 0;
((BBox*)pBV)->abox.Basis = m_Box.Basis;
((BBox*)pBV)->abox.bOriented = m_Box.bOriented;
((BBox*)pBV)->abox.center = m_Box.center;
((BBox*)pBV)->abox.size = m_Box.size;
}
void CSingleBoxTree::GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = 0;
box boxstatic;
boxstatic.Basis = m_Box.Basis;
boxstatic.bOriented = m_Box.bOriented;
boxstatic.center = m_Box.center;
boxstatic.size = m_Box.size;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &((BBox*)pBV)->abox);
}
void CSingleBoxTree::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = 0;
((BBox*)pBV)->abox.Basis = m_Box.Basis*Rw.T();
((BBox*)pBV)->abox.bOriented = 1;
((BBox*)pBV)->abox.center = Rw*m_Box.center*scalew + offsw;
((BBox*)pBV)->abox.size = m_Box.size*scalew;
}
void CSingleBoxTree::GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV,
const vectorf &sweepdir,float sweepstep, int iNode)
{
pBV = g_BBoxBuf + g_BBoxBufPos++;
pBV->type = box::type;
pBV->iNode = 0;
box boxstatic;
boxstatic.Basis = m_Box.Basis*Rw.T();
boxstatic.bOriented = 1;
boxstatic.center = Rw*m_Box.center*scalew + offsw;
boxstatic.size = m_Box.size*scalew;
ExtrudeBox(&boxstatic, sweepdir,sweepstep, &((BBox*)pBV)->abox);
}
int CSingleBoxTree::GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp)
{
return m_pGeom->GetPrimitiveList(0,m_nPrims, pBVCollider->type,*pBVCollider,bColliderLocal, pGTest,pGTestOp, pGTest->primbuf,pGTest->idbuf);
}
int CSingleBoxTree::PrepareForIntersectionTest(geometry_under_test *pGTest)
{
pGTest->pUsedNodesMap = &pGTest->nUsedNodes;
pGTest->nUsedNodes = 0;
return 1;
}
void CSingleBoxTree::MarkUsedTriangle(int itri, geometry_under_test *pGTest)
{
pGTest->nUsedNodes = 1;
}
void CSingleBoxTree::GetBBox(box *pbox)
{
pbox->Basis = m_Box.Basis;
pbox->bOriented = m_Box.bOriented;
pbox->center = m_Box.center;
pbox->size = m_Box.size;
}
void CSingleBoxTree::GetMemoryStatistics(ICrySizer *pSizer)
{
SIZER_COMPONENT_NAME(pSizer, "SingleBox trees");
pSizer->AddObject(this, sizeof(CSingleBoxTree));
}
void CSingleBoxTree::Save(CMemStream &stm)
{
stm.Write(m_Box);
stm.Write(m_nPrims);
}
void CSingleBoxTree::Load(CMemStream &stm, CGeometry *pGeom)
{
m_pGeom = pGeom;
stm.Read(m_Box);
stm.Read(m_nPrims);
}

31
CryPhysics/singleboxtree.h Normal file
View File

@@ -0,0 +1,31 @@
#ifndef singleboxtree_h
#define singleboxtree_h
class CSingleBoxTree : public CBVTree {
public:
CSingleBoxTree() { m_nPrims = 1; }
virtual int GetType() { return BVT_SINGLEBOX; }
virtual float Build(CGeometry *pGeom);
void SetBox(box *pbox);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest);
virtual void GetBBox(box *pbox);
virtual int MaxPrimsInNode() { return m_nPrims; }
virtual void GetNodeBV(BV *&pBV, int iNode=0);
virtual void GetNodeBV(BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, int iNode=0);
virtual void GetNodeBV(const matrix3x3f &Rw,const vectorf &offsw,float scalew, BV *&pBV, const vectorf &sweepdir,float sweepstep, int iNode=0);
virtual int GetNodeContents(int iNode, BV *pBVCollider,int bColliderUsed,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp);
virtual int GetNodeContentsIdx(int iNode, int &iStartPrim) { iStartPrim=0; return m_nPrims; }
virtual void MarkUsedTriangle(int itri, geometry_under_test *pGTest);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm, CGeometry *pGeom);
CGeometry *m_pGeom;
box m_Box;
int m_nPrims;
};
#endif

982
CryPhysics/softentity.cpp Normal file
View File

@@ -0,0 +1,982 @@
//////////////////////////////////////////////////////////////////////
//
// Soft Entity
//
// File: softentity.cpp
// Description : CSoftEntity class implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "bvtree.h"
#include "geometry.h"
#include "overlapchecks.h"
#include "intersectionchecks.h"
#include "raybv.h"
#include "raygeom.h"
#include "singleboxtree.h"
#include "trimesh.h"
#include "rigidbody.h"
#include "physicalplaceholder.h"
#include "physicalentity.h"
#include "geoman.h"
#include "physicalworld.h"
#include "softentity.h"
CSoftEntity::CSoftEntity(CPhysicalWorld *pworld) : CPhysicalEntity(pworld)
{
m_vtx=0; m_edges=0; m_pVtxEdges=0;
m_nVtx=m_nEdges=0;
m_maxAllowedStep = 0.1f;
m_ks = 200.0f;
m_thickness = 0.04f;
m_maxSafeStep = 0.2f;
m_kdRatio = 0.9f;
m_airResistance = 0;//10.0f;
m_wind.zero();
m_waterDensity = 0;
m_waterPlane.n.Set(0,0,1);
m_waterPlane.origin.Set(0,0,0);
m_waterFlow.zero();
m_waterResistance = 0;
m_waterDamping = 1.5f;
m_gravity.Set(0,0,-9.8f);
m_damping = 0;
m_Emin = sqr(0.01f);
m_iSimClass = 4;
m_accuracy = 0.01f;
m_nMaxIters = 1024;
m_prevTimeInterval = 0;
m_bAwake = 1;
m_nSlowFrames = 0;
m_friction = 0;
m_impulseScale = 0.05f;
m_explosionScale = 0.001f;
m_qrot0.SetIdentity();
m_collImpulseScale = 1.0f;
m_bMeshUpdated = 0;
m_collTypes = /*ent_terrain |*/ ent_static|ent_sleeping_rigid|ent_rigid|ent_living;
m_maxCollImpulse = 3000;
}
CSoftEntity::~CSoftEntity()
{
AlertNeighbourhoodND();
}
void CSoftEntity::AlertNeighbourhoodND()
{
if (m_vtx) { delete[] m_vtx; m_vtx=0; }
if (m_edges) { delete[] m_edges; m_edges=0; }
if (m_pVtxEdges) { delete[] m_pVtxEdges; m_pVtxEdges=0; }
m_nVtx=m_nEdges = 0;
}
int CSoftEntity::AddGeometry(phys_geometry *pgeom, pe_geomparams* params, int id)
{
if (!pgeom || pgeom->pGeom->GetType()!=GEOM_TRIMESH || m_nParts>0)
return -1;
int res = CPhysicalEntity::AddGeometry(pgeom,params,id);
int i,j,i0,i1,bDegen,iedge,itri,itri0,ivtx,itrinew,nVtxEdges,(*pInfo)[3];
float rvtxmass,vtxvol,len[3];
CTriMesh *pMesh = (CTriMesh*)pgeom->pGeom;
rvtxmass = pMesh->m_nVertices/params->mass;
vtxvol = 1.0f/(rvtxmass*params->density);
m_density = params->density;
m_vtx = new se_vertex[m_nVtx = pMesh->m_nVertices];
for(i=0;i<m_nVtx;i++) {
m_vtx[i].posorg = pMesh->m_pVertices[i];
m_vtx[i].pos = m_qrot0*(params->q*pMesh->m_pVertices[i]*params->scale+params->pos);
m_vtx[i].massinv = rvtxmass; m_vtx[i].volume = vtxvol;
m_vtx[i].vel.zero(); m_vtx[i].bSeparating=m_vtx[i].iSorted=m_vtx[i].bAttached = 0;
m_vtx[i].pContactEnt = 0; m_vtx[i].iContactNode = 0; m_vtx[i].ncontact.zero(); m_vtx[i].n.zero();
}
m_offs0 = m_vtx[0].pos;
for(i=1;i<m_nVtx;i++) m_vtx[i].pos -= m_vtx[0].pos;
m_vtx[0].pos.zero();
memset(pInfo=new int[pMesh->m_nTris][3],-1,pMesh->m_nTris*sizeof(pInfo[0]));
// count the number of edges - for each tri, mark each edge, unless buddy already did it
for(i=m_nEdges=0;i<pMesh->m_nTris;i++) {
for(j=bDegen=0;j<3;j++) {
len[j] = (pMesh->m_pVertices[pMesh->m_pIndices[i*3+j]]-pMesh->m_pVertices[pMesh->m_pIndices[i*3+inc_mod3[j]]]).len2();
bDegen |= iszero(len[j]);
}
iedge = idxmax3(len); j = iedge&-bDegen;
do {
if (pInfo[i][j]<0 && !(m_flags&se_skip_longest_edges && j==iedge && !bDegen)) {
if (pMesh->m_pTopology[i].ibuddy[j]>=0)
pInfo[ pMesh->m_pTopology[i].ibuddy[j] ][ pMesh->GetEdgeByBuddy(pMesh->m_pTopology[i].ibuddy[j],i) ] = m_nEdges;
pInfo[i][j] = m_nEdges++;
}
} while(++j<3 && !bDegen);
}
m_edges = new se_edge[m_nEdges];
m_pVtxEdges = new int[m_nEdges*2];
for(i=0;i<pMesh->m_nTris;i++) for(j=0;j<3;j++) if ((iedge=pInfo[i][j])>=0) {
i0 = m_edges[iedge].ivtx[0] = pMesh->m_pIndices[i*3+j];
i1 = m_edges[iedge].ivtx[1] = pMesh->m_pIndices[i*3+inc_mod3[j]];
m_edges[iedge].len0 = (pMesh->m_pVertices[i0]-pMesh->m_pVertices[i1]).len()*params->scale;
m_edges[pInfo[i][j]].kd = m_vtx[i0].massinv+m_vtx[i1].massinv>0 ?
sqrt_tpl(m_ks/(m_vtx[i0].massinv+m_vtx[i1].massinv))*2.0f*m_kdRatio : 0;
}
// for each vertex, trace ccw fan around it and store in m_pVtxEdges
m_BBox[0].zero(); m_BBox[1].zero();
for(i=nVtxEdges=0; i<pMesh->m_nTris; i++)
for(j=0;j<3;j++) if (!m_vtx[ivtx=pMesh->m_pIndices[i*3+j]].iSorted) {
itri=i; iedge=j;
m_vtx[ivtx].iStartEdge = nVtxEdges; m_vtx[ivtx].bFullFan = 1;
do { // first, trace cw fan until we find an open edge (if any)
if ((itrinew = pMesh->m_pTopology[itri].ibuddy[iedge])<=0)
break;
iedge = inc_mod3[pMesh->GetEdgeByBuddy(itrinew,itri)];
} while((itri=itrinew)!=i);
itri0 = itri;
do { // now trace ccw fan
if (pInfo[itri][iedge]>=0)
m_pVtxEdges[nVtxEdges++] = pInfo[itri][iedge];
if ((itrinew = pMesh->m_pTopology[itri].ibuddy[dec_mod3[iedge]])<0) {
if (pInfo[itri][dec_mod3[iedge]]>=0)
m_pVtxEdges[nVtxEdges++] = pInfo[itri][dec_mod3[iedge]];
m_vtx[ivtx].bFullFan = 0; break;
}
iedge = pMesh->GetEdgeByBuddy(itrinew,itri);
} while ((itri=itrinew)!=itri0);
m_vtx[ivtx].iEndEdge = nVtxEdges-1;
m_vtx[ivtx].rnEdges = 1.0f/(nVtxEdges-m_vtx[ivtx].iStartEdge);
m_vtx[ivtx].iSorted = 1;
m_vtx[ivtx].surface_idx[0] = pMesh->m_pIds ? pMesh->m_pIds[i]:-1;
m_BBox[0].x=min(m_BBox[0].x,m_vtx[ivtx].pos.x); m_BBox[1].x=max(m_BBox[1].x,m_vtx[ivtx].pos.x);
m_BBox[0].y=min(m_BBox[0].y,m_vtx[ivtx].pos.y); m_BBox[1].y=max(m_BBox[1].y,m_vtx[ivtx].pos.y);
m_BBox[0].z=min(m_BBox[0].z,m_vtx[ivtx].pos.z); m_BBox[1].z=max(m_BBox[1].z,m_vtx[ivtx].pos.z);
}
delete[] pInfo;
m_coverage = m_flags&se_skip_longest_edges ? 0.5f: 1.0f/3;
m_BBox[0] += m_pos+m_offs0; m_BBox[1] += m_pos+m_offs0;
box bbox;
bbox.Basis.SetIdentity();
bbox.bOriented = 0;
bbox.center = (m_BBox[0]+m_BBox[1])*0.5f-m_pos-m_parts[0].pos;
bbox.size = (m_BBox[1]-m_BBox[0])*(0.5f/params->scale);
if (pMesh->m_pTree)
delete pMesh->m_pTree;
CSingleBoxTree *pTree = new CSingleBoxTree;
pTree->SetBox(&bbox);
pTree->Build(pMesh);
pTree->m_nPrims = pMesh->m_nTris;
pMesh->m_pTree = pTree;
m_flags |= pef_use_geom_callbacks;
m_bMeshUpdated = 0;
return res;
}
void CSoftEntity::RemoveGeometry(int id)
{
int i;
for(i=0;i<m_nParts && m_parts[i].id!=id;i++);
if (i==m_nParts) return;
if (m_vtx) { delete[] m_vtx; m_vtx=0; }
if (m_edges) { delete[] m_edges; m_edges=0; }
if (m_pVtxEdges) { delete[] m_pVtxEdges; m_pVtxEdges=0; }
m_nVtx=m_nEdges = 0;
CPhysicalEntity::RemoveGeometry(id);
}
int CSoftEntity::SetParams(pe_params *_params)
{
int res = CPhysicalEntity::SetParams(_params);
if (res) {
if (_params->type==pe_params_pos::type_id && !is_unused(((pe_params_pos*)_params)->q)) {
if (m_nVtx>0 && (m_qrot0|m_qrot)<0.998f) {
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeom->pGeom;
int i; quaternionf qrot = m_bMeshUpdated ? m_qrot*!m_qrot0 : m_qrot;
for(i=0;i<m_nVtx;i++) {
m_vtx[i].pos = qrot*(m_parts[0].q*pMesh->m_pVertices[i]*m_parts[0].scale+m_parts[0].pos);
if (m_vtx[i].bAttached==2 && m_vtx[i].pContactEnt) {
RigidBody *pbody = m_vtx[i].pContactEnt->GetRigidBody(m_vtx[i].iContactPart);
m_vtx[i].ptAttach = (m_vtx[i].pos+m_pos-pbody->pos)*pbody->q;
}
}
m_offs0 = m_vtx[0].pos;
for(i=1;i<m_nVtx;i++) m_vtx[i].pos -= m_vtx[0].pos;
m_vtx[0].pos.zero();
}
m_qrot0 = m_qrot; m_qrot.SetIdentity();
}
return res;
}
if (_params->type==pe_simulation_params::type_id) {
pe_simulation_params *params = (pe_simulation_params*)_params;
if (!is_unused(params->gravity)) m_gravity = params->gravity;
if (!is_unused(params->maxTimeStep)) m_maxAllowedStep = params->maxTimeStep;
if (!is_unused(params->minEnergy)) m_Emin = params->minEnergy;
if (!is_unused(params->damping)) m_damping = params->damping;
if (!is_unused(params->density) && params->density>=0 && m_nParts>0) {
for(int i=0;i<m_nVtx;i++) if (m_vtx[i].massinv>0)
m_vtx[i].volume = 1.0f/(m_vtx[i].massinv*params->density);
m_density = params->density;
}
if (!is_unused(params->mass) && params->mass>=0 && m_nParts>0) {
float rvtxmass = m_nVtx/params->mass;
for(int i=0;i<m_nVtx;i++) if (m_vtx[i].massinv>0)
m_vtx[i].massinv = rvtxmass;
m_parts[0].mass = params->mass;
}
if (!is_unused(params->iSimClass)) {
m_iSimClass = params->iSimClass;
m_pWorld->RepositionEntity(this,2);
}
return 1;
}
if (_params->type==pe_params_buoyancy::type_id) {
pe_params_buoyancy *params = (pe_params_buoyancy*)_params;
if (!is_unused(params->waterDensity)) {
if (m_waterDensity!=params->waterDensity)
m_bAwake = 1;
m_waterDensity = params->waterDensity;
}
if (!is_unused(params->waterDamping)) m_waterDamping = params->waterDamping;
if (!is_unused(params->waterPlane.n)) {
if ((m_waterPlane.n-params->waterPlane.n).len2()>0)
m_bAwake = 1;
m_waterPlane.n = params->waterPlane.n;
}
if (!is_unused(params->waterPlane.origin)) {
if ((m_waterPlane.origin-params->waterPlane.origin).len2()>0)
m_bAwake = 1;
m_waterPlane.origin = params->waterPlane.origin;
}
if (!is_unused(params->waterFlow)) {
if ((m_waterFlow-params->waterFlow).len2()>0)
m_bAwake = 1;
m_waterFlow = params->waterFlow;
}
if (!is_unused(params->waterResistance)) m_waterResistance = params->waterResistance;
return 1;
}
if (_params->type==pe_params_softbody::type_id) {
pe_params_softbody *params = (pe_params_softbody*)_params;
if (!is_unused(params->thickness)) m_thickness = params->thickness;
if (!is_unused(params->friction)) m_friction = params->friction;
if (!is_unused(params->ks)) m_ks = params->ks;
if (!is_unused(params->kdRatio)) m_kdRatio = params->kdRatio;
if (!is_unused(params->ks) || !is_unused(params->kdRatio)) {
for(int i=0;i<m_nEdges;i++) if (m_vtx[m_edges[i].ivtx[0]].massinv+m_vtx[m_edges[i].ivtx[1]].massinv>0)
m_edges[i].kd = sqrt_tpl(m_ks/(m_vtx[m_edges[i].ivtx[0]].massinv+m_vtx[m_edges[i].ivtx[1]].massinv))*2.0f*m_kdRatio;
}
if (!is_unused(params->airResistance)) m_airResistance = params->airResistance;
if (!is_unused(params->wind)) m_wind = params->wind;
if (!is_unused(params->accuracy)) m_accuracy = params->accuracy;
if (!is_unused(params->nMaxIters)) m_nMaxIters = params->nMaxIters;
if (!is_unused(params->maxSafeStep)) m_maxSafeStep = params->maxSafeStep;
if (!is_unused(params->impulseScale)) m_impulseScale = params->impulseScale;
if (!is_unused(params->explosionScale)) m_explosionScale = params->explosionScale;
if (!is_unused(params->collisionImpulseScale)) m_collImpulseScale = params->collisionImpulseScale;
if (!is_unused(params->maxCollisionImpulse)) m_maxCollImpulse = params->maxCollisionImpulse;
if (!is_unused(params->collTypes)) m_collTypes = params->collTypes;
return 1;
}
return CPhysicalEntity::SetParams(_params);
}
int CSoftEntity::GetParams(pe_params *_params)
{
if (_params->type==pe_simulation_params::type_id) {
pe_simulation_params *params = (pe_simulation_params*)_params;
params->gravity=params->gravityFreefall = m_gravity;
params->maxTimeStep = m_maxAllowedStep;
params->minEnergy = m_Emin;
params->damping=params->dampingFreefall = m_damping;
params->mass = m_parts[0].mass;
params->density = m_density;
params->iSimClass = m_iSimClass;
return 1;
}
if (_params->type==pe_params_buoyancy::type_id) {
pe_params_buoyancy *params = (pe_params_buoyancy*)_params;
params->waterDensity = m_waterDensity;
params->waterDamping = m_waterDamping;
params->waterPlane = m_waterPlane;
params->waterFlow = m_waterFlow;
params->waterResistance = m_waterResistance;
params->waterEmin = m_Emin;
return 1;
}
if (_params->type==pe_params_softbody::type_id) {
pe_params_softbody *params = (pe_params_softbody*)_params;
params->thickness = m_thickness;
params->friction = m_friction;
params->ks = m_ks;
params->kdRatio = m_kdRatio;
params->airResistance = m_airResistance;
params->wind = m_wind;
params->accuracy = m_accuracy;
params->nMaxIters = m_nMaxIters;
params->maxSafeStep = m_maxSafeStep;
params->impulseScale = m_impulseScale;
params->explosionScale = m_explosionScale;
params->collisionImpulseScale = m_collImpulseScale;
params->maxCollisionImpulse = m_maxCollImpulse;
params->collTypes = m_collTypes;
return 1;
}
return CPhysicalEntity::GetParams(_params);
}
int CSoftEntity::GetStatus(pe_status *_status)
{
int res;
if (res = CPhysicalEntity::GetStatus(_status)) {
if (_status->type==pe_status_caps::type_id) {
pe_status_caps *status = (pe_status_caps*)_status;
status->bCanAlterOrientation = 1;
}
return res;
}
if (_status->type==pe_status_softvtx::type_id) {
if (m_nVtx<=0)
return 0;
pe_status_softvtx *status = (pe_status_softvtx*)_status;
status->nVtx = m_nVtx;
status->pVtx = ((CTriMesh*)m_parts[0].pPhysGeomProxy->pGeom)->m_pVertices;
status->pNormals.data = &m_vtx[0].n;
status->pNormals.iStride = sizeof(m_vtx[0]);
return 1;
}
if (_status->type==pe_status_collisions::type_id) {
pe_status_collisions *status = (pe_status_collisions*)_status;
int i,j,n;
if (status->len<=0)
return 0;
for(i=n=0;i<m_nVtx;i++) if (!m_vtx[i].bAttached && m_vtx[i].pContactEnt) {
if (n==status->len) {
for(n=1,j=0; n<status->len; n++)
if ((status->pHistory[n].v[0]-status->pHistory[n].v[1]).len2() < (status->pHistory[j].v[0]-status->pHistory[j].v[1]).len2())
j = n;
if ((status->pHistory[j].v[0]-status->pHistory[j].v[1]).len2() > (m_vtx[i].vel-m_vtx[i].vcontact).len2())
continue;
} else j=n++;
status->pHistory[j].pt = m_vtx[i].pos;
status->pHistory[j].n = m_vtx[i].ncontact;
status->pHistory[j].v[0] = m_vtx[i].vel;
status->pHistory[j].v[1] = m_vtx[i].vcontact;
status->pHistory[j].mass[0] = m_parts[0].mass;
status->pHistory[j].mass[1] = m_vtx[i].pContactEnt->GetMassInv();
if (status->pHistory[j].mass[1]>0)
status->pHistory[j].mass[1] = 1.0f/status->pHistory[j].mass[1];
status->pHistory[j].age = 0;
status->pHistory[j].idCollider = m_pWorld->GetPhysicalEntityId(m_vtx[i].pContactEnt);
status->pHistory[j].partid[0] = 0;
status->pHistory[j].partid[1] = m_vtx[i].iContactPart;
status->pHistory[j].idmat[0] = m_vtx[i].surface_idx[0];
status->pHistory[j].idmat[1] = m_vtx[i].surface_idx[1];
}
return status->len = n;
}
return 0;
}
int CSoftEntity::Action(pe_action *_action)
{
if (_action->type==pe_action_impulse::type_id) {
pe_action_impulse *action = (pe_action_impulse*)_action;
ENTITY_VALIDATE("CSoftEntity:Action(action_impulse)",action);
if (m_nVtx>0 && !is_unused(action->point) && !is_unused(action->impulse)) {
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeom->pGeom;
vectorf pt=action->point-(m_pos+m_offs0), impulse=action->impulse*m_impulseScale;
int i,j,bBest;
if (!is_unused(action->ipart)) i = action->ipart;
else i = action->partid;
m_bAwake = 1;
if ((unsigned int)i < (unsigned int)pMesh->m_nTris) {
float rarea,k; int idx[3];
for(j=0;j<3;j++) idx[j] = pMesh->m_pIndices[i*3+j];
rarea = (m_vtx[idx[1]].pos-m_vtx[idx[0]].pos ^ m_vtx[idx[2]].pos-m_vtx[idx[0]].pos).len();
if (rarea>1E-4f) {
rarea = 1.0f/rarea;
for(j=0;j<3;j++) {
k = (m_vtx[idx[inc_mod3[j]]].pos-pt ^ m_vtx[idx[j]].pos-pt).len()*rarea;
m_vtx[idx[dec_mod3[j]]].vel += impulse*(m_vtx[idx[dec_mod3[j]]].massinv*k);
}
} else
m_vtx[idx[0]].vel += impulse*m_vtx[idx[0]].massinv;
} else {
for(i=1,j=0;i<m_nVtx;i++) {
bBest = -isneg((m_vtx[i].pos-pt).len2()-(m_vtx[j].pos-pt).len2());
j = i&bBest | j&~bBest;
}
m_vtx[j].vel += impulse*m_vtx[j].massinv;
}
}
return 1;
}
if (_action->type==pe_action_attach_points::type_id) {
pe_action_attach_points *action = (pe_action_attach_points*)_action;
CPhysicalEntity* pent = action->pEntity==WORLD_ENTITY ? &g_StaticPhysicalEntity :
action->pEntity ? ((CPhysicalPlaceholder*)action->pEntity)->GetEntity() : 0;
int ipart=0, bAttached=iszero((intptr_t)pent)^1;
if (bAttached && is_unused(action->points))
bAttached = 2;
float rvtxmass = pent ? 0 : m_nVtx/m_parts[0].mass;
if (!is_unused(action->partid)) {
for(ipart=0; ipart<pent->m_nParts && pent->m_parts[ipart].id!=action->partid; ipart++);
if (ipart>=pent->m_nParts)
return 0;
}
RigidBody *pbody;
if (bAttached)
pbody = pent->GetRigidBody(ipart);
for(int i=0;i<action->nPoints;i++) {
if (m_vtx[action->piVtx[i]].pContactEnt) m_vtx[action->piVtx[i]].pContactEnt->Release();
if (m_vtx[action->piVtx[i]].pContactEnt = pent)
pent->AddRef();
m_vtx[action->piVtx[i]].massinv = rvtxmass;
m_vtx[action->piVtx[i]].iContactPart = ipart;
if (m_vtx[action->piVtx[i]].bAttached = bAttached) {
if (!is_unused(action->points))
m_vtx[action->piVtx[i]].ptAttach = action->points[i];
else
m_vtx[action->piVtx[i]].ptAttach = m_vtx[action->piVtx[i]].pos+m_pos+m_offs0;
m_vtx[action->piVtx[i]].ptAttach = (m_vtx[action->piVtx[i]].ptAttach-pbody->pos)*pbody->q;
}
}
return 1;
}
if (_action->type==pe_action_reset::type_id) {
if (m_nVtx>0) {
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeomProxy->pGeom;
int i;
for(i=0;i<m_nVtx;i++) {
if (m_vtx[i].pContactEnt) m_vtx[i].pContactEnt->Release();
m_vtx[i].pContactEnt = 0;
m_vtx[i].vel.zero();
pMesh->m_pVertices[i] = m_vtx[i].posorg;
}
for(i=0;i<pMesh->m_nTris;i++)
pMesh->m_pNormals[i] = (pMesh->m_pVertices[pMesh->m_pIndices[i*3+1]]-pMesh->m_pVertices[pMesh->m_pIndices[i*3]] ^
pMesh->m_pVertices[pMesh->m_pIndices[i*3+2]]-pMesh->m_pVertices[pMesh->m_pIndices[i*3]]).normalized();
}
return 1;
}
return CPhysicalEntity::Action(_action);
}
void CSoftEntity::StartStep(float time_interval)
{
m_timeStepPerformed = 0;
m_timeStepFull = time_interval;
}
float CSoftEntity::GetMaxTimeStep(float time_interval)
{
if (m_timeStepPerformed > m_timeStepFull-0.001f)
return time_interval;
return min(min(m_timeStepFull-m_timeStepPerformed,m_maxAllowedStep),time_interval);
}
inline int GetCheckPart(CPhysicalEntity *pent, int ipart)
{
int i = (pent->m_bProcessed_aux&0xFFFFFF) & (1<<ipart)-1;
return (pent->m_bProcessed_aux>>24) + g_bitcount[i&0xFF] + g_bitcount[i>>8&0xFF] + g_bitcount[i>>16];
}
int CSoftEntity::Step(float time_interval)
{
if (m_nVtx<=0 || m_timeStepPerformed>m_timeStepFull-0.001f)
return 1;
int i,j,j1,nEnts,i0,i1,nContactVtx,nContactVtx0,nCheckParts,iter,imask,bUnstable,iFullIter=4;
vectorf d,l,llTd,v,F,w,center,BBox0[2];
float rl,l0,rmax,windage,kr,rsep,friction=max(0.5f,1.0f-m_friction*time_interval),ks=m_ks,kdScale=1.0f;
real r2,r2new,a,b,dAd;
struct check_part {
vectorf offset;
matrix3x3f R;
box bbox;
CPhysicalEntity *pent;
int ipart;
RigidBody *pbody;
CGeometry *pGeom;
int bPrimitive;
int surface_idx;
vectorf P,L;
};
box boxent;
check_part checkParts[20];
CRayGeom aray;
intersection_params ip;
geom_contact *pcontacts;
CPhysicalEntity **pentlist,*pent;
geom_world_data gwd;
RigidBody *pbody;
g_Overlapper.Init();
boxent.size = (m_BBox[1]-m_BBox[0])*0.5f+vectorf(m_thickness,m_thickness,m_thickness)*2;
center = (m_BBox[0]+m_BBox[1])*0.5f;
boxent.bOriented = 1;
ip.bStopAtFirstTri = true;
for(i=0;i<m_nVtx;i++) if (m_vtx[i].pContactEnt && m_vtx[i].pContactEnt->m_iSimClass==7) {
m_vtx[i].pContactEnt = 0; m_vtx[i].bAttached = 0;
}
if (!m_bAwake)
return 1;
FUNCTION_PROFILER( GetISystem(),PROFILE_PHYSICS );
PHYS_ENTITY_PROFILER
nEnts = m_pWorld->GetEntitiesAround(m_BBox[0]-vectorf(m_thickness,m_thickness,m_thickness)*2,
m_BBox[1]+vectorf(m_thickness,m_thickness,m_thickness)*2, pentlist,
m_collTypes|ent_sort_by_mass|ent_ignore_noncolliding|ent_triggers, this);
for(i=j=nCheckParts=0;i<nEnts;i++) if (pentlist[i]!=this)
for(j=0,pentlist[i]->m_bProcessed_aux=nCheckParts<<24; j<pentlist[i]->m_nParts; j++) {
pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom->GetBBox(&checkParts[nCheckParts].bbox);
checkParts[nCheckParts].bbox.center *= pentlist[i]->m_parts[j].scale;
checkParts[nCheckParts].bbox.size *= pentlist[i]->m_parts[j].scale;
//(pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q).getmatrix(boxrope.Basis); //Q2M_IVO
boxent.Basis = matrix3x3f(pentlist[i]->m_qrot*pentlist[i]->m_parts[j].q);
checkParts[nCheckParts].offset = pentlist[i]->m_pos+pentlist[i]->m_qrot*pentlist[i]->m_parts[j].pos;
boxent.center = (center-checkParts[nCheckParts].offset)*boxent.Basis;
boxent.bOriented++;
if (box_box_overlap_check(&boxent,&checkParts[nCheckParts].bbox)) {
checkParts[nCheckParts].pent = pentlist[i];
checkParts[nCheckParts].ipart = j;
checkParts[nCheckParts].R = boxent.Basis;
pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom->PrepareForRayTest(
pentlist[i]->m_parts[j].scale==1.0f ? m_thickness*2 : m_thickness*2/pentlist[i]->m_parts[j].scale);
checkParts[nCheckParts].offset -= m_pos+m_offs0;
checkParts[nCheckParts].pbody = pentlist[i]->GetRigidBody(j);
checkParts[nCheckParts].pGeom = (CGeometry*)pentlist[i]->m_parts[j].pPhysGeomProxy->pGeom;
checkParts[nCheckParts].bPrimitive = isneg(checkParts[nCheckParts].pGeom->GetPrimitiveCount()-2);
checkParts[nCheckParts].surface_idx = pentlist[i]->m_parts[j].surface_idx;
pentlist[i]->m_bProcessed_aux |= 1u<<j;
pentlist[i]->m_bProcessed = 1;
if (++nCheckParts==sizeof(checkParts)/sizeof(checkParts[0]))
goto enoughgeoms;
}
} enoughgeoms:
for(i=nContactVtx=0; i<m_nVtx; i++) if (!m_vtx[i].bAttached) { // detect collisions for free vertices
// calculate normal
for(j=m_vtx[i].iStartEdge,m_vtx[i].n.zero(); j<m_vtx[i].iEndEdge+m_vtx[i].bFullFan; j++) {
imask = j-m_vtx[i].iEndEdge>>31; j1 = j+1&imask | m_vtx[i].iStartEdge&~imask;
m_vtx[i].n +=
(m_vtx[m_edges[m_pVtxEdges[j]].ivtx[1]].pos-m_vtx[m_edges[m_pVtxEdges[j]].ivtx[0]].pos)*(iszero(i^m_edges[m_pVtxEdges[j]].ivtx[0])*2-1) ^
(m_vtx[m_edges[m_pVtxEdges[j1]].ivtx[1]].pos-m_vtx[m_edges[m_pVtxEdges[j1]].ivtx[0]].pos)*(iszero(i^m_edges[m_pVtxEdges[j1]].ivtx[0])*2-1);
}
m_vtx[i].area = m_vtx[i].n.len();
m_vtx[i].n /= m_vtx[i].area; m_vtx[i].area *= m_coverage*0.5f;
m_vtx[i].pos0 = m_vtx[i].pos;
rsep = m_thickness;
if (pent = m_vtx[i].pContactEnt) {
pbody = m_vtx[i].pContactEnt->GetRigidBody(m_vtx[i].iContactPart);
m_vtx[i].vcontact = pbody->v+(pbody->w^m_vtx[i].pos+m_pos+m_offs0-pbody->pos);
if (pent->m_bProcessed && (!m_vtx[i].bSeparating || (m_vtx[i].vel-m_vtx[i].vcontact)*m_vtx[i].ncontact<0.1f)) {
if (checkParts[GetCheckPart(m_vtx[i].pContactEnt,m_vtx[i].iContactPart)].bPrimitive) {
rsep = m_thickness*1.5f; m_vtx[i].pContactEnt = 0;
} else {
j = m_vtx[i].iContactPart;
gwd.offset = pent->m_pos-m_pos-m_offs0+pent->m_qrot*pent->m_parts[j].pos;
gwd.R = matrix3x3f(pent->m_qrot*pent->m_parts[j].q);
gwd.scale = pent->m_parts[j].scale;
aray.m_dirn = -m_vtx[i].ncontact;
aray.m_ray.origin = m_vtx[i].pos;
aray.m_ray.dir = aray.m_dirn*(m_thickness*1.5f);
gwd.iStartNode = m_vtx[i].iContactNode;
if (pent->m_parts[j].pPhysGeomProxy->pGeom->Intersect(&aray,&gwd,0,&ip,pcontacts)) {
m_vtx[i].pos = pcontacts->pt+pcontacts->n*m_thickness;
m_vtx[i].ncontact = pcontacts->n;
m_vtx[i].iContactNode = pcontacts->iNode[0];
} else
m_vtx[i].pContactEnt = 0;
}
} else
m_vtx[i].pContactEnt = 0;
}
for(j=0; j<nCheckParts; j++) {
v = checkParts[j].pbody->v+(checkParts[j].pbody->w^m_vtx[i].pos+m_pos+m_offs0-checkParts[j].pbody->pos);
if (checkParts[j].bPrimitive) {
contact acontact;
if (checkParts[j].pGeom->UnprojectSphere((m_vtx[i].pos-checkParts[j].offset)*checkParts[j].R, m_thickness,rsep, &acontact)) {
m_vtx[i].pos = checkParts[j].R*(acontact.pt+acontact.n*m_thickness)+checkParts[j].offset;
m_vtx[i].ncontact = checkParts[j].R*acontact.n;
m_vtx[i].pContactEnt = checkParts[j].pent;
m_vtx[i].iContactPart = checkParts[j].ipart;
m_vtx[i].vcontact = v;
m_vtx[i].surface_idx[1] = checkParts[j].surface_idx;
}
} else {
aray.m_ray.dir = ((v-m_vtx[i].vel)*checkParts[j].R)*m_prevTimeInterval;
aray.m_dirn = aray.m_ray.dir.normalized();
aray.m_ray.origin = (m_vtx[i].pos-checkParts[j].offset)*checkParts[j].R - aray.m_dirn*m_thickness;
aray.m_ray.dir += aray.m_dirn*m_thickness;
if (box_ray_overlap_check(&checkParts[j].bbox,&aray.m_ray)) {
gwd.scale = checkParts[j].pent->m_parts[checkParts[j].ipart].scale;
gwd.offset.zero(); gwd.R.SetIdentity();
if (checkParts[j].pent->m_parts[checkParts[j].ipart].pPhysGeomProxy->pGeom->Intersect(&aray,&gwd,0,&ip,pcontacts) &&
pcontacts->n*aray.m_dirn>0)
{
m_vtx[i].pos = checkParts[j].R*(pcontacts->pt+aray.m_dirn*m_thickness)+checkParts[j].offset;
m_vtx[i].ncontact = checkParts[j].R*pcontacts->n;
m_vtx[i].pContactEnt = checkParts[j].pent;
m_vtx[i].iContactPart = checkParts[j].ipart;
m_vtx[i].iContactNode = pcontacts->iNode[0];
m_vtx[i].vcontact = v;
imask = pcontacts->id[0]>>31;
m_vtx[i].surface_idx[1] = checkParts[j].surface_idx&imask | pcontacts->id[0]&~imask;
}
}
}
}
if (pent!=m_vtx[i].pContactEnt) {
if (pent) pent->Release();
if (m_vtx[i].pContactEnt) m_vtx[i].pContactEnt->AddRef();
}
if (m_vtx[i].pContactEnt) {
m_vtx[nContactVtx++].iSorted = i;
if ((m_vtx[i].pos0-m_vtx[i].pos).len2()>sqr(m_maxSafeStep*0.75f))
m_vtx[i].pos = m_vtx[i].pos0+(m_vtx[i].pos-m_vtx[i].pos0).normalized()*(m_maxSafeStep*0.75f);
}
} else { // synchronize attached vertices w/ hosts
pbody = m_vtx[i].pContactEnt->GetRigidBody(m_vtx[i].iContactPart);
m_vtx[i].pos = pbody->pos+pbody->q*m_vtx[i].ptAttach;
m_vtx[i].vcontact = m_vtx[i].vel = pbody->v+(pbody->w^m_vtx[i].pos-pbody->pos);
m_vtx[i].pos -= m_pos+m_offs0;
}
for(i=0; i<nCheckParts; i++)
checkParts[i].pent->m_bProcessed = 0;
for(i=0; i<m_nEdges; i++) { // calculate edge lengths
l = m_vtx[m_edges[i].ivtx[0]].pos - m_vtx[m_edges[i].ivtx[1]].pos;
m_edges[i].rlen = 1.0f/max(1E-4f,m_edges[i].len = sqrt_tpl(l.len2()));
}
for(i=0; i<m_nVtx; i++) { // apply gravity, buoyancy, wind (or water resistance)
if (!m_vtx[i].bAttached) {
if ((m_vtx[i].pos+m_pos-m_waterPlane.origin)*m_waterPlane.n<0) {
m_vtx[i].vel -= m_gravity*(m_waterDensity*m_vtx[i].volume*time_interval);
kr = m_waterResistance; w = m_waterFlow;
} else {
kr = m_airResistance; w = m_wind;
}
for(j=m_vtx[i].iStartEdge,windage=0; j<=m_vtx[i].iEndEdge; j++)
windage += ((m_vtx[m_edges[m_pVtxEdges[j]].ivtx[1]].pos-m_vtx[m_edges[m_pVtxEdges[j]].ivtx[0]].pos)*m_vtx[i].n)*
(iszero(i^m_edges[m_pVtxEdges[j]].ivtx[0])*2-1)*m_edges[m_pVtxEdges[j]].rlen;
m_vtx[i].vel += m_vtx[i].n*((m_vtx[i].n*(w-m_vtx[i].vel))*m_vtx[i].area*(windage*m_vtx[i].rnEdges+1)*kr*time_interval);
m_vtx[i].vel += m_gravity*time_interval;
}
if (m_vtx[i].pContactEnt)
m_vtx[i].vel -= m_vtx[i].ncontact*(m_vtx[i].ncontact*(m_vtx[i].vel-m_vtx[i].vcontact));
m_vtx[i].r.zero(); m_vtx[i].bSeparating = 0;
m_vtx[i].pos0=m_vtx[i].pos; m_vtx[i].vel0=m_vtx[i].vel; m_vtx[i].iSorted0=m_vtx[i].iSorted;
}
nContactVtx0 = nContactVtx;
BBox0[0] = m_BBox[0]; BBox0[1] = m_BBox[1];
reiterate:
for(i=0;i<nCheckParts;i++) checkParts[i].P=checkParts[i].L.zero();
for(i=0; i<m_nEdges; i++) { // calculate residual for the solver
i0 = m_edges[i].ivtx[0]; i1 = m_edges[i].ivtx[1];
l0 = m_edges[i].len0; rl = m_edges[i].rlen;
l = m_vtx[i0].pos - m_vtx[i1].pos;
v = m_vtx[i0].vel - m_vtx[i1].vel;
d = v*time_interval;//+m_vtx[i0].n*m_vtx[i0].shift-m_vtx[i1].n*m_vtx[i1].shift;
llTd = l*((l*d)*sqr(rl));
F = l*(rl*(l0-m_edges[i].len)*ks); // Felastic
F -= l*((v*l)*sqr(rl)*m_edges[i].kd*kdScale); // Fviscous
F += (d*(l0*rl-1)-llTd*(l0*rl))*ks; // dFelastic/dx * shift
F -= ((l*v)*(d-llTd)+l*(v*(d-llTd)))*(m_edges[i].kd*kdScale*sqr(rl)); // dFviscous/dx * shift
F *= time_interval;
m_vtx[i0].r += F*m_vtx[i0].massinv;
m_vtx[i1].r -= F*m_vtx[i1].massinv;
}
for(i=j=0; i<nContactVtx; i++) { // remove vertices that have 'separating' residuals from the contacting vertices list
i1 = m_vtx[i].iSorted;
float rn = m_vtx[i1].ncontact*m_vtx[i1].r;
if (rn>0)
m_vtx[i1].bSeparating = 1;
else {
j1 = GetCheckPart(m_vtx[i1].pContactEnt,m_vtx[i1].iContactPart);
checkParts[j1].P += m_vtx[i1].ncontact*rn;
checkParts[j1].L += m_vtx[i1].pos+m_pos-m_offs0-checkParts[j1].pbody->pos ^ m_vtx[i1].ncontact*rn;
/*imask = m_vtx[i].surface_idx[0]>>31;
float friction = max(0.0f, (m_pWorld->m_DynFrictionTable[(m_parts[0].surface_idx&imask|m_vtx[i1].surface_idx[0]&~imask)&NSURFACETYPES-1] +
m_pWorld->m_DynFrictionTable[m_vtx[i1].surface_idx[1]&NSURFACETYPES-1])*0.5f);
if (rn*friction<0) {
if ((m_vtx[i1].vel-m_vtx[i1].vcontact).len2()>sqr(rn*friction))
m_vtx[i1].vel += (m_vtx[i1].vel-m_vtx[i1].vcontact).normalized()*(rn*friction);
else
m_vtx[i1].vel = m_vtx[i1].vcontact;
}*/
m_vtx[i1].vel = m_vtx[i1].vel*friction+m_vtx[i1].vcontact*(1.0f-friction);
m_vtx[i1].r -= m_vtx[i1].ncontact*rn;
m_vtx[j++].iSorted = i1;
}
}
nContactVtx = j;
for(i=0,r2=0; i<m_nVtx; i++) {
r2+=m_vtx[i].r.len2(); m_vtx[i].d=m_vtx[i].r; m_vtx[i].P.zero();
}
iter = min(m_nMaxIters,m_nVtx);
do { // conjugate gradient solver for implicit Euler step
for(i=0; i<m_nVtx; i++)
m_vtx[i].dv = m_vtx[i].d*m_vtx[i].massinv;
for(i=0; i<m_nEdges; i++) {
i0 = m_edges[i].ivtx[0]; i1 = m_edges[i].ivtx[1];
d = m_vtx[i0].d*m_vtx[i0].massinv-m_vtx[i1].d*m_vtx[i1].massinv;
l0 = m_edges[i].len0; rl = m_edges[i].rlen;
l = m_vtx[i0].pos - m_vtx[i1].pos;
v = m_vtx[i0].vel - m_vtx[i1].vel;
llTd = l*((l*d)*sqr(rl));
F = (d*(l0*rl-1)-llTd*(l0*rl))*ks; // dFelastic/dx
F -= ((l*v)*(d-llTd)+l*(v*(d-llTd)))*(m_edges[i].kd*kdScale*sqr(rl)); // dFviscous/dx - probably not worth the calculation amount
F *= sqr(time_interval);
F += llTd*(-m_edges[i].kd*kdScale*time_interval); // dFviscous/dv
m_vtx[i0].dv -= F*m_vtx[i0].massinv;
m_vtx[i1].dv += F*m_vtx[i1].massinv;
}
for(i=0; i<nContactVtx; i++) // filter away constrained components of contacting vertices
m_vtx[m_vtx[i].iSorted].dv -= m_vtx[m_vtx[i].iSorted].ncontact*(m_vtx[m_vtx[i].iSorted].ncontact*m_vtx[m_vtx[i].iSorted].dv);
for(i=0,dAd=0; i<m_nVtx; i++)
dAd += m_vtx[i].d*m_vtx[i].dv;
if (dAd<m_accuracy*0.01f)
break;
a = min((real)100.0,r2/dAd);
for(i=0,r2new=rmax=0; i<m_nVtx; i++) {
r2new += (m_vtx[i].r -= m_vtx[i].dv*a).len2();
m_vtx[i].P += m_vtx[i].d*a;
rmax = max(rmax,m_vtx[i].r.len2());
}
b = r2new/r2; r2 = r2new;
for(i=0;i<m_nVtx;i++)
(m_vtx[i].d*=b) += m_vtx[i].r;
} while(--iter && rmax>sqr(m_accuracy));
m_vtx[0].pos += (m_vtx[0].vel+=m_vtx[0].P*m_vtx[0].massinv)*time_interval;
bUnstable = isneg(sqr(m_maxSafeStep)-m_vtx[0].pos.len2());
kr = 1.0f-m_damping*time_interval;
m_BBox[0].zero(); m_BBox[1].zero(); rmax=0;
for(i=1; i<m_nVtx; i++) {
m_vtx[i].pos += (m_vtx[i].vel+=m_vtx[i].P*m_vtx[i].massinv)*time_interval - m_vtx[0].pos;
bUnstable += isneg(sqr(m_maxSafeStep)-(m_vtx[i].pos-m_vtx[i].pos0).len2());
rmax = max(rmax,m_vtx[i].vel.len2());
m_vtx[i].vel *= kr;
m_BBox[0].x=min(m_BBox[0].x,m_vtx[i].pos.x); m_BBox[1].x=max(m_BBox[1].x,m_vtx[i].pos.x);
m_BBox[0].y=min(m_BBox[0].y,m_vtx[i].pos.y); m_BBox[1].y=max(m_BBox[1].y,m_vtx[i].pos.y);
m_BBox[0].z=min(m_BBox[0].z,m_vtx[i].pos.z); m_BBox[1].z=max(m_BBox[1].z,m_vtx[i].pos.z);
}
if (bUnstable) {
for(i=0;i<m_nVtx;i++) {
m_vtx[i].pos=m_vtx[i].pos0; m_vtx[i].vel=m_vtx[i].vel0; m_vtx[i].iSorted=m_vtx[i].iSorted0;
}
nContactVtx = nContactVtx0;
time_interval *= 0.5f; ks *= 0.49f; kdScale *= 0.7f;
if (--iFullIter>0)
goto reiterate;
else for(i=0;i<m_nVtx;i++) {
m_vtx[i].vel.zero();
m_BBox[0] = BBox0[0]-(m_pos+m_offs0);
m_BBox[1] = BBox0[1]-(m_pos+m_offs0);
}
}
pe_action_impulse ai;
ai.iApplyTime = 0;
if (m_collImpulseScale>0)
for(i=0;i<nCheckParts;i++) if (checkParts[i].P.len2()+checkParts[i].L.len2()>0 && checkParts[i].pent->GetType()==PE_LIVING) {
if (checkParts[i].P.len2()>sqr(m_maxCollImpulse)) {
a = m_maxCollImpulse/checkParts[i].P.len();
checkParts[i].P *= a; checkParts[i].L *= a;
}
ai.impulse = checkParts[i].P*m_collImpulseScale;
ai.momentum = checkParts[i].L*m_collImpulseScale;
ai.ipart = checkParts[i].ipart;
checkParts[i].pent->Action(&ai);
}
m_pos += m_vtx[0].pos; m_vtx[0].pos.zero();
m_BBox[0] += m_pos+m_offs0; m_BBox[1] += m_pos+m_offs0;
m_prevTimeInterval = time_interval;
if (m_wind.len2()*m_airResistance>0 || rmax>m_Emin) {
m_nSlowFrames = 0; m_bAwake = 1;
} else if (++m_nSlowFrames>=3)
m_bAwake = 0;
float rscale = m_parts[0].scale==1.0f ? 1.0f:1.0f/m_parts[0].scale;
box bbox;
bbox.Basis = matrix3x3f(!m_parts[0].q);
bbox.center = (m_BBox[0]+m_BBox[1])*0.5f-m_pos-m_parts[0].pos;
bbox.size = (m_BBox[1]-m_BBox[0])*(0.5f*rscale);
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeomProxy->pGeom;
((CSingleBoxTree*)pMesh->m_pTree)->SetBox(&bbox);
d = m_offs0-m_parts[0].pos;
if (m_parts[0].scale==1.0f && m_parts[0].q.w==1.0f)
for(i=0;i<m_nVtx;i++) pMesh->m_pVertices[i] = m_vtx[i].pos+d;
else
for(i=0;i<m_nVtx;i++) pMesh->m_pVertices[i] = ((m_vtx[i].pos+d)*rscale)*m_parts[0].q;
m_bMeshUpdated = 1;
//for(i=0;i<pMesh->m_nTris;i++)
// MARK_UNUSED pMesh->m_pNormals[i];
if (m_flags & pef_traceable)
m_pWorld->RepositionEntity(this, 1);
return 1;
}
int CSoftEntity::RayTrace(CRayGeom *pRay,geom_contact *&pcontacts)
{
static geom_contact g_SoftContact;
if (m_nVtx>0) {
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeom->pGeom;
prim_inters inters;
triangle atri;
int i,j;
for(i=0;i<pMesh->m_nTris;i++) {
for(j=0;j<3;j++) atri.pt[j] = m_vtx[pMesh->m_pIndices[i*3+j]].pos+m_pos+m_offs0;
atri.n = atri.pt[1]-atri.pt[0] ^ atri.pt[2]-atri.pt[0];
if (ray_tri_intersection(&pRay->m_ray,&atri,&inters)) {
pcontacts = &g_SoftContact;
pcontacts->pt = inters.pt[0];
pcontacts->t = (inters.pt[0]-pRay->m_ray.origin)*pRay->m_dirn;
pcontacts->id[0] = pMesh->m_pIds ? pMesh->m_pIds[i] : m_parts[0].surface_idx;
pcontacts->iNode[0] = i;
pcontacts->n = atri.n.normalized()*-sgnnz(atri.n*pRay->m_dirn);
return 1;
}
}
}
return 0;
}
void CSoftEntity::ApplyVolumetricPressure(const vectorf &epicenter, float kr, float rmin)
{
if (m_nVtx>0) {
CTriMesh *pMesh = (CTriMesh*)m_parts[0].pPhysGeom->pGeom;
vectorf ptc,r,n,pt[3],dP;
int i,j,idx[3];
float r2,rmin2=sqr(rmin);
kr *= m_explosionScale;
for(i=0;i<pMesh->m_nTris;i++) {
for(j=0;j<3;j++) pt[j]=m_vtx[idx[j]=pMesh->m_pIndices[i*3+j]].pos+m_pos+m_offs0;
ptc = (pt[0]+pt[1]+pt[2])*(1.0f/3); r = ptc-epicenter; r2 = r.len2();
n = pt[1]-pt[0]^pt[2]-pt[0]; n *= sgnnz(r*n);
dP = n*((n*r)*0.5f*kr/(sqrt_tpl(n.len2()*r2)*max(rmin2,r2)));
for(j=0;j<3;j++)
m_vtx[idx[j]].vel += dP*m_vtx[idx[j]].massinv;
}
m_bAwake = 1;
}
}
int CSoftEntity::GetStateSnapshot(CStream &stm,float time_back,int flags)
{
stm.WriteNumberInBits(SNAPSHOT_VERSION,4);
stm.WriteBits((BYTE*)&m_qrot0,sizeof(m_qrot0)*8);
stm.Write(m_bAwake!=0);
return 1;
}
int CSoftEntity::SetStateFromSnapshot(CStream &stm, int flags)
{
int ver=0;
bool bnz;
stm.ReadNumberInBits(ver,4);
if (ver!=SNAPSHOT_VERSION)
return 0;
if (!(flags & ssf_no_update)) {
pe_params_pos pp;
stm.ReadBits((BYTE*)&pp.q,sizeof(pp.q)*8);
SetParams(&pp);
stm.Read(bnz);
m_bAwake = bnz? 1:0;
} else
stm.Seek(stm.GetReadPos()+sizeof(quaternionf)*8+1);
return 1;
}
void CSoftEntity::DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags)
{
CPhysicalEntity::DrawHelperInformation(DrawLineFunc,flags);
int i;
vectorf offs = m_pos+m_offs0;
if (flags & pe_helper_geometry) for(i=0;i<m_nEdges;i++)
DrawLineFunc(m_vtx[m_edges[i].ivtx[0]].pos+offs, m_vtx[m_edges[i].ivtx[1]].pos+offs);
if (flags & pe_helper_collisions) for(i=0;i<m_nVtx;i++) if (m_vtx[i].pContactEnt && !m_vtx[i].bSeparating)
DrawLineFunc(m_vtx[i].pos+offs-m_vtx[i].ncontact*m_thickness, m_vtx[i].pos+offs);
}
void CSoftEntity::GetMemoryStatistics(ICrySizer *pSizer)
{
CPhysicalEntity::GetMemoryStatistics(pSizer);
if (GetType()==PE_SOFT)
pSizer->AddObject(this, sizeof(CSoftEntity));
pSizer->AddObject(m_vtx, m_nVtx*sizeof(m_vtx[0]));
pSizer->AddObject(m_edges, m_nEdges*sizeof(m_edges[0]));
pSizer->AddObject(m_pVtxEdges, m_nEdges*2*sizeof(m_pVtxEdges[0]));
}

119
CryPhysics/softentity.h Normal file
View File

@@ -0,0 +1,119 @@
//////////////////////////////////////////////////////////////////////
//
// Soft Entity header
//
// File: softentity.h
// Description : SoftEntity class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef softentity_h
#define softentity_h
#pragma once
struct se_vertex {
~se_vertex() { if (pContactEnt) pContactEnt->Release(); }
vectorf pos,pos0,posorg;
vectorf vel,vel0;
float massinv;
float volume;
vectorf n,ncontact;
int bSeparating;
int iSorted,iSorted0;
float area;
int iStartEdge,iEndEdge,bFullFan;
float rnEdges;
CPhysicalEntity *pContactEnt;
int iContactPart;
int iContactNode;
vectorf vcontact;
int surface_idx[2];
int bAttached;
vectorf ptAttach;
vectorf P,dv,r,d;
};
struct se_edge {
int ivtx[2];
float len0;
float len,rlen;
float kd;
};
class CSoftEntity : public CPhysicalEntity {
public:
CSoftEntity(CPhysicalWorld *pworld);
virtual ~CSoftEntity();
virtual pe_type GetType() { return PE_SOFT; }
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual int SetParams(pe_params *_params);
virtual int GetParams(pe_params *_params);
virtual int Action(pe_action*);
virtual int GetStatus(pe_status*);
virtual int Awake(int bAwake=1,int iSource=0) { if (m_bAwake=bAwake) m_nSlowFrames=0; return 1; }
virtual int IsAwake(int ipart=-1) { return m_bAwake; }
virtual void AlertNeighbourhoodND();
virtual void StartStep(float time_interval);
virtual float GetMaxTimeStep(float time_interval);
virtual int Step(float time_interval);
virtual int RayTrace(CRayGeom *pRay, geom_contact *&pcontacts);
virtual void ApplyVolumetricPressure(const vectorf &epicenter, float kr, float rmin);
enum snapver { SNAPSHOT_VERSION = 10 };
virtual int GetStateSnapshot(CStream &stm, float time_back=0,int flags=0);
virtual int SetStateFromSnapshot(CStream &stm, int flags);
virtual void DrawHelperInformation(void (*DrawLineFunc)(float*,float*), int flags);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
se_vertex *m_vtx;
se_edge *m_edges;
int *m_pVtxEdges;
int m_nVtx,m_nEdges;
vectorf m_offs0;
quaternionf m_qrot0;
int m_bMeshUpdated;
float m_timeStepFull;
float m_timeStepPerformed;
vectorf m_gravity;
float m_Emin;
float m_maxAllowedStep;
int m_bAwake,m_nSlowFrames;
float m_damping;
float m_accuracy;
int m_nMaxIters;
float m_prevTimeInterval;
float m_thickness;
float m_ks,m_kdRatio;
float m_maxSafeStep;
float m_density;
float m_coverage;
float m_friction;
float m_impulseScale;
float m_explosionScale;
float m_collImpulseScale;
float m_maxCollImpulse;
int m_collTypes;
plane m_waterPlane;
float m_waterDensity;
float m_waterDamping;
float m_waterResistance;
vectorf m_waterFlow;
float m_airResistance;
vectorf m_wind;
};
#endif

253
CryPhysics/spheregeom.cpp Normal file
View File

@@ -0,0 +1,253 @@
#include "stdafx.h"
#include "utils.h"
#include "primitives.h"
#include "unprojectionchecks.h"
#include "bvtree.h"
#include "singleboxtree.h"
#include "geometry.h"
#include "spheregeom.h"
CSphereGeom* CSphereGeom::CreateSphere(sphere *psphere)
{
m_sphere.center = psphere->center;
m_sphere.r = psphere->r;
box bbox;
bbox.Basis.SetIdentity();
bbox.center = m_sphere.center;
bbox.size.Set(m_sphere.r,m_sphere.r,m_sphere.r);
m_Tree.SetBox(&bbox);
m_Tree.Build(this);
m_minVtxDist = m_sphere.r*1E-4f;
return this;
}
int CSphereGeom::CalcPhysicalProperties(phys_geometry *pgeom)
{
pgeom->pGeom = this;
pgeom->origin = m_sphere.center;
pgeom->q.SetIdentity();
pgeom->V = 4.0f/3*pi*cube(m_sphere.r);
float x2 = sqr(m_sphere.r)*0.4f;
pgeom->Ibody.Set(x2,x2,x2);
return 1;
}
int CSphereGeom::PointInsideStatus(const vectorf &pt)
{
return (isneg((pt-m_sphere.center).len2()-sqr(m_sphere.r))<<1)-1;
}
int CSphereGeom::PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts)
{
static short g_SphIdBuf[1];
pGTest->pGeometry = this;
pGTest->pBVtree = &m_Tree;
m_Tree.PrepareForIntersectionTest(pGTest);
pGTest->primbuf = pGTest->primbuf1 = g_SphBuf+g_SphBufPos++;
pGTest->szprimbuf = 1;
pGTest->typeprim = sphere::type;
pGTest->szprim = sizeof(sphere);
pGTest->idbuf = g_SphIdBuf;
pGTest->surfaces = 0;
pGTest->edges = 0;
pGTest->minAreaEdge = 1E10f;
return 1;
}
int CSphereGeom::PreparePrimitive(geom_world_data *pgwd,primitive *&pprim)
{
sphere *psph = g_SphBuf+g_SphBufPos;
g_SphBufPos = g_SphBufPos+1 & sizeof(g_SphBuf)/sizeof(g_SphBuf[0])-1;
psph->center = pgwd->R*m_sphere.center*pgwd->scale + pgwd->offset;
psph->r = m_sphere.r*pgwd->scale;
pprim = psph;
return sphere::type;
}
int CSphereGeom::GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId)
{
((sphere*)pRes)->center = pGTest->R*m_sphere.center*pGTest->scale + pGTest->offset;
((sphere*)pRes)->r = m_sphere.r*pGTest->scale;
pGTest->bTransformUpdated = 0; *pResId = -1;
return 1;
}
int CSphereGeom::GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest)
{
pprim = pGTest->primbuf1;
((sphere*)pprim)->center = pGTest->R*m_sphere.center*pGTest->scale + pGTest->offset;
((sphere*)pprim)->r = m_sphere.r*pGTest->scale;
pGTest->idbuf[0] = -1;
pGTest->nSurfaces = 0;
pGTest->nEdges = 0;
return 1;
}
int CSphereGeom::FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters)
{
vectorf center = pgwd->R*m_sphere.center*pgwd->scale + pgwd->offset;
float r = m_sphere.r*pgwd->scale;
ptres[1] = ptdst0;
if ((ptdst0-center).len2()<r*r) {
ptres[0].Set(1E10f,1E10f,1E10f);
return -1;
}
ptres[0] = center + (ptdst0-center).normalized()*r;
return 1;
}
int CSphereGeom::UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact)
{
vectorf dc = center-m_sphere.center;
if (dc.len2() > sqr(m_sphere.r+rsep))
return 0;
pcontact->n = dc.normalized();
pcontact->t = dc*pcontact->n-m_sphere.r-r;
pcontact->pt = m_sphere.center+pcontact->n*m_sphere.r;
return 1;
}
float CSphereGeom::CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter)
{
float r=m_sphere.r*pgwd->scale,x;
vectorf n=pplane->n, center=pgwd->R*m_sphere.center+pgwd->offset;
massCenter = center;
x = (pplane->origin-center)*n;
if (x>r)
return 0;
if (x<-r)
return (4.0f/3)*pi*cube(r);
massCenter += n*(pi*0.5f*(x*x*(r*r-x*x*0.5f)-r*r*r*r*0.5f)-x);
return pi*((2.0f/3)*cube(r)+x*(r*r-x*x*(1.0f/3)));
}
void CSphereGeom::CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres)
{
vectorf center,v,vn,axisx,axisy,n;
float r,x,vxn,vxninv,nv,cx,ry,l,lx,ly,circ_S,circ_x,ell_S,ell_x,S;
center = pgwd->R*m_sphere.center*pgwd->scale+pgwd->offset;
r = m_sphere.r*pgwd->scale; n = pplane->n;
v = pgwd->v + (pgwd->w ^ center-pgwd->centerOfMass);
x = (pplane->origin-center)*n;
if (fabsf(x)>r) {
dLres.zero(); dPres.zero();
if (x>r)
dPres = v*(-pi*r*r);
return;
}
vn = v.normalized();
axisy = vn^n; vxn = axisy.len();
if (vxn<0.01f) {
//axisy = vn.orthogonal().normalized(); l = sgnnz(x)*r;
axisy = GetOrthogonal(vn).normalized(); l = sgnnz(x)*r;
} else { // l - water plane - v+ plane intersection line along x
axisy *= (vxninv=1.0f/vxn); l = x*vxninv;
}
axisx = axisy^vn;
nv = n*v;
cx = x*vxn; // ellipse center along x
ry = max(0.001f,sqrt_tpl(r*r-x*x)); // ellipse radius along y
lx = max(-r*0.999f,min(r*0.999f,l)); ly = sqrt_tpl(r*r-lx*lx);
circ_S = (lx*ly+r*r*(pi*0.5f+asin_tpl(lx/r)))*0.5f;
circ_x = cube(ly)*(-1.0f/3);
lx = max(-ry*0.999f,min(ry*0.999f,(l-cx)/max(0.001f,fabs_tpl(nv)))); ly = sqrt_tpl(ry*ry-lx*lx);
ell_S = (lx*ly+ry*ry*(pi*0.5f+asin_tpl(lx/ry)*sgnnz(nv)))*fabs_tpl(nv)*0.5f;
ell_x = cube(ly)*(-1.0f/3)*nv+cx;
S = circ_S+ell_S;
x = (circ_x*circ_S+ell_x*ell_S)/S;
dPres = -v*S;
center += axisx*x + vn*sqrt_tpl(max(0.0f,r*r-x*x));
dLres = center-pgwd->centerOfMass ^ dPres;
}
void CSphereGeom::CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L)
{
vectorf dc = gwd->R*m_sphere.center*gwd->scale+gwd->offset-epicenter;
P += dc*(k/(dc.len()*max(dc.len2(),sqr(rmin))));
}
int CSphereGeom::DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale)
{
vectorf center=pgwd->R*m_sphere.center*pgwd->scale+pgwd->offset, axisx,axisy,pt[8];
float x=m_sphere.r*pgwd->scale,y=0;
const float step=1.0f/sqrt2;
//axisx = center.orthogonal().normalized(); axisy = (axisx^center).normalized();
axisx = GetOrthogonal(center).normalized(); axisy = (axisx^center).normalized();
for(int i=0;i<8;i++) {
pt[i] = center + axisx*x + axisy*y;
x = (x-y)*(1.0f/sqrt2);
y = x+y*sqrt2;
}
RasterizePolygonIntoCubemap(pt,8, iPass, pGrid,nRes, rmin,rmax,zscale);
return 1;
}
void CSphereGeom::DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel)
{
if (iLevel==0) {
int i,ix,iy,iz;
vectorf pt0,pt1;
const float stepcos=cry_cosf(pi/8),stepsin=cry_sinf(pi/8);
for(iz=0;iz<3;iz++) {
ix = inc_mod3[iz]; iy = dec_mod3[iz];
pt0[ix]=m_sphere.r*gwd->scale; pt0[iy]=pt0[iz]=pt1[iz]=0;
for(i=0;i<16;i++) {
pt1[ix] = pt0[ix]*stepcos-pt0[iy]*stepsin;
pt1[iy] = pt0[ix]*stepsin+pt0[iy]*stepcos;
DrawLineFunc(gwd->R*(pt0+m_sphere.center)+gwd->offset, gwd->R*(pt1+m_sphere.center)+gwd->offset);
pt0 = pt1;
}
}
} else {
BV *pbbox;
ResetGlobalPrimsBuffers();
m_Tree.GetNodeBV(gwd->R,gwd->offset,gwd->scale, pbbox);
DrawBBox(DrawLineFunc,gwd, &m_Tree,(BBox*)pbbox,iLevel-1);
}
}
void CSphereGeom::GetMemoryStatistics(ICrySizer *pSizer)
{
pSizer->AddObject(this, sizeof(CSphereGeom));
}
void CSphereGeom::Save(CMemStream &stm)
{
stm.Write(m_sphere);
m_Tree.Save(stm);
}
void CSphereGeom::Load(CMemStream &stm)
{
stm.Read(m_sphere);
m_Tree.Load(stm,this);
}

43
CryPhysics/spheregeom.h Normal file
View File

@@ -0,0 +1,43 @@
#ifndef spheregeom_h
#define spheregeom_h
#pragma once
class CSphereGeom : public CPrimitive {
public:
CSphereGeom() { m_iCollPriority = 3; m_minVtxDist = 0; }
CSphereGeom* CreateSphere(sphere *pcyl);
virtual int GetType() { return GEOM_SPHERE; }
virtual void GetBBox(box *pbox) { m_Tree.GetBBox(pbox); }
virtual int CalcPhysicalProperties(phys_geometry *pgeom);
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters);
virtual int PointInsideStatus(const vectorf &pt);
virtual float CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter);
virtual void CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres);
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L);
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale);
virtual CBVTree *GetBVTree() { return &m_Tree; }
virtual int UnprojectSphere(vectorf center,float r,float rsep, contact *pcontact);
virtual int GetPrimitive(int iPrim, primitive *pprim) { *(sphere*)pprim = m_sphere; return sizeof(sphere); }
virtual void DrawWireframe(void (*DrawLineFunc)(float*,float*), geom_world_data *gwd, int iLevel);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false);
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim);
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId);
virtual int GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest);
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm);
sphere m_sphere;
CSingleBoxTree m_Tree;
};
#endif

2180
CryPhysics/trimesh.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

111
CryPhysics/trimesh.h Normal file
View File

@@ -0,0 +1,111 @@
#ifndef trimesh_h
#define trimesh_h
struct trinfo {
index_t ibuddy[3];
};
struct border_trace {
vectorf *pt;
int (*itri)[2];
float *seglen;
int npt,szbuf;
vectorf pt_end;
int itri_end;
int iedge_end;
float end_dist2;
int iMark,nLoop;
vectorr n_sum[2];
vectorf n_best;
int ntris[2];
};
class CTriMesh : public CGeometry {
public:
CTriMesh();
virtual ~CTriMesh();
CTriMesh* CreateTriMesh(strided_pointer<const vectorf> pVertices,index_t *pIndices,const short *pIds,int nTris, int flags,
bool bCanSortTriangles=false,bool bCopyVertices=true, int nMinTrisPerNode=2,int nMaxTrisPerNode=4, float favorAABB=1.0f);
virtual int GetType() { return GEOM_TRIMESH; }
virtual int Intersect(IGeometry *pCollider, geom_world_data *pdata1,geom_world_data *pdata2, intersection_params *pparams, geom_contact *&pcontacts);
virtual void GetBBox(box *pbox) { m_pTree->GetBBox(pbox); }
virtual int FindClosestPoint(geom_world_data *pgwd, int &iPrim,int &iFeature, const vectorf &ptdst0,const vectorf &ptdst1,
vectorf *ptres, int nMaxIters=10);
virtual int CalcPhysicalProperties(phys_geometry *pgeom);
virtual int PointInsideStatus(const vectorf &pt);
virtual void DrawWireframe(void (*DrawLineFunc)(float*,float*),geom_world_data *gwd, int iLevel);
virtual void CalcVolumetricPressure(geom_world_data *gwd, const vectorf &epicenter,float k,float rmin,
const vectorf &centerOfMass, vectorf &P,vectorf &L);
virtual float CalculateBuoyancy(const plane *pplane, const geom_world_data *pgwd, vectorf &massCenter);
virtual void CalculateMediumResistance(const plane *pplane, const geom_world_data *pgwd, vectorf &dPres,vectorf &dLres);
virtual int GetPrimitiveId(int iPrim,int iFeature) { return m_pIds ? m_pIds[iPrim]:-1; }
virtual int IsConvex(float tolerance);
virtual void PrepareForRayTest(float raylen);
virtual int DrawToOcclusionCubemap(const geom_world_data *pgwd, int iStartPrim,int nPrims, int iPass, int *pGrid[6],int nRes,
float rmin,float rmax,float zscale);
virtual CBVTree *GetBVTree() { return m_pTree; }
virtual int GetPrimitiveCount() { return m_nTris; }
virtual int GetPrimitive(int iPrim, primitive *pprim) {
if ((unsigned int)iPrim>=(unsigned int)m_nTris)
return 0;
((triangle*)pprim)->pt[0] = m_pVertices[m_pIndices[iPrim*3+0]];
((triangle*)pprim)->pt[1] = m_pVertices[m_pIndices[iPrim*3+1]];
((triangle*)pprim)->pt[2] = m_pVertices[m_pIndices[iPrim*3+2]];
((triangle*)pprim)->n = m_pNormals[iPrim];
return sizeof(triangle);
}
virtual int GetFeature(int iPrim,int iFeature, vectorf *pt);
virtual int PreparePrimitive(geom_world_data *pgwd,primitive *&pprim);
virtual void RemapFaceIds(short *pMap,int sz) { if (m_pIds) for(int i=0;i<m_nTris;i++) m_pIds[i]=pMap[min(m_pIds[i],sz-1)]; }
virtual void GetMemoryStatistics(ICrySizer *pSizer);
virtual void Save(CMemStream &stm);
virtual void Load(CMemStream &stm);
virtual int PrepareForIntersectionTest(geometry_under_test *pGTest, CGeometry *pCollider,geometry_under_test *pGTestColl, bool bKeepPrevContacts=false);
virtual int RegisterIntersection(primitive *pprim1,primitive *pprim2, geometry_under_test *pGTest1,geometry_under_test *pGTest2,
prim_inters *pinters);
virtual void CleanupAfterIntersectionTest(geometry_under_test *pGTest);
virtual int GetPrimitiveList(int iStart,int nPrims, int typeCollider,primitive *pCollider,int bColliderLocal,
geometry_under_test *pGTest,geometry_under_test *pGTestOp, primitive *pRes,short *pResId);
virtual int GetUnprojectionCandidates(int iop,const contact *pcontact, primitive *&pprim,int *&piFeature, geometry_under_test *pGTest);
virtual int PreparePolygon(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
virtual int PreparePolyline(coord_plane *psurface, int iPrim,int iFeature, geometry_under_test *pGTest, vector2df *&ptbuf,
int *&pVtxIdBuf,int *&pEdgeIdBuf);
int GetEdgeByBuddy(int itri,int itri_buddy) {
int iedge=0,imask;
imask = m_pTopology[itri].ibuddy[1]-itri_buddy; imask = imask-1>>31 ^ imask>>31; iedge = 1&imask;
imask = m_pTopology[itri].ibuddy[2]-itri_buddy; imask = imask-1>>31 ^ imask>>31; iedge = iedge&~imask | 2&imask;
return iedge;
}
int GetNeighbouringEdgeId(int vtxid, int ivtx);
void PrepareTriangle(int itri,triangle *ptri, const geometry_under_test *pGTest);
int TraceTriangleInters(int iop, primitive *pprims[], int idx_buddy,int type_buddy, prim_inters *pinters,
geometry_under_test *pGTest, border_trace *pborder);
void HashTrianglesToPlane(const coord_plane &hashplane, const vector2df &hashsize, grid &hashgrid,index_t *&pHashGrid,index_t *&pHashData,float cellsize=0);
CBVTree *m_pTree;
trinfo *m_pTopology;
index_t *m_pIndices;
short *m_pIds;
strided_pointer<vectorf> m_pVertices;
vectorf *m_pNormals;
int m_nTris,m_nVertices;
int m_nMaxVertexValency;
int m_flags;
index_t *m_pHashGrid[3],*m_pHashData[3];
grid m_hashgrid[3];
int m_nHashPlanes;
int m_bConvex[4];
float m_ConvexityTolerance[4];
int m_bMultipart;
};
#endif

93
CryPhysics/unprojectionchecks.h Normal file
View File

@@ -0,0 +1,93 @@
#ifndef unprojectionchecks_h
#define unprojectionchecks_h
struct unprojection_mode {
unprojection_mode() { bCheckContact=0; }
int imode;
vectorf dir; // direction or rotation axis
vectorf center; // center of rotation
float vel; // linear or angular velocity
float tmax; // maximum unprojection length (not time)
float minPtDist; // tolerance value
int bCheckContact;
matrix3x3f R0;
vectorf offset0;
};
typedef int (*unprojection_check)(unprojection_mode*, const primitive*,int,const primitive*,int, contact*, geom_contact_area*);
int default_unprojection(unprojection_mode*, const primitive*,int,const primitive*,int, contact*, geom_contact_area*);
int tri_tri_lin_unprojection(unprojection_mode *pmode, const triangle *ptri1,int iFeature1,const triangle *ptri2,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_box_lin_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const box *pbox,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int box_tri_lin_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_cylinder_lin_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const cylinder *pcyl,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int cylinder_tri_lin_unprojection(unprojection_mode *pmode, const cylinder *pcyl,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_sphere_lin_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const sphere *psphere,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int sphere_tri_lin_unprojection(unprojection_mode *pmode, const sphere *psphere,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_ray_lin_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int ray_tri_lin_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_plane_lin_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const plane *pplane,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int plane_tri_lin_unprojection(unprojection_mode *pmode, const plane *pplane,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int box_box_lin_unprojection(unprojection_mode *pmode, const box *pbox1,int iFeature1, const box *pbox2,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int box_cylinder_lin_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1,const cylinder *pcyl,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int cylinder_box_lin_unprojection(unprojection_mode *pmode, const cylinder *pcyl,int iFeature1,const box *pbox,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int box_sphere_lin_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1,const sphere *psph,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int sphere_box_lin_unprojection(unprojection_mode *pmode, const sphere *psph,int iFeature1,const box *pbox,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int cyl_cyl_lin_unprojection(unprojection_mode *pmode, const cylinder *pcyl1,int iFeature1,const cylinder *pcyl2,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int cylinder_sphere_lin_unprojection(unprojection_mode *pmode, const cylinder *pbox,int iFeature1,const sphere *psph,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int sphere_cylinder_lin_unprojection(unprojection_mode *pmode, const sphere *psph,int iFeature1,const cylinder *pcyl,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int sphere_sphere_lin_unprojection(unprojection_mode *pmode, const sphere *psph1,int iFeature1, const sphere *psph2,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_tri_rot_unprojection(unprojection_mode *pmode, const triangle *ptri1,int iFeature1,const triangle *ptri2,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int tri_ray_rot_unprojection(unprojection_mode *pmode, const triangle *ptri,int iFeature1,const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int ray_tri_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1,const triangle *ptri,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int cyl_ray_rot_unprojection(unprojection_mode *pmode, const cylinder *pcyl,int iFeature1,const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int ray_cyl_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1,const cylinder *pcyl,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int box_ray_rot_unprojection(unprojection_mode *pmode, const box *pbox,int iFeature1, const ray *pray,int iFeature2,
contact *pcontact, geom_contact_area *parea);
int ray_box_rot_unprojection(unprojection_mode *pmode, const ray *pray,int iFeature1, const box *pbox,int iFeature2,
contact *pcontact, geom_contact_area *parea);
class CUnprojectionChecker {
public:
CUnprojectionChecker();
int Check(unprojection_mode *pmode, int type1,int type2, const primitive *prim1,int iFeature1,const primitive *prim2,int iFeature2,
contact *pcontact, geom_contact_area *parea=0)
{
return table[pmode->imode][type1][type2](pmode, prim1,iFeature1,prim2,iFeature2, pcontact, parea);
}
int CheckExists(int imode, int type1,int type2) {
return table[imode][type1][type2]!=default_unprojection;
}
unprojection_check table[2][NPRIMS][NPRIMS];
};
extern CUnprojectionChecker g_Unprojector;
#endif

1270
CryPhysics/utils.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

344
CryPhysics/utils.h Normal file
View File

@@ -0,0 +1,344 @@
//////////////////////////////////////////////////////////////////////
//
// Utils header
//
// File: utils.h
// Description : Various utility functions definitions
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef utils_h
#define utils_h
#include <math.h>
#include "Cry_Math.h"
using namespace primitives;
void ExtrudeBox(const primitives::box *pbox, const vectorf& dir,float step, primitives::box *pextbox);
#define USE_MATRIX_POOLS
#include "matrixnm.h"
#include "vectorn.h"
#include "quotient.h"
#include "polynomial.h"
#include "primitives.h"
const float fmag = 1.5f*(1<<23);
union floatint {
float fval;
int ival;
};
#if defined(WIN64) || defined(LINUX64)
const int imag = (23+127)<<23 | 1<<22;
int float2int(float x);
#elif defined(LINUX32)
const int imag = (23+127)<<23 | 1<<22;
inline int float2int(float x) {
floatint u;
u.fval = x+fmag;
return u.ival-imag;
}
#else //WIN64
__declspec(naked) inline int64 GetTicks() {
__asm rdtsc
__asm ret
}
const int imag = -((23+127)<<23 | 1<<22);
// had to rewrite in assembly, since it's impossible to tell the compiler that in this case
// addition is not associative, i.e. (x+0.5)+fmag!=x+(fmag+0.5)
__declspec(naked) inline int float2int(float x)
{ __asm {
fld [esp+4]
fadd fmag
mov eax, imag
fstp [esp-4]
add eax, [esp-4]
ret
}}
#endif //WIN64
int qhull(strided_pointer<vectorf> pts, int npts, index_t*& pTris);
struct ptitem2d {
vector2df pt;
ptitem2d *next,*prev;
int iContact;
};
struct edgeitem {
ptitem2d *pvtx;
ptitem2d *plist;
edgeitem *next,*prev;
float area,areanorm2;
edgeitem *next1,*prev1;
int idx;
};
int qhull2d(ptitem2d *pts,int nVtx, edgeitem *edges);
real ComputeMassProperties(strided_pointer<const vectorf> points, const index_t *faces, int nfaces, vectorr &center,matrix3x3 &I);
template<class ftype,int stridei,int stridej>
void OffsetInertiaTensor(Matrix33_tpl<ftype,stridei,stridej> &I,const Vec3_tpl<ftype> &center,ftype M)
{
I(0,0) += M*(center.y*center.y + center.z*center.z);
I(1,1) += M*(center.x*center.x + center.z*center.z);
I(2,2) += M*(center.x*center.x + center.y*center.y);
I(1,0) = I(0,1) = I(0,1)-M*center.x*center.y;
I(2,0) = I(0,2) = I(0,2)-M*center.x*center.z;
I(2,1) = I(1,2) = I(1,2)-M*center.y*center.z;
}
void ComputeMeshEigenBasis(strided_pointer<const vectorf> pVertices,const index_t *pIndices,int nTris, vectorr *eigen_axes,vectorr &center);
enum booltype { bool_intersect=1 };
int boolean2d(booltype type, vector2df *ptbuf1,int npt1, vector2df *ptbuf2,int npt2,int bClosed, vector2df *&ptbuf,int *&pidbuf);
real RotatePointToPlane(const vectorr &pt, const vectorr &axis,const vectorr &center, const vectorr &n,const vectorr &origin);
inline void LeftOffsetSpatialMatrix(matrixf &mtx, const vectorf &offset){
matrix3x3f rmtx;
matrix3x3in6x6f &A((matrix3x3in6x6f&)mtx.data[0]),&B((matrix3x3in6x6f&)mtx.data[3]),
&C((matrix3x3in6x6f&)mtx.data[18]),&D((matrix3x3in6x6f&)mtx.data[21]);
C += (crossproduct_matrix(offset, rmtx)*=A);
D += (crossproduct_matrix(offset, rmtx)*=B);
}
inline void RightOffsetSpatialMatrix(matrixf &mtx, const vectorf &offset){
matrix3x3f rmtx,tmtx;
matrix3x3in6x6f &A((matrix3x3in6x6f&)mtx.data[0]),&B((matrix3x3in6x6f&)mtx.data[3]),
&C((matrix3x3in6x6f&)mtx.data[18]),&D((matrix3x3in6x6f&)mtx.data[21]);
A += ((tmtx=B)*=crossproduct_matrix(offset,rmtx));
C += ((tmtx=D)*=rmtx);
}
inline void SpatialTranspose(matrixf &src, matrixf& dst) {
int i,j;
dst.nRows = src.nCols; dst.nCols = src.nRows;
for(i=0;i<src.nRows;i++) for(j=0;j<3;j++) {
dst[j][i] = src[i][j+3];
dst[j+3][i] = src[i][j];
}
}
template<class ftype> inline Vec3_tpl<ftype> cross_with_ort(const Vec3_tpl<ftype> &vec, int iz) {
Vec3_tpl<ftype> res(zero);
int ix=inc_mod3[iz], iy=dec_mod3[iz];
res[iz]=0; res[ix]=vec[iy]; res[iy]=-vec[ix];
return res;
}
void get_xqs_from_matrices(float *pMtx3x3,float *pMtx3x3T,float *pMtx4x4,float *pMtx4x4T, vectorf &pos,quaternionf &q,float &scale);
int BreakPolygon(vector2df *ptSrc,int nPt, int nCellx,int nCelly, int maxPatchTris, vector2df *&ptout,int *&nPtOut,
float jointhresh=0.5f,int seed=-1);
void RasterizePolygonIntoCubemap(const vectorf *pt,int npt, int iPass, int *pGrid[6],int nRes, float rmin,float rmax,float zscale);
void GrowAndCompareCubemaps(int *pGridOcc[6],int *pGrid[6],int nRes, int nGrow, int &nCells,int &nOccludedCells);
void CalcMediumResistance(const vectorf *ptsrc,int npt, const vectorf& n, const plane &waterPlane,
const vectorf &vworld,const vectorf &wworld,const vectorf &com, vectorf &P,vectorf &L);
int CoverPolygonWithCircles(strided_pointer<vector2df> pt,int npt,bool bConsecutive, const vector2df &center, vector2df *&centers,
float *&radii, float minCircleRadius);
int ChoosePrimitiveForMesh(strided_pointer<const vectorf> pVertices,const index_t *pIndices,int nTris, const vectorr *eigen_axes,const vectorr &center,
int flags, float tolerance, primitive *&pprim);
void ExtrudeBox(const box *pbox, const vectorf& dir,float step, box *pextbox);
template<class CellChecker>
int DrawRayOnGrid(primitives::grid *pgrid, vectorf &origin,vectorf &dir, CellChecker &cell_checker)
{
int i,ishort,ilong,bStep,bPrevStep,ilastcell;
float dshort,dlong,frac;
quotientf tx[2],ty[2],t[2];
vector2di istep,icell,idirsgn;
// crop ray with grid bounds
idirsgn.set(sgnnz(dir.x),sgnnz(dir.y));
i = idirsgn.x;
tx[1-i>>1].set(-origin.x*i, dir.x*i);
tx[1+i>>1].set((pgrid->size.x*pgrid->step.x-origin.x)*i, dir.x*i);
i = idirsgn.y;
ty[1-i>>1].set(-origin.y*i, dir.y*i);
ty[1+i>>1].set((pgrid->size.y*pgrid->step.y-origin.y)*i, dir.y*i);
t[0] = max(t[0].set(0,1),max(tx[0],ty[0]));
t[1] = min(t[1].set(1,1),min(tx[1],ty[1]));
if (t[0]>=t[1])
return 0;
if (t[0]>0)
origin += dir*t[0].val();
if (t[0]>0 || t[1]<1)
dir *= (t[1]-t[0]).val();
ilong = isneg(fabs_tpl(dir.x)-fabs_tpl(dir.y));
ishort = ilong^1;
dshort = fabs_tpl(dir[ishort]); dlong = fabs_tpl(dir[ilong]);
istep[ilong] = idirsgn[ilong];
istep[ishort] = idirsgn[ishort];
icell.set(float2int(origin.x*pgrid->stepr.x-0.5f), float2int(origin.y*pgrid->stepr.y-0.5f));
ilastcell = float2int((origin.y+dir.y)*pgrid->stepr.y-0.5f)<<16 | float2int((origin.x+dir.x)*pgrid->stepr.x-0.5f);
if (cell_checker.check_cell(icell,ilastcell) || (icell.y<<16|icell.x)==ilastcell)
return 1;
t[0].set((icell[ilong]+(istep[ilong]+1>>1))*pgrid->step[ilong]-origin[ilong], dir[ilong]);
frac = (origin[ishort]+dir[ishort]*t[0].val() - icell[ishort]*pgrid->step[ishort])*pgrid->stepr[ishort];
i = isneg(dir[ishort]);
frac = i+frac*(1-(i<<1));
if (frac>1.0f) {
icell[ishort] += istep[ishort];
if (cell_checker.check_cell(icell,ilastcell) || (icell.y<<16|icell.x)==ilastcell)
return 1;
frac -= 1;
}
frac *= dlong;
icell[ilong] += istep[ilong];
bPrevStep = 0;
do {
if (cell_checker.check_cell(icell,ilastcell) || (icell.y<<16|icell.x)==ilastcell)
return 1;
frac += dshort*(bPrevStep^1); bStep = isneg(dlong-frac);
icell[ishort] += bStep*istep[ishort]; frac -= dlong*bStep;
icell[ilong] += istep[ilong]&~-bStep;
bPrevStep = bStep;
} while(true);
return 0;
}
int bin2ascii(const unsigned char *pin,int sz, unsigned char *pout);
int ascii2bin(const unsigned char *pin,int sz, unsigned char *pout);
template<class T> inline T *_align16(T *ptr) { return (T*)(((INT_PTR)ptr-1&~15)+16); }
#if !defined(LINUX)
template<class dtype> bool is_valid(const dtype &op) { return is_valid(op|op); }
inline bool is_valid(float op) { return op*op>=0 && op*op<1E30f; }
inline bool is_valid(int op) { return true; }
inline bool is_valid(unsigned int op) { return true; }
#endif
void WritePacked(CStream &stm, int num);
void WritePacked(CStream &stm, uint64 num);
void ReadPacked(CStream &stm,int &num);
void ReadPacked(CStream &stm,uint64 &num);
void WriteCompressedPos(CStream &stm, const vectorf &pos, bool bCompress=true);
void ReadCompressedPos(CStream &stm, vectorf &pos, bool &bWasCompressed);
vectorf CompressPos(const vectorf &pos);
const int CMP_QUAT_SZ = 49;
void WriteCompressedQuat(CStream &stm, const quaternionf &q);
void ReadCompressedQuat(CStream &stm,quaternionf &q);
quaternionf CompressQuat(const quaternionf &q);
const int CMP_VEL_SZ = 48;
inline void WriteCompressedVel(CStream &stm, const vectorf &vel, const float maxvel) {
stm.Write((short)max(-32768,min(32767,float2int(vel.x*(32767.0f/maxvel)))));
stm.Write((short)max(-32768,min(32767,float2int(vel.y*(32767.0f/maxvel)))));
stm.Write((short)max(-32768,min(32767,float2int(vel.z*(32767.0f/maxvel)))));
}
inline void ReadCompressedVel(CStream &stm, vectorf &vel, const float maxvel) {
short i;
stm.Read(i); vel.x = i*(maxvel/32767.0f);
stm.Read(i); vel.y = i*(maxvel/32767.0f);
stm.Read(i); vel.z = i*(maxvel/32767.0f);
}
inline vectorf CompressVel(const vectorf &vel, const float maxvel) {
vectorf res;
res.x = max(-32768,min(32767,float2int(vel.x*(32767.0f/maxvel)))) * (maxvel/32767.0f);
res.y = max(-32768,min(32767,float2int(vel.y*(32767.0f/maxvel)))) * (maxvel/32767.0f);
res.z = max(-32768,min(32767,float2int(vel.z*(32767.0f/maxvel)))) * (maxvel/32767.0f);
return res;
}
inline void WriteCompressedFloat(CStream &stm, float f, const float maxval,const int nBits) {
const int imax = (1<<nBits-1)-1;
stm.WriteNumberInBits(max(-imax-1,min(imax,float2int(f*(imax/maxval)))),nBits);
}
inline void ReadCompressedFloat(CStream &stm, float &f, const float maxval,const int nBits) {
const int imax = (1<<nBits-1)-1;
unsigned int num;
stm.ReadNumberInBits(num,nBits);
f = ((int)num | ((int)num<<32-nBits & 1<<31)>>31-nBits)*(maxval/imax);
}
inline float CompressFloat(float f,const float maxval,const int nBits) {
const int imax = (1<<nBits-1)-1;
return max(-imax-1,min(imax,float2int(f*(imax/maxval)))) * (maxval/imax);
}
inline intptr_t iszero_mask(void *ptr) { return (intptr_t)ptr>>sizeof(intptr_t)*8-1 ^ (intptr_t)ptr-1>>sizeof(intptr_t)*8-1; }
template<class ftype> inline int AABB_overlap(Vec3_tpl<ftype> *BBox0,Vec3_tpl<ftype> *BBox1) {
return isneg(fabs_tpl(BBox0[0].x+BBox0[1].x-BBox1[0].x-BBox1[1].x) - (BBox0[1].x-BBox0[0].x)-(BBox1[1].x-BBox1[0].x)) &
isneg(fabs_tpl(BBox0[0].y+BBox0[1].y-BBox1[0].y-BBox1[1].y) - (BBox0[1].y-BBox0[0].y)-(BBox1[1].y-BBox1[0].y)) &
isneg(fabs_tpl(BBox0[0].z+BBox0[1].z-BBox1[0].z-BBox1[1].z) - (BBox0[1].z-BBox0[0].z)-(BBox1[1].z-BBox1[0].z));
}
extern int g_bitcount[256];
const int SINCOSTABSZ = 1024, SINCOSTABSZ_LOG2 = 10;
extern float g_costab[SINCOSTABSZ],g_sintab[SINCOSTABSZ];
// uncomment the following block to effectively disable validations
/*#define VALIDATOR_LOG(pLog,str)
#define VALIDATORS_START
#define VALIDATOR(member)
#define VALIDATOR_NORM(member)
#define VALIDATOR_RANGE(member,minval,maxval)
#define VALIDATOR_RANGE2(member,minval,maxval)
#define VALIDATORS_END
#define ENTITY_VALIDATE(strSource,pStructure)*/
#if defined(WIN64) || defined(LINUX64)
#define DoBreak {assert(0);}
#else
#define DoBreak { DEBUG_BREAK; }
#endif
#if defined LINUX
#include "validator.h"
#else
#define VALIDATOR_LOG(pLog,str) pLog->Log(str) //OutputDebugString(str)
#define VALIDATORS_START bool validate( const char *strSource, ILog *pLog, const vectorf &pt,\
IPhysicsStreamer *pStreamer, void *param0, int param1, int param2 ) { bool res=true; char errmsg[256];
#define VALIDATOR(member) if (!is_unused(member) && !is_valid(member)) { \
res=false; sprintf(errmsg,"\002%s: (%.50s @ %.1f,%.1f,%.1f) Validation Error: %s is invalid",strSource,\
pStreamer->GetForeignName(param0,param1,param2),pt.x,pt.y,pt.z,#member); \
VALIDATOR_LOG(pLog,errmsg); }
#define VALIDATOR_NORM(member) if (!is_unused(member) && !(is_valid(member) && fabs_tpl((member|member)-1.0f)<0.01f)) { \
res=false; sprintf(errmsg,"\002%s: (%.50s @ %.1f,%.1f,%.1f) Validation Error: %s is invalid or unnormalized",\
strSource,pStreamer->GetForeignName(param0,param1,param2),pt.x,pt.y,pt.z,#member); VALIDATOR_LOG(pLog,errmsg); }
#define VALIDATOR_NORM_MSG(member,msg,member1) if (!is_unused(member) && !(is_valid(member) && fabs_tpl((member|member)-1.0f)<0.01f)) { \
res=false; sprintf(errmsg,"\002%s: (%.50s @ %.1f,%.1f,%.1f) Validation Error: %s is invalid or unnormalized %s",\
strSource,pStreamer->GetForeignName(param0,param1,param2),pt.x,pt.y,pt.z,#member,msg); \
if (!is_unused(member1)) sprintf(errmsg+strlen(errmsg)," "#member1": %.1f,%.1f,%.1f",member1.x,member1.y,member1.z); \
VALIDATOR_LOG(pLog,errmsg); }
#define VALIDATOR_RANGE(member,minval,maxval) if (!is_unused(member) && !(is_valid(member) && member>=minval && member<=maxval)) { \
res=false; sprintf(errmsg,"\002%s: (%.50s @ %.1f,%.1f,%.1f) Validation Error: %s is invalid or out of range",\
strSource,pStreamer->GetForeignName(param0,param1,param2),pt.x,pt.y,pt.z,#member); VALIDATOR_LOG(pLog,errmsg); }
#define VALIDATOR_RANGE2(member,minval,maxval) if (!is_unused(member) && !(is_valid(member) && member*member>=minval*minval && \
member*member<=maxval*maxval)) { \
res=false; sprintf(errmsg,"\002%s: (%.50s @ %.1f,%.1f,%.1f) Validation Error: %s is invalid or out of range",\
strSource,pStreamer->GetForeignName(param0,param1,param2),pt.x,pt.y,pt.z,#member); VALIDATOR_LOG(pLog,errmsg); }
#define VALIDATORS_END return res; }
#define ENTITY_VALIDATE(strSource,pStructure) if (!pStructure->validate(strSource,m_pWorld->m_pLog,m_pos,\
m_pWorld->m_pPhysicsStreamer,m_pForeignData,m_iForeignData,m_iForeignFlags)) { \
if (m_pWorld->m_vars.bBreakOnValidation) DoBreak return 0; }
#define ENTITY_VALIDATE_ERRCODE(strSource,pStructure,iErrCode) if (!pStructure->validate(strSource,m_pWorld->m_pLog,m_pos, \
m_pWorld->m_pPhysicsStreamer,m_pForeignData,m_iForeignData,m_iForeignFlags)) { \
if (m_pWorld->m_vars.bBreakOnValidation) DoBreak return iErrCode; }
#endif //LINUX
#endif

183
CryPhysics/vectorn.h Normal file
View File

@@ -0,0 +1,183 @@
//////////////////////////////////////////////////////////////////////
//
// Variable-length Vector header
//
// File: vectorn.h
// Description : vectorn template class declaration and inlined implementation
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef vectorn_h
#define vectorn_h
#pragma once
template <class ftype> class matrix_vector_product_tpl {
public:
matrix_vector_product_tpl(int nrows,int ncols,ftype *pmtxdata, ftype *pvecdata) {
nRows=nrows; nCols=ncols; mtxdata=pmtxdata; vecdata=pvecdata;
}
int nRows,nCols;
ftype *mtxdata,*vecdata;
};
template <class ftype> class vectorn_tpl {
public:
vectorn_tpl(int _len,ftype *pdata=0) {
len = _len;
if (pdata) { flags=mtx_foreign_data; data=pdata; }
#ifdef USE_MATRIX_POOLS
else if (len<64) {
if (vecn_pool_pos+len > vecn_pool_size)
vecn_pool_pos = 0;
data = vecn_pool+vecn_pool_pos;
vecn_pool_pos += len;
flags = mtx_foreign_data;
}
#endif
else { data=new ftype[len]; flags=0; }
}
vectorn_tpl(vectorn_tpl &src) { flags=src.flags & mtx_foreign_data; data=src.data; len=src.len; src.flags |= mtx_foreign_data; };
~vectorn_tpl() { if (!(flags & mtx_foreign_data)) delete[] data; }
vectorn_tpl& operator=(const vectorn_tpl<ftype> &src) {
if (src.len!=len && !(flags & mtx_foreign_data)) {
if (data) delete data;
data = new ftype[src.len];
}
len = src.len;
for(int i=0;i<len;i++) data[i]=src.data[i];
return *this;
}
template<class ftype1> vectorn_tpl& operator=(const vectorn_tpl<ftype1> &src) {
if (src.len!=len && !(flags & mtx_foreign_data)) {
if (data) delete data;
data = new ftype[src.len];
}
len = src.len;
for(int i=0;i<len;i++) data[i]=src.data[i];
return *this;
}
vectorn_tpl& operator=(const matrix_vector_product_tpl<ftype> &src) {
int i,j;
for(i=0;i<src.nRows;i++) for(data[i]=0,j=0;j<src.nCols;j++)
data[i] += src.mtxdata[i*src.nCols+j]*src.vecdata[j];
return *this;
}
vectorn_tpl& operator+=(const matrix_vector_product_tpl<ftype> &src) {
int i,j;
for(i=0;i<src.nRows;i++) for(j=0;j<src.nCols;j++)
data[i] += src.mtxdata[i*src.nCols+j]*src.vecdata[j];
return *this;
}
vectorn_tpl& operator-=(const matrix_vector_product_tpl<ftype> &src) {
int i,j;
for(i=0;i<src.nRows;i++) for(j=0;j<src.nCols;j++)
data[i] -= src.mtxdata[i*src.nCols+j]*src.vecdata[j];
return *this;
}
ftype len2() { ftype res=0; for(int i=0;i<len;i++) res += data[i]*data[i]; return res; }
ftype& operator[](int idx) const { return data[idx]; }
operator ftype*() { return data; }
vectorn_tpl& zero() { for(int i=0;i<len;i++) data[i]=0; return *this; }
vectorn_tpl& allocate() {
if (flags & mtx_foreign_data) {
ftype *newdata = new ftype[len];
for(int i=0;i<len;i++) newdata[i]=data[i];
data=newdata; flags&=~mtx_foreign_data;
}
}
vectorn_tpl operator*(ftype op) const {
vectorn_tpl<ftype> res(len);
for(int i=0;i<len;i++) res.data[i] = data[i]*op;
return res;
}
vectorn_tpl& operator*=(ftype op) {
for(int i=0;i<len;i++) data[i] *= op;
return *this;
}
ftype *data;
int len;
int flags;
#ifdef USE_MATRIX_POOLS
static ftype vecn_pool[];
static int vecn_pool_pos;
static int vecn_pool_size;
#endif
};
template<class ftype1,class ftype2>
ftype1 operator*(const vectorn_tpl<ftype1>& op1, const vectorn_tpl<ftype2>& op2) {
ftype1 res=0; for(int i=0;i<op1.len;i++) res += op1.data[i]*op2.data[i];
return res;
}
/*template<class ftype1,class ftype2>
vectorn_tpl<ftype1> operator+(const vectorn_tpl<ftype1> &op1,const vectorn_tpl<ftype2> &op2) {
vectorn_tpl<ftype1> res(len);
for(int i=0;i<len;i++) res.data[i] = op1.data[i]+op2.data[i];
return res;
}
*/
template<class ftype1,class ftype2>
vectorn_tpl<ftype1> operator-(const vectorn_tpl<ftype1> &op1, const vectorn_tpl<ftype2> &op2) {
vectorn_tpl<ftype1> res(op1.len);
for(int i=0;i<op1.len;i++) res.data[i] = op1.data[i]-op2.data[i];
return res;
}
template<class ftype1,class ftype2>
vectorn_tpl<ftype1>& operator+=(vectorn_tpl<ftype1> &op1, const vectorn_tpl<ftype2> &op2) {
for(int i=0;i<op1.len;i++) op1.data[i] += op2.data[i];
return op1;
}
template<class ftype1,class ftype2>
vectorn_tpl<ftype1>& operator-=(vectorn_tpl<ftype1> &op1, const vectorn_tpl<ftype2> &op2) {
for(int i=0;i<op1.len;i++) op1.data[i]-=op2.data[i];
return op1;
}
template<class ftype1,class ftype2>
vectorn_tpl<ftype1> operator*(const vectorn_tpl<ftype1> &vec, const matrix_tpl<ftype2> &mtx) {
int i,j; vectorn_tpl<ftype1> res(mtx.nCols);
for(i=0;i<mtx.nCols;i++) for(res.data[i]=0,j=0;j<vec.len;j++)
res.data[i] += vec[j]*mtx[j][i];
return res;
}
/*template<class ftype1,class ftype2>
vectorn_tpl<ftype1> operator*(const matrix_tpl<ftype1> &mtx, const vectorn_tpl<ftype2> &vec) {
int i,j; vectorn_tpl<ftype1> res(mtx.nRows);
for(i=0;i<mtx.nRows;i++) for(res.data[i]=0,j=0;j<vec.len;j++)
res.data[i] += mtx[i][j]*vec[j];
return res;
}*/
template<class ftype>
matrix_vector_product_tpl<ftype> operator*(const matrix_tpl<ftype> &mtx, const vectorn_tpl<ftype> &vec) {
return matrix_vector_product_tpl<ftype>(mtx.nRows,mtx.nCols,mtx.data, vec.data);
}
typedef vectorn_tpl<float> vectornf;
typedef vectorn_tpl<real> vectorn;
#if defined(LINUX)
#define DECLARE_VECTORN_POOL(ftype,sz) template<> ftype vectorn_tpl<ftype>::vecn_pool[sz] = {}; template<> int vectorn_tpl<ftype>::vecn_pool_pos=0; \
template<> int vectorn_tpl<ftype>::vecn_pool_size=sz;
#else
#define DECLARE_VECTORN_POOL(ftype,sz) template<> ftype vectorn_tpl<ftype>::vecn_pool[sz]; template<> int vectorn_tpl<ftype>::vecn_pool_pos=0; \
template<> int vectorn_tpl<ftype>::vecn_pool_size=sz;
#endif //LINUX
#endif

1243
CryPhysics/wheeledvehicleentity.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

115
CryPhysics/wheeledvehicleentity.h Normal file
View File

@@ -0,0 +1,115 @@
//////////////////////////////////////////////////////////////////////
//
// Wheeled Vehicle Entity header
//
// File: wheeledvehicleentity.h
// Description : CWheeledVehicleEntity class declaration
//
// History:
// -:Created by Anton Knyazev
//
//////////////////////////////////////////////////////////////////////
#ifndef wheeledvehicleentity_h
#define wheeledvehicleentity_h
#pragma once
struct suspension_point {
int bDriving; // if the corresponding wheel a driving wheel
int iAxle;
vectorf pt; // the uppermost suspension point in car frame
float fullen; // unconstrained length
float kStiffness; // stiffness coefficient
float kDamping,kDamping0; // damping coefficient
float len0; // initial length in model
float Mpt; // hull "mass" at suspension upper point along suspension direction
quaternionf q0; // used to calculate geometry transformation from wheel transformation
vectorf pos0,ptc0; // ...
float Iinv;
float minFriction,maxFriction;
int flags0,flagsCollider0;
int bCanBrake;
int iBuddy;
float r,rinv; // wheel radius, 1.0/radius
float width;
float curlen; // current length
float steer; // steering angle
float rot; // current wheel rotation angle
float w; // current rotation speed
float wa; // current angular acceleration
float T; // wheel's net torque
float prevTdt;
float prevw;
vectorf ncontact,ptcontact; // filled in RegisterPendingCollisions
int bSlip,bSlipPull;
int bContact;
int surface_idx[2];
vectorf vrel;
vectorf rworld;
float vworld;
float PN;
RigidBody *pbody;
CPhysicalEntity *pent;
int ipart;
};
class CWheeledVehicleEntity : public CRigidEntity, public IRigidBodyOwner {
public:
CWheeledVehicleEntity(CPhysicalWorld *pworld);
virtual pe_type GetType() { return PE_WHEELEDVEHICLE; }
virtual int SetParams(pe_params*);
virtual int GetParams(pe_params*);
virtual int Action(pe_action*);
virtual int GetStatus(pe_status*);
enum snapver { SNAPSHOT_VERSION = 1 };
virtual int GetSnapshotVersion() { return SNAPSHOT_VERSION; }
virtual int GetStateSnapshot(class CStream &stm, float time_back=0, int flags=0);
virtual int SetStateFromSnapshot(class CStream &stm, int flags=0);
virtual int AddGeometry(phys_geometry *pgeom, pe_geomparams* params,int id=-1);
virtual void RemoveGeometry(int id);
virtual float GetMaxTimeStep(float time_interval);
virtual float GetDamping(float time_interval);
virtual void CheckAdditionalGeometry(float time_interval, masktype &contact_mask);
virtual int HasContactsWith(CPhysicalEntity *pent);
virtual void AddAdditionalImpulses(float time_interval);
virtual int Update(float time_interval, float damping);
virtual void ComputeBBox();
//virtual RigidBody *GetRigidBody(int ipart=-1) { return &m_bodyStatic; }
virtual void AddImpulseAtContact(entity_contact *pcontact, int iop, const vectorf &dP) {};
virtual vectorf GetVelocityAtContact(entity_contact *pcontact, int iop) { return vectorf(zero); };
virtual int OnRegisterContact(entity_contact *pcontact, int iop);
virtual void OnSolverEvent(int iEvent) {};
virtual void GetMemoryStatistics(ICrySizer *pSizer);
void UpdateWheelsGeoms();
void RecalcSuspStiffness();
float ComputeDrivingTorque(float time_interval);
suspension_point m_susp[8];
float m_enginePower,m_maxSteer;
float m_engineMaxw,m_engineMinw,m_engineIdlew,m_engineShiftUpw,m_engineShiftDownw,m_gearDirSwitchw,m_engineStartw;
float m_axleFriction,m_brakeTorque,m_clutchSpeed,m_maxBrakingFriction,m_kDynFriction,m_slipThreshold;
float m_kStabilizer;
float m_enginePedal,m_steer,m_clutch,m_wengine;
float m_gears[8];
int m_nGears,m_iCurGear;
int m_bHandBrake;
int m_nHullParts;
int m_iIntegrationType;
float m_EminRigid,m_EminVehicle;
float m_maxAllowedStepVehicle,m_maxAllowedStepRigid;
float m_dampingVehicle;
vectorf m_Ffriction,m_Tfriction;
float m_timeNoContacts;
int m_nContacts,m_bHasContacts;
};
#endif