[deploy] Merge pull request #41 from KiwanoEngine/dev
Add Box2D physics engine
This commit is contained in:
Haibo
GitHub
commit
4ebd32e153
|
|
@ -0,0 +1,188 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Box2D.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2BroadPhase.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2Collision.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2Distance.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2DynamicTree.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2TimeOfImpact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2ChainShape.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2CircleShape.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2EdgeShape.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2PolygonShape.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2Shape.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2BlockAllocator.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Draw.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2GrowableStack.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Math.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Settings.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2StackAllocator.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Timer.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Body.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2ContactManager.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Fixture.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Island.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2TimeStep.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2World.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2WorldCallbacks.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndCircleContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndPolygonContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2CircleContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2Contact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ContactSolver.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndCircleContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndPolygonContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonAndCircleContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonContact.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2DistanceJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2FrictionJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2GearJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2Joint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MotorJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MouseJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PrismaticJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PulleyJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RevoluteJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RopeJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WeldJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WheelJoint.h" />
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Rope\b2Rope.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2BroadPhase.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollideCircle.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollideEdge.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollidePolygon.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2Collision.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2Distance.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2DynamicTree.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2TimeOfImpact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2ChainShape.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2CircleShape.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2EdgeShape.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2PolygonShape.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2BlockAllocator.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Draw.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Math.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Settings.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2StackAllocator.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Timer.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Body.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2ContactManager.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Fixture.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Island.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2World.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2WorldCallbacks.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndCircleContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndPolygonContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2CircleContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2Contact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ContactSolver.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndCircleContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndPolygonContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonAndCircleContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonContact.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2DistanceJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2FrictionJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2GearJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2Joint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MotorJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MouseJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PrismaticJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PulleyJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RevoluteJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RopeJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WeldJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WheelJoint.cpp" />
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Rope\b2Rope.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<ProjectGuid>{0CBA9295-F14D-4966-A7C4-1DD68158176C}</ProjectGuid>
|
||||||
|
<RootNamespace>libBox2D</RootNamespace>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>$(DefaultPlatformToolset)</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<WholeProgramOptimization>true</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>$(DefaultPlatformToolset)</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="Shared">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<OutDir>$(SolutionDir)\output\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>$(SolutionDir)\build\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<OutDir>$(SolutionDir)\output\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>$(SolutionDir)\build\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
<DebugInformationFormat>None</DebugInformationFormat>
|
||||||
|
<MultiProcessorCompilation>true</MultiProcessorCompilation>
|
||||||
|
<AdditionalIncludeDirectories>../../../src/3rd-party;</AdditionalIncludeDirectories>
|
||||||
|
<MinimalRebuild>false</MinimalRebuild>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<BufferSecurityCheck>false</BufferSecurityCheck>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
<DebugInformationFormat>None</DebugInformationFormat>
|
||||||
|
<MultiProcessorCompilation>true</MultiProcessorCompilation>
|
||||||
|
<AdditionalIncludeDirectories>../../../src/3rd-party;</AdditionalIncludeDirectories>
|
||||||
|
<MinimalRebuild>false</MinimalRebuild>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>false</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,307 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup>
|
||||||
|
<Filter Include="Collision">
|
||||||
|
<UniqueIdentifier>{100df8bf-ec25-491c-85d3-75e498f6afb2}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Collision\Shapes">
|
||||||
|
<UniqueIdentifier>{c1685b07-79ba-4fd0-bc68-167809eeac15}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Common">
|
||||||
|
<UniqueIdentifier>{890e188f-6f12-4236-904d-26a9251b2fbd}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Dynamics">
|
||||||
|
<UniqueIdentifier>{a32ce909-1b1c-4733-b5fc-6ec63219d303}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Rope">
|
||||||
|
<UniqueIdentifier>{c0c4ea0d-0f35-4eaf-b30e-bf63dc1cb485}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Dynamics\Joints">
|
||||||
|
<UniqueIdentifier>{beca7c8f-5051-4b18-a714-bcd98a3ab006}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
<Filter Include="Dynamics\Contacts">
|
||||||
|
<UniqueIdentifier>{0b4fe2be-741e-4723-83ca-ed057820ee89}</UniqueIdentifier>
|
||||||
|
</Filter>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Rope\b2Rope.h">
|
||||||
|
<Filter>Rope</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Body.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2ContactManager.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Fixture.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Island.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2TimeStep.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2World.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2WorldCallbacks.h">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2DistanceJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2FrictionJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2GearJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2Joint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MotorJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MouseJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PrismaticJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PulleyJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RevoluteJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RopeJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WeldJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WheelJoint.h">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndCircleContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndPolygonContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2CircleContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2Contact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ContactSolver.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndCircleContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndPolygonContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonAndCircleContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonContact.h">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2BlockAllocator.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Draw.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2GrowableStack.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Math.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Settings.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2StackAllocator.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Common\b2Timer.h">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2BroadPhase.h">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2Collision.h">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2Distance.h">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2DynamicTree.h">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\b2TimeOfImpact.h">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2ChainShape.h">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2CircleShape.h">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2EdgeShape.h">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2PolygonShape.h">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2Shape.h">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClInclude>
|
||||||
|
<ClInclude Include="..\..\..\src\3rd-party\Box2D\Box2D.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Rope\b2Rope.cpp">
|
||||||
|
<Filter>Rope</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Body.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2ContactManager.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Fixture.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2Island.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2World.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\b2WorldCallbacks.cpp">
|
||||||
|
<Filter>Dynamics</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2DistanceJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2FrictionJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2GearJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2Joint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MotorJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2MouseJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PrismaticJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2PulleyJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RevoluteJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2RopeJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WeldJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Joints\b2WheelJoint.cpp">
|
||||||
|
<Filter>Dynamics\Joints</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndCircleContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ChainAndPolygonContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2CircleContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2Contact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2ContactSolver.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndCircleContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2EdgeAndPolygonContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonAndCircleContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Dynamics\Contacts\b2PolygonContact.cpp">
|
||||||
|
<Filter>Dynamics\Contacts</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2BlockAllocator.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Draw.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Math.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Settings.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2StackAllocator.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Common\b2Timer.cpp">
|
||||||
|
<Filter>Common</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2BroadPhase.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollideCircle.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollideEdge.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2CollidePolygon.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2Collision.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2Distance.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2DynamicTree.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\b2TimeOfImpact.cpp">
|
||||||
|
<Filter>Collision</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2ChainShape.cpp">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2CircleShape.cpp">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2EdgeShape.cpp">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
<ClCompile Include="..\..\..\src\3rd-party\Box2D\Collision\Shapes\b2PolygonShape.cpp">
|
||||||
|
<Filter>Collision\Shapes</Filter>
|
||||||
|
</ClCompile>
|
||||||
|
</ItemGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -10,6 +10,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "kiwano-network", "kiwano-ne
|
||||||
EndProject
|
EndProject
|
||||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "kiwano-imgui", "kiwano-imgui\kiwano-imgui.vcxproj", "{A7062ED8-8910-48A5-A3BC-C1612672571F}"
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "kiwano-imgui", "kiwano-imgui\kiwano-imgui.vcxproj", "{A7062ED8-8910-48A5-A3BC-C1612672571F}"
|
||||||
EndProject
|
EndProject
|
||||||
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "kiwano-physics", "kiwano-physics\kiwano-physics.vcxproj", "{DF599AFB-744F-41E5-AF0C-2146F90575C8}"
|
||||||
|
EndProject
|
||||||
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "3rd-party", "3rd-party", "{2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}"
|
Project("{2150E333-8FDC-42A3-9474-1A3956D46DE8}") = "3rd-party", "3rd-party", "{2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}"
|
||||||
EndProject
|
EndProject
|
||||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libStackWalker", "3rd-party\StackWalker\libStackWalker.vcxproj", "{3A3948DC-9865-46B3-B7B9-7E5572704ED2}"
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libStackWalker", "3rd-party\StackWalker\libStackWalker.vcxproj", "{3A3948DC-9865-46B3-B7B9-7E5572704ED2}"
|
||||||
|
|
@ -20,6 +22,8 @@ Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libimgui", "3rd-party\imgui
|
||||||
EndProject
|
EndProject
|
||||||
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libcurl", "3rd-party\curl\libcurl.vcxproj", "{A9ABACC7-75A1-46BA-8E48-4105346D9719}"
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libcurl", "3rd-party\curl\libcurl.vcxproj", "{A9ABACC7-75A1-46BA-8E48-4105346D9719}"
|
||||||
EndProject
|
EndProject
|
||||||
|
Project("{8BC9CEB8-8B4A-11D0-8D11-00A0C91BC942}") = "libBox2D", "3rd-party\Box2D\libBox2D.vcxproj", "{0CBA9295-F14D-4966-A7C4-1DD68158176C}"
|
||||||
|
EndProject
|
||||||
Global
|
Global
|
||||||
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
GlobalSection(SolutionConfigurationPlatforms) = preSolution
|
||||||
Debug|Win32 = Debug|Win32
|
Debug|Win32 = Debug|Win32
|
||||||
|
|
@ -59,6 +63,14 @@ Global
|
||||||
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Debug|Win32.Build.0 = Debug|Win32
|
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Debug|Win32.Build.0 = Debug|Win32
|
||||||
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Release|Win32.ActiveCfg = Release|Win32
|
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Release|Win32.ActiveCfg = Release|Win32
|
||||||
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Release|Win32.Build.0 = Release|Win32
|
{A9ABACC7-75A1-46BA-8E48-4105346D9719}.Release|Win32.Build.0 = Release|Win32
|
||||||
|
{0CBA9295-F14D-4966-A7C4-1DD68158176C}.Debug|Win32.ActiveCfg = Debug|Win32
|
||||||
|
{0CBA9295-F14D-4966-A7C4-1DD68158176C}.Debug|Win32.Build.0 = Debug|Win32
|
||||||
|
{0CBA9295-F14D-4966-A7C4-1DD68158176C}.Release|Win32.ActiveCfg = Release|Win32
|
||||||
|
{0CBA9295-F14D-4966-A7C4-1DD68158176C}.Release|Win32.Build.0 = Release|Win32
|
||||||
|
{DF599AFB-744F-41E5-AF0C-2146F90575C8}.Debug|Win32.ActiveCfg = Debug|Win32
|
||||||
|
{DF599AFB-744F-41E5-AF0C-2146F90575C8}.Debug|Win32.Build.0 = Debug|Win32
|
||||||
|
{DF599AFB-744F-41E5-AF0C-2146F90575C8}.Release|Win32.ActiveCfg = Release|Win32
|
||||||
|
{DF599AFB-744F-41E5-AF0C-2146F90575C8}.Release|Win32.Build.0 = Release|Win32
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
GlobalSection(SolutionProperties) = preSolution
|
GlobalSection(SolutionProperties) = preSolution
|
||||||
HideSolutionNode = FALSE
|
HideSolutionNode = FALSE
|
||||||
|
|
@ -68,6 +80,7 @@ Global
|
||||||
{AB47E875-85E5-4105-A71E-88930EAAB910} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
{AB47E875-85E5-4105-A71E-88930EAAB910} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
||||||
{7FA1E56D-62AC-47D1-97D1-40B302724198} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
{7FA1E56D-62AC-47D1-97D1-40B302724198} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
||||||
{A9ABACC7-75A1-46BA-8E48-4105346D9719} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
{A9ABACC7-75A1-46BA-8E48-4105346D9719} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
||||||
|
{0CBA9295-F14D-4966-A7C4-1DD68158176C} = {2D8919F2-8922-4B3F-8F68-D4127C6BCBB7}
|
||||||
EndGlobalSection
|
EndGlobalSection
|
||||||
GlobalSection(ExtensibilityGlobals) = postSolution
|
GlobalSection(ExtensibilityGlobals) = postSolution
|
||||||
SolutionGuid = {FAC2BE2F-19AF-477A-8DC6-4645E66868A4}
|
SolutionGuid = {FAC2BE2F-19AF-477A-8DC6-4645E66868A4}
|
||||||
|
|
|
||||||
|
|
@ -18,11 +18,6 @@
|
||||||
<Platform>Win32</Platform>
|
<Platform>Win32</Platform>
|
||||||
</ProjectConfiguration>
|
</ProjectConfiguration>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
|
||||||
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
|
||||||
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
|
||||||
</ProjectReference>
|
|
||||||
</ItemGroup>
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClCompile Include="..\..\src\kiwano-audio\audio-modules.cpp" />
|
<ClCompile Include="..\..\src\kiwano-audio\audio-modules.cpp" />
|
||||||
<ClCompile Include="..\..\src\kiwano-audio\AudioEngine.cpp" />
|
<ClCompile Include="..\..\src\kiwano-audio\AudioEngine.cpp" />
|
||||||
|
|
@ -105,6 +100,11 @@
|
||||||
<OptimizeReferences>true</OptimizeReferences>
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
</Link>
|
</Link>
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
||||||
|
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
</ItemGroup>
|
||||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
<ImportGroup Label="ExtensionTargets">
|
<ImportGroup Label="ExtensionTargets">
|
||||||
</ImportGroup>
|
</ImportGroup>
|
||||||
|
|
|
||||||
|
|
@ -18,14 +18,6 @@
|
||||||
<Platform>Win32</Platform>
|
<Platform>Win32</Platform>
|
||||||
</ProjectConfiguration>
|
</ProjectConfiguration>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
|
||||||
<ProjectReference Include="..\3rd-party\imgui\libimgui.vcxproj">
|
|
||||||
<Project>{7fa1e56d-62ac-47d1-97d1-40b302724198}</Project>
|
|
||||||
</ProjectReference>
|
|
||||||
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
|
||||||
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
|
||||||
</ProjectReference>
|
|
||||||
</ItemGroup>
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClCompile Include="..\..\src\kiwano-imgui\ImGuiLayer.cpp" />
|
<ClCompile Include="..\..\src\kiwano-imgui\ImGuiLayer.cpp" />
|
||||||
<ClCompile Include="..\..\src\kiwano-imgui\ImGuiModule.cpp" />
|
<ClCompile Include="..\..\src\kiwano-imgui\ImGuiModule.cpp" />
|
||||||
|
|
@ -107,6 +99,14 @@
|
||||||
<OptimizeReferences>true</OptimizeReferences>
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
</Link>
|
</Link>
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ProjectReference Include="..\3rd-party\imgui\libimgui.vcxproj">
|
||||||
|
<Project>{7fa1e56d-62ac-47d1-97d1-40b302724198}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
||||||
|
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
</ItemGroup>
|
||||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
<ImportGroup Label="ExtensionTargets">
|
<ImportGroup Label="ExtensionTargets">
|
||||||
</ImportGroup>
|
</ImportGroup>
|
||||||
|
|
|
||||||
|
|
@ -16,14 +16,6 @@
|
||||||
<Platform>Win32</Platform>
|
<Platform>Win32</Platform>
|
||||||
</ProjectConfiguration>
|
</ProjectConfiguration>
|
||||||
</ItemGroup>
|
</ItemGroup>
|
||||||
<ItemGroup>
|
|
||||||
<ProjectReference Include="..\3rd-party\curl\libcurl.vcxproj">
|
|
||||||
<Project>{a9abacc7-75a1-46ba-8e48-4105346d9719}</Project>
|
|
||||||
</ProjectReference>
|
|
||||||
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
|
||||||
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
|
||||||
</ProjectReference>
|
|
||||||
</ItemGroup>
|
|
||||||
<ItemGroup>
|
<ItemGroup>
|
||||||
<ClCompile Include="..\..\src\kiwano-network\HttpClient.cpp" />
|
<ClCompile Include="..\..\src\kiwano-network\HttpClient.cpp" />
|
||||||
<ClCompile Include="..\..\src\kiwano-network\HttpRequest.cpp" />
|
<ClCompile Include="..\..\src\kiwano-network\HttpRequest.cpp" />
|
||||||
|
|
@ -103,6 +95,14 @@
|
||||||
<OptimizeReferences>true</OptimizeReferences>
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
</Link>
|
</Link>
|
||||||
</ItemDefinitionGroup>
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ProjectReference Include="..\3rd-party\curl\libcurl.vcxproj">
|
||||||
|
<Project>{a9abacc7-75a1-46ba-8e48-4105346d9719}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
||||||
|
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
</ItemGroup>
|
||||||
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
<ImportGroup Label="ExtensionTargets">
|
<ImportGroup Label="ExtensionTargets">
|
||||||
</ImportGroup>
|
</ImportGroup>
|
||||||
|
|
|
||||||
|
|
@ -0,0 +1,119 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project DefaultTargets="Build" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup Label="ProjectConfigurations">
|
||||||
|
<ProjectConfiguration Include="Debug|Win32">
|
||||||
|
<Configuration>Debug</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
<ProjectConfiguration Include="Release|Win32">
|
||||||
|
<Configuration>Release</Configuration>
|
||||||
|
<Platform>Win32</Platform>
|
||||||
|
</ProjectConfiguration>
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Body.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Contact.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\ContactListener.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Fixture.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\helper.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Joint.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\kiwano-physics.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Shape.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\World.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Body.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Contact.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\ContactListener.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Fixture.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Joint.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Shape.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\World.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
<PropertyGroup Label="Globals">
|
||||||
|
<ProjectGuid>{DF599AFB-744F-41E5-AF0C-2146F90575C8}</ProjectGuid>
|
||||||
|
<RootNamespace>kiwano-physics</RootNamespace>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.Default.props" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>true</UseDebugLibraries>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>$(DefaultPlatformToolset)</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'" Label="Configuration">
|
||||||
|
<ConfigurationType>StaticLibrary</ConfigurationType>
|
||||||
|
<UseDebugLibraries>false</UseDebugLibraries>
|
||||||
|
<WholeProgramOptimization>false</WholeProgramOptimization>
|
||||||
|
<CharacterSet>Unicode</CharacterSet>
|
||||||
|
<PlatformToolset>$(DefaultPlatformToolset)</PlatformToolset>
|
||||||
|
</PropertyGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.props" />
|
||||||
|
<ImportGroup Label="ExtensionSettings">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="Shared">
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<ImportGroup Label="PropertySheets" Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<Import Project="$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props" Condition="exists('$(UserRootDir)\Microsoft.Cpp.$(Platform).user.props')" Label="LocalAppDataPlatform" />
|
||||||
|
</ImportGroup>
|
||||||
|
<PropertyGroup Label="UserMacros" />
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<OutDir>$(SolutionDir)\output\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>$(SolutionDir)\build\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<PropertyGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<OutDir>$(SolutionDir)\output\$(PlatformToolset)\$(Platform)\$(Configuration)\</OutDir>
|
||||||
|
<IntDir>$(SolutionDir)\build\$(PlatformToolset)\$(Platform)\$(Configuration)\$(ProjectName)\</IntDir>
|
||||||
|
<LinkIncremental>true</LinkIncremental>
|
||||||
|
</PropertyGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Debug|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>Disabled</Optimization>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
<DebugInformationFormat>EditAndContinue</DebugInformationFormat>
|
||||||
|
<MultiProcessorCompilation>true</MultiProcessorCompilation>
|
||||||
|
<AdditionalIncludeDirectories>../../src;</AdditionalIncludeDirectories>
|
||||||
|
<MinimalRebuild>false</MinimalRebuild>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>true</GenerateDebugInformation>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemDefinitionGroup Condition="'$(Configuration)|$(Platform)'=='Release|Win32'">
|
||||||
|
<ClCompile>
|
||||||
|
<WarningLevel>Level3</WarningLevel>
|
||||||
|
<Optimization>MaxSpeed</Optimization>
|
||||||
|
<FunctionLevelLinking>true</FunctionLevelLinking>
|
||||||
|
<IntrinsicFunctions>true</IntrinsicFunctions>
|
||||||
|
<BufferSecurityCheck>false</BufferSecurityCheck>
|
||||||
|
<TreatWarningAsError>true</TreatWarningAsError>
|
||||||
|
<DebugInformationFormat>ProgramDatabase</DebugInformationFormat>
|
||||||
|
<MultiProcessorCompilation>true</MultiProcessorCompilation>
|
||||||
|
<AdditionalIncludeDirectories>../../src;</AdditionalIncludeDirectories>
|
||||||
|
<MinimalRebuild>false</MinimalRebuild>
|
||||||
|
</ClCompile>
|
||||||
|
<Link>
|
||||||
|
<SubSystem>Windows</SubSystem>
|
||||||
|
<GenerateDebugInformation>false</GenerateDebugInformation>
|
||||||
|
<EnableCOMDATFolding>true</EnableCOMDATFolding>
|
||||||
|
<OptimizeReferences>true</OptimizeReferences>
|
||||||
|
</Link>
|
||||||
|
</ItemDefinitionGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ProjectReference Include="..\3rd-party\Box2D\libBox2D.vcxproj">
|
||||||
|
<Project>{0cba9295-f14d-4966-a7c4-1dd68158176c}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
<ProjectReference Include="..\kiwano\kiwano.vcxproj">
|
||||||
|
<Project>{ff7f943d-a89c-4e6c-97cf-84f7d8ff8edf}</Project>
|
||||||
|
</ProjectReference>
|
||||||
|
</ItemGroup>
|
||||||
|
<Import Project="$(VCTargetsPath)\Microsoft.Cpp.targets" />
|
||||||
|
<ImportGroup Label="ExtensionTargets">
|
||||||
|
</ImportGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,23 @@
|
||||||
|
<?xml version="1.0" encoding="utf-8"?>
|
||||||
|
<Project ToolsVersion="4.0" xmlns="http://schemas.microsoft.com/developer/msbuild/2003">
|
||||||
|
<ItemGroup>
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\kiwano-physics.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Body.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\World.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Joint.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Shape.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\helper.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Fixture.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\Contact.h" />
|
||||||
|
<ClInclude Include="..\..\src\kiwano-physics\ContactListener.h" />
|
||||||
|
</ItemGroup>
|
||||||
|
<ItemGroup>
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Body.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\World.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Joint.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Shape.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Fixture.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\Contact.cpp" />
|
||||||
|
<ClCompile Include="..\..\src\kiwano-physics\ContactListener.cpp" />
|
||||||
|
</ItemGroup>
|
||||||
|
</Project>
|
||||||
|
|
@ -0,0 +1,68 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef BOX2D_H
|
||||||
|
#define BOX2D_H
|
||||||
|
|
||||||
|
/**
|
||||||
|
\mainpage Box2D API Documentation
|
||||||
|
|
||||||
|
\section intro_sec Getting Started
|
||||||
|
|
||||||
|
For documentation please see http://box2d.org/documentation.html
|
||||||
|
|
||||||
|
For discussion please visit http://box2d.org/forum
|
||||||
|
*/
|
||||||
|
|
||||||
|
// These include files constitute the main Box2D API
|
||||||
|
|
||||||
|
#include "Common/b2Settings.h"
|
||||||
|
#include "Common/b2Draw.h"
|
||||||
|
#include "Common/b2Timer.h"
|
||||||
|
|
||||||
|
#include "Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include "Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Collision/Shapes/b2PolygonShape.h"
|
||||||
|
|
||||||
|
#include "Collision/b2BroadPhase.h"
|
||||||
|
#include "Collision/b2Distance.h"
|
||||||
|
#include "Collision/b2DynamicTree.h"
|
||||||
|
#include "Collision/b2TimeOfImpact.h"
|
||||||
|
|
||||||
|
#include "Dynamics/b2Body.h"
|
||||||
|
#include "Dynamics/b2Fixture.h"
|
||||||
|
#include "Dynamics/b2WorldCallbacks.h"
|
||||||
|
#include "Dynamics/b2TimeStep.h"
|
||||||
|
#include "Dynamics/b2World.h"
|
||||||
|
|
||||||
|
#include "Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
#include "Dynamics/Joints/b2DistanceJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2FrictionJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2GearJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2MotorJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2MouseJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2PrismaticJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2PulleyJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2RevoluteJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2RopeJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2WeldJoint.h"
|
||||||
|
#include "Dynamics/Joints/b2WheelJoint.h"
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,198 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include <new>
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
b2ChainShape::~b2ChainShape()
|
||||||
|
{
|
||||||
|
Clear();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::Clear()
|
||||||
|
{
|
||||||
|
b2Free(m_vertices);
|
||||||
|
m_vertices = nullptr;
|
||||||
|
m_count = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
|
||||||
|
{
|
||||||
|
b2Assert(m_vertices == nullptr && m_count == 0);
|
||||||
|
b2Assert(count >= 3);
|
||||||
|
if (count < 3)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int32 i = 1; i < count; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 v1 = vertices[i-1];
|
||||||
|
b2Vec2 v2 = vertices[i];
|
||||||
|
// If the code crashes here, it means your vertices are too close together.
|
||||||
|
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_count = count + 1;
|
||||||
|
m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
|
||||||
|
memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
|
||||||
|
m_vertices[count] = m_vertices[0];
|
||||||
|
m_prevVertex = m_vertices[m_count - 2];
|
||||||
|
m_nextVertex = m_vertices[1];
|
||||||
|
m_hasPrevVertex = true;
|
||||||
|
m_hasNextVertex = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
|
||||||
|
{
|
||||||
|
b2Assert(m_vertices == nullptr && m_count == 0);
|
||||||
|
b2Assert(count >= 2);
|
||||||
|
for (int32 i = 1; i < count; ++i)
|
||||||
|
{
|
||||||
|
// If the code crashes here, it means your vertices are too close together.
|
||||||
|
b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_count = count;
|
||||||
|
m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
|
||||||
|
memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
|
||||||
|
|
||||||
|
m_hasPrevVertex = false;
|
||||||
|
m_hasNextVertex = false;
|
||||||
|
|
||||||
|
m_prevVertex.SetZero();
|
||||||
|
m_nextVertex.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
|
||||||
|
{
|
||||||
|
m_prevVertex = prevVertex;
|
||||||
|
m_hasPrevVertex = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
|
||||||
|
{
|
||||||
|
m_nextVertex = nextVertex;
|
||||||
|
m_hasNextVertex = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2ChainShape));
|
||||||
|
b2ChainShape* clone = new (mem) b2ChainShape;
|
||||||
|
clone->CreateChain(m_vertices, m_count);
|
||||||
|
clone->m_prevVertex = m_prevVertex;
|
||||||
|
clone->m_nextVertex = m_nextVertex;
|
||||||
|
clone->m_hasPrevVertex = m_hasPrevVertex;
|
||||||
|
clone->m_hasNextVertex = m_hasNextVertex;
|
||||||
|
return clone;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2ChainShape::GetChildCount() const
|
||||||
|
{
|
||||||
|
// edge count = vertex count - 1
|
||||||
|
return m_count - 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= index && index < m_count - 1);
|
||||||
|
edge->m_type = b2Shape::e_edge;
|
||||||
|
edge->m_radius = m_radius;
|
||||||
|
|
||||||
|
edge->m_vertex1 = m_vertices[index + 0];
|
||||||
|
edge->m_vertex2 = m_vertices[index + 1];
|
||||||
|
|
||||||
|
if (index > 0)
|
||||||
|
{
|
||||||
|
edge->m_vertex0 = m_vertices[index - 1];
|
||||||
|
edge->m_hasVertex0 = true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
edge->m_vertex0 = m_prevVertex;
|
||||||
|
edge->m_hasVertex0 = m_hasPrevVertex;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (index < m_count - 2)
|
||||||
|
{
|
||||||
|
edge->m_vertex3 = m_vertices[index + 2];
|
||||||
|
edge->m_hasVertex3 = true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
edge->m_vertex3 = m_nextVertex;
|
||||||
|
edge->m_hasVertex3 = m_hasNextVertex;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(xf);
|
||||||
|
B2_NOT_USED(p);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
b2Assert(childIndex < m_count);
|
||||||
|
|
||||||
|
b2EdgeShape edgeShape;
|
||||||
|
|
||||||
|
int32 i1 = childIndex;
|
||||||
|
int32 i2 = childIndex + 1;
|
||||||
|
if (i2 == m_count)
|
||||||
|
{
|
||||||
|
i2 = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
edgeShape.m_vertex1 = m_vertices[i1];
|
||||||
|
edgeShape.m_vertex2 = m_vertices[i2];
|
||||||
|
|
||||||
|
return edgeShape.RayCast(output, input, xf, 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
b2Assert(childIndex < m_count);
|
||||||
|
|
||||||
|
int32 i1 = childIndex;
|
||||||
|
int32 i2 = childIndex + 1;
|
||||||
|
if (i2 == m_count)
|
||||||
|
{
|
||||||
|
i2 = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
|
||||||
|
b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
|
||||||
|
|
||||||
|
aabb->lowerBound = b2Min(v1, v2);
|
||||||
|
aabb->upperBound = b2Max(v1, v2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(density);
|
||||||
|
|
||||||
|
massData->mass = 0.0f;
|
||||||
|
massData->center.SetZero();
|
||||||
|
massData->I = 0.0f;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,105 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CHAIN_SHAPE_H
|
||||||
|
#define B2_CHAIN_SHAPE_H
|
||||||
|
|
||||||
|
#include "b2Shape.h"
|
||||||
|
|
||||||
|
class b2EdgeShape;
|
||||||
|
|
||||||
|
/// A chain shape is a free form sequence of line segments.
|
||||||
|
/// The chain has two-sided collision, so you can use inside and outside collision.
|
||||||
|
/// Therefore, you may use any winding order.
|
||||||
|
/// Since there may be many vertices, they are allocated using b2Alloc.
|
||||||
|
/// Connectivity information is used to create smooth collisions.
|
||||||
|
/// WARNING: The chain will not collide properly if there are self-intersections.
|
||||||
|
class b2ChainShape : public b2Shape
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2ChainShape();
|
||||||
|
|
||||||
|
/// The destructor frees the vertices using b2Free.
|
||||||
|
~b2ChainShape();
|
||||||
|
|
||||||
|
/// Clear all data.
|
||||||
|
void Clear();
|
||||||
|
|
||||||
|
/// Create a loop. This automatically adjusts connectivity.
|
||||||
|
/// @param vertices an array of vertices, these are copied
|
||||||
|
/// @param count the vertex count
|
||||||
|
void CreateLoop(const b2Vec2* vertices, int32 count);
|
||||||
|
|
||||||
|
/// Create a chain with isolated end vertices.
|
||||||
|
/// @param vertices an array of vertices, these are copied
|
||||||
|
/// @param count the vertex count
|
||||||
|
void CreateChain(const b2Vec2* vertices, int32 count);
|
||||||
|
|
||||||
|
/// Establish connectivity to a vertex that precedes the first vertex.
|
||||||
|
/// Don't call this for loops.
|
||||||
|
void SetPrevVertex(const b2Vec2& prevVertex);
|
||||||
|
|
||||||
|
/// Establish connectivity to a vertex that follows the last vertex.
|
||||||
|
/// Don't call this for loops.
|
||||||
|
void SetNextVertex(const b2Vec2& nextVertex);
|
||||||
|
|
||||||
|
/// Implement b2Shape. Vertices are cloned using b2Alloc.
|
||||||
|
b2Shape* Clone(b2BlockAllocator* allocator) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::GetChildCount
|
||||||
|
int32 GetChildCount() const override;
|
||||||
|
|
||||||
|
/// Get a child edge.
|
||||||
|
void GetChildEdge(b2EdgeShape* edge, int32 index) const;
|
||||||
|
|
||||||
|
/// This always return false.
|
||||||
|
/// @see b2Shape::TestPoint
|
||||||
|
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeAABB
|
||||||
|
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// Chains have zero mass.
|
||||||
|
/// @see b2Shape::ComputeMass
|
||||||
|
void ComputeMass(b2MassData* massData, float32 density) const override;
|
||||||
|
|
||||||
|
/// The vertices. Owned by this class.
|
||||||
|
b2Vec2* m_vertices;
|
||||||
|
|
||||||
|
/// The vertex count.
|
||||||
|
int32 m_count;
|
||||||
|
|
||||||
|
b2Vec2 m_prevVertex, m_nextVertex;
|
||||||
|
bool m_hasPrevVertex, m_hasNextVertex;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2ChainShape::b2ChainShape()
|
||||||
|
{
|
||||||
|
m_type = e_chain;
|
||||||
|
m_radius = b2_polygonRadius;
|
||||||
|
m_vertices = nullptr;
|
||||||
|
m_count = 0;
|
||||||
|
m_hasPrevVertex = false;
|
||||||
|
m_hasNextVertex = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,99 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2CircleShape));
|
||||||
|
b2CircleShape* clone = new (mem) b2CircleShape;
|
||||||
|
*clone = *this;
|
||||||
|
return clone;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2CircleShape::GetChildCount() const
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
|
||||||
|
{
|
||||||
|
b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
|
||||||
|
b2Vec2 d = p - center;
|
||||||
|
return b2Dot(d, d) <= m_radius * m_radius;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
|
||||||
|
// From Section 3.1.2
|
||||||
|
// x = s + a * r
|
||||||
|
// norm(x) = radius
|
||||||
|
bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
|
||||||
|
b2Vec2 s = input.p1 - position;
|
||||||
|
float32 b = b2Dot(s, s) - m_radius * m_radius;
|
||||||
|
|
||||||
|
// Solve quadratic equation.
|
||||||
|
b2Vec2 r = input.p2 - input.p1;
|
||||||
|
float32 c = b2Dot(s, r);
|
||||||
|
float32 rr = b2Dot(r, r);
|
||||||
|
float32 sigma = c * c - rr * b;
|
||||||
|
|
||||||
|
// Check for negative discriminant and short segment.
|
||||||
|
if (sigma < 0.0f || rr < b2_epsilon)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Find the point of intersection of the line with the circle.
|
||||||
|
float32 a = -(c + b2Sqrt(sigma));
|
||||||
|
|
||||||
|
// Is the intersection point on the segment?
|
||||||
|
if (0.0f <= a && a <= input.maxFraction * rr)
|
||||||
|
{
|
||||||
|
a /= rr;
|
||||||
|
output->fraction = a;
|
||||||
|
output->normal = s + a * r;
|
||||||
|
output->normal.Normalize();
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
|
||||||
|
aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
|
||||||
|
aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
|
||||||
|
{
|
||||||
|
massData->mass = density * b2_pi * m_radius * m_radius;
|
||||||
|
massData->center = m_p;
|
||||||
|
|
||||||
|
// inertia about the local origin
|
||||||
|
massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,60 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CIRCLE_SHAPE_H
|
||||||
|
#define B2_CIRCLE_SHAPE_H
|
||||||
|
|
||||||
|
#include "b2Shape.h"
|
||||||
|
|
||||||
|
/// A circle shape.
|
||||||
|
class b2CircleShape : public b2Shape
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2CircleShape();
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
b2Shape* Clone(b2BlockAllocator* allocator) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::GetChildCount
|
||||||
|
int32 GetChildCount() const override;
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeAABB
|
||||||
|
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeMass
|
||||||
|
void ComputeMass(b2MassData* massData, float32 density) const override;
|
||||||
|
|
||||||
|
/// Position
|
||||||
|
b2Vec2 m_p;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2CircleShape::b2CircleShape()
|
||||||
|
{
|
||||||
|
m_type = e_circle;
|
||||||
|
m_radius = 0.0f;
|
||||||
|
m_p.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,138 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
|
||||||
|
{
|
||||||
|
m_vertex1 = v1;
|
||||||
|
m_vertex2 = v2;
|
||||||
|
m_hasVertex0 = false;
|
||||||
|
m_hasVertex3 = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2EdgeShape));
|
||||||
|
b2EdgeShape* clone = new (mem) b2EdgeShape;
|
||||||
|
*clone = *this;
|
||||||
|
return clone;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2EdgeShape::GetChildCount() const
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(xf);
|
||||||
|
B2_NOT_USED(p);
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// p = p1 + t * d
|
||||||
|
// v = v1 + s * e
|
||||||
|
// p1 + t * d = v1 + s * e
|
||||||
|
// s * e - t * d = p1 - v1
|
||||||
|
bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
// Put the ray into the edge's frame of reference.
|
||||||
|
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
|
||||||
|
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
|
||||||
|
b2Vec2 d = p2 - p1;
|
||||||
|
|
||||||
|
b2Vec2 v1 = m_vertex1;
|
||||||
|
b2Vec2 v2 = m_vertex2;
|
||||||
|
b2Vec2 e = v2 - v1;
|
||||||
|
b2Vec2 normal(e.y, -e.x);
|
||||||
|
normal.Normalize();
|
||||||
|
|
||||||
|
// q = p1 + t * d
|
||||||
|
// dot(normal, q - v1) = 0
|
||||||
|
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
|
||||||
|
float32 numerator = b2Dot(normal, v1 - p1);
|
||||||
|
float32 denominator = b2Dot(normal, d);
|
||||||
|
|
||||||
|
if (denominator == 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 t = numerator / denominator;
|
||||||
|
if (t < 0.0f || input.maxFraction < t)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 q = p1 + t * d;
|
||||||
|
|
||||||
|
// q = v1 + s * r
|
||||||
|
// s = dot(q - v1, r) / dot(r, r)
|
||||||
|
b2Vec2 r = v2 - v1;
|
||||||
|
float32 rr = b2Dot(r, r);
|
||||||
|
if (rr == 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 s = b2Dot(q - v1, r) / rr;
|
||||||
|
if (s < 0.0f || 1.0f < s)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
output->fraction = t;
|
||||||
|
if (numerator > 0.0f)
|
||||||
|
{
|
||||||
|
output->normal = -b2Mul(xf.q, normal);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
output->normal = b2Mul(xf.q, normal);
|
||||||
|
}
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
b2Vec2 v1 = b2Mul(xf, m_vertex1);
|
||||||
|
b2Vec2 v2 = b2Mul(xf, m_vertex2);
|
||||||
|
|
||||||
|
b2Vec2 lower = b2Min(v1, v2);
|
||||||
|
b2Vec2 upper = b2Max(v1, v2);
|
||||||
|
|
||||||
|
b2Vec2 r(m_radius, m_radius);
|
||||||
|
aabb->lowerBound = lower - r;
|
||||||
|
aabb->upperBound = upper + r;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(density);
|
||||||
|
|
||||||
|
massData->mass = 0.0f;
|
||||||
|
massData->center = 0.5f * (m_vertex1 + m_vertex2);
|
||||||
|
massData->I = 0.0f;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,74 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_EDGE_SHAPE_H
|
||||||
|
#define B2_EDGE_SHAPE_H
|
||||||
|
|
||||||
|
#include "b2Shape.h"
|
||||||
|
|
||||||
|
/// A line segment (edge) shape. These can be connected in chains or loops
|
||||||
|
/// to other edge shapes. The connectivity information is used to ensure
|
||||||
|
/// correct contact normals.
|
||||||
|
class b2EdgeShape : public b2Shape
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2EdgeShape();
|
||||||
|
|
||||||
|
/// Set this as an isolated edge.
|
||||||
|
void Set(const b2Vec2& v1, const b2Vec2& v2);
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
b2Shape* Clone(b2BlockAllocator* allocator) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::GetChildCount
|
||||||
|
int32 GetChildCount() const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::TestPoint
|
||||||
|
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeAABB
|
||||||
|
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeMass
|
||||||
|
void ComputeMass(b2MassData* massData, float32 density) const override;
|
||||||
|
|
||||||
|
/// These are the edge vertices
|
||||||
|
b2Vec2 m_vertex1, m_vertex2;
|
||||||
|
|
||||||
|
/// Optional adjacent vertices. These are used for smooth collision.
|
||||||
|
b2Vec2 m_vertex0, m_vertex3;
|
||||||
|
bool m_hasVertex0, m_hasVertex3;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2EdgeShape::b2EdgeShape()
|
||||||
|
{
|
||||||
|
m_type = e_edge;
|
||||||
|
m_radius = b2_polygonRadius;
|
||||||
|
m_vertex0.x = 0.0f;
|
||||||
|
m_vertex0.y = 0.0f;
|
||||||
|
m_vertex3.x = 0.0f;
|
||||||
|
m_vertex3.y = 0.0f;
|
||||||
|
m_hasVertex0 = false;
|
||||||
|
m_hasVertex3 = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,468 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2PolygonShape));
|
||||||
|
b2PolygonShape* clone = new (mem) b2PolygonShape;
|
||||||
|
*clone = *this;
|
||||||
|
return clone;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
|
||||||
|
{
|
||||||
|
m_count = 4;
|
||||||
|
m_vertices[0].Set(-hx, -hy);
|
||||||
|
m_vertices[1].Set( hx, -hy);
|
||||||
|
m_vertices[2].Set( hx, hy);
|
||||||
|
m_vertices[3].Set(-hx, hy);
|
||||||
|
m_normals[0].Set(0.0f, -1.0f);
|
||||||
|
m_normals[1].Set(1.0f, 0.0f);
|
||||||
|
m_normals[2].Set(0.0f, 1.0f);
|
||||||
|
m_normals[3].Set(-1.0f, 0.0f);
|
||||||
|
m_centroid.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
|
||||||
|
{
|
||||||
|
m_count = 4;
|
||||||
|
m_vertices[0].Set(-hx, -hy);
|
||||||
|
m_vertices[1].Set( hx, -hy);
|
||||||
|
m_vertices[2].Set( hx, hy);
|
||||||
|
m_vertices[3].Set(-hx, hy);
|
||||||
|
m_normals[0].Set(0.0f, -1.0f);
|
||||||
|
m_normals[1].Set(1.0f, 0.0f);
|
||||||
|
m_normals[2].Set(0.0f, 1.0f);
|
||||||
|
m_normals[3].Set(-1.0f, 0.0f);
|
||||||
|
m_centroid = center;
|
||||||
|
|
||||||
|
b2Transform xf;
|
||||||
|
xf.p = center;
|
||||||
|
xf.q.Set(angle);
|
||||||
|
|
||||||
|
// Transform vertices and normals.
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
m_vertices[i] = b2Mul(xf, m_vertices[i]);
|
||||||
|
m_normals[i] = b2Mul(xf.q, m_normals[i]);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2PolygonShape::GetChildCount() const
|
||||||
|
{
|
||||||
|
return 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
|
||||||
|
{
|
||||||
|
b2Assert(count >= 3);
|
||||||
|
|
||||||
|
b2Vec2 c; c.Set(0.0f, 0.0f);
|
||||||
|
float32 area = 0.0f;
|
||||||
|
|
||||||
|
// pRef is the reference point for forming triangles.
|
||||||
|
// It's location doesn't change the result (except for rounding error).
|
||||||
|
b2Vec2 pRef(0.0f, 0.0f);
|
||||||
|
#if 0
|
||||||
|
// This code would put the reference point inside the polygon.
|
||||||
|
for (int32 i = 0; i < count; ++i)
|
||||||
|
{
|
||||||
|
pRef += vs[i];
|
||||||
|
}
|
||||||
|
pRef *= 1.0f / count;
|
||||||
|
#endif
|
||||||
|
|
||||||
|
const float32 inv3 = 1.0f / 3.0f;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < count; ++i)
|
||||||
|
{
|
||||||
|
// Triangle vertices.
|
||||||
|
b2Vec2 p1 = pRef;
|
||||||
|
b2Vec2 p2 = vs[i];
|
||||||
|
b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
|
||||||
|
|
||||||
|
b2Vec2 e1 = p2 - p1;
|
||||||
|
b2Vec2 e2 = p3 - p1;
|
||||||
|
|
||||||
|
float32 D = b2Cross(e1, e2);
|
||||||
|
|
||||||
|
float32 triangleArea = 0.5f * D;
|
||||||
|
area += triangleArea;
|
||||||
|
|
||||||
|
// Area weighted centroid
|
||||||
|
c += triangleArea * inv3 * (p1 + p2 + p3);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Centroid
|
||||||
|
b2Assert(area > b2_epsilon);
|
||||||
|
c *= 1.0f / area;
|
||||||
|
return c;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
|
||||||
|
{
|
||||||
|
b2Assert(3 <= count && count <= b2_maxPolygonVertices);
|
||||||
|
if (count < 3)
|
||||||
|
{
|
||||||
|
SetAsBox(1.0f, 1.0f);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 n = b2Min(count, b2_maxPolygonVertices);
|
||||||
|
|
||||||
|
// Perform welding and copy vertices into local buffer.
|
||||||
|
b2Vec2 ps[b2_maxPolygonVertices];
|
||||||
|
int32 tempCount = 0;
|
||||||
|
for (int32 i = 0; i < n; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 v = vertices[i];
|
||||||
|
|
||||||
|
bool unique = true;
|
||||||
|
for (int32 j = 0; j < tempCount; ++j)
|
||||||
|
{
|
||||||
|
if (b2DistanceSquared(v, ps[j]) < ((0.5f * b2_linearSlop) * (0.5f * b2_linearSlop)))
|
||||||
|
{
|
||||||
|
unique = false;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (unique)
|
||||||
|
{
|
||||||
|
ps[tempCount++] = v;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
n = tempCount;
|
||||||
|
if (n < 3)
|
||||||
|
{
|
||||||
|
// Polygon is degenerate.
|
||||||
|
b2Assert(false);
|
||||||
|
SetAsBox(1.0f, 1.0f);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create the convex hull using the Gift wrapping algorithm
|
||||||
|
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
|
||||||
|
|
||||||
|
// Find the right most point on the hull
|
||||||
|
int32 i0 = 0;
|
||||||
|
float32 x0 = ps[0].x;
|
||||||
|
for (int32 i = 1; i < n; ++i)
|
||||||
|
{
|
||||||
|
float32 x = ps[i].x;
|
||||||
|
if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
|
||||||
|
{
|
||||||
|
i0 = i;
|
||||||
|
x0 = x;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 hull[b2_maxPolygonVertices];
|
||||||
|
int32 m = 0;
|
||||||
|
int32 ih = i0;
|
||||||
|
|
||||||
|
for (;;)
|
||||||
|
{
|
||||||
|
b2Assert(m < b2_maxPolygonVertices);
|
||||||
|
hull[m] = ih;
|
||||||
|
|
||||||
|
int32 ie = 0;
|
||||||
|
for (int32 j = 1; j < n; ++j)
|
||||||
|
{
|
||||||
|
if (ie == ih)
|
||||||
|
{
|
||||||
|
ie = j;
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 r = ps[ie] - ps[hull[m]];
|
||||||
|
b2Vec2 v = ps[j] - ps[hull[m]];
|
||||||
|
float32 c = b2Cross(r, v);
|
||||||
|
if (c < 0.0f)
|
||||||
|
{
|
||||||
|
ie = j;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Collinearity check
|
||||||
|
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
|
||||||
|
{
|
||||||
|
ie = j;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
++m;
|
||||||
|
ih = ie;
|
||||||
|
|
||||||
|
if (ie == i0)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m < 3)
|
||||||
|
{
|
||||||
|
// Polygon is degenerate.
|
||||||
|
b2Assert(false);
|
||||||
|
SetAsBox(1.0f, 1.0f);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_count = m;
|
||||||
|
|
||||||
|
// Copy vertices.
|
||||||
|
for (int32 i = 0; i < m; ++i)
|
||||||
|
{
|
||||||
|
m_vertices[i] = ps[hull[i]];
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute normals. Ensure the edges have non-zero length.
|
||||||
|
for (int32 i = 0; i < m; ++i)
|
||||||
|
{
|
||||||
|
int32 i1 = i;
|
||||||
|
int32 i2 = i + 1 < m ? i + 1 : 0;
|
||||||
|
b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
|
||||||
|
b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
|
||||||
|
m_normals[i] = b2Cross(edge, 1.0f);
|
||||||
|
m_normals[i].Normalize();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the polygon centroid.
|
||||||
|
m_centroid = ComputeCentroid(m_vertices, m);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
|
||||||
|
{
|
||||||
|
b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
|
||||||
|
if (dot > 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
// Put the ray into the polygon's frame of reference.
|
||||||
|
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
|
||||||
|
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
|
||||||
|
b2Vec2 d = p2 - p1;
|
||||||
|
|
||||||
|
float32 lower = 0.0f, upper = input.maxFraction;
|
||||||
|
|
||||||
|
int32 index = -1;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
// p = p1 + a * d
|
||||||
|
// dot(normal, p - v) = 0
|
||||||
|
// dot(normal, p1 - v) + a * dot(normal, d) = 0
|
||||||
|
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
|
||||||
|
float32 denominator = b2Dot(m_normals[i], d);
|
||||||
|
|
||||||
|
if (denominator == 0.0f)
|
||||||
|
{
|
||||||
|
if (numerator < 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Note: we want this predicate without division:
|
||||||
|
// lower < numerator / denominator, where denominator < 0
|
||||||
|
// Since denominator < 0, we have to flip the inequality:
|
||||||
|
// lower < numerator / denominator <==> denominator * lower > numerator.
|
||||||
|
if (denominator < 0.0f && numerator < lower * denominator)
|
||||||
|
{
|
||||||
|
// Increase lower.
|
||||||
|
// The segment enters this half-space.
|
||||||
|
lower = numerator / denominator;
|
||||||
|
index = i;
|
||||||
|
}
|
||||||
|
else if (denominator > 0.0f && numerator < upper * denominator)
|
||||||
|
{
|
||||||
|
// Decrease upper.
|
||||||
|
// The segment exits this half-space.
|
||||||
|
upper = numerator / denominator;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// The use of epsilon here causes the assert on lower to trip
|
||||||
|
// in some cases. Apparently the use of epsilon was to make edge
|
||||||
|
// shapes work, but now those are handled separately.
|
||||||
|
//if (upper < lower - b2_epsilon)
|
||||||
|
if (upper < lower)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(0.0f <= lower && lower <= input.maxFraction);
|
||||||
|
|
||||||
|
if (index >= 0)
|
||||||
|
{
|
||||||
|
output->fraction = lower;
|
||||||
|
output->normal = b2Mul(xf.q, m_normals[index]);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(childIndex);
|
||||||
|
|
||||||
|
b2Vec2 lower = b2Mul(xf, m_vertices[0]);
|
||||||
|
b2Vec2 upper = lower;
|
||||||
|
|
||||||
|
for (int32 i = 1; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 v = b2Mul(xf, m_vertices[i]);
|
||||||
|
lower = b2Min(lower, v);
|
||||||
|
upper = b2Max(upper, v);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 r(m_radius, m_radius);
|
||||||
|
aabb->lowerBound = lower - r;
|
||||||
|
aabb->upperBound = upper + r;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
|
||||||
|
{
|
||||||
|
// Polygon mass, centroid, and inertia.
|
||||||
|
// Let rho be the polygon density in mass per unit area.
|
||||||
|
// Then:
|
||||||
|
// mass = rho * int(dA)
|
||||||
|
// centroid.x = (1/mass) * rho * int(x * dA)
|
||||||
|
// centroid.y = (1/mass) * rho * int(y * dA)
|
||||||
|
// I = rho * int((x*x + y*y) * dA)
|
||||||
|
//
|
||||||
|
// We can compute these integrals by summing all the integrals
|
||||||
|
// for each triangle of the polygon. To evaluate the integral
|
||||||
|
// for a single triangle, we make a change of variables to
|
||||||
|
// the (u,v) coordinates of the triangle:
|
||||||
|
// x = x0 + e1x * u + e2x * v
|
||||||
|
// y = y0 + e1y * u + e2y * v
|
||||||
|
// where 0 <= u && 0 <= v && u + v <= 1.
|
||||||
|
//
|
||||||
|
// We integrate u from [0,1-v] and then v from [0,1].
|
||||||
|
// We also need to use the Jacobian of the transformation:
|
||||||
|
// D = cross(e1, e2)
|
||||||
|
//
|
||||||
|
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
|
||||||
|
//
|
||||||
|
// The rest of the derivation is handled by computer algebra.
|
||||||
|
|
||||||
|
b2Assert(m_count >= 3);
|
||||||
|
|
||||||
|
b2Vec2 center; center.Set(0.0f, 0.0f);
|
||||||
|
float32 area = 0.0f;
|
||||||
|
float32 I = 0.0f;
|
||||||
|
|
||||||
|
// s is the reference point for forming triangles.
|
||||||
|
// It's location doesn't change the result (except for rounding error).
|
||||||
|
b2Vec2 s(0.0f, 0.0f);
|
||||||
|
|
||||||
|
// This code would put the reference point inside the polygon.
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
s += m_vertices[i];
|
||||||
|
}
|
||||||
|
s *= 1.0f / m_count;
|
||||||
|
|
||||||
|
const float32 k_inv3 = 1.0f / 3.0f;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
// Triangle vertices.
|
||||||
|
b2Vec2 e1 = m_vertices[i] - s;
|
||||||
|
b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
|
||||||
|
|
||||||
|
float32 D = b2Cross(e1, e2);
|
||||||
|
|
||||||
|
float32 triangleArea = 0.5f * D;
|
||||||
|
area += triangleArea;
|
||||||
|
|
||||||
|
// Area weighted centroid
|
||||||
|
center += triangleArea * k_inv3 * (e1 + e2);
|
||||||
|
|
||||||
|
float32 ex1 = e1.x, ey1 = e1.y;
|
||||||
|
float32 ex2 = e2.x, ey2 = e2.y;
|
||||||
|
|
||||||
|
float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
|
||||||
|
float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
|
||||||
|
|
||||||
|
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Total mass
|
||||||
|
massData->mass = density * area;
|
||||||
|
|
||||||
|
// Center of mass
|
||||||
|
b2Assert(area > b2_epsilon);
|
||||||
|
center *= 1.0f / area;
|
||||||
|
massData->center = center + s;
|
||||||
|
|
||||||
|
// Inertia tensor relative to the local origin (point s).
|
||||||
|
massData->I = density * I;
|
||||||
|
|
||||||
|
// Shift to center of mass then to original body origin.
|
||||||
|
massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PolygonShape::Validate() const
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
int32 i1 = i;
|
||||||
|
int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
|
||||||
|
b2Vec2 p = m_vertices[i1];
|
||||||
|
b2Vec2 e = m_vertices[i2] - p;
|
||||||
|
|
||||||
|
for (int32 j = 0; j < m_count; ++j)
|
||||||
|
{
|
||||||
|
if (j == i1 || j == i2)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 v = m_vertices[j] - p;
|
||||||
|
float32 c = b2Cross(e, v);
|
||||||
|
if (c < 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,89 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_POLYGON_SHAPE_H
|
||||||
|
#define B2_POLYGON_SHAPE_H
|
||||||
|
|
||||||
|
#include "b2Shape.h"
|
||||||
|
|
||||||
|
/// A convex polygon. It is assumed that the interior of the polygon is to
|
||||||
|
/// the left of each edge.
|
||||||
|
/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
|
||||||
|
/// In most cases you should not need many vertices for a convex polygon.
|
||||||
|
class b2PolygonShape : public b2Shape
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2PolygonShape();
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
b2Shape* Clone(b2BlockAllocator* allocator) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::GetChildCount
|
||||||
|
int32 GetChildCount() const override;
|
||||||
|
|
||||||
|
/// Create a convex hull from the given array of local points.
|
||||||
|
/// The count must be in the range [3, b2_maxPolygonVertices].
|
||||||
|
/// @warning the points may be re-ordered, even if they form a convex polygon
|
||||||
|
/// @warning collinear points are handled but not removed. Collinear points
|
||||||
|
/// may lead to poor stacking behavior.
|
||||||
|
void Set(const b2Vec2* points, int32 count);
|
||||||
|
|
||||||
|
/// Build vertices to represent an axis-aligned box centered on the local origin.
|
||||||
|
/// @param hx the half-width.
|
||||||
|
/// @param hy the half-height.
|
||||||
|
void SetAsBox(float32 hx, float32 hy);
|
||||||
|
|
||||||
|
/// Build vertices to represent an oriented box.
|
||||||
|
/// @param hx the half-width.
|
||||||
|
/// @param hy the half-height.
|
||||||
|
/// @param center the center of the box in local coordinates.
|
||||||
|
/// @param angle the rotation of the box in local coordinates.
|
||||||
|
void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
|
||||||
|
|
||||||
|
/// @see b2Shape::TestPoint
|
||||||
|
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const override;
|
||||||
|
|
||||||
|
/// Implement b2Shape.
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeAABB
|
||||||
|
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const override;
|
||||||
|
|
||||||
|
/// @see b2Shape::ComputeMass
|
||||||
|
void ComputeMass(b2MassData* massData, float32 density) const override;
|
||||||
|
|
||||||
|
/// Validate convexity. This is a very time consuming operation.
|
||||||
|
/// @returns true if valid
|
||||||
|
bool Validate() const;
|
||||||
|
|
||||||
|
b2Vec2 m_centroid;
|
||||||
|
b2Vec2 m_vertices[b2_maxPolygonVertices];
|
||||||
|
b2Vec2 m_normals[b2_maxPolygonVertices];
|
||||||
|
int32 m_count;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2PolygonShape::b2PolygonShape()
|
||||||
|
{
|
||||||
|
m_type = e_polygon;
|
||||||
|
m_radius = b2_polygonRadius;
|
||||||
|
m_count = 0;
|
||||||
|
m_centroid.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,104 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_SHAPE_H
|
||||||
|
#define B2_SHAPE_H
|
||||||
|
|
||||||
|
#include "../../Common/b2BlockAllocator.h"
|
||||||
|
#include "../../Common/b2Math.h"
|
||||||
|
#include "../../Collision/b2Collision.h"
|
||||||
|
|
||||||
|
/// This holds the mass data computed for a shape.
|
||||||
|
struct b2MassData
|
||||||
|
{
|
||||||
|
/// The mass of the shape, usually in kilograms.
|
||||||
|
float32 mass;
|
||||||
|
|
||||||
|
/// The position of the shape's centroid relative to the shape's origin.
|
||||||
|
b2Vec2 center;
|
||||||
|
|
||||||
|
/// The rotational inertia of the shape about the local origin.
|
||||||
|
float32 I;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A shape is used for collision detection. You can create a shape however you like.
|
||||||
|
/// Shapes used for simulation in b2World are created automatically when a b2Fixture
|
||||||
|
/// is created. Shapes may encapsulate a one or more child shapes.
|
||||||
|
class b2Shape
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
enum Type
|
||||||
|
{
|
||||||
|
e_circle = 0,
|
||||||
|
e_edge = 1,
|
||||||
|
e_polygon = 2,
|
||||||
|
e_chain = 3,
|
||||||
|
e_typeCount = 4
|
||||||
|
};
|
||||||
|
|
||||||
|
virtual ~b2Shape() {}
|
||||||
|
|
||||||
|
/// Clone the concrete shape using the provided allocator.
|
||||||
|
virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
|
||||||
|
|
||||||
|
/// Get the type of this shape. You can use this to down cast to the concrete shape.
|
||||||
|
/// @return the shape type.
|
||||||
|
Type GetType() const;
|
||||||
|
|
||||||
|
/// Get the number of child primitives.
|
||||||
|
virtual int32 GetChildCount() const = 0;
|
||||||
|
|
||||||
|
/// Test a point for containment in this shape. This only works for convex shapes.
|
||||||
|
/// @param xf the shape world transform.
|
||||||
|
/// @param p a point in world coordinates.
|
||||||
|
virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
|
||||||
|
|
||||||
|
/// Cast a ray against a child shape.
|
||||||
|
/// @param output the ray-cast results.
|
||||||
|
/// @param input the ray-cast input parameters.
|
||||||
|
/// @param transform the transform to be applied to the shape.
|
||||||
|
/// @param childIndex the child shape index
|
||||||
|
virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
|
||||||
|
const b2Transform& transform, int32 childIndex) const = 0;
|
||||||
|
|
||||||
|
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
|
||||||
|
/// @param aabb returns the axis aligned box.
|
||||||
|
/// @param xf the world transform of the shape.
|
||||||
|
/// @param childIndex the child shape
|
||||||
|
virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
|
||||||
|
|
||||||
|
/// Compute the mass properties of this shape using its dimensions and density.
|
||||||
|
/// The inertia tensor is computed about the local origin.
|
||||||
|
/// @param massData returns the mass data for this shape.
|
||||||
|
/// @param density the density in kilograms per meter squared.
|
||||||
|
virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;
|
||||||
|
|
||||||
|
Type m_type;
|
||||||
|
|
||||||
|
/// Radius of a shape. For polygonal shapes this must be b2_polygonRadius. There is no support for
|
||||||
|
/// making rounded polygons.
|
||||||
|
float32 m_radius;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2Shape::Type b2Shape::GetType() const
|
||||||
|
{
|
||||||
|
return m_type;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,119 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2BroadPhase.h"
|
||||||
|
|
||||||
|
b2BroadPhase::b2BroadPhase()
|
||||||
|
{
|
||||||
|
m_proxyCount = 0;
|
||||||
|
|
||||||
|
m_pairCapacity = 16;
|
||||||
|
m_pairCount = 0;
|
||||||
|
m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
|
||||||
|
|
||||||
|
m_moveCapacity = 16;
|
||||||
|
m_moveCount = 0;
|
||||||
|
m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2BroadPhase::~b2BroadPhase()
|
||||||
|
{
|
||||||
|
b2Free(m_moveBuffer);
|
||||||
|
b2Free(m_pairBuffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
|
||||||
|
{
|
||||||
|
int32 proxyId = m_tree.CreateProxy(aabb, userData);
|
||||||
|
++m_proxyCount;
|
||||||
|
BufferMove(proxyId);
|
||||||
|
return proxyId;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BroadPhase::DestroyProxy(int32 proxyId)
|
||||||
|
{
|
||||||
|
UnBufferMove(proxyId);
|
||||||
|
--m_proxyCount;
|
||||||
|
m_tree.DestroyProxy(proxyId);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
|
||||||
|
{
|
||||||
|
bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
|
||||||
|
if (buffer)
|
||||||
|
{
|
||||||
|
BufferMove(proxyId);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BroadPhase::TouchProxy(int32 proxyId)
|
||||||
|
{
|
||||||
|
BufferMove(proxyId);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BroadPhase::BufferMove(int32 proxyId)
|
||||||
|
{
|
||||||
|
if (m_moveCount == m_moveCapacity)
|
||||||
|
{
|
||||||
|
int32* oldBuffer = m_moveBuffer;
|
||||||
|
m_moveCapacity *= 2;
|
||||||
|
m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
|
||||||
|
memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
|
||||||
|
b2Free(oldBuffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_moveBuffer[m_moveCount] = proxyId;
|
||||||
|
++m_moveCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BroadPhase::UnBufferMove(int32 proxyId)
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_moveCount; ++i)
|
||||||
|
{
|
||||||
|
if (m_moveBuffer[i] == proxyId)
|
||||||
|
{
|
||||||
|
m_moveBuffer[i] = e_nullProxy;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is called from b2DynamicTree::Query when we are gathering pairs.
|
||||||
|
bool b2BroadPhase::QueryCallback(int32 proxyId)
|
||||||
|
{
|
||||||
|
// A proxy cannot form a pair with itself.
|
||||||
|
if (proxyId == m_queryProxyId)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Grow the pair buffer as needed.
|
||||||
|
if (m_pairCount == m_pairCapacity)
|
||||||
|
{
|
||||||
|
b2Pair* oldBuffer = m_pairBuffer;
|
||||||
|
m_pairCapacity *= 2;
|
||||||
|
m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
|
||||||
|
memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
|
||||||
|
b2Free(oldBuffer);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
|
||||||
|
m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
|
||||||
|
++m_pairCount;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,257 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_BROAD_PHASE_H
|
||||||
|
#define B2_BROAD_PHASE_H
|
||||||
|
|
||||||
|
#include "../Common/b2Settings.h"
|
||||||
|
#include "b2Collision.h"
|
||||||
|
#include "b2DynamicTree.h"
|
||||||
|
#include <algorithm>
|
||||||
|
|
||||||
|
struct b2Pair
|
||||||
|
{
|
||||||
|
int32 proxyIdA;
|
||||||
|
int32 proxyIdB;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
|
||||||
|
/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
|
||||||
|
/// It is up to the client to consume the new pairs and to track subsequent overlap.
|
||||||
|
class b2BroadPhase
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
e_nullProxy = -1
|
||||||
|
};
|
||||||
|
|
||||||
|
b2BroadPhase();
|
||||||
|
~b2BroadPhase();
|
||||||
|
|
||||||
|
/// Create a proxy with an initial AABB. Pairs are not reported until
|
||||||
|
/// UpdatePairs is called.
|
||||||
|
int32 CreateProxy(const b2AABB& aabb, void* userData);
|
||||||
|
|
||||||
|
/// Destroy a proxy. It is up to the client to remove any pairs.
|
||||||
|
void DestroyProxy(int32 proxyId);
|
||||||
|
|
||||||
|
/// Call MoveProxy as many times as you like, then when you are done
|
||||||
|
/// call UpdatePairs to finalized the proxy pairs (for your time step).
|
||||||
|
void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
|
||||||
|
|
||||||
|
/// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
|
||||||
|
void TouchProxy(int32 proxyId);
|
||||||
|
|
||||||
|
/// Get the fat AABB for a proxy.
|
||||||
|
const b2AABB& GetFatAABB(int32 proxyId) const;
|
||||||
|
|
||||||
|
/// Get user data from a proxy. Returns nullptr if the id is invalid.
|
||||||
|
void* GetUserData(int32 proxyId) const;
|
||||||
|
|
||||||
|
/// Test overlap of fat AABBs.
|
||||||
|
bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
|
||||||
|
|
||||||
|
/// Get the number of proxies.
|
||||||
|
int32 GetProxyCount() const;
|
||||||
|
|
||||||
|
/// Update the pairs. This results in pair callbacks. This can only add pairs.
|
||||||
|
template <typename T>
|
||||||
|
void UpdatePairs(T* callback);
|
||||||
|
|
||||||
|
/// Query an AABB for overlapping proxies. The callback class
|
||||||
|
/// is called for each proxy that overlaps the supplied AABB.
|
||||||
|
template <typename T>
|
||||||
|
void Query(T* callback, const b2AABB& aabb) const;
|
||||||
|
|
||||||
|
/// Ray-cast against the proxies in the tree. This relies on the callback
|
||||||
|
/// to perform a exact ray-cast in the case were the proxy contains a shape.
|
||||||
|
/// The callback also performs the any collision filtering. This has performance
|
||||||
|
/// roughly equal to k * log(n), where k is the number of collisions and n is the
|
||||||
|
/// number of proxies in the tree.
|
||||||
|
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
|
||||||
|
/// @param callback a callback class that is called for each proxy that is hit by the ray.
|
||||||
|
template <typename T>
|
||||||
|
void RayCast(T* callback, const b2RayCastInput& input) const;
|
||||||
|
|
||||||
|
/// Get the height of the embedded tree.
|
||||||
|
int32 GetTreeHeight() const;
|
||||||
|
|
||||||
|
/// Get the balance of the embedded tree.
|
||||||
|
int32 GetTreeBalance() const;
|
||||||
|
|
||||||
|
/// Get the quality metric of the embedded tree.
|
||||||
|
float32 GetTreeQuality() const;
|
||||||
|
|
||||||
|
/// Shift the world origin. Useful for large worlds.
|
||||||
|
/// The shift formula is: position -= newOrigin
|
||||||
|
/// @param newOrigin the new origin with respect to the old origin
|
||||||
|
void ShiftOrigin(const b2Vec2& newOrigin);
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
friend class b2DynamicTree;
|
||||||
|
|
||||||
|
void BufferMove(int32 proxyId);
|
||||||
|
void UnBufferMove(int32 proxyId);
|
||||||
|
|
||||||
|
bool QueryCallback(int32 proxyId);
|
||||||
|
|
||||||
|
b2DynamicTree m_tree;
|
||||||
|
|
||||||
|
int32 m_proxyCount;
|
||||||
|
|
||||||
|
int32* m_moveBuffer;
|
||||||
|
int32 m_moveCapacity;
|
||||||
|
int32 m_moveCount;
|
||||||
|
|
||||||
|
b2Pair* m_pairBuffer;
|
||||||
|
int32 m_pairCapacity;
|
||||||
|
int32 m_pairCount;
|
||||||
|
|
||||||
|
int32 m_queryProxyId;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is used to sort pairs.
|
||||||
|
inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
|
||||||
|
{
|
||||||
|
if (pair1.proxyIdA < pair2.proxyIdA)
|
||||||
|
{
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (pair1.proxyIdA == pair2.proxyIdA)
|
||||||
|
{
|
||||||
|
return pair1.proxyIdB < pair2.proxyIdB;
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void* b2BroadPhase::GetUserData(int32 proxyId) const
|
||||||
|
{
|
||||||
|
return m_tree.GetUserData(proxyId);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const
|
||||||
|
{
|
||||||
|
const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
|
||||||
|
const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
|
||||||
|
return b2TestOverlap(aabbA, aabbB);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const
|
||||||
|
{
|
||||||
|
return m_tree.GetFatAABB(proxyId);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2BroadPhase::GetProxyCount() const
|
||||||
|
{
|
||||||
|
return m_proxyCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2BroadPhase::GetTreeHeight() const
|
||||||
|
{
|
||||||
|
return m_tree.GetHeight();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2BroadPhase::GetTreeBalance() const
|
||||||
|
{
|
||||||
|
return m_tree.GetMaxBalance();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2BroadPhase::GetTreeQuality() const
|
||||||
|
{
|
||||||
|
return m_tree.GetAreaRatio();
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
void b2BroadPhase::UpdatePairs(T* callback)
|
||||||
|
{
|
||||||
|
// Reset pair buffer
|
||||||
|
m_pairCount = 0;
|
||||||
|
|
||||||
|
// Perform tree queries for all moving proxies.
|
||||||
|
for (int32 i = 0; i < m_moveCount; ++i)
|
||||||
|
{
|
||||||
|
m_queryProxyId = m_moveBuffer[i];
|
||||||
|
if (m_queryProxyId == e_nullProxy)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// We have to query the tree with the fat AABB so that
|
||||||
|
// we don't fail to create a pair that may touch later.
|
||||||
|
const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
|
||||||
|
|
||||||
|
// Query tree, create pairs and add them pair buffer.
|
||||||
|
m_tree.Query(this, fatAABB);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reset move buffer
|
||||||
|
m_moveCount = 0;
|
||||||
|
|
||||||
|
// Sort the pair buffer to expose duplicates.
|
||||||
|
std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
|
||||||
|
|
||||||
|
// Send the pairs back to the client.
|
||||||
|
int32 i = 0;
|
||||||
|
while (i < m_pairCount)
|
||||||
|
{
|
||||||
|
b2Pair* primaryPair = m_pairBuffer + i;
|
||||||
|
void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
|
||||||
|
void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
|
||||||
|
|
||||||
|
callback->AddPair(userDataA, userDataB);
|
||||||
|
++i;
|
||||||
|
|
||||||
|
// Skip any duplicate pairs.
|
||||||
|
while (i < m_pairCount)
|
||||||
|
{
|
||||||
|
b2Pair* pair = m_pairBuffer + i;
|
||||||
|
if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
++i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Try to keep the tree balanced.
|
||||||
|
//m_tree.Rebalance(4);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const
|
||||||
|
{
|
||||||
|
m_tree.Query(callback, aabb);
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const
|
||||||
|
{
|
||||||
|
m_tree.RayCast(callback, input);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
|
||||||
|
{
|
||||||
|
m_tree.ShiftOrigin(newOrigin);
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,154 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
|
||||||
|
void b2CollideCircles(
|
||||||
|
b2Manifold* manifold,
|
||||||
|
const b2CircleShape* circleA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
manifold->pointCount = 0;
|
||||||
|
|
||||||
|
b2Vec2 pA = b2Mul(xfA, circleA->m_p);
|
||||||
|
b2Vec2 pB = b2Mul(xfB, circleB->m_p);
|
||||||
|
|
||||||
|
b2Vec2 d = pB - pA;
|
||||||
|
float32 distSqr = b2Dot(d, d);
|
||||||
|
float32 rA = circleA->m_radius, rB = circleB->m_radius;
|
||||||
|
float32 radius = rA + rB;
|
||||||
|
if (distSqr > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->type = b2Manifold::e_circles;
|
||||||
|
manifold->localPoint = circleA->m_p;
|
||||||
|
manifold->localNormal.SetZero();
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CollidePolygonAndCircle(
|
||||||
|
b2Manifold* manifold,
|
||||||
|
const b2PolygonShape* polygonA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
manifold->pointCount = 0;
|
||||||
|
|
||||||
|
// Compute circle position in the frame of the polygon.
|
||||||
|
b2Vec2 c = b2Mul(xfB, circleB->m_p);
|
||||||
|
b2Vec2 cLocal = b2MulT(xfA, c);
|
||||||
|
|
||||||
|
// Find the min separating edge.
|
||||||
|
int32 normalIndex = 0;
|
||||||
|
float32 separation = -b2_maxFloat;
|
||||||
|
float32 radius = polygonA->m_radius + circleB->m_radius;
|
||||||
|
int32 vertexCount = polygonA->m_count;
|
||||||
|
const b2Vec2* vertices = polygonA->m_vertices;
|
||||||
|
const b2Vec2* normals = polygonA->m_normals;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < vertexCount; ++i)
|
||||||
|
{
|
||||||
|
float32 s = b2Dot(normals[i], cLocal - vertices[i]);
|
||||||
|
|
||||||
|
if (s > radius)
|
||||||
|
{
|
||||||
|
// Early out.
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (s > separation)
|
||||||
|
{
|
||||||
|
separation = s;
|
||||||
|
normalIndex = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Vertices that subtend the incident face.
|
||||||
|
int32 vertIndex1 = normalIndex;
|
||||||
|
int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
|
||||||
|
b2Vec2 v1 = vertices[vertIndex1];
|
||||||
|
b2Vec2 v2 = vertices[vertIndex2];
|
||||||
|
|
||||||
|
// If the center is inside the polygon ...
|
||||||
|
if (separation < b2_epsilon)
|
||||||
|
{
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
manifold->localNormal = normals[normalIndex];
|
||||||
|
manifold->localPoint = 0.5f * (v1 + v2);
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute barycentric coordinates
|
||||||
|
float32 u1 = b2Dot(cLocal - v1, v2 - v1);
|
||||||
|
float32 u2 = b2Dot(cLocal - v2, v1 - v2);
|
||||||
|
if (u1 <= 0.0f)
|
||||||
|
{
|
||||||
|
if (b2DistanceSquared(cLocal, v1) > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
manifold->localNormal = cLocal - v1;
|
||||||
|
manifold->localNormal.Normalize();
|
||||||
|
manifold->localPoint = v1;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
}
|
||||||
|
else if (u2 <= 0.0f)
|
||||||
|
{
|
||||||
|
if (b2DistanceSquared(cLocal, v2) > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
manifold->localNormal = cLocal - v2;
|
||||||
|
manifold->localNormal.Normalize();
|
||||||
|
manifold->localPoint = v2;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 faceCenter = 0.5f * (v1 + v2);
|
||||||
|
float32 s = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
|
||||||
|
if (s > radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
manifold->localNormal = normals[vertIndex1];
|
||||||
|
manifold->localPoint = faceCenter;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,698 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
|
||||||
|
|
||||||
|
// Compute contact points for edge versus circle.
|
||||||
|
// This accounts for edge connectivity.
|
||||||
|
void b2CollideEdgeAndCircle(b2Manifold* manifold,
|
||||||
|
const b2EdgeShape* edgeA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
manifold->pointCount = 0;
|
||||||
|
|
||||||
|
// Compute circle in frame of edge
|
||||||
|
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
|
||||||
|
|
||||||
|
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
|
||||||
|
b2Vec2 e = B - A;
|
||||||
|
|
||||||
|
// Barycentric coordinates
|
||||||
|
float32 u = b2Dot(e, B - Q);
|
||||||
|
float32 v = b2Dot(e, Q - A);
|
||||||
|
|
||||||
|
float32 radius = edgeA->m_radius + circleB->m_radius;
|
||||||
|
|
||||||
|
b2ContactFeature cf;
|
||||||
|
cf.indexB = 0;
|
||||||
|
cf.typeB = b2ContactFeature::e_vertex;
|
||||||
|
|
||||||
|
// Region A
|
||||||
|
if (v <= 0.0f)
|
||||||
|
{
|
||||||
|
b2Vec2 P = A;
|
||||||
|
b2Vec2 d = Q - P;
|
||||||
|
float32 dd = b2Dot(d, d);
|
||||||
|
if (dd > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Is there an edge connected to A?
|
||||||
|
if (edgeA->m_hasVertex0)
|
||||||
|
{
|
||||||
|
b2Vec2 A1 = edgeA->m_vertex0;
|
||||||
|
b2Vec2 B1 = A;
|
||||||
|
b2Vec2 e1 = B1 - A1;
|
||||||
|
float32 u1 = b2Dot(e1, B1 - Q);
|
||||||
|
|
||||||
|
// Is the circle in Region AB of the previous edge?
|
||||||
|
if (u1 > 0.0f)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
cf.indexA = 0;
|
||||||
|
cf.typeA = b2ContactFeature::e_vertex;
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_circles;
|
||||||
|
manifold->localNormal.SetZero();
|
||||||
|
manifold->localPoint = P;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
manifold->points[0].id.cf = cf;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Region B
|
||||||
|
if (u <= 0.0f)
|
||||||
|
{
|
||||||
|
b2Vec2 P = B;
|
||||||
|
b2Vec2 d = Q - P;
|
||||||
|
float32 dd = b2Dot(d, d);
|
||||||
|
if (dd > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Is there an edge connected to B?
|
||||||
|
if (edgeA->m_hasVertex3)
|
||||||
|
{
|
||||||
|
b2Vec2 B2 = edgeA->m_vertex3;
|
||||||
|
b2Vec2 A2 = B;
|
||||||
|
b2Vec2 e2 = B2 - A2;
|
||||||
|
float32 v2 = b2Dot(e2, Q - A2);
|
||||||
|
|
||||||
|
// Is the circle in Region AB of the next edge?
|
||||||
|
if (v2 > 0.0f)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
cf.indexA = 1;
|
||||||
|
cf.typeA = b2ContactFeature::e_vertex;
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_circles;
|
||||||
|
manifold->localNormal.SetZero();
|
||||||
|
manifold->localPoint = P;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
manifold->points[0].id.cf = cf;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Region AB
|
||||||
|
float32 den = b2Dot(e, e);
|
||||||
|
b2Assert(den > 0.0f);
|
||||||
|
b2Vec2 P = (1.0f / den) * (u * A + v * B);
|
||||||
|
b2Vec2 d = Q - P;
|
||||||
|
float32 dd = b2Dot(d, d);
|
||||||
|
if (dd > radius * radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 n(-e.y, e.x);
|
||||||
|
if (b2Dot(n, Q - A) < 0.0f)
|
||||||
|
{
|
||||||
|
n.Set(-n.x, -n.y);
|
||||||
|
}
|
||||||
|
n.Normalize();
|
||||||
|
|
||||||
|
cf.indexA = 0;
|
||||||
|
cf.typeA = b2ContactFeature::e_face;
|
||||||
|
manifold->pointCount = 1;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
manifold->localNormal = n;
|
||||||
|
manifold->localPoint = A;
|
||||||
|
manifold->points[0].id.key = 0;
|
||||||
|
manifold->points[0].id.cf = cf;
|
||||||
|
manifold->points[0].localPoint = circleB->m_p;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This structure is used to keep track of the best separating axis.
|
||||||
|
struct b2EPAxis
|
||||||
|
{
|
||||||
|
enum Type
|
||||||
|
{
|
||||||
|
e_unknown,
|
||||||
|
e_edgeA,
|
||||||
|
e_edgeB
|
||||||
|
};
|
||||||
|
|
||||||
|
Type type;
|
||||||
|
int32 index;
|
||||||
|
float32 separation;
|
||||||
|
};
|
||||||
|
|
||||||
|
// This holds polygon B expressed in frame A.
|
||||||
|
struct b2TempPolygon
|
||||||
|
{
|
||||||
|
b2Vec2 vertices[b2_maxPolygonVertices];
|
||||||
|
b2Vec2 normals[b2_maxPolygonVertices];
|
||||||
|
int32 count;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Reference face used for clipping
|
||||||
|
struct b2ReferenceFace
|
||||||
|
{
|
||||||
|
int32 i1, i2;
|
||||||
|
|
||||||
|
b2Vec2 v1, v2;
|
||||||
|
|
||||||
|
b2Vec2 normal;
|
||||||
|
|
||||||
|
b2Vec2 sideNormal1;
|
||||||
|
float32 sideOffset1;
|
||||||
|
|
||||||
|
b2Vec2 sideNormal2;
|
||||||
|
float32 sideOffset2;
|
||||||
|
};
|
||||||
|
|
||||||
|
// This class collides and edge and a polygon, taking into account edge adjacency.
|
||||||
|
struct b2EPCollider
|
||||||
|
{
|
||||||
|
void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* polygonB, const b2Transform& xfB);
|
||||||
|
b2EPAxis ComputeEdgeSeparation();
|
||||||
|
b2EPAxis ComputePolygonSeparation();
|
||||||
|
|
||||||
|
enum VertexType
|
||||||
|
{
|
||||||
|
e_isolated,
|
||||||
|
e_concave,
|
||||||
|
e_convex
|
||||||
|
};
|
||||||
|
|
||||||
|
b2TempPolygon m_polygonB;
|
||||||
|
|
||||||
|
b2Transform m_xf;
|
||||||
|
b2Vec2 m_centroidB;
|
||||||
|
b2Vec2 m_v0, m_v1, m_v2, m_v3;
|
||||||
|
b2Vec2 m_normal0, m_normal1, m_normal2;
|
||||||
|
b2Vec2 m_normal;
|
||||||
|
VertexType m_type1, m_type2;
|
||||||
|
b2Vec2 m_lowerLimit, m_upperLimit;
|
||||||
|
float32 m_radius;
|
||||||
|
bool m_front;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Algorithm:
|
||||||
|
// 1. Classify v1 and v2
|
||||||
|
// 2. Classify polygon centroid as front or back
|
||||||
|
// 3. Flip normal if necessary
|
||||||
|
// 4. Initialize normal range to [-pi, pi] about face normal
|
||||||
|
// 5. Adjust normal range according to adjacent edges
|
||||||
|
// 6. Visit each separating axes, only accept axes within the range
|
||||||
|
// 7. Return if _any_ axis indicates separation
|
||||||
|
// 8. Clip
|
||||||
|
void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* polygonB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
m_xf = b2MulT(xfA, xfB);
|
||||||
|
|
||||||
|
m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
|
||||||
|
|
||||||
|
m_v0 = edgeA->m_vertex0;
|
||||||
|
m_v1 = edgeA->m_vertex1;
|
||||||
|
m_v2 = edgeA->m_vertex2;
|
||||||
|
m_v3 = edgeA->m_vertex3;
|
||||||
|
|
||||||
|
bool hasVertex0 = edgeA->m_hasVertex0;
|
||||||
|
bool hasVertex3 = edgeA->m_hasVertex3;
|
||||||
|
|
||||||
|
b2Vec2 edge1 = m_v2 - m_v1;
|
||||||
|
edge1.Normalize();
|
||||||
|
m_normal1.Set(edge1.y, -edge1.x);
|
||||||
|
float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
|
||||||
|
float32 offset0 = 0.0f, offset2 = 0.0f;
|
||||||
|
bool convex1 = false, convex2 = false;
|
||||||
|
|
||||||
|
// Is there a preceding edge?
|
||||||
|
if (hasVertex0)
|
||||||
|
{
|
||||||
|
b2Vec2 edge0 = m_v1 - m_v0;
|
||||||
|
edge0.Normalize();
|
||||||
|
m_normal0.Set(edge0.y, -edge0.x);
|
||||||
|
convex1 = b2Cross(edge0, edge1) >= 0.0f;
|
||||||
|
offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Is there a following edge?
|
||||||
|
if (hasVertex3)
|
||||||
|
{
|
||||||
|
b2Vec2 edge2 = m_v3 - m_v2;
|
||||||
|
edge2.Normalize();
|
||||||
|
m_normal2.Set(edge2.y, -edge2.x);
|
||||||
|
convex2 = b2Cross(edge1, edge2) > 0.0f;
|
||||||
|
offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Determine front or back collision. Determine collision normal limits.
|
||||||
|
if (hasVertex0 && hasVertex3)
|
||||||
|
{
|
||||||
|
if (convex1 && convex2)
|
||||||
|
{
|
||||||
|
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal0;
|
||||||
|
m_upperLimit = m_normal2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (convex1)
|
||||||
|
{
|
||||||
|
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal0;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal2;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (convex2)
|
||||||
|
{
|
||||||
|
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = m_normal2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = -m_normal0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal2;
|
||||||
|
m_upperLimit = -m_normal0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (hasVertex0)
|
||||||
|
{
|
||||||
|
if (convex1)
|
||||||
|
{
|
||||||
|
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal0;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = -m_normal0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (hasVertex3)
|
||||||
|
{
|
||||||
|
if (convex2)
|
||||||
|
{
|
||||||
|
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = m_normal2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = -m_normal2;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_front = offset1 >= 0.0f;
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
m_normal = m_normal1;
|
||||||
|
m_lowerLimit = -m_normal1;
|
||||||
|
m_upperLimit = -m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_normal = -m_normal1;
|
||||||
|
m_lowerLimit = m_normal1;
|
||||||
|
m_upperLimit = m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get polygonB in frameA
|
||||||
|
m_polygonB.count = polygonB->m_count;
|
||||||
|
for (int32 i = 0; i < polygonB->m_count; ++i)
|
||||||
|
{
|
||||||
|
m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
|
||||||
|
m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_radius = polygonB->m_radius + edgeA->m_radius;
|
||||||
|
|
||||||
|
manifold->pointCount = 0;
|
||||||
|
|
||||||
|
b2EPAxis edgeAxis = ComputeEdgeSeparation();
|
||||||
|
|
||||||
|
// If no valid normal can be found than this edge should not collide.
|
||||||
|
if (edgeAxis.type == b2EPAxis::e_unknown)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (edgeAxis.separation > m_radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2EPAxis polygonAxis = ComputePolygonSeparation();
|
||||||
|
if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Use hysteresis for jitter reduction.
|
||||||
|
const float32 k_relativeTol = 0.98f;
|
||||||
|
const float32 k_absoluteTol = 0.001f;
|
||||||
|
|
||||||
|
b2EPAxis primaryAxis;
|
||||||
|
if (polygonAxis.type == b2EPAxis::e_unknown)
|
||||||
|
{
|
||||||
|
primaryAxis = edgeAxis;
|
||||||
|
}
|
||||||
|
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
|
||||||
|
{
|
||||||
|
primaryAxis = polygonAxis;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
primaryAxis = edgeAxis;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ClipVertex ie[2];
|
||||||
|
b2ReferenceFace rf;
|
||||||
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
||||||
|
{
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
|
||||||
|
// Search for the polygon normal that is most anti-parallel to the edge normal.
|
||||||
|
int32 bestIndex = 0;
|
||||||
|
float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
|
||||||
|
for (int32 i = 1; i < m_polygonB.count; ++i)
|
||||||
|
{
|
||||||
|
float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
|
||||||
|
if (value < bestValue)
|
||||||
|
{
|
||||||
|
bestValue = value;
|
||||||
|
bestIndex = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 i1 = bestIndex;
|
||||||
|
int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
|
||||||
|
|
||||||
|
ie[0].v = m_polygonB.vertices[i1];
|
||||||
|
ie[0].id.cf.indexA = 0;
|
||||||
|
ie[0].id.cf.indexB = static_cast<uint8>(i1);
|
||||||
|
ie[0].id.cf.typeA = b2ContactFeature::e_face;
|
||||||
|
ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
|
||||||
|
|
||||||
|
ie[1].v = m_polygonB.vertices[i2];
|
||||||
|
ie[1].id.cf.indexA = 0;
|
||||||
|
ie[1].id.cf.indexB = static_cast<uint8>(i2);
|
||||||
|
ie[1].id.cf.typeA = b2ContactFeature::e_face;
|
||||||
|
ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
|
||||||
|
|
||||||
|
if (m_front)
|
||||||
|
{
|
||||||
|
rf.i1 = 0;
|
||||||
|
rf.i2 = 1;
|
||||||
|
rf.v1 = m_v1;
|
||||||
|
rf.v2 = m_v2;
|
||||||
|
rf.normal = m_normal1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
rf.i1 = 1;
|
||||||
|
rf.i2 = 0;
|
||||||
|
rf.v1 = m_v2;
|
||||||
|
rf.v2 = m_v1;
|
||||||
|
rf.normal = -m_normal1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
manifold->type = b2Manifold::e_faceB;
|
||||||
|
|
||||||
|
ie[0].v = m_v1;
|
||||||
|
ie[0].id.cf.indexA = 0;
|
||||||
|
ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
|
||||||
|
ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
|
||||||
|
ie[0].id.cf.typeB = b2ContactFeature::e_face;
|
||||||
|
|
||||||
|
ie[1].v = m_v2;
|
||||||
|
ie[1].id.cf.indexA = 0;
|
||||||
|
ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
|
||||||
|
ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
|
||||||
|
ie[1].id.cf.typeB = b2ContactFeature::e_face;
|
||||||
|
|
||||||
|
rf.i1 = primaryAxis.index;
|
||||||
|
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
|
||||||
|
rf.v1 = m_polygonB.vertices[rf.i1];
|
||||||
|
rf.v2 = m_polygonB.vertices[rf.i2];
|
||||||
|
rf.normal = m_polygonB.normals[rf.i1];
|
||||||
|
}
|
||||||
|
|
||||||
|
rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
|
||||||
|
rf.sideNormal2 = -rf.sideNormal1;
|
||||||
|
rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
|
||||||
|
rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
|
||||||
|
|
||||||
|
// Clip incident edge against extruded edge1 side edges.
|
||||||
|
b2ClipVertex clipPoints1[2];
|
||||||
|
b2ClipVertex clipPoints2[2];
|
||||||
|
int32 np;
|
||||||
|
|
||||||
|
// Clip to box side 1
|
||||||
|
np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
|
||||||
|
|
||||||
|
if (np < b2_maxManifoldPoints)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clip to negative box side 1
|
||||||
|
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
|
||||||
|
|
||||||
|
if (np < b2_maxManifoldPoints)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Now clipPoints2 contains the clipped points.
|
||||||
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
||||||
|
{
|
||||||
|
manifold->localNormal = rf.normal;
|
||||||
|
manifold->localPoint = rf.v1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
manifold->localNormal = polygonB->m_normals[rf.i1];
|
||||||
|
manifold->localPoint = polygonB->m_vertices[rf.i1];
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 pointCount = 0;
|
||||||
|
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
|
||||||
|
{
|
||||||
|
float32 separation;
|
||||||
|
|
||||||
|
separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
|
||||||
|
|
||||||
|
if (separation <= m_radius)
|
||||||
|
{
|
||||||
|
b2ManifoldPoint* cp = manifold->points + pointCount;
|
||||||
|
|
||||||
|
if (primaryAxis.type == b2EPAxis::e_edgeA)
|
||||||
|
{
|
||||||
|
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
|
||||||
|
cp->id = clipPoints2[i].id;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
cp->localPoint = clipPoints2[i].v;
|
||||||
|
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
|
||||||
|
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
|
||||||
|
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
|
||||||
|
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
|
||||||
|
}
|
||||||
|
|
||||||
|
++pointCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->pointCount = pointCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2EPAxis b2EPCollider::ComputeEdgeSeparation()
|
||||||
|
{
|
||||||
|
b2EPAxis axis;
|
||||||
|
axis.type = b2EPAxis::e_edgeA;
|
||||||
|
axis.index = m_front ? 0 : 1;
|
||||||
|
axis.separation = FLT_MAX;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_polygonB.count; ++i)
|
||||||
|
{
|
||||||
|
float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
|
||||||
|
if (s < axis.separation)
|
||||||
|
{
|
||||||
|
axis.separation = s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return axis;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2EPAxis b2EPCollider::ComputePolygonSeparation()
|
||||||
|
{
|
||||||
|
b2EPAxis axis;
|
||||||
|
axis.type = b2EPAxis::e_unknown;
|
||||||
|
axis.index = -1;
|
||||||
|
axis.separation = -FLT_MAX;
|
||||||
|
|
||||||
|
b2Vec2 perp(-m_normal.y, m_normal.x);
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_polygonB.count; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 n = -m_polygonB.normals[i];
|
||||||
|
|
||||||
|
float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
|
||||||
|
float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
|
||||||
|
float32 s = b2Min(s1, s2);
|
||||||
|
|
||||||
|
if (s > m_radius)
|
||||||
|
{
|
||||||
|
// No collision
|
||||||
|
axis.type = b2EPAxis::e_edgeB;
|
||||||
|
axis.index = i;
|
||||||
|
axis.separation = s;
|
||||||
|
return axis;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Adjacency
|
||||||
|
if (b2Dot(n, perp) >= 0.0f)
|
||||||
|
{
|
||||||
|
if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (s > axis.separation)
|
||||||
|
{
|
||||||
|
axis.type = b2EPAxis::e_edgeB;
|
||||||
|
axis.index = i;
|
||||||
|
axis.separation = s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return axis;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
|
||||||
|
const b2EdgeShape* edgeA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* polygonB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2EPCollider collider;
|
||||||
|
collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,239 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
|
||||||
|
// Find the max separation between poly1 and poly2 using edge normals from poly1.
|
||||||
|
static float32 b2FindMaxSeparation(int32* edgeIndex,
|
||||||
|
const b2PolygonShape* poly1, const b2Transform& xf1,
|
||||||
|
const b2PolygonShape* poly2, const b2Transform& xf2)
|
||||||
|
{
|
||||||
|
int32 count1 = poly1->m_count;
|
||||||
|
int32 count2 = poly2->m_count;
|
||||||
|
const b2Vec2* n1s = poly1->m_normals;
|
||||||
|
const b2Vec2* v1s = poly1->m_vertices;
|
||||||
|
const b2Vec2* v2s = poly2->m_vertices;
|
||||||
|
b2Transform xf = b2MulT(xf2, xf1);
|
||||||
|
|
||||||
|
int32 bestIndex = 0;
|
||||||
|
float32 maxSeparation = -b2_maxFloat;
|
||||||
|
for (int32 i = 0; i < count1; ++i)
|
||||||
|
{
|
||||||
|
// Get poly1 normal in frame2.
|
||||||
|
b2Vec2 n = b2Mul(xf.q, n1s[i]);
|
||||||
|
b2Vec2 v1 = b2Mul(xf, v1s[i]);
|
||||||
|
|
||||||
|
// Find deepest point for normal i.
|
||||||
|
float32 si = b2_maxFloat;
|
||||||
|
for (int32 j = 0; j < count2; ++j)
|
||||||
|
{
|
||||||
|
float32 sij = b2Dot(n, v2s[j] - v1);
|
||||||
|
if (sij < si)
|
||||||
|
{
|
||||||
|
si = sij;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (si > maxSeparation)
|
||||||
|
{
|
||||||
|
maxSeparation = si;
|
||||||
|
bestIndex = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
*edgeIndex = bestIndex;
|
||||||
|
return maxSeparation;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void b2FindIncidentEdge(b2ClipVertex c[2],
|
||||||
|
const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
|
||||||
|
const b2PolygonShape* poly2, const b2Transform& xf2)
|
||||||
|
{
|
||||||
|
const b2Vec2* normals1 = poly1->m_normals;
|
||||||
|
|
||||||
|
int32 count2 = poly2->m_count;
|
||||||
|
const b2Vec2* vertices2 = poly2->m_vertices;
|
||||||
|
const b2Vec2* normals2 = poly2->m_normals;
|
||||||
|
|
||||||
|
b2Assert(0 <= edge1 && edge1 < poly1->m_count);
|
||||||
|
|
||||||
|
// Get the normal of the reference edge in poly2's frame.
|
||||||
|
b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
|
||||||
|
|
||||||
|
// Find the incident edge on poly2.
|
||||||
|
int32 index = 0;
|
||||||
|
float32 minDot = b2_maxFloat;
|
||||||
|
for (int32 i = 0; i < count2; ++i)
|
||||||
|
{
|
||||||
|
float32 dot = b2Dot(normal1, normals2[i]);
|
||||||
|
if (dot < minDot)
|
||||||
|
{
|
||||||
|
minDot = dot;
|
||||||
|
index = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Build the clip vertices for the incident edge.
|
||||||
|
int32 i1 = index;
|
||||||
|
int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
|
||||||
|
|
||||||
|
c[0].v = b2Mul(xf2, vertices2[i1]);
|
||||||
|
c[0].id.cf.indexA = (uint8)edge1;
|
||||||
|
c[0].id.cf.indexB = (uint8)i1;
|
||||||
|
c[0].id.cf.typeA = b2ContactFeature::e_face;
|
||||||
|
c[0].id.cf.typeB = b2ContactFeature::e_vertex;
|
||||||
|
|
||||||
|
c[1].v = b2Mul(xf2, vertices2[i2]);
|
||||||
|
c[1].id.cf.indexA = (uint8)edge1;
|
||||||
|
c[1].id.cf.indexB = (uint8)i2;
|
||||||
|
c[1].id.cf.typeA = b2ContactFeature::e_face;
|
||||||
|
c[1].id.cf.typeB = b2ContactFeature::e_vertex;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Find edge normal of max separation on A - return if separating axis is found
|
||||||
|
// Find edge normal of max separation on B - return if separation axis is found
|
||||||
|
// Choose reference edge as min(minA, minB)
|
||||||
|
// Find incident edge
|
||||||
|
// Clip
|
||||||
|
|
||||||
|
// The normal points from 1 to 2
|
||||||
|
void b2CollidePolygons(b2Manifold* manifold,
|
||||||
|
const b2PolygonShape* polyA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* polyB, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
manifold->pointCount = 0;
|
||||||
|
float32 totalRadius = polyA->m_radius + polyB->m_radius;
|
||||||
|
|
||||||
|
int32 edgeA = 0;
|
||||||
|
float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
|
||||||
|
if (separationA > totalRadius)
|
||||||
|
return;
|
||||||
|
|
||||||
|
int32 edgeB = 0;
|
||||||
|
float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
|
||||||
|
if (separationB > totalRadius)
|
||||||
|
return;
|
||||||
|
|
||||||
|
const b2PolygonShape* poly1; // reference polygon
|
||||||
|
const b2PolygonShape* poly2; // incident polygon
|
||||||
|
b2Transform xf1, xf2;
|
||||||
|
int32 edge1; // reference edge
|
||||||
|
uint8 flip;
|
||||||
|
const float32 k_tol = 0.1f * b2_linearSlop;
|
||||||
|
|
||||||
|
if (separationB > separationA + k_tol)
|
||||||
|
{
|
||||||
|
poly1 = polyB;
|
||||||
|
poly2 = polyA;
|
||||||
|
xf1 = xfB;
|
||||||
|
xf2 = xfA;
|
||||||
|
edge1 = edgeB;
|
||||||
|
manifold->type = b2Manifold::e_faceB;
|
||||||
|
flip = 1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
poly1 = polyA;
|
||||||
|
poly2 = polyB;
|
||||||
|
xf1 = xfA;
|
||||||
|
xf2 = xfB;
|
||||||
|
edge1 = edgeA;
|
||||||
|
manifold->type = b2Manifold::e_faceA;
|
||||||
|
flip = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ClipVertex incidentEdge[2];
|
||||||
|
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
|
||||||
|
|
||||||
|
int32 count1 = poly1->m_count;
|
||||||
|
const b2Vec2* vertices1 = poly1->m_vertices;
|
||||||
|
|
||||||
|
int32 iv1 = edge1;
|
||||||
|
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
|
||||||
|
|
||||||
|
b2Vec2 v11 = vertices1[iv1];
|
||||||
|
b2Vec2 v12 = vertices1[iv2];
|
||||||
|
|
||||||
|
b2Vec2 localTangent = v12 - v11;
|
||||||
|
localTangent.Normalize();
|
||||||
|
|
||||||
|
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
|
||||||
|
b2Vec2 planePoint = 0.5f * (v11 + v12);
|
||||||
|
|
||||||
|
b2Vec2 tangent = b2Mul(xf1.q, localTangent);
|
||||||
|
b2Vec2 normal = b2Cross(tangent, 1.0f);
|
||||||
|
|
||||||
|
v11 = b2Mul(xf1, v11);
|
||||||
|
v12 = b2Mul(xf1, v12);
|
||||||
|
|
||||||
|
// Face offset.
|
||||||
|
float32 frontOffset = b2Dot(normal, v11);
|
||||||
|
|
||||||
|
// Side offsets, extended by polytope skin thickness.
|
||||||
|
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
|
||||||
|
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
|
||||||
|
|
||||||
|
// Clip incident edge against extruded edge1 side edges.
|
||||||
|
b2ClipVertex clipPoints1[2];
|
||||||
|
b2ClipVertex clipPoints2[2];
|
||||||
|
int np;
|
||||||
|
|
||||||
|
// Clip to box side 1
|
||||||
|
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
|
||||||
|
|
||||||
|
if (np < 2)
|
||||||
|
return;
|
||||||
|
|
||||||
|
// Clip to negative box side 1
|
||||||
|
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
|
||||||
|
|
||||||
|
if (np < 2)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Now clipPoints2 contains the clipped points.
|
||||||
|
manifold->localNormal = localNormal;
|
||||||
|
manifold->localPoint = planePoint;
|
||||||
|
|
||||||
|
int32 pointCount = 0;
|
||||||
|
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
|
||||||
|
{
|
||||||
|
float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
|
||||||
|
|
||||||
|
if (separation <= totalRadius)
|
||||||
|
{
|
||||||
|
b2ManifoldPoint* cp = manifold->points + pointCount;
|
||||||
|
cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
|
||||||
|
cp->id = clipPoints2[i].id;
|
||||||
|
if (flip)
|
||||||
|
{
|
||||||
|
// Swap features
|
||||||
|
b2ContactFeature cf = cp->id.cf;
|
||||||
|
cp->id.cf.indexA = cf.indexB;
|
||||||
|
cp->id.cf.indexB = cf.indexA;
|
||||||
|
cp->id.cf.typeA = cf.typeB;
|
||||||
|
cp->id.cf.typeB = cf.typeA;
|
||||||
|
}
|
||||||
|
++pointCount;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
manifold->pointCount = pointCount;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,252 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/b2Distance.h"
|
||||||
|
|
||||||
|
void b2WorldManifold::Initialize(const b2Manifold* manifold,
|
||||||
|
const b2Transform& xfA, float32 radiusA,
|
||||||
|
const b2Transform& xfB, float32 radiusB)
|
||||||
|
{
|
||||||
|
if (manifold->pointCount == 0)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (manifold->type)
|
||||||
|
{
|
||||||
|
case b2Manifold::e_circles:
|
||||||
|
{
|
||||||
|
normal.Set(1.0f, 0.0f);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
|
||||||
|
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
|
||||||
|
{
|
||||||
|
normal = pointB - pointA;
|
||||||
|
normal.Normalize();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 cA = pointA + radiusA * normal;
|
||||||
|
b2Vec2 cB = pointB - radiusB * normal;
|
||||||
|
points[0] = 0.5f * (cA + cB);
|
||||||
|
separations[0] = b2Dot(cB - cA, normal);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Manifold::e_faceA:
|
||||||
|
{
|
||||||
|
normal = b2Mul(xfA.q, manifold->localNormal);
|
||||||
|
b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
|
||||||
|
|
||||||
|
for (int32 i = 0; i < manifold->pointCount; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
|
||||||
|
b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
|
||||||
|
b2Vec2 cB = clipPoint - radiusB * normal;
|
||||||
|
points[i] = 0.5f * (cA + cB);
|
||||||
|
separations[i] = b2Dot(cB - cA, normal);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Manifold::e_faceB:
|
||||||
|
{
|
||||||
|
normal = b2Mul(xfB.q, manifold->localNormal);
|
||||||
|
b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
|
||||||
|
|
||||||
|
for (int32 i = 0; i < manifold->pointCount; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
|
||||||
|
b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
|
||||||
|
b2Vec2 cA = clipPoint - radiusA * normal;
|
||||||
|
points[i] = 0.5f * (cA + cB);
|
||||||
|
separations[i] = b2Dot(cA - cB, normal);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Ensure normal points from A to B.
|
||||||
|
normal = -normal;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
|
||||||
|
const b2Manifold* manifold1, const b2Manifold* manifold2)
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
|
||||||
|
{
|
||||||
|
state1[i] = b2_nullState;
|
||||||
|
state2[i] = b2_nullState;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Detect persists and removes.
|
||||||
|
for (int32 i = 0; i < manifold1->pointCount; ++i)
|
||||||
|
{
|
||||||
|
b2ContactID id = manifold1->points[i].id;
|
||||||
|
|
||||||
|
state1[i] = b2_removeState;
|
||||||
|
|
||||||
|
for (int32 j = 0; j < manifold2->pointCount; ++j)
|
||||||
|
{
|
||||||
|
if (manifold2->points[j].id.key == id.key)
|
||||||
|
{
|
||||||
|
state1[i] = b2_persistState;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Detect persists and adds.
|
||||||
|
for (int32 i = 0; i < manifold2->pointCount; ++i)
|
||||||
|
{
|
||||||
|
b2ContactID id = manifold2->points[i].id;
|
||||||
|
|
||||||
|
state2[i] = b2_addState;
|
||||||
|
|
||||||
|
for (int32 j = 0; j < manifold1->pointCount; ++j)
|
||||||
|
{
|
||||||
|
if (manifold1->points[j].id.key == id.key)
|
||||||
|
{
|
||||||
|
state2[i] = b2_persistState;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// From Real-time Collision Detection, p179.
|
||||||
|
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
|
||||||
|
{
|
||||||
|
float32 tmin = -b2_maxFloat;
|
||||||
|
float32 tmax = b2_maxFloat;
|
||||||
|
|
||||||
|
b2Vec2 p = input.p1;
|
||||||
|
b2Vec2 d = input.p2 - input.p1;
|
||||||
|
b2Vec2 absD = b2Abs(d);
|
||||||
|
|
||||||
|
b2Vec2 normal;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < 2; ++i)
|
||||||
|
{
|
||||||
|
if (absD(i) < b2_epsilon)
|
||||||
|
{
|
||||||
|
// Parallel.
|
||||||
|
if (p(i) < lowerBound(i) || upperBound(i) < p(i))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
float32 inv_d = 1.0f / d(i);
|
||||||
|
float32 t1 = (lowerBound(i) - p(i)) * inv_d;
|
||||||
|
float32 t2 = (upperBound(i) - p(i)) * inv_d;
|
||||||
|
|
||||||
|
// Sign of the normal vector.
|
||||||
|
float32 s = -1.0f;
|
||||||
|
|
||||||
|
if (t1 > t2)
|
||||||
|
{
|
||||||
|
b2Swap(t1, t2);
|
||||||
|
s = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Push the min up
|
||||||
|
if (t1 > tmin)
|
||||||
|
{
|
||||||
|
normal.SetZero();
|
||||||
|
normal(i) = s;
|
||||||
|
tmin = t1;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Pull the max down
|
||||||
|
tmax = b2Min(tmax, t2);
|
||||||
|
|
||||||
|
if (tmin > tmax)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Does the ray start inside the box?
|
||||||
|
// Does the ray intersect beyond the max fraction?
|
||||||
|
if (tmin < 0.0f || input.maxFraction < tmin)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Intersection.
|
||||||
|
output->fraction = tmin;
|
||||||
|
output->normal = normal;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Sutherland-Hodgman clipping.
|
||||||
|
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
|
||||||
|
const b2Vec2& normal, float32 offset, int32 vertexIndexA)
|
||||||
|
{
|
||||||
|
// Start with no output points
|
||||||
|
int32 numOut = 0;
|
||||||
|
|
||||||
|
// Calculate the distance of end points to the line
|
||||||
|
float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
|
||||||
|
float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
|
||||||
|
|
||||||
|
// If the points are behind the plane
|
||||||
|
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
|
||||||
|
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
|
||||||
|
|
||||||
|
// If the points are on different sides of the plane
|
||||||
|
if (distance0 * distance1 < 0.0f)
|
||||||
|
{
|
||||||
|
// Find intersection point of edge and plane
|
||||||
|
float32 interp = distance0 / (distance0 - distance1);
|
||||||
|
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
|
||||||
|
|
||||||
|
// VertexA is hitting edgeB.
|
||||||
|
vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
|
||||||
|
vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
|
||||||
|
vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
|
||||||
|
vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
|
||||||
|
++numOut;
|
||||||
|
}
|
||||||
|
|
||||||
|
return numOut;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,
|
||||||
|
const b2Shape* shapeB, int32 indexB,
|
||||||
|
const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2DistanceInput input;
|
||||||
|
input.proxyA.Set(shapeA, indexA);
|
||||||
|
input.proxyB.Set(shapeB, indexB);
|
||||||
|
input.transformA = xfA;
|
||||||
|
input.transformB = xfB;
|
||||||
|
input.useRadii = true;
|
||||||
|
|
||||||
|
b2SimplexCache cache;
|
||||||
|
cache.count = 0;
|
||||||
|
|
||||||
|
b2DistanceOutput output;
|
||||||
|
|
||||||
|
b2Distance(&output, &cache, &input);
|
||||||
|
|
||||||
|
return output.distance < 10.0f * b2_epsilon;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,277 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_COLLISION_H
|
||||||
|
#define B2_COLLISION_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
#include <limits.h>
|
||||||
|
|
||||||
|
/// @file
|
||||||
|
/// Structures and functions used for computing contact points, distance
|
||||||
|
/// queries, and TOI queries.
|
||||||
|
|
||||||
|
class b2Shape;
|
||||||
|
class b2CircleShape;
|
||||||
|
class b2EdgeShape;
|
||||||
|
class b2PolygonShape;
|
||||||
|
|
||||||
|
const uint8 b2_nullFeature = UCHAR_MAX;
|
||||||
|
|
||||||
|
/// The features that intersect to form the contact point
|
||||||
|
/// This must be 4 bytes or less.
|
||||||
|
struct b2ContactFeature
|
||||||
|
{
|
||||||
|
enum Type
|
||||||
|
{
|
||||||
|
e_vertex = 0,
|
||||||
|
e_face = 1
|
||||||
|
};
|
||||||
|
|
||||||
|
uint8 indexA; ///< Feature index on shapeA
|
||||||
|
uint8 indexB; ///< Feature index on shapeB
|
||||||
|
uint8 typeA; ///< The feature type on shapeA
|
||||||
|
uint8 typeB; ///< The feature type on shapeB
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Contact ids to facilitate warm starting.
|
||||||
|
union b2ContactID
|
||||||
|
{
|
||||||
|
b2ContactFeature cf;
|
||||||
|
uint32 key; ///< Used to quickly compare contact ids.
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A manifold point is a contact point belonging to a contact
|
||||||
|
/// manifold. It holds details related to the geometry and dynamics
|
||||||
|
/// of the contact points.
|
||||||
|
/// The local point usage depends on the manifold type:
|
||||||
|
/// -e_circles: the local center of circleB
|
||||||
|
/// -e_faceA: the local center of cirlceB or the clip point of polygonB
|
||||||
|
/// -e_faceB: the clip point of polygonA
|
||||||
|
/// This structure is stored across time steps, so we keep it small.
|
||||||
|
/// Note: the impulses are used for internal caching and may not
|
||||||
|
/// provide reliable contact forces, especially for high speed collisions.
|
||||||
|
struct b2ManifoldPoint
|
||||||
|
{
|
||||||
|
b2Vec2 localPoint; ///< usage depends on manifold type
|
||||||
|
float32 normalImpulse; ///< the non-penetration impulse
|
||||||
|
float32 tangentImpulse; ///< the friction impulse
|
||||||
|
b2ContactID id; ///< uniquely identifies a contact point between two shapes
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A manifold for two touching convex shapes.
|
||||||
|
/// Box2D supports multiple types of contact:
|
||||||
|
/// - clip point versus plane with radius
|
||||||
|
/// - point versus point with radius (circles)
|
||||||
|
/// The local point usage depends on the manifold type:
|
||||||
|
/// -e_circles: the local center of circleA
|
||||||
|
/// -e_faceA: the center of faceA
|
||||||
|
/// -e_faceB: the center of faceB
|
||||||
|
/// Similarly the local normal usage:
|
||||||
|
/// -e_circles: not used
|
||||||
|
/// -e_faceA: the normal on polygonA
|
||||||
|
/// -e_faceB: the normal on polygonB
|
||||||
|
/// We store contacts in this way so that position correction can
|
||||||
|
/// account for movement, which is critical for continuous physics.
|
||||||
|
/// All contact scenarios must be expressed in one of these types.
|
||||||
|
/// This structure is stored across time steps, so we keep it small.
|
||||||
|
struct b2Manifold
|
||||||
|
{
|
||||||
|
enum Type
|
||||||
|
{
|
||||||
|
e_circles,
|
||||||
|
e_faceA,
|
||||||
|
e_faceB
|
||||||
|
};
|
||||||
|
|
||||||
|
b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact
|
||||||
|
b2Vec2 localNormal; ///< not use for Type::e_points
|
||||||
|
b2Vec2 localPoint; ///< usage depends on manifold type
|
||||||
|
Type type;
|
||||||
|
int32 pointCount; ///< the number of manifold points
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is used to compute the current state of a contact manifold.
|
||||||
|
struct b2WorldManifold
|
||||||
|
{
|
||||||
|
/// Evaluate the manifold with supplied transforms. This assumes
|
||||||
|
/// modest motion from the original state. This does not change the
|
||||||
|
/// point count, impulses, etc. The radii must come from the shapes
|
||||||
|
/// that generated the manifold.
|
||||||
|
void Initialize(const b2Manifold* manifold,
|
||||||
|
const b2Transform& xfA, float32 radiusA,
|
||||||
|
const b2Transform& xfB, float32 radiusB);
|
||||||
|
|
||||||
|
b2Vec2 normal; ///< world vector pointing from A to B
|
||||||
|
b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection)
|
||||||
|
float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is used for determining the state of contact points.
|
||||||
|
enum b2PointState
|
||||||
|
{
|
||||||
|
b2_nullState, ///< point does not exist
|
||||||
|
b2_addState, ///< point was added in the update
|
||||||
|
b2_persistState, ///< point persisted across the update
|
||||||
|
b2_removeState ///< point was removed in the update
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
|
||||||
|
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
|
||||||
|
void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
|
||||||
|
const b2Manifold* manifold1, const b2Manifold* manifold2);
|
||||||
|
|
||||||
|
/// Used for computing contact manifolds.
|
||||||
|
struct b2ClipVertex
|
||||||
|
{
|
||||||
|
b2Vec2 v;
|
||||||
|
b2ContactID id;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
|
||||||
|
struct b2RayCastInput
|
||||||
|
{
|
||||||
|
b2Vec2 p1, p2;
|
||||||
|
float32 maxFraction;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
|
||||||
|
/// come from b2RayCastInput.
|
||||||
|
struct b2RayCastOutput
|
||||||
|
{
|
||||||
|
b2Vec2 normal;
|
||||||
|
float32 fraction;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// An axis aligned bounding box.
|
||||||
|
struct b2AABB
|
||||||
|
{
|
||||||
|
/// Verify that the bounds are sorted.
|
||||||
|
bool IsValid() const;
|
||||||
|
|
||||||
|
/// Get the center of the AABB.
|
||||||
|
b2Vec2 GetCenter() const
|
||||||
|
{
|
||||||
|
return 0.5f * (lowerBound + upperBound);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the extents of the AABB (half-widths).
|
||||||
|
b2Vec2 GetExtents() const
|
||||||
|
{
|
||||||
|
return 0.5f * (upperBound - lowerBound);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the perimeter length
|
||||||
|
float32 GetPerimeter() const
|
||||||
|
{
|
||||||
|
float32 wx = upperBound.x - lowerBound.x;
|
||||||
|
float32 wy = upperBound.y - lowerBound.y;
|
||||||
|
return 2.0f * (wx + wy);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Combine an AABB into this one.
|
||||||
|
void Combine(const b2AABB& aabb)
|
||||||
|
{
|
||||||
|
lowerBound = b2Min(lowerBound, aabb.lowerBound);
|
||||||
|
upperBound = b2Max(upperBound, aabb.upperBound);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Combine two AABBs into this one.
|
||||||
|
void Combine(const b2AABB& aabb1, const b2AABB& aabb2)
|
||||||
|
{
|
||||||
|
lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
|
||||||
|
upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Does this aabb contain the provided AABB.
|
||||||
|
bool Contains(const b2AABB& aabb) const
|
||||||
|
{
|
||||||
|
bool result = true;
|
||||||
|
result = result && lowerBound.x <= aabb.lowerBound.x;
|
||||||
|
result = result && lowerBound.y <= aabb.lowerBound.y;
|
||||||
|
result = result && aabb.upperBound.x <= upperBound.x;
|
||||||
|
result = result && aabb.upperBound.y <= upperBound.y;
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
|
||||||
|
|
||||||
|
b2Vec2 lowerBound; ///< the lower vertex
|
||||||
|
b2Vec2 upperBound; ///< the upper vertex
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Compute the collision manifold between two circles.
|
||||||
|
void b2CollideCircles(b2Manifold* manifold,
|
||||||
|
const b2CircleShape* circleA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB);
|
||||||
|
|
||||||
|
/// Compute the collision manifold between a polygon and a circle.
|
||||||
|
void b2CollidePolygonAndCircle(b2Manifold* manifold,
|
||||||
|
const b2PolygonShape* polygonA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB);
|
||||||
|
|
||||||
|
/// Compute the collision manifold between two polygons.
|
||||||
|
void b2CollidePolygons(b2Manifold* manifold,
|
||||||
|
const b2PolygonShape* polygonA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* polygonB, const b2Transform& xfB);
|
||||||
|
|
||||||
|
/// Compute the collision manifold between an edge and a circle.
|
||||||
|
void b2CollideEdgeAndCircle(b2Manifold* manifold,
|
||||||
|
const b2EdgeShape* polygonA, const b2Transform& xfA,
|
||||||
|
const b2CircleShape* circleB, const b2Transform& xfB);
|
||||||
|
|
||||||
|
/// Compute the collision manifold between an edge and a circle.
|
||||||
|
void b2CollideEdgeAndPolygon(b2Manifold* manifold,
|
||||||
|
const b2EdgeShape* edgeA, const b2Transform& xfA,
|
||||||
|
const b2PolygonShape* circleB, const b2Transform& xfB);
|
||||||
|
|
||||||
|
/// Clipping for contact manifolds.
|
||||||
|
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
|
||||||
|
const b2Vec2& normal, float32 offset, int32 vertexIndexA);
|
||||||
|
|
||||||
|
/// Determine if two generic shapes overlap.
|
||||||
|
bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,
|
||||||
|
const b2Shape* shapeB, int32 indexB,
|
||||||
|
const b2Transform& xfA, const b2Transform& xfB);
|
||||||
|
|
||||||
|
// ---------------- Inline Functions ------------------------------------------
|
||||||
|
|
||||||
|
inline bool b2AABB::IsValid() const
|
||||||
|
{
|
||||||
|
b2Vec2 d = upperBound - lowerBound;
|
||||||
|
bool valid = d.x >= 0.0f && d.y >= 0.0f;
|
||||||
|
valid = valid && lowerBound.IsValid() && upperBound.IsValid();
|
||||||
|
return valid;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)
|
||||||
|
{
|
||||||
|
b2Vec2 d1, d2;
|
||||||
|
d1 = b.lowerBound - a.upperBound;
|
||||||
|
d2 = a.lowerBound - b.upperBound;
|
||||||
|
|
||||||
|
if (d1.x > 0.0f || d1.y > 0.0f)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
if (d2.x > 0.0f || d2.y > 0.0f)
|
||||||
|
return false;
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,737 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Distance.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
|
||||||
|
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
|
||||||
|
int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
|
||||||
|
|
||||||
|
void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
|
||||||
|
{
|
||||||
|
switch (shape->GetType())
|
||||||
|
{
|
||||||
|
case b2Shape::e_circle:
|
||||||
|
{
|
||||||
|
const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
|
||||||
|
m_vertices = &circle->m_p;
|
||||||
|
m_count = 1;
|
||||||
|
m_radius = circle->m_radius;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_polygon:
|
||||||
|
{
|
||||||
|
const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
|
||||||
|
m_vertices = polygon->m_vertices;
|
||||||
|
m_count = polygon->m_count;
|
||||||
|
m_radius = polygon->m_radius;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_chain:
|
||||||
|
{
|
||||||
|
const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
|
||||||
|
b2Assert(0 <= index && index < chain->m_count);
|
||||||
|
|
||||||
|
m_buffer[0] = chain->m_vertices[index];
|
||||||
|
if (index + 1 < chain->m_count)
|
||||||
|
{
|
||||||
|
m_buffer[1] = chain->m_vertices[index + 1];
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_buffer[1] = chain->m_vertices[0];
|
||||||
|
}
|
||||||
|
|
||||||
|
m_vertices = m_buffer;
|
||||||
|
m_count = 2;
|
||||||
|
m_radius = chain->m_radius;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_edge:
|
||||||
|
{
|
||||||
|
const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
|
||||||
|
m_vertices = &edge->m_vertex1;
|
||||||
|
m_count = 2;
|
||||||
|
m_radius = edge->m_radius;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DistanceProxy::Set(const b2Vec2* vertices, int32 count, float32 radius)
|
||||||
|
{
|
||||||
|
m_vertices = vertices;
|
||||||
|
m_count = count;
|
||||||
|
m_radius = radius;
|
||||||
|
}
|
||||||
|
|
||||||
|
struct b2SimplexVertex
|
||||||
|
{
|
||||||
|
b2Vec2 wA; // support point in proxyA
|
||||||
|
b2Vec2 wB; // support point in proxyB
|
||||||
|
b2Vec2 w; // wB - wA
|
||||||
|
float32 a; // barycentric coordinate for closest point
|
||||||
|
int32 indexA; // wA index
|
||||||
|
int32 indexB; // wB index
|
||||||
|
};
|
||||||
|
|
||||||
|
struct b2Simplex
|
||||||
|
{
|
||||||
|
void ReadCache( const b2SimplexCache* cache,
|
||||||
|
const b2DistanceProxy* proxyA, const b2Transform& transformA,
|
||||||
|
const b2DistanceProxy* proxyB, const b2Transform& transformB)
|
||||||
|
{
|
||||||
|
b2Assert(cache->count <= 3);
|
||||||
|
|
||||||
|
// Copy data from cache.
|
||||||
|
m_count = cache->count;
|
||||||
|
b2SimplexVertex* vertices = &m_v1;
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2SimplexVertex* v = vertices + i;
|
||||||
|
v->indexA = cache->indexA[i];
|
||||||
|
v->indexB = cache->indexB[i];
|
||||||
|
b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
|
||||||
|
b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
|
||||||
|
v->wA = b2Mul(transformA, wALocal);
|
||||||
|
v->wB = b2Mul(transformB, wBLocal);
|
||||||
|
v->w = v->wB - v->wA;
|
||||||
|
v->a = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the new simplex metric, if it is substantially different than
|
||||||
|
// old metric then flush the simplex.
|
||||||
|
if (m_count > 1)
|
||||||
|
{
|
||||||
|
float32 metric1 = cache->metric;
|
||||||
|
float32 metric2 = GetMetric();
|
||||||
|
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
|
||||||
|
{
|
||||||
|
// Reset the simplex.
|
||||||
|
m_count = 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If the cache is empty or invalid ...
|
||||||
|
if (m_count == 0)
|
||||||
|
{
|
||||||
|
b2SimplexVertex* v = vertices + 0;
|
||||||
|
v->indexA = 0;
|
||||||
|
v->indexB = 0;
|
||||||
|
b2Vec2 wALocal = proxyA->GetVertex(0);
|
||||||
|
b2Vec2 wBLocal = proxyB->GetVertex(0);
|
||||||
|
v->wA = b2Mul(transformA, wALocal);
|
||||||
|
v->wB = b2Mul(transformB, wBLocal);
|
||||||
|
v->w = v->wB - v->wA;
|
||||||
|
v->a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void WriteCache(b2SimplexCache* cache) const
|
||||||
|
{
|
||||||
|
cache->metric = GetMetric();
|
||||||
|
cache->count = uint16(m_count);
|
||||||
|
const b2SimplexVertex* vertices = &m_v1;
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
cache->indexA[i] = uint8(vertices[i].indexA);
|
||||||
|
cache->indexB[i] = uint8(vertices[i].indexB);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 GetSearchDirection() const
|
||||||
|
{
|
||||||
|
switch (m_count)
|
||||||
|
{
|
||||||
|
case 1:
|
||||||
|
return -m_v1.w;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
{
|
||||||
|
b2Vec2 e12 = m_v2.w - m_v1.w;
|
||||||
|
float32 sgn = b2Cross(e12, -m_v1.w);
|
||||||
|
if (sgn > 0.0f)
|
||||||
|
{
|
||||||
|
// Origin is left of e12.
|
||||||
|
return b2Cross(1.0f, e12);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Origin is right of e12.
|
||||||
|
return b2Cross(e12, 1.0f);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
return b2Vec2_zero;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 GetClosestPoint() const
|
||||||
|
{
|
||||||
|
switch (m_count)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
b2Assert(false);
|
||||||
|
return b2Vec2_zero;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
return m_v1.w;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
|
||||||
|
|
||||||
|
case 3:
|
||||||
|
return b2Vec2_zero;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
return b2Vec2_zero;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
|
||||||
|
{
|
||||||
|
switch (m_count)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
b2Assert(false);
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
*pA = m_v1.wA;
|
||||||
|
*pB = m_v1.wB;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
|
||||||
|
*pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3:
|
||||||
|
*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
|
||||||
|
*pB = *pA;
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 GetMetric() const
|
||||||
|
{
|
||||||
|
switch (m_count)
|
||||||
|
{
|
||||||
|
case 0:
|
||||||
|
b2Assert(false);
|
||||||
|
return 0.0f;
|
||||||
|
|
||||||
|
case 1:
|
||||||
|
return 0.0f;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
return b2Distance(m_v1.w, m_v2.w);
|
||||||
|
|
||||||
|
case 3:
|
||||||
|
return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Solve2();
|
||||||
|
void Solve3();
|
||||||
|
|
||||||
|
b2SimplexVertex m_v1, m_v2, m_v3;
|
||||||
|
int32 m_count;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
// Solve a line segment using barycentric coordinates.
|
||||||
|
//
|
||||||
|
// p = a1 * w1 + a2 * w2
|
||||||
|
// a1 + a2 = 1
|
||||||
|
//
|
||||||
|
// The vector from the origin to the closest point on the line is
|
||||||
|
// perpendicular to the line.
|
||||||
|
// e12 = w2 - w1
|
||||||
|
// dot(p, e) = 0
|
||||||
|
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
|
||||||
|
//
|
||||||
|
// 2-by-2 linear system
|
||||||
|
// [1 1 ][a1] = [1]
|
||||||
|
// [w1.e12 w2.e12][a2] = [0]
|
||||||
|
//
|
||||||
|
// Define
|
||||||
|
// d12_1 = dot(w2, e12)
|
||||||
|
// d12_2 = -dot(w1, e12)
|
||||||
|
// d12 = d12_1 + d12_2
|
||||||
|
//
|
||||||
|
// Solution
|
||||||
|
// a1 = d12_1 / d12
|
||||||
|
// a2 = d12_2 / d12
|
||||||
|
void b2Simplex::Solve2()
|
||||||
|
{
|
||||||
|
b2Vec2 w1 = m_v1.w;
|
||||||
|
b2Vec2 w2 = m_v2.w;
|
||||||
|
b2Vec2 e12 = w2 - w1;
|
||||||
|
|
||||||
|
// w1 region
|
||||||
|
float32 d12_2 = -b2Dot(w1, e12);
|
||||||
|
if (d12_2 <= 0.0f)
|
||||||
|
{
|
||||||
|
// a2 <= 0, so we clamp it to 0
|
||||||
|
m_v1.a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// w2 region
|
||||||
|
float32 d12_1 = b2Dot(w2, e12);
|
||||||
|
if (d12_1 <= 0.0f)
|
||||||
|
{
|
||||||
|
// a1 <= 0, so we clamp it to 0
|
||||||
|
m_v2.a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
m_v1 = m_v2;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Must be in e12 region.
|
||||||
|
float32 inv_d12 = 1.0f / (d12_1 + d12_2);
|
||||||
|
m_v1.a = d12_1 * inv_d12;
|
||||||
|
m_v2.a = d12_2 * inv_d12;
|
||||||
|
m_count = 2;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Possible regions:
|
||||||
|
// - points[2]
|
||||||
|
// - edge points[0]-points[2]
|
||||||
|
// - edge points[1]-points[2]
|
||||||
|
// - inside the triangle
|
||||||
|
void b2Simplex::Solve3()
|
||||||
|
{
|
||||||
|
b2Vec2 w1 = m_v1.w;
|
||||||
|
b2Vec2 w2 = m_v2.w;
|
||||||
|
b2Vec2 w3 = m_v3.w;
|
||||||
|
|
||||||
|
// Edge12
|
||||||
|
// [1 1 ][a1] = [1]
|
||||||
|
// [w1.e12 w2.e12][a2] = [0]
|
||||||
|
// a3 = 0
|
||||||
|
b2Vec2 e12 = w2 - w1;
|
||||||
|
float32 w1e12 = b2Dot(w1, e12);
|
||||||
|
float32 w2e12 = b2Dot(w2, e12);
|
||||||
|
float32 d12_1 = w2e12;
|
||||||
|
float32 d12_2 = -w1e12;
|
||||||
|
|
||||||
|
// Edge13
|
||||||
|
// [1 1 ][a1] = [1]
|
||||||
|
// [w1.e13 w3.e13][a3] = [0]
|
||||||
|
// a2 = 0
|
||||||
|
b2Vec2 e13 = w3 - w1;
|
||||||
|
float32 w1e13 = b2Dot(w1, e13);
|
||||||
|
float32 w3e13 = b2Dot(w3, e13);
|
||||||
|
float32 d13_1 = w3e13;
|
||||||
|
float32 d13_2 = -w1e13;
|
||||||
|
|
||||||
|
// Edge23
|
||||||
|
// [1 1 ][a2] = [1]
|
||||||
|
// [w2.e23 w3.e23][a3] = [0]
|
||||||
|
// a1 = 0
|
||||||
|
b2Vec2 e23 = w3 - w2;
|
||||||
|
float32 w2e23 = b2Dot(w2, e23);
|
||||||
|
float32 w3e23 = b2Dot(w3, e23);
|
||||||
|
float32 d23_1 = w3e23;
|
||||||
|
float32 d23_2 = -w2e23;
|
||||||
|
|
||||||
|
// Triangle123
|
||||||
|
float32 n123 = b2Cross(e12, e13);
|
||||||
|
|
||||||
|
float32 d123_1 = n123 * b2Cross(w2, w3);
|
||||||
|
float32 d123_2 = n123 * b2Cross(w3, w1);
|
||||||
|
float32 d123_3 = n123 * b2Cross(w1, w2);
|
||||||
|
|
||||||
|
// w1 region
|
||||||
|
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
|
||||||
|
{
|
||||||
|
m_v1.a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// e12
|
||||||
|
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
|
||||||
|
{
|
||||||
|
float32 inv_d12 = 1.0f / (d12_1 + d12_2);
|
||||||
|
m_v1.a = d12_1 * inv_d12;
|
||||||
|
m_v2.a = d12_2 * inv_d12;
|
||||||
|
m_count = 2;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// e13
|
||||||
|
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
|
||||||
|
{
|
||||||
|
float32 inv_d13 = 1.0f / (d13_1 + d13_2);
|
||||||
|
m_v1.a = d13_1 * inv_d13;
|
||||||
|
m_v3.a = d13_2 * inv_d13;
|
||||||
|
m_count = 2;
|
||||||
|
m_v2 = m_v3;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// w2 region
|
||||||
|
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
|
||||||
|
{
|
||||||
|
m_v2.a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
m_v1 = m_v2;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// w3 region
|
||||||
|
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
|
||||||
|
{
|
||||||
|
m_v3.a = 1.0f;
|
||||||
|
m_count = 1;
|
||||||
|
m_v1 = m_v3;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// e23
|
||||||
|
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
|
||||||
|
{
|
||||||
|
float32 inv_d23 = 1.0f / (d23_1 + d23_2);
|
||||||
|
m_v2.a = d23_1 * inv_d23;
|
||||||
|
m_v3.a = d23_2 * inv_d23;
|
||||||
|
m_count = 2;
|
||||||
|
m_v1 = m_v3;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Must be in triangle123
|
||||||
|
float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
|
||||||
|
m_v1.a = d123_1 * inv_d123;
|
||||||
|
m_v2.a = d123_2 * inv_d123;
|
||||||
|
m_v3.a = d123_3 * inv_d123;
|
||||||
|
m_count = 3;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Distance(b2DistanceOutput* output,
|
||||||
|
b2SimplexCache* cache,
|
||||||
|
const b2DistanceInput* input)
|
||||||
|
{
|
||||||
|
++b2_gjkCalls;
|
||||||
|
|
||||||
|
const b2DistanceProxy* proxyA = &input->proxyA;
|
||||||
|
const b2DistanceProxy* proxyB = &input->proxyB;
|
||||||
|
|
||||||
|
b2Transform transformA = input->transformA;
|
||||||
|
b2Transform transformB = input->transformB;
|
||||||
|
|
||||||
|
// Initialize the simplex.
|
||||||
|
b2Simplex simplex;
|
||||||
|
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
|
||||||
|
|
||||||
|
// Get simplex vertices as an array.
|
||||||
|
b2SimplexVertex* vertices = &simplex.m_v1;
|
||||||
|
const int32 k_maxIters = 20;
|
||||||
|
|
||||||
|
// These store the vertices of the last simplex so that we
|
||||||
|
// can check for duplicates and prevent cycling.
|
||||||
|
int32 saveA[3], saveB[3];
|
||||||
|
int32 saveCount = 0;
|
||||||
|
|
||||||
|
// Main iteration loop.
|
||||||
|
int32 iter = 0;
|
||||||
|
while (iter < k_maxIters)
|
||||||
|
{
|
||||||
|
// Copy simplex so we can identify duplicates.
|
||||||
|
saveCount = simplex.m_count;
|
||||||
|
for (int32 i = 0; i < saveCount; ++i)
|
||||||
|
{
|
||||||
|
saveA[i] = vertices[i].indexA;
|
||||||
|
saveB[i] = vertices[i].indexB;
|
||||||
|
}
|
||||||
|
|
||||||
|
switch (simplex.m_count)
|
||||||
|
{
|
||||||
|
case 1:
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
simplex.Solve2();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3:
|
||||||
|
simplex.Solve3();
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we have 3 points, then the origin is in the corresponding triangle.
|
||||||
|
if (simplex.m_count == 3)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get search direction.
|
||||||
|
b2Vec2 d = simplex.GetSearchDirection();
|
||||||
|
|
||||||
|
// Ensure the search direction is numerically fit.
|
||||||
|
if (d.LengthSquared() < b2_epsilon * b2_epsilon)
|
||||||
|
{
|
||||||
|
// The origin is probably contained by a line segment
|
||||||
|
// or triangle. Thus the shapes are overlapped.
|
||||||
|
|
||||||
|
// We can't return zero here even though there may be overlap.
|
||||||
|
// In case the simplex is a point, segment, or triangle it is difficult
|
||||||
|
// to determine if the origin is contained in the CSO or very close to it.
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute a tentative new simplex vertex using support points.
|
||||||
|
b2SimplexVertex* vertex = vertices + simplex.m_count;
|
||||||
|
vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
|
||||||
|
vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
|
||||||
|
b2Vec2 wBLocal;
|
||||||
|
vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
|
||||||
|
vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
|
||||||
|
vertex->w = vertex->wB - vertex->wA;
|
||||||
|
|
||||||
|
// Iteration count is equated to the number of support point calls.
|
||||||
|
++iter;
|
||||||
|
++b2_gjkIters;
|
||||||
|
|
||||||
|
// Check for duplicate support points. This is the main termination criteria.
|
||||||
|
bool duplicate = false;
|
||||||
|
for (int32 i = 0; i < saveCount; ++i)
|
||||||
|
{
|
||||||
|
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
|
||||||
|
{
|
||||||
|
duplicate = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we found a duplicate support point we must exit to avoid cycling.
|
||||||
|
if (duplicate)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// New vertex is ok and needed.
|
||||||
|
++simplex.m_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
|
||||||
|
|
||||||
|
// Prepare output.
|
||||||
|
simplex.GetWitnessPoints(&output->pointA, &output->pointB);
|
||||||
|
output->distance = b2Distance(output->pointA, output->pointB);
|
||||||
|
output->iterations = iter;
|
||||||
|
|
||||||
|
// Cache the simplex.
|
||||||
|
simplex.WriteCache(cache);
|
||||||
|
|
||||||
|
// Apply radii if requested.
|
||||||
|
if (input->useRadii)
|
||||||
|
{
|
||||||
|
float32 rA = proxyA->m_radius;
|
||||||
|
float32 rB = proxyB->m_radius;
|
||||||
|
|
||||||
|
if (output->distance > rA + rB && output->distance > b2_epsilon)
|
||||||
|
{
|
||||||
|
// Shapes are still no overlapped.
|
||||||
|
// Move the witness points to the outer surface.
|
||||||
|
output->distance -= rA + rB;
|
||||||
|
b2Vec2 normal = output->pointB - output->pointA;
|
||||||
|
normal.Normalize();
|
||||||
|
output->pointA += rA * normal;
|
||||||
|
output->pointB -= rB * normal;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Shapes are overlapped when radii are considered.
|
||||||
|
// Move the witness points to the middle.
|
||||||
|
b2Vec2 p = 0.5f * (output->pointA + output->pointB);
|
||||||
|
output->pointA = p;
|
||||||
|
output->pointB = p;
|
||||||
|
output->distance = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// GJK-raycast
|
||||||
|
// Algorithm by Gino van den Bergen.
|
||||||
|
// "Smooth Mesh Contacts with GJK" in Game Physics Pearls. 2010
|
||||||
|
bool b2ShapeCast(b2ShapeCastOutput * output, const b2ShapeCastInput * input)
|
||||||
|
{
|
||||||
|
output->iterations = 0;
|
||||||
|
output->lambda = 1.0f;
|
||||||
|
output->normal.SetZero();
|
||||||
|
output->point.SetZero();
|
||||||
|
|
||||||
|
const b2DistanceProxy* proxyA = &input->proxyA;
|
||||||
|
const b2DistanceProxy* proxyB = &input->proxyB;
|
||||||
|
|
||||||
|
float32 radiusA = b2Max(proxyA->m_radius, b2_polygonRadius);
|
||||||
|
float32 radiusB = b2Max(proxyB->m_radius, b2_polygonRadius);
|
||||||
|
float32 radius = radiusA + radiusB;
|
||||||
|
|
||||||
|
b2Transform xfA = input->transformA;
|
||||||
|
b2Transform xfB = input->transformB;
|
||||||
|
|
||||||
|
b2Vec2 r = input->translationB;
|
||||||
|
b2Vec2 n(0.0f, 0.0f);
|
||||||
|
float32 lambda = 0.0f;
|
||||||
|
|
||||||
|
// Initial simplex
|
||||||
|
b2Simplex simplex;
|
||||||
|
simplex.m_count = 0;
|
||||||
|
|
||||||
|
// Get simplex vertices as an array.
|
||||||
|
b2SimplexVertex* vertices = &simplex.m_v1;
|
||||||
|
|
||||||
|
// Get support point in -r direction
|
||||||
|
int32 indexA = proxyA->GetSupport(b2MulT(xfA.q, -r));
|
||||||
|
b2Vec2 wA = b2Mul(xfA, proxyA->GetVertex(indexA));
|
||||||
|
int32 indexB = proxyB->GetSupport(b2MulT(xfB.q, r));
|
||||||
|
b2Vec2 wB = b2Mul(xfB, proxyB->GetVertex(indexB));
|
||||||
|
b2Vec2 v = wA - wB;
|
||||||
|
|
||||||
|
// Sigma is the target distance between polygons
|
||||||
|
float32 sigma = b2Max(b2_polygonRadius, radius - b2_polygonRadius);
|
||||||
|
const float32 tolerance = 0.5f * b2_linearSlop;
|
||||||
|
|
||||||
|
// Main iteration loop.
|
||||||
|
const int32 k_maxIters = 20;
|
||||||
|
int32 iter = 0;
|
||||||
|
while (iter < k_maxIters && b2Abs(v.Length() - sigma) > tolerance)
|
||||||
|
{
|
||||||
|
b2Assert(simplex.m_count < 3);
|
||||||
|
|
||||||
|
output->iterations += 1;
|
||||||
|
|
||||||
|
// Support in direction -v (A - B)
|
||||||
|
indexA = proxyA->GetSupport(b2MulT(xfA.q, -v));
|
||||||
|
wA = b2Mul(xfA, proxyA->GetVertex(indexA));
|
||||||
|
indexB = proxyB->GetSupport(b2MulT(xfB.q, v));
|
||||||
|
wB = b2Mul(xfB, proxyB->GetVertex(indexB));
|
||||||
|
b2Vec2 p = wA - wB;
|
||||||
|
|
||||||
|
// -v is a normal at p
|
||||||
|
v.Normalize();
|
||||||
|
|
||||||
|
// Intersect ray with plane
|
||||||
|
float32 vp = b2Dot(v, p);
|
||||||
|
float32 vr = b2Dot(v, r);
|
||||||
|
if (vp - sigma > lambda * vr)
|
||||||
|
{
|
||||||
|
if (vr <= 0.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
lambda = (vp - sigma) / vr;
|
||||||
|
if (lambda > 1.0f)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
n = -v;
|
||||||
|
simplex.m_count = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Reverse simplex since it works with B - A.
|
||||||
|
// Shift by lambda * r because we want the closest point to the current clip point.
|
||||||
|
// Note that the support point p is not shifted because we want the plane equation
|
||||||
|
// to be formed in unshifted space.
|
||||||
|
b2SimplexVertex* vertex = vertices + simplex.m_count;
|
||||||
|
vertex->indexA = indexB;
|
||||||
|
vertex->wA = wB + lambda * r;
|
||||||
|
vertex->indexB = indexA;
|
||||||
|
vertex->wB = wA;
|
||||||
|
vertex->w = vertex->wB - vertex->wA;
|
||||||
|
vertex->a = 1.0f;
|
||||||
|
simplex.m_count += 1;
|
||||||
|
|
||||||
|
switch (simplex.m_count)
|
||||||
|
{
|
||||||
|
case 1:
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 2:
|
||||||
|
simplex.Solve2();
|
||||||
|
break;
|
||||||
|
|
||||||
|
case 3:
|
||||||
|
simplex.Solve3();
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we have 3 points, then the origin is in the corresponding triangle.
|
||||||
|
if (simplex.m_count == 3)
|
||||||
|
{
|
||||||
|
// Overlap
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Get search direction.
|
||||||
|
v = simplex.GetClosestPoint();
|
||||||
|
|
||||||
|
// Iteration count is equated to the number of support point calls.
|
||||||
|
++iter;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Prepare output.
|
||||||
|
b2Vec2 pointA, pointB;
|
||||||
|
simplex.GetWitnessPoints(&pointB, &pointA);
|
||||||
|
|
||||||
|
if (v.LengthSquared() > 0.0f)
|
||||||
|
{
|
||||||
|
n = -v;
|
||||||
|
n.Normalize();
|
||||||
|
}
|
||||||
|
|
||||||
|
output->point = pointA + radiusA * n;
|
||||||
|
output->normal = n;
|
||||||
|
output->lambda = lambda;
|
||||||
|
output->iterations = iter;
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,166 @@
|
||||||
|
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_DISTANCE_H
|
||||||
|
#define B2_DISTANCE_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
|
||||||
|
class b2Shape;
|
||||||
|
|
||||||
|
/// A distance proxy is used by the GJK algorithm.
|
||||||
|
/// It encapsulates any shape.
|
||||||
|
struct b2DistanceProxy
|
||||||
|
{
|
||||||
|
b2DistanceProxy() : m_vertices(nullptr), m_count(0), m_radius(0.0f) {}
|
||||||
|
|
||||||
|
/// Initialize the proxy using the given shape. The shape
|
||||||
|
/// must remain in scope while the proxy is in use.
|
||||||
|
void Set(const b2Shape* shape, int32 index);
|
||||||
|
|
||||||
|
/// Initialize the proxy using a vertex cloud and radius. The vertices
|
||||||
|
/// must remain in scope while the proxy is in use.
|
||||||
|
void Set(const b2Vec2* vertices, int32 count, float32 radius);
|
||||||
|
|
||||||
|
/// Get the supporting vertex index in the given direction.
|
||||||
|
int32 GetSupport(const b2Vec2& d) const;
|
||||||
|
|
||||||
|
/// Get the supporting vertex in the given direction.
|
||||||
|
const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
|
||||||
|
|
||||||
|
/// Get the vertex count.
|
||||||
|
int32 GetVertexCount() const;
|
||||||
|
|
||||||
|
/// Get a vertex by index. Used by b2Distance.
|
||||||
|
const b2Vec2& GetVertex(int32 index) const;
|
||||||
|
|
||||||
|
b2Vec2 m_buffer[2];
|
||||||
|
const b2Vec2* m_vertices;
|
||||||
|
int32 m_count;
|
||||||
|
float32 m_radius;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Used to warm start b2Distance.
|
||||||
|
/// Set count to zero on first call.
|
||||||
|
struct b2SimplexCache
|
||||||
|
{
|
||||||
|
float32 metric; ///< length or area
|
||||||
|
uint16 count;
|
||||||
|
uint8 indexA[3]; ///< vertices on shape A
|
||||||
|
uint8 indexB[3]; ///< vertices on shape B
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Input for b2Distance.
|
||||||
|
/// You have to option to use the shape radii
|
||||||
|
/// in the computation. Even
|
||||||
|
struct b2DistanceInput
|
||||||
|
{
|
||||||
|
b2DistanceProxy proxyA;
|
||||||
|
b2DistanceProxy proxyB;
|
||||||
|
b2Transform transformA;
|
||||||
|
b2Transform transformB;
|
||||||
|
bool useRadii;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Output for b2Distance.
|
||||||
|
struct b2DistanceOutput
|
||||||
|
{
|
||||||
|
b2Vec2 pointA; ///< closest point on shapeA
|
||||||
|
b2Vec2 pointB; ///< closest point on shapeB
|
||||||
|
float32 distance;
|
||||||
|
int32 iterations; ///< number of GJK iterations used
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Compute the closest points between two shapes. Supports any combination of:
|
||||||
|
/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
|
||||||
|
/// On the first call set b2SimplexCache.count to zero.
|
||||||
|
void b2Distance(b2DistanceOutput* output,
|
||||||
|
b2SimplexCache* cache,
|
||||||
|
const b2DistanceInput* input);
|
||||||
|
|
||||||
|
/// Input parameters for b2ShapeCast
|
||||||
|
struct b2ShapeCastInput
|
||||||
|
{
|
||||||
|
b2DistanceProxy proxyA;
|
||||||
|
b2DistanceProxy proxyB;
|
||||||
|
b2Transform transformA;
|
||||||
|
b2Transform transformB;
|
||||||
|
b2Vec2 translationB;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Output results for b2ShapeCast
|
||||||
|
struct b2ShapeCastOutput
|
||||||
|
{
|
||||||
|
b2Vec2 point;
|
||||||
|
b2Vec2 normal;
|
||||||
|
float32 lambda;
|
||||||
|
int32 iterations;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Perform a linear shape cast of shape B moving and shape A fixed. Determines the hit point, normal, and translation fraction.
|
||||||
|
bool b2ShapeCast(b2ShapeCastOutput* output, const b2ShapeCastInput* input);
|
||||||
|
|
||||||
|
//////////////////////////////////////////////////////////////////////////
|
||||||
|
|
||||||
|
inline int32 b2DistanceProxy::GetVertexCount() const
|
||||||
|
{
|
||||||
|
return m_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= index && index < m_count);
|
||||||
|
return m_vertices[index];
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
|
||||||
|
{
|
||||||
|
int32 bestIndex = 0;
|
||||||
|
float32 bestValue = b2Dot(m_vertices[0], d);
|
||||||
|
for (int32 i = 1; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
float32 value = b2Dot(m_vertices[i], d);
|
||||||
|
if (value > bestValue)
|
||||||
|
{
|
||||||
|
bestIndex = i;
|
||||||
|
bestValue = value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return bestIndex;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
|
||||||
|
{
|
||||||
|
int32 bestIndex = 0;
|
||||||
|
float32 bestValue = b2Dot(m_vertices[0], d);
|
||||||
|
for (int32 i = 1; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
float32 value = b2Dot(m_vertices[i], d);
|
||||||
|
if (value > bestValue)
|
||||||
|
{
|
||||||
|
bestIndex = i;
|
||||||
|
bestValue = value;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return m_vertices[bestIndex];
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,780 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2DynamicTree.h"
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
b2DynamicTree::b2DynamicTree()
|
||||||
|
{
|
||||||
|
m_root = b2_nullNode;
|
||||||
|
|
||||||
|
m_nodeCapacity = 16;
|
||||||
|
m_nodeCount = 0;
|
||||||
|
m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
|
||||||
|
memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
|
||||||
|
|
||||||
|
// Build a linked list for the free list.
|
||||||
|
for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
|
||||||
|
{
|
||||||
|
m_nodes[i].next = i + 1;
|
||||||
|
m_nodes[i].height = -1;
|
||||||
|
}
|
||||||
|
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
|
||||||
|
m_nodes[m_nodeCapacity-1].height = -1;
|
||||||
|
m_freeList = 0;
|
||||||
|
|
||||||
|
m_path = 0;
|
||||||
|
|
||||||
|
m_insertionCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2DynamicTree::~b2DynamicTree()
|
||||||
|
{
|
||||||
|
// This frees the entire tree in one shot.
|
||||||
|
b2Free(m_nodes);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Allocate a node from the pool. Grow the pool if necessary.
|
||||||
|
int32 b2DynamicTree::AllocateNode()
|
||||||
|
{
|
||||||
|
// Expand the node pool as needed.
|
||||||
|
if (m_freeList == b2_nullNode)
|
||||||
|
{
|
||||||
|
b2Assert(m_nodeCount == m_nodeCapacity);
|
||||||
|
|
||||||
|
// The free list is empty. Rebuild a bigger pool.
|
||||||
|
b2TreeNode* oldNodes = m_nodes;
|
||||||
|
m_nodeCapacity *= 2;
|
||||||
|
m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
|
||||||
|
memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
|
||||||
|
b2Free(oldNodes);
|
||||||
|
|
||||||
|
// Build a linked list for the free list. The parent
|
||||||
|
// pointer becomes the "next" pointer.
|
||||||
|
for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
|
||||||
|
{
|
||||||
|
m_nodes[i].next = i + 1;
|
||||||
|
m_nodes[i].height = -1;
|
||||||
|
}
|
||||||
|
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
|
||||||
|
m_nodes[m_nodeCapacity-1].height = -1;
|
||||||
|
m_freeList = m_nodeCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Peel a node off the free list.
|
||||||
|
int32 nodeId = m_freeList;
|
||||||
|
m_freeList = m_nodes[nodeId].next;
|
||||||
|
m_nodes[nodeId].parent = b2_nullNode;
|
||||||
|
m_nodes[nodeId].child1 = b2_nullNode;
|
||||||
|
m_nodes[nodeId].child2 = b2_nullNode;
|
||||||
|
m_nodes[nodeId].height = 0;
|
||||||
|
m_nodes[nodeId].userData = nullptr;
|
||||||
|
++m_nodeCount;
|
||||||
|
return nodeId;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Return a node to the pool.
|
||||||
|
void b2DynamicTree::FreeNode(int32 nodeId)
|
||||||
|
{
|
||||||
|
b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
|
||||||
|
b2Assert(0 < m_nodeCount);
|
||||||
|
m_nodes[nodeId].next = m_freeList;
|
||||||
|
m_nodes[nodeId].height = -1;
|
||||||
|
m_freeList = nodeId;
|
||||||
|
--m_nodeCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Create a proxy in the tree as a leaf node. We return the index
|
||||||
|
// of the node instead of a pointer so that we can grow
|
||||||
|
// the node pool.
|
||||||
|
int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
|
||||||
|
{
|
||||||
|
int32 proxyId = AllocateNode();
|
||||||
|
|
||||||
|
// Fatten the aabb.
|
||||||
|
b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
|
||||||
|
m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
|
||||||
|
m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
|
||||||
|
m_nodes[proxyId].userData = userData;
|
||||||
|
m_nodes[proxyId].height = 0;
|
||||||
|
|
||||||
|
InsertLeaf(proxyId);
|
||||||
|
|
||||||
|
return proxyId;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::DestroyProxy(int32 proxyId)
|
||||||
|
{
|
||||||
|
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
|
||||||
|
b2Assert(m_nodes[proxyId].IsLeaf());
|
||||||
|
|
||||||
|
RemoveLeaf(proxyId);
|
||||||
|
FreeNode(proxyId);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
|
||||||
|
{
|
||||||
|
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
|
||||||
|
|
||||||
|
b2Assert(m_nodes[proxyId].IsLeaf());
|
||||||
|
|
||||||
|
if (m_nodes[proxyId].aabb.Contains(aabb))
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
RemoveLeaf(proxyId);
|
||||||
|
|
||||||
|
// Extend AABB.
|
||||||
|
b2AABB b = aabb;
|
||||||
|
b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
|
||||||
|
b.lowerBound = b.lowerBound - r;
|
||||||
|
b.upperBound = b.upperBound + r;
|
||||||
|
|
||||||
|
// Predict AABB displacement.
|
||||||
|
b2Vec2 d = b2_aabbMultiplier * displacement;
|
||||||
|
|
||||||
|
if (d.x < 0.0f)
|
||||||
|
{
|
||||||
|
b.lowerBound.x += d.x;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b.upperBound.x += d.x;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (d.y < 0.0f)
|
||||||
|
{
|
||||||
|
b.lowerBound.y += d.y;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b.upperBound.y += d.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_nodes[proxyId].aabb = b;
|
||||||
|
|
||||||
|
InsertLeaf(proxyId);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::InsertLeaf(int32 leaf)
|
||||||
|
{
|
||||||
|
++m_insertionCount;
|
||||||
|
|
||||||
|
if (m_root == b2_nullNode)
|
||||||
|
{
|
||||||
|
m_root = leaf;
|
||||||
|
m_nodes[m_root].parent = b2_nullNode;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Find the best sibling for this node
|
||||||
|
b2AABB leafAABB = m_nodes[leaf].aabb;
|
||||||
|
int32 index = m_root;
|
||||||
|
while (m_nodes[index].IsLeaf() == false)
|
||||||
|
{
|
||||||
|
int32 child1 = m_nodes[index].child1;
|
||||||
|
int32 child2 = m_nodes[index].child2;
|
||||||
|
|
||||||
|
float32 area = m_nodes[index].aabb.GetPerimeter();
|
||||||
|
|
||||||
|
b2AABB combinedAABB;
|
||||||
|
combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
|
||||||
|
float32 combinedArea = combinedAABB.GetPerimeter();
|
||||||
|
|
||||||
|
// Cost of creating a new parent for this node and the new leaf
|
||||||
|
float32 cost = 2.0f * combinedArea;
|
||||||
|
|
||||||
|
// Minimum cost of pushing the leaf further down the tree
|
||||||
|
float32 inheritanceCost = 2.0f * (combinedArea - area);
|
||||||
|
|
||||||
|
// Cost of descending into child1
|
||||||
|
float32 cost1;
|
||||||
|
if (m_nodes[child1].IsLeaf())
|
||||||
|
{
|
||||||
|
b2AABB aabb;
|
||||||
|
aabb.Combine(leafAABB, m_nodes[child1].aabb);
|
||||||
|
cost1 = aabb.GetPerimeter() + inheritanceCost;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2AABB aabb;
|
||||||
|
aabb.Combine(leafAABB, m_nodes[child1].aabb);
|
||||||
|
float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
|
||||||
|
float32 newArea = aabb.GetPerimeter();
|
||||||
|
cost1 = (newArea - oldArea) + inheritanceCost;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Cost of descending into child2
|
||||||
|
float32 cost2;
|
||||||
|
if (m_nodes[child2].IsLeaf())
|
||||||
|
{
|
||||||
|
b2AABB aabb;
|
||||||
|
aabb.Combine(leafAABB, m_nodes[child2].aabb);
|
||||||
|
cost2 = aabb.GetPerimeter() + inheritanceCost;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2AABB aabb;
|
||||||
|
aabb.Combine(leafAABB, m_nodes[child2].aabb);
|
||||||
|
float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
|
||||||
|
float32 newArea = aabb.GetPerimeter();
|
||||||
|
cost2 = newArea - oldArea + inheritanceCost;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Descend according to the minimum cost.
|
||||||
|
if (cost < cost1 && cost < cost2)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Descend
|
||||||
|
if (cost1 < cost2)
|
||||||
|
{
|
||||||
|
index = child1;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
index = child2;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 sibling = index;
|
||||||
|
|
||||||
|
// Create a new parent.
|
||||||
|
int32 oldParent = m_nodes[sibling].parent;
|
||||||
|
int32 newParent = AllocateNode();
|
||||||
|
m_nodes[newParent].parent = oldParent;
|
||||||
|
m_nodes[newParent].userData = nullptr;
|
||||||
|
m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
|
||||||
|
m_nodes[newParent].height = m_nodes[sibling].height + 1;
|
||||||
|
|
||||||
|
if (oldParent != b2_nullNode)
|
||||||
|
{
|
||||||
|
// The sibling was not the root.
|
||||||
|
if (m_nodes[oldParent].child1 == sibling)
|
||||||
|
{
|
||||||
|
m_nodes[oldParent].child1 = newParent;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_nodes[oldParent].child2 = newParent;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_nodes[newParent].child1 = sibling;
|
||||||
|
m_nodes[newParent].child2 = leaf;
|
||||||
|
m_nodes[sibling].parent = newParent;
|
||||||
|
m_nodes[leaf].parent = newParent;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// The sibling was the root.
|
||||||
|
m_nodes[newParent].child1 = sibling;
|
||||||
|
m_nodes[newParent].child2 = leaf;
|
||||||
|
m_nodes[sibling].parent = newParent;
|
||||||
|
m_nodes[leaf].parent = newParent;
|
||||||
|
m_root = newParent;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Walk back up the tree fixing heights and AABBs
|
||||||
|
index = m_nodes[leaf].parent;
|
||||||
|
while (index != b2_nullNode)
|
||||||
|
{
|
||||||
|
index = Balance(index);
|
||||||
|
|
||||||
|
int32 child1 = m_nodes[index].child1;
|
||||||
|
int32 child2 = m_nodes[index].child2;
|
||||||
|
|
||||||
|
b2Assert(child1 != b2_nullNode);
|
||||||
|
b2Assert(child2 != b2_nullNode);
|
||||||
|
|
||||||
|
m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
|
||||||
|
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
|
||||||
|
|
||||||
|
index = m_nodes[index].parent;
|
||||||
|
}
|
||||||
|
|
||||||
|
//Validate();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::RemoveLeaf(int32 leaf)
|
||||||
|
{
|
||||||
|
if (leaf == m_root)
|
||||||
|
{
|
||||||
|
m_root = b2_nullNode;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 parent = m_nodes[leaf].parent;
|
||||||
|
int32 grandParent = m_nodes[parent].parent;
|
||||||
|
int32 sibling;
|
||||||
|
if (m_nodes[parent].child1 == leaf)
|
||||||
|
{
|
||||||
|
sibling = m_nodes[parent].child2;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
sibling = m_nodes[parent].child1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (grandParent != b2_nullNode)
|
||||||
|
{
|
||||||
|
// Destroy parent and connect sibling to grandParent.
|
||||||
|
if (m_nodes[grandParent].child1 == parent)
|
||||||
|
{
|
||||||
|
m_nodes[grandParent].child1 = sibling;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_nodes[grandParent].child2 = sibling;
|
||||||
|
}
|
||||||
|
m_nodes[sibling].parent = grandParent;
|
||||||
|
FreeNode(parent);
|
||||||
|
|
||||||
|
// Adjust ancestor bounds.
|
||||||
|
int32 index = grandParent;
|
||||||
|
while (index != b2_nullNode)
|
||||||
|
{
|
||||||
|
index = Balance(index);
|
||||||
|
|
||||||
|
int32 child1 = m_nodes[index].child1;
|
||||||
|
int32 child2 = m_nodes[index].child2;
|
||||||
|
|
||||||
|
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
|
||||||
|
m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
|
||||||
|
|
||||||
|
index = m_nodes[index].parent;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_root = sibling;
|
||||||
|
m_nodes[sibling].parent = b2_nullNode;
|
||||||
|
FreeNode(parent);
|
||||||
|
}
|
||||||
|
|
||||||
|
//Validate();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Perform a left or right rotation if node A is imbalanced.
|
||||||
|
// Returns the new root index.
|
||||||
|
int32 b2DynamicTree::Balance(int32 iA)
|
||||||
|
{
|
||||||
|
b2Assert(iA != b2_nullNode);
|
||||||
|
|
||||||
|
b2TreeNode* A = m_nodes + iA;
|
||||||
|
if (A->IsLeaf() || A->height < 2)
|
||||||
|
{
|
||||||
|
return iA;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 iB = A->child1;
|
||||||
|
int32 iC = A->child2;
|
||||||
|
b2Assert(0 <= iB && iB < m_nodeCapacity);
|
||||||
|
b2Assert(0 <= iC && iC < m_nodeCapacity);
|
||||||
|
|
||||||
|
b2TreeNode* B = m_nodes + iB;
|
||||||
|
b2TreeNode* C = m_nodes + iC;
|
||||||
|
|
||||||
|
int32 balance = C->height - B->height;
|
||||||
|
|
||||||
|
// Rotate C up
|
||||||
|
if (balance > 1)
|
||||||
|
{
|
||||||
|
int32 iF = C->child1;
|
||||||
|
int32 iG = C->child2;
|
||||||
|
b2TreeNode* F = m_nodes + iF;
|
||||||
|
b2TreeNode* G = m_nodes + iG;
|
||||||
|
b2Assert(0 <= iF && iF < m_nodeCapacity);
|
||||||
|
b2Assert(0 <= iG && iG < m_nodeCapacity);
|
||||||
|
|
||||||
|
// Swap A and C
|
||||||
|
C->child1 = iA;
|
||||||
|
C->parent = A->parent;
|
||||||
|
A->parent = iC;
|
||||||
|
|
||||||
|
// A's old parent should point to C
|
||||||
|
if (C->parent != b2_nullNode)
|
||||||
|
{
|
||||||
|
if (m_nodes[C->parent].child1 == iA)
|
||||||
|
{
|
||||||
|
m_nodes[C->parent].child1 = iC;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Assert(m_nodes[C->parent].child2 == iA);
|
||||||
|
m_nodes[C->parent].child2 = iC;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_root = iC;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Rotate
|
||||||
|
if (F->height > G->height)
|
||||||
|
{
|
||||||
|
C->child2 = iF;
|
||||||
|
A->child2 = iG;
|
||||||
|
G->parent = iA;
|
||||||
|
A->aabb.Combine(B->aabb, G->aabb);
|
||||||
|
C->aabb.Combine(A->aabb, F->aabb);
|
||||||
|
|
||||||
|
A->height = 1 + b2Max(B->height, G->height);
|
||||||
|
C->height = 1 + b2Max(A->height, F->height);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
C->child2 = iG;
|
||||||
|
A->child2 = iF;
|
||||||
|
F->parent = iA;
|
||||||
|
A->aabb.Combine(B->aabb, F->aabb);
|
||||||
|
C->aabb.Combine(A->aabb, G->aabb);
|
||||||
|
|
||||||
|
A->height = 1 + b2Max(B->height, F->height);
|
||||||
|
C->height = 1 + b2Max(A->height, G->height);
|
||||||
|
}
|
||||||
|
|
||||||
|
return iC;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Rotate B up
|
||||||
|
if (balance < -1)
|
||||||
|
{
|
||||||
|
int32 iD = B->child1;
|
||||||
|
int32 iE = B->child2;
|
||||||
|
b2TreeNode* D = m_nodes + iD;
|
||||||
|
b2TreeNode* E = m_nodes + iE;
|
||||||
|
b2Assert(0 <= iD && iD < m_nodeCapacity);
|
||||||
|
b2Assert(0 <= iE && iE < m_nodeCapacity);
|
||||||
|
|
||||||
|
// Swap A and B
|
||||||
|
B->child1 = iA;
|
||||||
|
B->parent = A->parent;
|
||||||
|
A->parent = iB;
|
||||||
|
|
||||||
|
// A's old parent should point to B
|
||||||
|
if (B->parent != b2_nullNode)
|
||||||
|
{
|
||||||
|
if (m_nodes[B->parent].child1 == iA)
|
||||||
|
{
|
||||||
|
m_nodes[B->parent].child1 = iB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Assert(m_nodes[B->parent].child2 == iA);
|
||||||
|
m_nodes[B->parent].child2 = iB;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_root = iB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Rotate
|
||||||
|
if (D->height > E->height)
|
||||||
|
{
|
||||||
|
B->child2 = iD;
|
||||||
|
A->child1 = iE;
|
||||||
|
E->parent = iA;
|
||||||
|
A->aabb.Combine(C->aabb, E->aabb);
|
||||||
|
B->aabb.Combine(A->aabb, D->aabb);
|
||||||
|
|
||||||
|
A->height = 1 + b2Max(C->height, E->height);
|
||||||
|
B->height = 1 + b2Max(A->height, D->height);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
B->child2 = iE;
|
||||||
|
A->child1 = iD;
|
||||||
|
D->parent = iA;
|
||||||
|
A->aabb.Combine(C->aabb, D->aabb);
|
||||||
|
B->aabb.Combine(A->aabb, E->aabb);
|
||||||
|
|
||||||
|
A->height = 1 + b2Max(C->height, D->height);
|
||||||
|
B->height = 1 + b2Max(A->height, E->height);
|
||||||
|
}
|
||||||
|
|
||||||
|
return iB;
|
||||||
|
}
|
||||||
|
|
||||||
|
return iA;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2DynamicTree::GetHeight() const
|
||||||
|
{
|
||||||
|
if (m_root == b2_nullNode)
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
return m_nodes[m_root].height;
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
float32 b2DynamicTree::GetAreaRatio() const
|
||||||
|
{
|
||||||
|
if (m_root == b2_nullNode)
|
||||||
|
{
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2TreeNode* root = m_nodes + m_root;
|
||||||
|
float32 rootArea = root->aabb.GetPerimeter();
|
||||||
|
|
||||||
|
float32 totalArea = 0.0f;
|
||||||
|
for (int32 i = 0; i < m_nodeCapacity; ++i)
|
||||||
|
{
|
||||||
|
const b2TreeNode* node = m_nodes + i;
|
||||||
|
if (node->height < 0)
|
||||||
|
{
|
||||||
|
// Free node in pool
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
totalArea += node->aabb.GetPerimeter();
|
||||||
|
}
|
||||||
|
|
||||||
|
return totalArea / rootArea;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the height of a sub-tree.
|
||||||
|
int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
|
||||||
|
b2TreeNode* node = m_nodes + nodeId;
|
||||||
|
|
||||||
|
if (node->IsLeaf())
|
||||||
|
{
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 height1 = ComputeHeight(node->child1);
|
||||||
|
int32 height2 = ComputeHeight(node->child2);
|
||||||
|
return 1 + b2Max(height1, height2);
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2DynamicTree::ComputeHeight() const
|
||||||
|
{
|
||||||
|
int32 height = ComputeHeight(m_root);
|
||||||
|
return height;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::ValidateStructure(int32 index) const
|
||||||
|
{
|
||||||
|
if (index == b2_nullNode)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (index == m_root)
|
||||||
|
{
|
||||||
|
b2Assert(m_nodes[index].parent == b2_nullNode);
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2TreeNode* node = m_nodes + index;
|
||||||
|
|
||||||
|
int32 child1 = node->child1;
|
||||||
|
int32 child2 = node->child2;
|
||||||
|
|
||||||
|
if (node->IsLeaf())
|
||||||
|
{
|
||||||
|
b2Assert(child1 == b2_nullNode);
|
||||||
|
b2Assert(child2 == b2_nullNode);
|
||||||
|
b2Assert(node->height == 0);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(0 <= child1 && child1 < m_nodeCapacity);
|
||||||
|
b2Assert(0 <= child2 && child2 < m_nodeCapacity);
|
||||||
|
|
||||||
|
b2Assert(m_nodes[child1].parent == index);
|
||||||
|
b2Assert(m_nodes[child2].parent == index);
|
||||||
|
|
||||||
|
ValidateStructure(child1);
|
||||||
|
ValidateStructure(child2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::ValidateMetrics(int32 index) const
|
||||||
|
{
|
||||||
|
if (index == b2_nullNode)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2TreeNode* node = m_nodes + index;
|
||||||
|
|
||||||
|
int32 child1 = node->child1;
|
||||||
|
int32 child2 = node->child2;
|
||||||
|
|
||||||
|
if (node->IsLeaf())
|
||||||
|
{
|
||||||
|
b2Assert(child1 == b2_nullNode);
|
||||||
|
b2Assert(child2 == b2_nullNode);
|
||||||
|
b2Assert(node->height == 0);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(0 <= child1 && child1 < m_nodeCapacity);
|
||||||
|
b2Assert(0 <= child2 && child2 < m_nodeCapacity);
|
||||||
|
|
||||||
|
int32 height1 = m_nodes[child1].height;
|
||||||
|
int32 height2 = m_nodes[child2].height;
|
||||||
|
int32 height;
|
||||||
|
height = 1 + b2Max(height1, height2);
|
||||||
|
b2Assert(node->height == height);
|
||||||
|
|
||||||
|
b2AABB aabb;
|
||||||
|
aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
|
||||||
|
|
||||||
|
b2Assert(aabb.lowerBound == node->aabb.lowerBound);
|
||||||
|
b2Assert(aabb.upperBound == node->aabb.upperBound);
|
||||||
|
|
||||||
|
ValidateMetrics(child1);
|
||||||
|
ValidateMetrics(child2);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::Validate() const
|
||||||
|
{
|
||||||
|
#if defined(b2DEBUG)
|
||||||
|
ValidateStructure(m_root);
|
||||||
|
ValidateMetrics(m_root);
|
||||||
|
|
||||||
|
int32 freeCount = 0;
|
||||||
|
int32 freeIndex = m_freeList;
|
||||||
|
while (freeIndex != b2_nullNode)
|
||||||
|
{
|
||||||
|
b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
|
||||||
|
freeIndex = m_nodes[freeIndex].next;
|
||||||
|
++freeCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(GetHeight() == ComputeHeight());
|
||||||
|
|
||||||
|
b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
|
||||||
|
#endif
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2DynamicTree::GetMaxBalance() const
|
||||||
|
{
|
||||||
|
int32 maxBalance = 0;
|
||||||
|
for (int32 i = 0; i < m_nodeCapacity; ++i)
|
||||||
|
{
|
||||||
|
const b2TreeNode* node = m_nodes + i;
|
||||||
|
if (node->height <= 1)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(node->IsLeaf() == false);
|
||||||
|
|
||||||
|
int32 child1 = node->child1;
|
||||||
|
int32 child2 = node->child2;
|
||||||
|
int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
|
||||||
|
maxBalance = b2Max(maxBalance, balance);
|
||||||
|
}
|
||||||
|
|
||||||
|
return maxBalance;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::RebuildBottomUp()
|
||||||
|
{
|
||||||
|
int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
|
||||||
|
int32 count = 0;
|
||||||
|
|
||||||
|
// Build array of leaves. Free the rest.
|
||||||
|
for (int32 i = 0; i < m_nodeCapacity; ++i)
|
||||||
|
{
|
||||||
|
if (m_nodes[i].height < 0)
|
||||||
|
{
|
||||||
|
// free node in pool
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_nodes[i].IsLeaf())
|
||||||
|
{
|
||||||
|
m_nodes[i].parent = b2_nullNode;
|
||||||
|
nodes[count] = i;
|
||||||
|
++count;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
FreeNode(i);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
while (count > 1)
|
||||||
|
{
|
||||||
|
float32 minCost = b2_maxFloat;
|
||||||
|
int32 iMin = -1, jMin = -1;
|
||||||
|
for (int32 i = 0; i < count; ++i)
|
||||||
|
{
|
||||||
|
b2AABB aabbi = m_nodes[nodes[i]].aabb;
|
||||||
|
|
||||||
|
for (int32 j = i + 1; j < count; ++j)
|
||||||
|
{
|
||||||
|
b2AABB aabbj = m_nodes[nodes[j]].aabb;
|
||||||
|
b2AABB b;
|
||||||
|
b.Combine(aabbi, aabbj);
|
||||||
|
float32 cost = b.GetPerimeter();
|
||||||
|
if (cost < minCost)
|
||||||
|
{
|
||||||
|
iMin = i;
|
||||||
|
jMin = j;
|
||||||
|
minCost = cost;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 index1 = nodes[iMin];
|
||||||
|
int32 index2 = nodes[jMin];
|
||||||
|
b2TreeNode* child1 = m_nodes + index1;
|
||||||
|
b2TreeNode* child2 = m_nodes + index2;
|
||||||
|
|
||||||
|
int32 parentIndex = AllocateNode();
|
||||||
|
b2TreeNode* parent = m_nodes + parentIndex;
|
||||||
|
parent->child1 = index1;
|
||||||
|
parent->child2 = index2;
|
||||||
|
parent->height = 1 + b2Max(child1->height, child2->height);
|
||||||
|
parent->aabb.Combine(child1->aabb, child2->aabb);
|
||||||
|
parent->parent = b2_nullNode;
|
||||||
|
|
||||||
|
child1->parent = parentIndex;
|
||||||
|
child2->parent = parentIndex;
|
||||||
|
|
||||||
|
nodes[jMin] = nodes[count-1];
|
||||||
|
nodes[iMin] = parentIndex;
|
||||||
|
--count;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_root = nodes[0];
|
||||||
|
b2Free(nodes);
|
||||||
|
|
||||||
|
Validate();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
|
||||||
|
{
|
||||||
|
// Build array of leaves. Free the rest.
|
||||||
|
for (int32 i = 0; i < m_nodeCapacity; ++i)
|
||||||
|
{
|
||||||
|
m_nodes[i].aabb.lowerBound -= newOrigin;
|
||||||
|
m_nodes[i].aabb.upperBound -= newOrigin;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,289 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_DYNAMIC_TREE_H
|
||||||
|
#define B2_DYNAMIC_TREE_H
|
||||||
|
|
||||||
|
#include "b2Collision.h"
|
||||||
|
#include "../Common/b2GrowableStack.h"
|
||||||
|
|
||||||
|
#define b2_nullNode (-1)
|
||||||
|
|
||||||
|
/// A node in the dynamic tree. The client does not interact with this directly.
|
||||||
|
struct b2TreeNode
|
||||||
|
{
|
||||||
|
bool IsLeaf() const
|
||||||
|
{
|
||||||
|
return child1 == b2_nullNode;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Enlarged AABB
|
||||||
|
b2AABB aabb;
|
||||||
|
|
||||||
|
void* userData;
|
||||||
|
|
||||||
|
union
|
||||||
|
{
|
||||||
|
int32 parent;
|
||||||
|
int32 next;
|
||||||
|
};
|
||||||
|
|
||||||
|
int32 child1;
|
||||||
|
int32 child2;
|
||||||
|
|
||||||
|
// leaf = 0, free node = -1
|
||||||
|
int32 height;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
|
||||||
|
/// A dynamic tree arranges data in a binary tree to accelerate
|
||||||
|
/// queries such as volume queries and ray casts. Leafs are proxies
|
||||||
|
/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
|
||||||
|
/// so that the proxy AABB is bigger than the client object. This allows the client
|
||||||
|
/// object to move by small amounts without triggering a tree update.
|
||||||
|
///
|
||||||
|
/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
|
||||||
|
class b2DynamicTree
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/// Constructing the tree initializes the node pool.
|
||||||
|
b2DynamicTree();
|
||||||
|
|
||||||
|
/// Destroy the tree, freeing the node pool.
|
||||||
|
~b2DynamicTree();
|
||||||
|
|
||||||
|
/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
|
||||||
|
int32 CreateProxy(const b2AABB& aabb, void* userData);
|
||||||
|
|
||||||
|
/// Destroy a proxy. This asserts if the id is invalid.
|
||||||
|
void DestroyProxy(int32 proxyId);
|
||||||
|
|
||||||
|
/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
|
||||||
|
/// then the proxy is removed from the tree and re-inserted. Otherwise
|
||||||
|
/// the function returns immediately.
|
||||||
|
/// @return true if the proxy was re-inserted.
|
||||||
|
bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
|
||||||
|
|
||||||
|
/// Get proxy user data.
|
||||||
|
/// @return the proxy user data or 0 if the id is invalid.
|
||||||
|
void* GetUserData(int32 proxyId) const;
|
||||||
|
|
||||||
|
/// Get the fat AABB for a proxy.
|
||||||
|
const b2AABB& GetFatAABB(int32 proxyId) const;
|
||||||
|
|
||||||
|
/// Query an AABB for overlapping proxies. The callback class
|
||||||
|
/// is called for each proxy that overlaps the supplied AABB.
|
||||||
|
template <typename T>
|
||||||
|
void Query(T* callback, const b2AABB& aabb) const;
|
||||||
|
|
||||||
|
/// Ray-cast against the proxies in the tree. This relies on the callback
|
||||||
|
/// to perform a exact ray-cast in the case were the proxy contains a shape.
|
||||||
|
/// The callback also performs the any collision filtering. This has performance
|
||||||
|
/// roughly equal to k * log(n), where k is the number of collisions and n is the
|
||||||
|
/// number of proxies in the tree.
|
||||||
|
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
|
||||||
|
/// @param callback a callback class that is called for each proxy that is hit by the ray.
|
||||||
|
template <typename T>
|
||||||
|
void RayCast(T* callback, const b2RayCastInput& input) const;
|
||||||
|
|
||||||
|
/// Validate this tree. For testing.
|
||||||
|
void Validate() const;
|
||||||
|
|
||||||
|
/// Compute the height of the binary tree in O(N) time. Should not be
|
||||||
|
/// called often.
|
||||||
|
int32 GetHeight() const;
|
||||||
|
|
||||||
|
/// Get the maximum balance of an node in the tree. The balance is the difference
|
||||||
|
/// in height of the two children of a node.
|
||||||
|
int32 GetMaxBalance() const;
|
||||||
|
|
||||||
|
/// Get the ratio of the sum of the node areas to the root area.
|
||||||
|
float32 GetAreaRatio() const;
|
||||||
|
|
||||||
|
/// Build an optimal tree. Very expensive. For testing.
|
||||||
|
void RebuildBottomUp();
|
||||||
|
|
||||||
|
/// Shift the world origin. Useful for large worlds.
|
||||||
|
/// The shift formula is: position -= newOrigin
|
||||||
|
/// @param newOrigin the new origin with respect to the old origin
|
||||||
|
void ShiftOrigin(const b2Vec2& newOrigin);
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
int32 AllocateNode();
|
||||||
|
void FreeNode(int32 node);
|
||||||
|
|
||||||
|
void InsertLeaf(int32 node);
|
||||||
|
void RemoveLeaf(int32 node);
|
||||||
|
|
||||||
|
int32 Balance(int32 index);
|
||||||
|
|
||||||
|
int32 ComputeHeight() const;
|
||||||
|
int32 ComputeHeight(int32 nodeId) const;
|
||||||
|
|
||||||
|
void ValidateStructure(int32 index) const;
|
||||||
|
void ValidateMetrics(int32 index) const;
|
||||||
|
|
||||||
|
int32 m_root;
|
||||||
|
|
||||||
|
b2TreeNode* m_nodes;
|
||||||
|
int32 m_nodeCount;
|
||||||
|
int32 m_nodeCapacity;
|
||||||
|
|
||||||
|
int32 m_freeList;
|
||||||
|
|
||||||
|
/// This is used to incrementally traverse the tree for re-balancing.
|
||||||
|
uint32 m_path;
|
||||||
|
|
||||||
|
int32 m_insertionCount;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline void* b2DynamicTree::GetUserData(int32 proxyId) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
|
||||||
|
return m_nodes[proxyId].userData;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
|
||||||
|
return m_nodes[proxyId].aabb;
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
|
||||||
|
{
|
||||||
|
b2GrowableStack<int32, 256> stack;
|
||||||
|
stack.Push(m_root);
|
||||||
|
|
||||||
|
while (stack.GetCount() > 0)
|
||||||
|
{
|
||||||
|
int32 nodeId = stack.Pop();
|
||||||
|
if (nodeId == b2_nullNode)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2TreeNode* node = m_nodes + nodeId;
|
||||||
|
|
||||||
|
if (b2TestOverlap(node->aabb, aabb))
|
||||||
|
{
|
||||||
|
if (node->IsLeaf())
|
||||||
|
{
|
||||||
|
bool proceed = callback->QueryCallback(nodeId);
|
||||||
|
if (proceed == false)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
stack.Push(node->child1);
|
||||||
|
stack.Push(node->child2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
|
||||||
|
{
|
||||||
|
b2Vec2 p1 = input.p1;
|
||||||
|
b2Vec2 p2 = input.p2;
|
||||||
|
b2Vec2 r = p2 - p1;
|
||||||
|
b2Assert(r.LengthSquared() > 0.0f);
|
||||||
|
r.Normalize();
|
||||||
|
|
||||||
|
// v is perpendicular to the segment.
|
||||||
|
b2Vec2 v = b2Cross(1.0f, r);
|
||||||
|
b2Vec2 abs_v = b2Abs(v);
|
||||||
|
|
||||||
|
// Separating axis for segment (Gino, p80).
|
||||||
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
||||||
|
|
||||||
|
float32 maxFraction = input.maxFraction;
|
||||||
|
|
||||||
|
// Build a bounding box for the segment.
|
||||||
|
b2AABB segmentAABB;
|
||||||
|
{
|
||||||
|
b2Vec2 t = p1 + maxFraction * (p2 - p1);
|
||||||
|
segmentAABB.lowerBound = b2Min(p1, t);
|
||||||
|
segmentAABB.upperBound = b2Max(p1, t);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2GrowableStack<int32, 256> stack;
|
||||||
|
stack.Push(m_root);
|
||||||
|
|
||||||
|
while (stack.GetCount() > 0)
|
||||||
|
{
|
||||||
|
int32 nodeId = stack.Pop();
|
||||||
|
if (nodeId == b2_nullNode)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2TreeNode* node = m_nodes + nodeId;
|
||||||
|
|
||||||
|
if (b2TestOverlap(node->aabb, segmentAABB) == false)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Separating axis for segment (Gino, p80).
|
||||||
|
// |dot(v, p1 - c)| > dot(|v|, h)
|
||||||
|
b2Vec2 c = node->aabb.GetCenter();
|
||||||
|
b2Vec2 h = node->aabb.GetExtents();
|
||||||
|
float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
|
||||||
|
if (separation > 0.0f)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (node->IsLeaf())
|
||||||
|
{
|
||||||
|
b2RayCastInput subInput;
|
||||||
|
subInput.p1 = input.p1;
|
||||||
|
subInput.p2 = input.p2;
|
||||||
|
subInput.maxFraction = maxFraction;
|
||||||
|
|
||||||
|
float32 value = callback->RayCastCallback(subInput, nodeId);
|
||||||
|
|
||||||
|
if (value == 0.0f)
|
||||||
|
{
|
||||||
|
// The client has terminated the ray cast.
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (value > 0.0f)
|
||||||
|
{
|
||||||
|
// Update segment bounding box.
|
||||||
|
maxFraction = value;
|
||||||
|
b2Vec2 t = p1 + maxFraction * (p2 - p1);
|
||||||
|
segmentAABB.lowerBound = b2Min(p1, t);
|
||||||
|
segmentAABB.upperBound = b2Max(p1, t);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
stack.Push(node->child1);
|
||||||
|
stack.Push(node->child2);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,486 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/b2Distance.h"
|
||||||
|
#include "Box2D/Collision/b2TimeOfImpact.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
#include "Box2D/Common/b2Timer.h"
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
float32 b2_toiTime, b2_toiMaxTime;
|
||||||
|
int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
|
||||||
|
int32 b2_toiRootIters, b2_toiMaxRootIters;
|
||||||
|
|
||||||
|
//
|
||||||
|
struct b2SeparationFunction
|
||||||
|
{
|
||||||
|
enum Type
|
||||||
|
{
|
||||||
|
e_points,
|
||||||
|
e_faceA,
|
||||||
|
e_faceB
|
||||||
|
};
|
||||||
|
|
||||||
|
// TODO_ERIN might not need to return the separation
|
||||||
|
|
||||||
|
float32 Initialize(const b2SimplexCache* cache,
|
||||||
|
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
|
||||||
|
const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
|
||||||
|
float32 t1)
|
||||||
|
{
|
||||||
|
m_proxyA = proxyA;
|
||||||
|
m_proxyB = proxyB;
|
||||||
|
int32 count = cache->count;
|
||||||
|
b2Assert(0 < count && count < 3);
|
||||||
|
|
||||||
|
m_sweepA = sweepA;
|
||||||
|
m_sweepB = sweepB;
|
||||||
|
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
m_sweepA.GetTransform(&xfA, t1);
|
||||||
|
m_sweepB.GetTransform(&xfB, t1);
|
||||||
|
|
||||||
|
if (count == 1)
|
||||||
|
{
|
||||||
|
m_type = e_points;
|
||||||
|
b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
m_axis = pointB - pointA;
|
||||||
|
float32 s = m_axis.Normalize();
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
else if (cache->indexA[0] == cache->indexA[1])
|
||||||
|
{
|
||||||
|
// Two points on B and one on A.
|
||||||
|
m_type = e_faceB;
|
||||||
|
b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
|
||||||
|
b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
|
||||||
|
|
||||||
|
m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
|
||||||
|
m_axis.Normalize();
|
||||||
|
b2Vec2 normal = b2Mul(xfB.q, m_axis);
|
||||||
|
|
||||||
|
m_localPoint = 0.5f * (localPointB1 + localPointB2);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
|
||||||
|
float32 s = b2Dot(pointA - pointB, normal);
|
||||||
|
if (s < 0.0f)
|
||||||
|
{
|
||||||
|
m_axis = -m_axis;
|
||||||
|
s = -s;
|
||||||
|
}
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Two points on A and one or two points on B.
|
||||||
|
m_type = e_faceA;
|
||||||
|
b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
|
||||||
|
b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
|
||||||
|
|
||||||
|
m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
|
||||||
|
m_axis.Normalize();
|
||||||
|
b2Vec2 normal = b2Mul(xfA.q, m_axis);
|
||||||
|
|
||||||
|
m_localPoint = 0.5f * (localPointA1 + localPointA2);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
|
||||||
|
float32 s = b2Dot(pointB - pointA, normal);
|
||||||
|
if (s < 0.0f)
|
||||||
|
{
|
||||||
|
m_axis = -m_axis;
|
||||||
|
s = -s;
|
||||||
|
}
|
||||||
|
return s;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
|
||||||
|
{
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
m_sweepA.GetTransform(&xfA, t);
|
||||||
|
m_sweepB.GetTransform(&xfB, t);
|
||||||
|
|
||||||
|
switch (m_type)
|
||||||
|
{
|
||||||
|
case e_points:
|
||||||
|
{
|
||||||
|
b2Vec2 axisA = b2MulT(xfA.q, m_axis);
|
||||||
|
b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
|
||||||
|
|
||||||
|
*indexA = m_proxyA->GetSupport(axisA);
|
||||||
|
*indexB = m_proxyB->GetSupport(axisB);
|
||||||
|
|
||||||
|
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
|
||||||
|
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
|
||||||
|
float32 separation = b2Dot(pointB - pointA, m_axis);
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
case e_faceA:
|
||||||
|
{
|
||||||
|
b2Vec2 normal = b2Mul(xfA.q, m_axis);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 axisB = b2MulT(xfB.q, -normal);
|
||||||
|
|
||||||
|
*indexA = -1;
|
||||||
|
*indexB = m_proxyB->GetSupport(axisB);
|
||||||
|
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
|
||||||
|
float32 separation = b2Dot(pointB - pointA, normal);
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
case e_faceB:
|
||||||
|
{
|
||||||
|
b2Vec2 normal = b2Mul(xfB.q, m_axis);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 axisA = b2MulT(xfA.q, -normal);
|
||||||
|
|
||||||
|
*indexB = -1;
|
||||||
|
*indexA = m_proxyA->GetSupport(axisA);
|
||||||
|
|
||||||
|
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
|
||||||
|
float32 separation = b2Dot(pointA - pointB, normal);
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
*indexA = -1;
|
||||||
|
*indexB = -1;
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
|
||||||
|
{
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
m_sweepA.GetTransform(&xfA, t);
|
||||||
|
m_sweepB.GetTransform(&xfB, t);
|
||||||
|
|
||||||
|
switch (m_type)
|
||||||
|
{
|
||||||
|
case e_points:
|
||||||
|
{
|
||||||
|
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
|
||||||
|
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
float32 separation = b2Dot(pointB - pointA, m_axis);
|
||||||
|
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
case e_faceA:
|
||||||
|
{
|
||||||
|
b2Vec2 normal = b2Mul(xfA.q, m_axis);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, localPointB);
|
||||||
|
|
||||||
|
float32 separation = b2Dot(pointB - pointA, normal);
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
case e_faceB:
|
||||||
|
{
|
||||||
|
b2Vec2 normal = b2Mul(xfB.q, m_axis);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
|
||||||
|
|
||||||
|
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, localPointA);
|
||||||
|
|
||||||
|
float32 separation = b2Dot(pointA - pointB, normal);
|
||||||
|
return separation;
|
||||||
|
}
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2DistanceProxy* m_proxyA;
|
||||||
|
const b2DistanceProxy* m_proxyB;
|
||||||
|
b2Sweep m_sweepA, m_sweepB;
|
||||||
|
Type m_type;
|
||||||
|
b2Vec2 m_localPoint;
|
||||||
|
b2Vec2 m_axis;
|
||||||
|
};
|
||||||
|
|
||||||
|
// CCD via the local separating axis method. This seeks progression
|
||||||
|
// by computing the largest time at which separation is maintained.
|
||||||
|
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
|
||||||
|
{
|
||||||
|
b2Timer timer;
|
||||||
|
|
||||||
|
++b2_toiCalls;
|
||||||
|
|
||||||
|
output->state = b2TOIOutput::e_unknown;
|
||||||
|
output->t = input->tMax;
|
||||||
|
|
||||||
|
const b2DistanceProxy* proxyA = &input->proxyA;
|
||||||
|
const b2DistanceProxy* proxyB = &input->proxyB;
|
||||||
|
|
||||||
|
b2Sweep sweepA = input->sweepA;
|
||||||
|
b2Sweep sweepB = input->sweepB;
|
||||||
|
|
||||||
|
// Large rotations can make the root finder fail, so we normalize the
|
||||||
|
// sweep angles.
|
||||||
|
sweepA.Normalize();
|
||||||
|
sweepB.Normalize();
|
||||||
|
|
||||||
|
float32 tMax = input->tMax;
|
||||||
|
|
||||||
|
float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
|
||||||
|
float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
|
||||||
|
float32 tolerance = 0.25f * b2_linearSlop;
|
||||||
|
b2Assert(target > tolerance);
|
||||||
|
|
||||||
|
float32 t1 = 0.0f;
|
||||||
|
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
|
||||||
|
int32 iter = 0;
|
||||||
|
|
||||||
|
// Prepare input for distance query.
|
||||||
|
b2SimplexCache cache;
|
||||||
|
cache.count = 0;
|
||||||
|
b2DistanceInput distanceInput;
|
||||||
|
distanceInput.proxyA = input->proxyA;
|
||||||
|
distanceInput.proxyB = input->proxyB;
|
||||||
|
distanceInput.useRadii = false;
|
||||||
|
|
||||||
|
// The outer loop progressively attempts to compute new separating axes.
|
||||||
|
// This loop terminates when an axis is repeated (no progress is made).
|
||||||
|
for(;;)
|
||||||
|
{
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
sweepA.GetTransform(&xfA, t1);
|
||||||
|
sweepB.GetTransform(&xfB, t1);
|
||||||
|
|
||||||
|
// Get the distance between shapes. We can also use the results
|
||||||
|
// to get a separating axis.
|
||||||
|
distanceInput.transformA = xfA;
|
||||||
|
distanceInput.transformB = xfB;
|
||||||
|
b2DistanceOutput distanceOutput;
|
||||||
|
b2Distance(&distanceOutput, &cache, &distanceInput);
|
||||||
|
|
||||||
|
// If the shapes are overlapped, we give up on continuous collision.
|
||||||
|
if (distanceOutput.distance <= 0.0f)
|
||||||
|
{
|
||||||
|
// Failure!
|
||||||
|
output->state = b2TOIOutput::e_overlapped;
|
||||||
|
output->t = 0.0f;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (distanceOutput.distance < target + tolerance)
|
||||||
|
{
|
||||||
|
// Victory!
|
||||||
|
output->state = b2TOIOutput::e_touching;
|
||||||
|
output->t = t1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize the separating axis.
|
||||||
|
b2SeparationFunction fcn;
|
||||||
|
fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
|
||||||
|
#if 0
|
||||||
|
// Dump the curve seen by the root finder
|
||||||
|
{
|
||||||
|
const int32 N = 100;
|
||||||
|
float32 dx = 1.0f / N;
|
||||||
|
float32 xs[N+1];
|
||||||
|
float32 fs[N+1];
|
||||||
|
|
||||||
|
float32 x = 0.0f;
|
||||||
|
|
||||||
|
for (int32 i = 0; i <= N; ++i)
|
||||||
|
{
|
||||||
|
sweepA.GetTransform(&xfA, x);
|
||||||
|
sweepB.GetTransform(&xfB, x);
|
||||||
|
float32 f = fcn.Evaluate(xfA, xfB) - target;
|
||||||
|
|
||||||
|
printf("%g %g\n", x, f);
|
||||||
|
|
||||||
|
xs[i] = x;
|
||||||
|
fs[i] = f;
|
||||||
|
|
||||||
|
x += dx;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Compute the TOI on the separating axis. We do this by successively
|
||||||
|
// resolving the deepest point. This loop is bounded by the number of vertices.
|
||||||
|
bool done = false;
|
||||||
|
float32 t2 = tMax;
|
||||||
|
int32 pushBackIter = 0;
|
||||||
|
for (;;)
|
||||||
|
{
|
||||||
|
// Find the deepest point at t2. Store the witness point indices.
|
||||||
|
int32 indexA, indexB;
|
||||||
|
float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
|
||||||
|
|
||||||
|
// Is the final configuration separated?
|
||||||
|
if (s2 > target + tolerance)
|
||||||
|
{
|
||||||
|
// Victory!
|
||||||
|
output->state = b2TOIOutput::e_separated;
|
||||||
|
output->t = tMax;
|
||||||
|
done = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Has the separation reached tolerance?
|
||||||
|
if (s2 > target - tolerance)
|
||||||
|
{
|
||||||
|
// Advance the sweeps
|
||||||
|
t1 = t2;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute the initial separation of the witness points.
|
||||||
|
float32 s1 = fcn.Evaluate(indexA, indexB, t1);
|
||||||
|
|
||||||
|
// Check for initial overlap. This might happen if the root finder
|
||||||
|
// runs out of iterations.
|
||||||
|
if (s1 < target - tolerance)
|
||||||
|
{
|
||||||
|
output->state = b2TOIOutput::e_failed;
|
||||||
|
output->t = t1;
|
||||||
|
done = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check for touching
|
||||||
|
if (s1 <= target + tolerance)
|
||||||
|
{
|
||||||
|
// Victory! t1 should hold the TOI (could be 0.0).
|
||||||
|
output->state = b2TOIOutput::e_touching;
|
||||||
|
output->t = t1;
|
||||||
|
done = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute 1D root of: f(x) - target = 0
|
||||||
|
int32 rootIterCount = 0;
|
||||||
|
float32 a1 = t1, a2 = t2;
|
||||||
|
for (;;)
|
||||||
|
{
|
||||||
|
// Use a mix of the secant rule and bisection.
|
||||||
|
float32 t;
|
||||||
|
if (rootIterCount & 1)
|
||||||
|
{
|
||||||
|
// Secant rule to improve convergence.
|
||||||
|
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Bisection to guarantee progress.
|
||||||
|
t = 0.5f * (a1 + a2);
|
||||||
|
}
|
||||||
|
|
||||||
|
++rootIterCount;
|
||||||
|
++b2_toiRootIters;
|
||||||
|
|
||||||
|
float32 s = fcn.Evaluate(indexA, indexB, t);
|
||||||
|
|
||||||
|
if (b2Abs(s - target) < tolerance)
|
||||||
|
{
|
||||||
|
// t2 holds a tentative value for t1
|
||||||
|
t2 = t;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Ensure we continue to bracket the root.
|
||||||
|
if (s > target)
|
||||||
|
{
|
||||||
|
a1 = t;
|
||||||
|
s1 = s;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
a2 = t;
|
||||||
|
s2 = s;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rootIterCount == 50)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
|
||||||
|
|
||||||
|
++pushBackIter;
|
||||||
|
|
||||||
|
if (pushBackIter == b2_maxPolygonVertices)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
++iter;
|
||||||
|
++b2_toiIters;
|
||||||
|
|
||||||
|
if (done)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (iter == k_maxIterations)
|
||||||
|
{
|
||||||
|
// Root finder got stuck. Semi-victory.
|
||||||
|
output->state = b2TOIOutput::e_failed;
|
||||||
|
output->t = t1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
|
||||||
|
|
||||||
|
float32 time = timer.GetMilliseconds();
|
||||||
|
b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
|
||||||
|
b2_toiTime += time;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,58 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_TIME_OF_IMPACT_H
|
||||||
|
#define B2_TIME_OF_IMPACT_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
#include "b2Distance.h"
|
||||||
|
|
||||||
|
/// Input parameters for b2TimeOfImpact
|
||||||
|
struct b2TOIInput
|
||||||
|
{
|
||||||
|
b2DistanceProxy proxyA;
|
||||||
|
b2DistanceProxy proxyB;
|
||||||
|
b2Sweep sweepA;
|
||||||
|
b2Sweep sweepB;
|
||||||
|
float32 tMax; // defines sweep interval [0, tMax]
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Output parameters for b2TimeOfImpact.
|
||||||
|
struct b2TOIOutput
|
||||||
|
{
|
||||||
|
enum State
|
||||||
|
{
|
||||||
|
e_unknown,
|
||||||
|
e_failed,
|
||||||
|
e_overlapped,
|
||||||
|
e_touching,
|
||||||
|
e_separated
|
||||||
|
};
|
||||||
|
|
||||||
|
State state;
|
||||||
|
float32 t;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
|
||||||
|
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
|
||||||
|
/// non-tunneling collisions. If you change the time interval, you should call this function
|
||||||
|
/// again.
|
||||||
|
/// Note: use b2Distance to compute the contact point and normal at the time of impact.
|
||||||
|
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,215 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include <limits.h>
|
||||||
|
#include <string.h>
|
||||||
|
#include <stddef.h>
|
||||||
|
|
||||||
|
int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =
|
||||||
|
{
|
||||||
|
16, // 0
|
||||||
|
32, // 1
|
||||||
|
64, // 2
|
||||||
|
96, // 3
|
||||||
|
128, // 4
|
||||||
|
160, // 5
|
||||||
|
192, // 6
|
||||||
|
224, // 7
|
||||||
|
256, // 8
|
||||||
|
320, // 9
|
||||||
|
384, // 10
|
||||||
|
448, // 11
|
||||||
|
512, // 12
|
||||||
|
640, // 13
|
||||||
|
};
|
||||||
|
uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
|
||||||
|
bool b2BlockAllocator::s_blockSizeLookupInitialized;
|
||||||
|
|
||||||
|
struct b2Chunk
|
||||||
|
{
|
||||||
|
int32 blockSize;
|
||||||
|
b2Block* blocks;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct b2Block
|
||||||
|
{
|
||||||
|
b2Block* next;
|
||||||
|
};
|
||||||
|
|
||||||
|
b2BlockAllocator::b2BlockAllocator()
|
||||||
|
{
|
||||||
|
b2Assert(b2_blockSizes < UCHAR_MAX);
|
||||||
|
|
||||||
|
m_chunkSpace = b2_chunkArrayIncrement;
|
||||||
|
m_chunkCount = 0;
|
||||||
|
m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
|
||||||
|
|
||||||
|
memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
|
||||||
|
memset(m_freeLists, 0, sizeof(m_freeLists));
|
||||||
|
|
||||||
|
if (s_blockSizeLookupInitialized == false)
|
||||||
|
{
|
||||||
|
int32 j = 0;
|
||||||
|
for (int32 i = 1; i <= b2_maxBlockSize; ++i)
|
||||||
|
{
|
||||||
|
b2Assert(j < b2_blockSizes);
|
||||||
|
if (i <= s_blockSizes[j])
|
||||||
|
{
|
||||||
|
s_blockSizeLookup[i] = (uint8)j;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
++j;
|
||||||
|
s_blockSizeLookup[i] = (uint8)j;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
s_blockSizeLookupInitialized = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2BlockAllocator::~b2BlockAllocator()
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_chunkCount; ++i)
|
||||||
|
{
|
||||||
|
b2Free(m_chunks[i].blocks);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Free(m_chunks);
|
||||||
|
}
|
||||||
|
|
||||||
|
void* b2BlockAllocator::Allocate(int32 size)
|
||||||
|
{
|
||||||
|
if (size == 0)
|
||||||
|
return nullptr;
|
||||||
|
|
||||||
|
b2Assert(0 < size);
|
||||||
|
|
||||||
|
if (size > b2_maxBlockSize)
|
||||||
|
{
|
||||||
|
return b2Alloc(size);
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 index = s_blockSizeLookup[size];
|
||||||
|
b2Assert(0 <= index && index < b2_blockSizes);
|
||||||
|
|
||||||
|
if (m_freeLists[index])
|
||||||
|
{
|
||||||
|
b2Block* block = m_freeLists[index];
|
||||||
|
m_freeLists[index] = block->next;
|
||||||
|
return block;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if (m_chunkCount == m_chunkSpace)
|
||||||
|
{
|
||||||
|
b2Chunk* oldChunks = m_chunks;
|
||||||
|
m_chunkSpace += b2_chunkArrayIncrement;
|
||||||
|
m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
|
||||||
|
memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
|
||||||
|
memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
|
||||||
|
b2Free(oldChunks);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Chunk* chunk = m_chunks + m_chunkCount;
|
||||||
|
chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
|
||||||
|
#if defined(_DEBUG)
|
||||||
|
memset(chunk->blocks, 0xcd, b2_chunkSize);
|
||||||
|
#endif
|
||||||
|
int32 blockSize = s_blockSizes[index];
|
||||||
|
chunk->blockSize = blockSize;
|
||||||
|
int32 blockCount = b2_chunkSize / blockSize;
|
||||||
|
b2Assert(blockCount * blockSize <= b2_chunkSize);
|
||||||
|
for (int32 i = 0; i < blockCount - 1; ++i)
|
||||||
|
{
|
||||||
|
b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
|
||||||
|
b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
|
||||||
|
block->next = next;
|
||||||
|
}
|
||||||
|
b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
|
||||||
|
last->next = nullptr;
|
||||||
|
|
||||||
|
m_freeLists[index] = chunk->blocks->next;
|
||||||
|
++m_chunkCount;
|
||||||
|
|
||||||
|
return chunk->blocks;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BlockAllocator::Free(void* p, int32 size)
|
||||||
|
{
|
||||||
|
if (size == 0)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(0 < size);
|
||||||
|
|
||||||
|
if (size > b2_maxBlockSize)
|
||||||
|
{
|
||||||
|
b2Free(p);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 index = s_blockSizeLookup[size];
|
||||||
|
b2Assert(0 <= index && index < b2_blockSizes);
|
||||||
|
|
||||||
|
#ifdef _DEBUG
|
||||||
|
// Verify the memory address and size is valid.
|
||||||
|
int32 blockSize = s_blockSizes[index];
|
||||||
|
bool found = false;
|
||||||
|
for (int32 i = 0; i < m_chunkCount; ++i)
|
||||||
|
{
|
||||||
|
b2Chunk* chunk = m_chunks + i;
|
||||||
|
if (chunk->blockSize != blockSize)
|
||||||
|
{
|
||||||
|
b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks ||
|
||||||
|
(int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
|
||||||
|
{
|
||||||
|
found = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(found);
|
||||||
|
|
||||||
|
memset(p, 0xfd, blockSize);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
b2Block* block = (b2Block*)p;
|
||||||
|
block->next = m_freeLists[index];
|
||||||
|
m_freeLists[index] = block;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2BlockAllocator::Clear()
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_chunkCount; ++i)
|
||||||
|
{
|
||||||
|
b2Free(m_chunks[i].blocks);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_chunkCount = 0;
|
||||||
|
memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
|
||||||
|
|
||||||
|
memset(m_freeLists, 0, sizeof(m_freeLists));
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,62 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_BLOCK_ALLOCATOR_H
|
||||||
|
#define B2_BLOCK_ALLOCATOR_H
|
||||||
|
|
||||||
|
#include "b2Settings.h"
|
||||||
|
|
||||||
|
const int32 b2_chunkSize = 16 * 1024;
|
||||||
|
const int32 b2_maxBlockSize = 640;
|
||||||
|
const int32 b2_blockSizes = 14;
|
||||||
|
const int32 b2_chunkArrayIncrement = 128;
|
||||||
|
|
||||||
|
struct b2Block;
|
||||||
|
struct b2Chunk;
|
||||||
|
|
||||||
|
/// This is a small object allocator used for allocating small
|
||||||
|
/// objects that persist for more than one time step.
|
||||||
|
/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
|
||||||
|
class b2BlockAllocator
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2BlockAllocator();
|
||||||
|
~b2BlockAllocator();
|
||||||
|
|
||||||
|
/// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
|
||||||
|
void* Allocate(int32 size);
|
||||||
|
|
||||||
|
/// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
|
||||||
|
void Free(void* p, int32 size);
|
||||||
|
|
||||||
|
void Clear();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
b2Chunk* m_chunks;
|
||||||
|
int32 m_chunkCount;
|
||||||
|
int32 m_chunkSpace;
|
||||||
|
|
||||||
|
b2Block* m_freeLists[b2_blockSizes];
|
||||||
|
|
||||||
|
static int32 s_blockSizes[b2_blockSizes];
|
||||||
|
static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];
|
||||||
|
static bool s_blockSizeLookupInitialized;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2011 Erin Catto http://box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Draw.h"
|
||||||
|
|
||||||
|
b2Draw::b2Draw()
|
||||||
|
{
|
||||||
|
m_drawFlags = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Draw::SetFlags(uint32 flags)
|
||||||
|
{
|
||||||
|
m_drawFlags = flags;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32 b2Draw::GetFlags() const
|
||||||
|
{
|
||||||
|
return m_drawFlags;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Draw::AppendFlags(uint32 flags)
|
||||||
|
{
|
||||||
|
m_drawFlags |= flags;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Draw::ClearFlags(uint32 flags)
|
||||||
|
{
|
||||||
|
m_drawFlags &= ~flags;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,97 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2011 Erin Catto http://box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_DRAW_H
|
||||||
|
#define B2_DRAW_H
|
||||||
|
|
||||||
|
#include "b2Math.h"
|
||||||
|
|
||||||
|
/// Color for debug drawing. Each value has the range [0,1].
|
||||||
|
struct b2Color
|
||||||
|
{
|
||||||
|
b2Color() {}
|
||||||
|
b2Color(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f)
|
||||||
|
{
|
||||||
|
r = rIn; g = gIn; b = bIn; a = aIn;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Set(float32 rIn, float32 gIn, float32 bIn, float32 aIn = 1.0f)
|
||||||
|
{
|
||||||
|
r = rIn; g = gIn; b = bIn; a = aIn;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 r, g, b, a;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Implement and register this class with a b2World to provide debug drawing of physics
|
||||||
|
/// entities in your game.
|
||||||
|
class b2Draw
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Draw();
|
||||||
|
|
||||||
|
virtual ~b2Draw() {}
|
||||||
|
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
e_shapeBit = 0x0001, ///< draw shapes
|
||||||
|
e_jointBit = 0x0002, ///< draw joint connections
|
||||||
|
e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes
|
||||||
|
e_pairBit = 0x0008, ///< draw broad-phase pairs
|
||||||
|
e_centerOfMassBit = 0x0010 ///< draw center of mass frame
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Set the drawing flags.
|
||||||
|
void SetFlags(uint32 flags);
|
||||||
|
|
||||||
|
/// Get the drawing flags.
|
||||||
|
uint32 GetFlags() const;
|
||||||
|
|
||||||
|
/// Append flags to the current flags.
|
||||||
|
void AppendFlags(uint32 flags);
|
||||||
|
|
||||||
|
/// Clear flags from the current flags.
|
||||||
|
void ClearFlags(uint32 flags);
|
||||||
|
|
||||||
|
/// Draw a closed polygon provided in CCW order.
|
||||||
|
virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
/// Draw a solid closed polygon provided in CCW order.
|
||||||
|
virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
/// Draw a circle.
|
||||||
|
virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
/// Draw a solid circle.
|
||||||
|
virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
/// Draw a line segment.
|
||||||
|
virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
/// Draw a transform. Choose your own length scale.
|
||||||
|
/// @param xf a transform.
|
||||||
|
virtual void DrawTransform(const b2Transform& xf) = 0;
|
||||||
|
|
||||||
|
/// Draw a point.
|
||||||
|
virtual void DrawPoint(const b2Vec2& p, float32 size, const b2Color& color) = 0;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
uint32 m_drawFlags;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,85 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_GROWABLE_STACK_H
|
||||||
|
#define B2_GROWABLE_STACK_H
|
||||||
|
#include "b2Settings.h"
|
||||||
|
#include <string.h>
|
||||||
|
|
||||||
|
/// This is a growable LIFO stack with an initial capacity of N.
|
||||||
|
/// If the stack size exceeds the initial capacity, the heap is used
|
||||||
|
/// to increase the size of the stack.
|
||||||
|
template <typename T, int32 N>
|
||||||
|
class b2GrowableStack
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2GrowableStack()
|
||||||
|
{
|
||||||
|
m_stack = m_array;
|
||||||
|
m_count = 0;
|
||||||
|
m_capacity = N;
|
||||||
|
}
|
||||||
|
|
||||||
|
~b2GrowableStack()
|
||||||
|
{
|
||||||
|
if (m_stack != m_array)
|
||||||
|
{
|
||||||
|
b2Free(m_stack);
|
||||||
|
m_stack = nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void Push(const T& element)
|
||||||
|
{
|
||||||
|
if (m_count == m_capacity)
|
||||||
|
{
|
||||||
|
T* old = m_stack;
|
||||||
|
m_capacity *= 2;
|
||||||
|
m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
|
||||||
|
memcpy(m_stack, old, m_count * sizeof(T));
|
||||||
|
if (old != m_array)
|
||||||
|
{
|
||||||
|
b2Free(old);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
m_stack[m_count] = element;
|
||||||
|
++m_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
T Pop()
|
||||||
|
{
|
||||||
|
b2Assert(m_count > 0);
|
||||||
|
--m_count;
|
||||||
|
return m_stack[m_count];
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 GetCount()
|
||||||
|
{
|
||||||
|
return m_count;
|
||||||
|
}
|
||||||
|
|
||||||
|
private:
|
||||||
|
T* m_stack;
|
||||||
|
T m_array[N];
|
||||||
|
int32 m_count;
|
||||||
|
int32 m_capacity;
|
||||||
|
};
|
||||||
|
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,94 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Math.h"
|
||||||
|
|
||||||
|
const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
|
||||||
|
|
||||||
|
/// Solve A * x = b, where b is a column vector. This is more efficient
|
||||||
|
/// than computing the inverse in one-shot cases.
|
||||||
|
b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
|
||||||
|
{
|
||||||
|
float32 det = b2Dot(ex, b2Cross(ey, ez));
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
b2Vec3 x;
|
||||||
|
x.x = det * b2Dot(b, b2Cross(ey, ez));
|
||||||
|
x.y = det * b2Dot(ex, b2Cross(b, ez));
|
||||||
|
x.z = det * b2Dot(ex, b2Cross(ey, b));
|
||||||
|
return x;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Solve A * x = b, where b is a column vector. This is more efficient
|
||||||
|
/// than computing the inverse in one-shot cases.
|
||||||
|
b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
|
||||||
|
{
|
||||||
|
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
|
||||||
|
float32 det = a11 * a22 - a12 * a21;
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
b2Vec2 x;
|
||||||
|
x.x = det * (a22 * b.x - a12 * b.y);
|
||||||
|
x.y = det * (a11 * b.y - a21 * b.x);
|
||||||
|
return x;
|
||||||
|
}
|
||||||
|
|
||||||
|
///
|
||||||
|
void b2Mat33::GetInverse22(b2Mat33* M) const
|
||||||
|
{
|
||||||
|
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
|
||||||
|
float32 det = a * d - b * c;
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
|
||||||
|
M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f;
|
||||||
|
M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f;
|
||||||
|
M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Returns the zero matrix if singular.
|
||||||
|
void b2Mat33::GetSymInverse33(b2Mat33* M) const
|
||||||
|
{
|
||||||
|
float32 det = b2Dot(ex, b2Cross(ey, ez));
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
|
||||||
|
float32 a22 = ey.y, a23 = ez.y;
|
||||||
|
float32 a33 = ez.z;
|
||||||
|
|
||||||
|
M->ex.x = det * (a22 * a33 - a23 * a23);
|
||||||
|
M->ex.y = det * (a13 * a23 - a12 * a33);
|
||||||
|
M->ex.z = det * (a12 * a23 - a13 * a22);
|
||||||
|
|
||||||
|
M->ey.x = M->ex.y;
|
||||||
|
M->ey.y = det * (a11 * a33 - a13 * a13);
|
||||||
|
M->ey.z = det * (a13 * a12 - a11 * a23);
|
||||||
|
|
||||||
|
M->ez.x = M->ex.z;
|
||||||
|
M->ez.y = M->ey.z;
|
||||||
|
M->ez.z = det * (a11 * a22 - a12 * a12);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,707 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_MATH_H
|
||||||
|
#define B2_MATH_H
|
||||||
|
|
||||||
|
#include "b2Settings.h"
|
||||||
|
#include <math.h>
|
||||||
|
|
||||||
|
/// This function is used to ensure that a floating point number is not a NaN or infinity.
|
||||||
|
inline bool b2IsValid(float32 x)
|
||||||
|
{
|
||||||
|
return isfinite(x);
|
||||||
|
}
|
||||||
|
|
||||||
|
#define b2Sqrt(x) sqrtf(x)
|
||||||
|
#define b2Atan2(y, x) atan2f(y, x)
|
||||||
|
|
||||||
|
/// A 2D column vector.
|
||||||
|
struct b2Vec2
|
||||||
|
{
|
||||||
|
/// Default constructor does nothing (for performance).
|
||||||
|
b2Vec2() {}
|
||||||
|
|
||||||
|
/// Construct using coordinates.
|
||||||
|
b2Vec2(float32 xIn, float32 yIn) : x(xIn), y(yIn) {}
|
||||||
|
|
||||||
|
/// Set this vector to all zeros.
|
||||||
|
void SetZero() { x = 0.0f; y = 0.0f; }
|
||||||
|
|
||||||
|
/// Set this vector to some specified coordinates.
|
||||||
|
void Set(float32 x_, float32 y_) { x = x_; y = y_; }
|
||||||
|
|
||||||
|
/// Negate this vector.
|
||||||
|
b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
|
||||||
|
|
||||||
|
/// Read from and indexed element.
|
||||||
|
float32 operator () (int32 i) const
|
||||||
|
{
|
||||||
|
return (&x)[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Write to an indexed element.
|
||||||
|
float32& operator () (int32 i)
|
||||||
|
{
|
||||||
|
return (&x)[i];
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Add a vector to this vector.
|
||||||
|
void operator += (const b2Vec2& v)
|
||||||
|
{
|
||||||
|
x += v.x; y += v.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Subtract a vector from this vector.
|
||||||
|
void operator -= (const b2Vec2& v)
|
||||||
|
{
|
||||||
|
x -= v.x; y -= v.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply this vector by a scalar.
|
||||||
|
void operator *= (float32 a)
|
||||||
|
{
|
||||||
|
x *= a; y *= a;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the length of this vector (the norm).
|
||||||
|
float32 Length() const
|
||||||
|
{
|
||||||
|
return b2Sqrt(x * x + y * y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the length squared. For performance, use this instead of
|
||||||
|
/// b2Vec2::Length (if possible).
|
||||||
|
float32 LengthSquared() const
|
||||||
|
{
|
||||||
|
return x * x + y * y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Convert this vector into a unit vector. Returns the length.
|
||||||
|
float32 Normalize()
|
||||||
|
{
|
||||||
|
float32 length = Length();
|
||||||
|
if (length < b2_epsilon)
|
||||||
|
{
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
float32 invLength = 1.0f / length;
|
||||||
|
x *= invLength;
|
||||||
|
y *= invLength;
|
||||||
|
|
||||||
|
return length;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Does this vector contain finite coordinates?
|
||||||
|
bool IsValid() const
|
||||||
|
{
|
||||||
|
return b2IsValid(x) && b2IsValid(y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
|
||||||
|
b2Vec2 Skew() const
|
||||||
|
{
|
||||||
|
return b2Vec2(-y, x);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 x, y;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A 2D column vector with 3 elements.
|
||||||
|
struct b2Vec3
|
||||||
|
{
|
||||||
|
/// Default constructor does nothing (for performance).
|
||||||
|
b2Vec3() {}
|
||||||
|
|
||||||
|
/// Construct using coordinates.
|
||||||
|
b2Vec3(float32 xIn, float32 yIn, float32 zIn) : x(xIn), y(yIn), z(zIn) {}
|
||||||
|
|
||||||
|
/// Set this vector to all zeros.
|
||||||
|
void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
|
||||||
|
|
||||||
|
/// Set this vector to some specified coordinates.
|
||||||
|
void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
|
||||||
|
|
||||||
|
/// Negate this vector.
|
||||||
|
b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
|
||||||
|
|
||||||
|
/// Add a vector to this vector.
|
||||||
|
void operator += (const b2Vec3& v)
|
||||||
|
{
|
||||||
|
x += v.x; y += v.y; z += v.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Subtract a vector from this vector.
|
||||||
|
void operator -= (const b2Vec3& v)
|
||||||
|
{
|
||||||
|
x -= v.x; y -= v.y; z -= v.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply this vector by a scalar.
|
||||||
|
void operator *= (float32 s)
|
||||||
|
{
|
||||||
|
x *= s; y *= s; z *= s;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 x, y, z;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A 2-by-2 matrix. Stored in column-major order.
|
||||||
|
struct b2Mat22
|
||||||
|
{
|
||||||
|
/// The default constructor does nothing (for performance).
|
||||||
|
b2Mat22() {}
|
||||||
|
|
||||||
|
/// Construct this matrix using columns.
|
||||||
|
b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
|
||||||
|
{
|
||||||
|
ex = c1;
|
||||||
|
ey = c2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Construct this matrix using scalars.
|
||||||
|
b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
|
||||||
|
{
|
||||||
|
ex.x = a11; ex.y = a21;
|
||||||
|
ey.x = a12; ey.y = a22;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize this matrix using columns.
|
||||||
|
void Set(const b2Vec2& c1, const b2Vec2& c2)
|
||||||
|
{
|
||||||
|
ex = c1;
|
||||||
|
ey = c2;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set this to the identity matrix.
|
||||||
|
void SetIdentity()
|
||||||
|
{
|
||||||
|
ex.x = 1.0f; ey.x = 0.0f;
|
||||||
|
ex.y = 0.0f; ey.y = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set this matrix to all zeros.
|
||||||
|
void SetZero()
|
||||||
|
{
|
||||||
|
ex.x = 0.0f; ey.x = 0.0f;
|
||||||
|
ex.y = 0.0f; ey.y = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Mat22 GetInverse() const
|
||||||
|
{
|
||||||
|
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
|
||||||
|
b2Mat22 B;
|
||||||
|
float32 det = a * d - b * c;
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
B.ex.x = det * d; B.ey.x = -det * b;
|
||||||
|
B.ex.y = -det * c; B.ey.y = det * a;
|
||||||
|
return B;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Solve A * x = b, where b is a column vector. This is more efficient
|
||||||
|
/// than computing the inverse in one-shot cases.
|
||||||
|
b2Vec2 Solve(const b2Vec2& b) const
|
||||||
|
{
|
||||||
|
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
|
||||||
|
float32 det = a11 * a22 - a12 * a21;
|
||||||
|
if (det != 0.0f)
|
||||||
|
{
|
||||||
|
det = 1.0f / det;
|
||||||
|
}
|
||||||
|
b2Vec2 x;
|
||||||
|
x.x = det * (a22 * b.x - a12 * b.y);
|
||||||
|
x.y = det * (a11 * b.y - a21 * b.x);
|
||||||
|
return x;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 ex, ey;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A 3-by-3 matrix. Stored in column-major order.
|
||||||
|
struct b2Mat33
|
||||||
|
{
|
||||||
|
/// The default constructor does nothing (for performance).
|
||||||
|
b2Mat33() {}
|
||||||
|
|
||||||
|
/// Construct this matrix using columns.
|
||||||
|
b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
|
||||||
|
{
|
||||||
|
ex = c1;
|
||||||
|
ey = c2;
|
||||||
|
ez = c3;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set this matrix to all zeros.
|
||||||
|
void SetZero()
|
||||||
|
{
|
||||||
|
ex.SetZero();
|
||||||
|
ey.SetZero();
|
||||||
|
ez.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Solve A * x = b, where b is a column vector. This is more efficient
|
||||||
|
/// than computing the inverse in one-shot cases.
|
||||||
|
b2Vec3 Solve33(const b2Vec3& b) const;
|
||||||
|
|
||||||
|
/// Solve A * x = b, where b is a column vector. This is more efficient
|
||||||
|
/// than computing the inverse in one-shot cases. Solve only the upper
|
||||||
|
/// 2-by-2 matrix equation.
|
||||||
|
b2Vec2 Solve22(const b2Vec2& b) const;
|
||||||
|
|
||||||
|
/// Get the inverse of this matrix as a 2-by-2.
|
||||||
|
/// Returns the zero matrix if singular.
|
||||||
|
void GetInverse22(b2Mat33* M) const;
|
||||||
|
|
||||||
|
/// Get the symmetric inverse of this matrix as a 3-by-3.
|
||||||
|
/// Returns the zero matrix if singular.
|
||||||
|
void GetSymInverse33(b2Mat33* M) const;
|
||||||
|
|
||||||
|
b2Vec3 ex, ey, ez;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Rotation
|
||||||
|
struct b2Rot
|
||||||
|
{
|
||||||
|
b2Rot() {}
|
||||||
|
|
||||||
|
/// Initialize from an angle in radians
|
||||||
|
explicit b2Rot(float32 angle)
|
||||||
|
{
|
||||||
|
/// TODO_ERIN optimize
|
||||||
|
s = sinf(angle);
|
||||||
|
c = cosf(angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set using an angle in radians.
|
||||||
|
void Set(float32 angle)
|
||||||
|
{
|
||||||
|
/// TODO_ERIN optimize
|
||||||
|
s = sinf(angle);
|
||||||
|
c = cosf(angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set to the identity rotation
|
||||||
|
void SetIdentity()
|
||||||
|
{
|
||||||
|
s = 0.0f;
|
||||||
|
c = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the angle in radians
|
||||||
|
float32 GetAngle() const
|
||||||
|
{
|
||||||
|
return b2Atan2(s, c);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the x-axis
|
||||||
|
b2Vec2 GetXAxis() const
|
||||||
|
{
|
||||||
|
return b2Vec2(c, s);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the u-axis
|
||||||
|
b2Vec2 GetYAxis() const
|
||||||
|
{
|
||||||
|
return b2Vec2(-s, c);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Sine and cosine
|
||||||
|
float32 s, c;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A transform contains translation and rotation. It is used to represent
|
||||||
|
/// the position and orientation of rigid frames.
|
||||||
|
struct b2Transform
|
||||||
|
{
|
||||||
|
/// The default constructor does nothing.
|
||||||
|
b2Transform() {}
|
||||||
|
|
||||||
|
/// Initialize using a position vector and a rotation.
|
||||||
|
b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
|
||||||
|
|
||||||
|
/// Set this to the identity transform.
|
||||||
|
void SetIdentity()
|
||||||
|
{
|
||||||
|
p.SetZero();
|
||||||
|
q.SetIdentity();
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Set this based on the position and angle.
|
||||||
|
void Set(const b2Vec2& position, float32 angle)
|
||||||
|
{
|
||||||
|
p = position;
|
||||||
|
q.Set(angle);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 p;
|
||||||
|
b2Rot q;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This describes the motion of a body/shape for TOI computation.
|
||||||
|
/// Shapes are defined with respect to the body origin, which may
|
||||||
|
/// no coincide with the center of mass. However, to support dynamics
|
||||||
|
/// we must interpolate the center of mass position.
|
||||||
|
struct b2Sweep
|
||||||
|
{
|
||||||
|
/// Get the interpolated transform at a specific time.
|
||||||
|
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
|
||||||
|
void GetTransform(b2Transform* xfb, float32 beta) const;
|
||||||
|
|
||||||
|
/// Advance the sweep forward, yielding a new initial state.
|
||||||
|
/// @param alpha the new initial time.
|
||||||
|
void Advance(float32 alpha);
|
||||||
|
|
||||||
|
/// Normalize the angles.
|
||||||
|
void Normalize();
|
||||||
|
|
||||||
|
b2Vec2 localCenter; ///< local center of mass position
|
||||||
|
b2Vec2 c0, c; ///< center world positions
|
||||||
|
float32 a0, a; ///< world angles
|
||||||
|
|
||||||
|
/// Fraction of the current time step in the range [0,1]
|
||||||
|
/// c0 and a0 are the positions at alpha0.
|
||||||
|
float32 alpha0;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Useful constant
|
||||||
|
extern const b2Vec2 b2Vec2_zero;
|
||||||
|
|
||||||
|
/// Perform the dot product on two vectors.
|
||||||
|
inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return a.x * b.x + a.y * b.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the cross product on two vectors. In 2D this produces a scalar.
|
||||||
|
inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return a.x * b.y - a.y * b.x;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the cross product on a vector and a scalar. In 2D this produces
|
||||||
|
/// a vector.
|
||||||
|
inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
|
||||||
|
{
|
||||||
|
return b2Vec2(s * a.y, -s * a.x);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the cross product on a scalar and a vector. In 2D this produces
|
||||||
|
/// a vector.
|
||||||
|
inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
|
||||||
|
{
|
||||||
|
return b2Vec2(-s * a.y, s * a.x);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply a matrix times a vector. If a rotation matrix is provided,
|
||||||
|
/// then this transforms the vector from one frame to another.
|
||||||
|
inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
|
||||||
|
/// then this transforms the vector from one frame to another (inverse transform).
|
||||||
|
inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Add two vectors component-wise.
|
||||||
|
inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return b2Vec2(a.x + b.x, a.y + b.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Subtract two vectors component-wise.
|
||||||
|
inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return b2Vec2(a.x - b.x, a.y - b.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 operator * (float32 s, const b2Vec2& a)
|
||||||
|
{
|
||||||
|
return b2Vec2(s * a.x, s * a.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool operator == (const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return a.x == b.x && a.y == b.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool operator != (const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return a.x != b.x || a.y != b.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
b2Vec2 c = a - b;
|
||||||
|
return c.Length();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
b2Vec2 c = a - b;
|
||||||
|
return b2Dot(c, c);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec3 operator * (float32 s, const b2Vec3& a)
|
||||||
|
{
|
||||||
|
return b2Vec3(s * a.x, s * a.y, s * a.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Add two vectors component-wise.
|
||||||
|
inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
|
||||||
|
{
|
||||||
|
return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Subtract two vectors component-wise.
|
||||||
|
inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
|
||||||
|
{
|
||||||
|
return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the dot product on two vectors.
|
||||||
|
inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
|
||||||
|
{
|
||||||
|
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Perform the cross product on two vectors.
|
||||||
|
inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
|
||||||
|
{
|
||||||
|
return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
|
||||||
|
{
|
||||||
|
return b2Mat22(A.ex + B.ex, A.ey + B.ey);
|
||||||
|
}
|
||||||
|
|
||||||
|
// A * B
|
||||||
|
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
|
||||||
|
{
|
||||||
|
return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
|
||||||
|
}
|
||||||
|
|
||||||
|
// A^T * B
|
||||||
|
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
|
||||||
|
{
|
||||||
|
b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
|
||||||
|
b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
|
||||||
|
return b2Mat22(c1, c2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply a matrix times a vector.
|
||||||
|
inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
|
||||||
|
{
|
||||||
|
return v.x * A.ex + v.y * A.ey + v.z * A.ez;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply a matrix times a vector.
|
||||||
|
inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Multiply two rotations: q * r
|
||||||
|
inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
|
||||||
|
{
|
||||||
|
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
|
||||||
|
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
|
||||||
|
// s = qs * rc + qc * rs
|
||||||
|
// c = qc * rc - qs * rs
|
||||||
|
b2Rot qr;
|
||||||
|
qr.s = q.s * r.c + q.c * r.s;
|
||||||
|
qr.c = q.c * r.c - q.s * r.s;
|
||||||
|
return qr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Transpose multiply two rotations: qT * r
|
||||||
|
inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
|
||||||
|
{
|
||||||
|
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
|
||||||
|
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
|
||||||
|
// s = qc * rs - qs * rc
|
||||||
|
// c = qc * rc + qs * rs
|
||||||
|
b2Rot qr;
|
||||||
|
qr.s = q.c * r.s - q.s * r.c;
|
||||||
|
qr.c = q.c * r.c + q.s * r.s;
|
||||||
|
return qr;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Rotate a vector
|
||||||
|
inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Inverse rotate a vector
|
||||||
|
inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
|
||||||
|
float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
|
||||||
|
|
||||||
|
return b2Vec2(x, y);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
|
||||||
|
{
|
||||||
|
float32 px = v.x - T.p.x;
|
||||||
|
float32 py = v.y - T.p.y;
|
||||||
|
float32 x = (T.q.c * px + T.q.s * py);
|
||||||
|
float32 y = (-T.q.s * px + T.q.c * py);
|
||||||
|
|
||||||
|
return b2Vec2(x, y);
|
||||||
|
}
|
||||||
|
|
||||||
|
// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
|
||||||
|
// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
|
||||||
|
inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
|
||||||
|
{
|
||||||
|
b2Transform C;
|
||||||
|
C.q = b2Mul(A.q, B.q);
|
||||||
|
C.p = b2Mul(A.q, B.p) + A.p;
|
||||||
|
return C;
|
||||||
|
}
|
||||||
|
|
||||||
|
// v2 = A.q' * (B.q * v1 + B.p - A.p)
|
||||||
|
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
|
||||||
|
inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
|
||||||
|
{
|
||||||
|
b2Transform C;
|
||||||
|
C.q = b2MulT(A.q, B.q);
|
||||||
|
C.p = b2MulT(A.q, B.p - A.p);
|
||||||
|
return C;
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline T b2Abs(T a)
|
||||||
|
{
|
||||||
|
return a > T(0) ? a : -a;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Abs(const b2Vec2& a)
|
||||||
|
{
|
||||||
|
return b2Vec2(b2Abs(a.x), b2Abs(a.y));
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Mat22 b2Abs(const b2Mat22& A)
|
||||||
|
{
|
||||||
|
return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline T b2Min(T a, T b)
|
||||||
|
{
|
||||||
|
return a < b ? a : b;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline T b2Max(T a, T b)
|
||||||
|
{
|
||||||
|
return a > b ? a : b;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
|
||||||
|
{
|
||||||
|
return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
|
||||||
|
}
|
||||||
|
|
||||||
|
template <typename T>
|
||||||
|
inline T b2Clamp(T a, T low, T high)
|
||||||
|
{
|
||||||
|
return b2Max(low, b2Min(a, high));
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
|
||||||
|
{
|
||||||
|
return b2Max(low, b2Min(a, high));
|
||||||
|
}
|
||||||
|
|
||||||
|
template<typename T> inline void b2Swap(T& a, T& b)
|
||||||
|
{
|
||||||
|
T tmp = a;
|
||||||
|
a = b;
|
||||||
|
b = tmp;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// "Next Largest Power of 2
|
||||||
|
/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
|
||||||
|
/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
|
||||||
|
/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
|
||||||
|
/// largest power of 2. For a 32-bit value:"
|
||||||
|
inline uint32 b2NextPowerOfTwo(uint32 x)
|
||||||
|
{
|
||||||
|
x |= (x >> 1);
|
||||||
|
x |= (x >> 2);
|
||||||
|
x |= (x >> 4);
|
||||||
|
x |= (x >> 8);
|
||||||
|
x |= (x >> 16);
|
||||||
|
return x + 1;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2IsPowerOfTwo(uint32 x)
|
||||||
|
{
|
||||||
|
bool result = x > 0 && (x & (x - 1)) == 0;
|
||||||
|
return result;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
|
||||||
|
{
|
||||||
|
xf->p = (1.0f - beta) * c0 + beta * c;
|
||||||
|
float32 angle = (1.0f - beta) * a0 + beta * a;
|
||||||
|
xf->q.Set(angle);
|
||||||
|
|
||||||
|
// Shift to origin
|
||||||
|
xf->p -= b2Mul(xf->q, localCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Sweep::Advance(float32 alpha)
|
||||||
|
{
|
||||||
|
b2Assert(alpha0 < 1.0f);
|
||||||
|
float32 beta = (alpha - alpha0) / (1.0f - alpha0);
|
||||||
|
c0 += beta * (c - c0);
|
||||||
|
a0 += beta * (a - a0);
|
||||||
|
alpha0 = alpha;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Normalize an angle in radians to be between -pi and pi
|
||||||
|
inline void b2Sweep::Normalize()
|
||||||
|
{
|
||||||
|
float32 twoPi = 2.0f * b2_pi;
|
||||||
|
float32 d = twoPi * floorf(a0 / twoPi);
|
||||||
|
a0 -= d;
|
||||||
|
a -= d;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,44 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Settings.h"
|
||||||
|
#include <stdio.h>
|
||||||
|
#include <stdarg.h>
|
||||||
|
#include <stdlib.h>
|
||||||
|
|
||||||
|
b2Version b2_version = {2, 3, 2};
|
||||||
|
|
||||||
|
// Memory allocators. Modify these to use your own allocator.
|
||||||
|
void* b2Alloc(int32 size)
|
||||||
|
{
|
||||||
|
return malloc(size);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Free(void* mem)
|
||||||
|
{
|
||||||
|
free(mem);
|
||||||
|
}
|
||||||
|
|
||||||
|
// You can modify this to use your logging facility.
|
||||||
|
void b2Log(const char* string, ...)
|
||||||
|
{
|
||||||
|
va_list args;
|
||||||
|
va_start(args, string);
|
||||||
|
vprintf(string, args);
|
||||||
|
va_end(args);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,155 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_SETTINGS_H
|
||||||
|
#define B2_SETTINGS_H
|
||||||
|
|
||||||
|
#include <stddef.h>
|
||||||
|
#include <assert.h>
|
||||||
|
#include <float.h>
|
||||||
|
|
||||||
|
#if !defined(NDEBUG)
|
||||||
|
#define b2DEBUG
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#define B2_NOT_USED(x) ((void)(x))
|
||||||
|
#define b2Assert(A) assert(A)
|
||||||
|
|
||||||
|
typedef signed char int8;
|
||||||
|
typedef signed short int16;
|
||||||
|
typedef signed int int32;
|
||||||
|
typedef unsigned char uint8;
|
||||||
|
typedef unsigned short uint16;
|
||||||
|
typedef unsigned int uint32;
|
||||||
|
typedef float float32;
|
||||||
|
typedef double float64;
|
||||||
|
|
||||||
|
#define b2_maxFloat FLT_MAX
|
||||||
|
#define b2_epsilon FLT_EPSILON
|
||||||
|
#define b2_pi 3.14159265359f
|
||||||
|
|
||||||
|
/// @file
|
||||||
|
/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
|
||||||
|
///
|
||||||
|
|
||||||
|
// Collision
|
||||||
|
|
||||||
|
/// The maximum number of contact points between two convex shapes. Do
|
||||||
|
/// not change this value.
|
||||||
|
#define b2_maxManifoldPoints 2
|
||||||
|
|
||||||
|
/// The maximum number of vertices on a convex polygon. You cannot increase
|
||||||
|
/// this too much because b2BlockAllocator has a maximum object size.
|
||||||
|
#define b2_maxPolygonVertices 8
|
||||||
|
|
||||||
|
/// This is used to fatten AABBs in the dynamic tree. This allows proxies
|
||||||
|
/// to move by a small amount without triggering a tree adjustment.
|
||||||
|
/// This is in meters.
|
||||||
|
#define b2_aabbExtension 0.1f
|
||||||
|
|
||||||
|
/// This is used to fatten AABBs in the dynamic tree. This is used to predict
|
||||||
|
/// the future position based on the current displacement.
|
||||||
|
/// This is a dimensionless multiplier.
|
||||||
|
#define b2_aabbMultiplier 2.0f
|
||||||
|
|
||||||
|
/// A small length used as a collision and constraint tolerance. Usually it is
|
||||||
|
/// chosen to be numerically significant, but visually insignificant.
|
||||||
|
#define b2_linearSlop 0.005f
|
||||||
|
|
||||||
|
/// A small angle used as a collision and constraint tolerance. Usually it is
|
||||||
|
/// chosen to be numerically significant, but visually insignificant.
|
||||||
|
#define b2_angularSlop (2.0f / 180.0f * b2_pi)
|
||||||
|
|
||||||
|
/// The radius of the polygon/edge shape skin. This should not be modified. Making
|
||||||
|
/// this smaller means polygons will have an insufficient buffer for continuous collision.
|
||||||
|
/// Making it larger may create artifacts for vertex collision.
|
||||||
|
#define b2_polygonRadius (2.0f * b2_linearSlop)
|
||||||
|
|
||||||
|
/// Maximum number of sub-steps per contact in continuous physics simulation.
|
||||||
|
#define b2_maxSubSteps 8
|
||||||
|
|
||||||
|
|
||||||
|
// Dynamics
|
||||||
|
|
||||||
|
/// Maximum number of contacts to be handled to solve a TOI impact.
|
||||||
|
#define b2_maxTOIContacts 32
|
||||||
|
|
||||||
|
/// A velocity threshold for elastic collisions. Any collision with a relative linear
|
||||||
|
/// velocity below this threshold will be treated as inelastic.
|
||||||
|
#define b2_velocityThreshold 1.0f
|
||||||
|
|
||||||
|
/// The maximum linear position correction used when solving constraints. This helps to
|
||||||
|
/// prevent overshoot.
|
||||||
|
#define b2_maxLinearCorrection 0.2f
|
||||||
|
|
||||||
|
/// The maximum angular position correction used when solving constraints. This helps to
|
||||||
|
/// prevent overshoot.
|
||||||
|
#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi)
|
||||||
|
|
||||||
|
/// The maximum linear velocity of a body. This limit is very large and is used
|
||||||
|
/// to prevent numerical problems. You shouldn't need to adjust this.
|
||||||
|
#define b2_maxTranslation 2.0f
|
||||||
|
#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation)
|
||||||
|
|
||||||
|
/// The maximum angular velocity of a body. This limit is very large and is used
|
||||||
|
/// to prevent numerical problems. You shouldn't need to adjust this.
|
||||||
|
#define b2_maxRotation (0.5f * b2_pi)
|
||||||
|
#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation)
|
||||||
|
|
||||||
|
/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
|
||||||
|
/// that overlap is removed in one time step. However using values close to 1 often lead
|
||||||
|
/// to overshoot.
|
||||||
|
#define b2_baumgarte 0.2f
|
||||||
|
#define b2_toiBaugarte 0.75f
|
||||||
|
|
||||||
|
|
||||||
|
// Sleep
|
||||||
|
|
||||||
|
/// The time that a body must be still before it will go to sleep.
|
||||||
|
#define b2_timeToSleep 0.5f
|
||||||
|
|
||||||
|
/// A body cannot sleep if its linear velocity is above this tolerance.
|
||||||
|
#define b2_linearSleepTolerance 0.01f
|
||||||
|
|
||||||
|
/// A body cannot sleep if its angular velocity is above this tolerance.
|
||||||
|
#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi)
|
||||||
|
|
||||||
|
// Memory Allocation
|
||||||
|
|
||||||
|
/// Implement this function to use your own memory allocator.
|
||||||
|
void* b2Alloc(int32 size);
|
||||||
|
|
||||||
|
/// If you implement b2Alloc, you should also implement this function.
|
||||||
|
void b2Free(void* mem);
|
||||||
|
|
||||||
|
/// Logging function.
|
||||||
|
void b2Log(const char* string, ...);
|
||||||
|
|
||||||
|
/// Version numbering scheme.
|
||||||
|
/// See http://en.wikipedia.org/wiki/Software_versioning
|
||||||
|
struct b2Version
|
||||||
|
{
|
||||||
|
int32 major; ///< significant changes
|
||||||
|
int32 minor; ///< incremental changes
|
||||||
|
int32 revision; ///< bug fixes
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Current version.
|
||||||
|
extern b2Version b2_version;
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,83 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2StackAllocator.h"
|
||||||
|
#include "Box2D/Common/b2Math.h"
|
||||||
|
|
||||||
|
b2StackAllocator::b2StackAllocator()
|
||||||
|
{
|
||||||
|
m_index = 0;
|
||||||
|
m_allocation = 0;
|
||||||
|
m_maxAllocation = 0;
|
||||||
|
m_entryCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2StackAllocator::~b2StackAllocator()
|
||||||
|
{
|
||||||
|
b2Assert(m_index == 0);
|
||||||
|
b2Assert(m_entryCount == 0);
|
||||||
|
}
|
||||||
|
|
||||||
|
void* b2StackAllocator::Allocate(int32 size)
|
||||||
|
{
|
||||||
|
b2Assert(m_entryCount < b2_maxStackEntries);
|
||||||
|
|
||||||
|
b2StackEntry* entry = m_entries + m_entryCount;
|
||||||
|
entry->size = size;
|
||||||
|
if (m_index + size > b2_stackSize)
|
||||||
|
{
|
||||||
|
entry->data = (char*)b2Alloc(size);
|
||||||
|
entry->usedMalloc = true;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
entry->data = m_data + m_index;
|
||||||
|
entry->usedMalloc = false;
|
||||||
|
m_index += size;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_allocation += size;
|
||||||
|
m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
|
||||||
|
++m_entryCount;
|
||||||
|
|
||||||
|
return entry->data;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2StackAllocator::Free(void* p)
|
||||||
|
{
|
||||||
|
b2Assert(m_entryCount > 0);
|
||||||
|
b2StackEntry* entry = m_entries + m_entryCount - 1;
|
||||||
|
b2Assert(p == entry->data);
|
||||||
|
if (entry->usedMalloc)
|
||||||
|
{
|
||||||
|
b2Free(p);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_index -= entry->size;
|
||||||
|
}
|
||||||
|
m_allocation -= entry->size;
|
||||||
|
--m_entryCount;
|
||||||
|
|
||||||
|
p = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 b2StackAllocator::GetMaxAllocation() const
|
||||||
|
{
|
||||||
|
return m_maxAllocation;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,60 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_STACK_ALLOCATOR_H
|
||||||
|
#define B2_STACK_ALLOCATOR_H
|
||||||
|
|
||||||
|
#include "b2Settings.h"
|
||||||
|
|
||||||
|
const int32 b2_stackSize = 100 * 1024; // 100k
|
||||||
|
const int32 b2_maxStackEntries = 32;
|
||||||
|
|
||||||
|
struct b2StackEntry
|
||||||
|
{
|
||||||
|
char* data;
|
||||||
|
int32 size;
|
||||||
|
bool usedMalloc;
|
||||||
|
};
|
||||||
|
|
||||||
|
// This is a stack allocator used for fast per step allocations.
|
||||||
|
// You must nest allocate/free pairs. The code will assert
|
||||||
|
// if you try to interleave multiple allocate/free pairs.
|
||||||
|
class b2StackAllocator
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2StackAllocator();
|
||||||
|
~b2StackAllocator();
|
||||||
|
|
||||||
|
void* Allocate(int32 size);
|
||||||
|
void Free(void* p);
|
||||||
|
|
||||||
|
int32 GetMaxAllocation() const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
char m_data[b2_stackSize];
|
||||||
|
int32 m_index;
|
||||||
|
|
||||||
|
int32 m_allocation;
|
||||||
|
int32 m_maxAllocation;
|
||||||
|
|
||||||
|
b2StackEntry m_entries[b2_maxStackEntries];
|
||||||
|
int32 m_entryCount;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,121 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2011 Erin Catto http://box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Timer.h"
|
||||||
|
|
||||||
|
#if defined(_WIN32)
|
||||||
|
|
||||||
|
float64 b2Timer::s_invFrequency = 0.0f;
|
||||||
|
|
||||||
|
#ifndef WIN32_LEAN_AND_MEAN
|
||||||
|
#define WIN32_LEAN_AND_MEAN
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#include <windows.h>
|
||||||
|
|
||||||
|
b2Timer::b2Timer()
|
||||||
|
{
|
||||||
|
LARGE_INTEGER largeInteger;
|
||||||
|
|
||||||
|
if (s_invFrequency == 0.0f)
|
||||||
|
{
|
||||||
|
QueryPerformanceFrequency(&largeInteger);
|
||||||
|
s_invFrequency = float64(largeInteger.QuadPart);
|
||||||
|
if (s_invFrequency > 0.0f)
|
||||||
|
{
|
||||||
|
s_invFrequency = 1000.0f / s_invFrequency;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
QueryPerformanceCounter(&largeInteger);
|
||||||
|
m_start = float64(largeInteger.QuadPart);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Timer::Reset()
|
||||||
|
{
|
||||||
|
LARGE_INTEGER largeInteger;
|
||||||
|
QueryPerformanceCounter(&largeInteger);
|
||||||
|
m_start = float64(largeInteger.QuadPart);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2Timer::GetMilliseconds() const
|
||||||
|
{
|
||||||
|
LARGE_INTEGER largeInteger;
|
||||||
|
QueryPerformanceCounter(&largeInteger);
|
||||||
|
float64 count = float64(largeInteger.QuadPart);
|
||||||
|
float32 ms = float32(s_invFrequency * (count - m_start));
|
||||||
|
return ms;
|
||||||
|
}
|
||||||
|
|
||||||
|
#elif defined(__linux__) || defined (__APPLE__)
|
||||||
|
|
||||||
|
#include <sys/time.h>
|
||||||
|
|
||||||
|
b2Timer::b2Timer()
|
||||||
|
{
|
||||||
|
Reset();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Timer::Reset()
|
||||||
|
{
|
||||||
|
timeval t;
|
||||||
|
gettimeofday(&t, 0);
|
||||||
|
m_start_sec = t.tv_sec;
|
||||||
|
m_start_usec = t.tv_usec;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2Timer::GetMilliseconds() const
|
||||||
|
{
|
||||||
|
timeval t;
|
||||||
|
gettimeofday(&t, 0);
|
||||||
|
time_t start_sec = m_start_sec;
|
||||||
|
suseconds_t start_usec = m_start_usec;
|
||||||
|
|
||||||
|
// http://www.gnu.org/software/libc/manual/html_node/Elapsed-Time.html
|
||||||
|
if (t.tv_usec < start_usec)
|
||||||
|
{
|
||||||
|
int nsec = (start_usec - t.tv_usec) / 1000000 + 1;
|
||||||
|
start_usec -= 1000000 * nsec;
|
||||||
|
start_sec += nsec;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (t.tv_usec - start_usec > 1000000)
|
||||||
|
{
|
||||||
|
int nsec = (t.tv_usec - start_usec) / 1000000;
|
||||||
|
start_usec += 1000000 * nsec;
|
||||||
|
start_sec -= nsec;
|
||||||
|
}
|
||||||
|
return 1000.0f * (t.tv_sec - start_sec) + 0.001f * (t.tv_usec - start_usec);
|
||||||
|
}
|
||||||
|
|
||||||
|
#else
|
||||||
|
|
||||||
|
b2Timer::b2Timer()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Timer::Reset()
|
||||||
|
{
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2Timer::GetMilliseconds() const
|
||||||
|
{
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,50 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2011 Erin Catto http://box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_TIMER_H
|
||||||
|
#define B2_TIMER_H
|
||||||
|
|
||||||
|
#include "b2Settings.h"
|
||||||
|
|
||||||
|
/// Timer for profiling. This has platform specific code and may
|
||||||
|
/// not work on every platform.
|
||||||
|
class b2Timer
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Constructor
|
||||||
|
b2Timer();
|
||||||
|
|
||||||
|
/// Reset the timer.
|
||||||
|
void Reset();
|
||||||
|
|
||||||
|
/// Get the time since construction or the last reset.
|
||||||
|
float32 GetMilliseconds() const;
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
#if defined(_WIN32)
|
||||||
|
float64 m_start;
|
||||||
|
static float64 s_invFrequency;
|
||||||
|
#elif defined(__linux__) || defined (__APPLE__)
|
||||||
|
unsigned long long m_start_sec;
|
||||||
|
unsigned long long m_start_usec;
|
||||||
|
#endif
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,53 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
|
||||||
|
return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
|
||||||
|
allocator->Free(contact, sizeof(b2ChainAndCircleContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
|
||||||
|
: b2Contact(fixtureA, indexA, fixtureB, indexB)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
|
||||||
|
b2EdgeShape edge;
|
||||||
|
chain->GetChildEdge(&edge, m_indexA);
|
||||||
|
b2CollideEdgeAndCircle( manifold, &edge, xfA,
|
||||||
|
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CHAIN_AND_CIRCLE_CONTACT_H
|
||||||
|
#define B2_CHAIN_AND_CIRCLE_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2ChainAndCircleContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
|
||||||
|
~b2ChainAndCircleContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,53 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
|
||||||
|
return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
|
||||||
|
allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
|
||||||
|
: b2Contact(fixtureA, indexA, fixtureB, indexB)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
|
||||||
|
b2EdgeShape edge;
|
||||||
|
chain->GetChildEdge(&edge, m_indexA);
|
||||||
|
b2CollideEdgeAndPolygon( manifold, &edge, xfA,
|
||||||
|
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CHAIN_AND_POLYGON_CONTACT_H
|
||||||
|
#define B2_CHAIN_AND_POLYGON_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2ChainAndPolygonContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
|
||||||
|
~b2ChainAndPolygonContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2CircleContact.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2WorldCallbacks.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Collision/b2TimeOfImpact.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2CircleContact));
|
||||||
|
return new (mem) b2CircleContact(fixtureA, fixtureB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2CircleContact*)contact)->~b2CircleContact();
|
||||||
|
allocator->Free(contact, sizeof(b2CircleContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
: b2Contact(fixtureA, 0, fixtureB, 0)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2CollideCircles(manifold,
|
||||||
|
(b2CircleShape*)m_fixtureA->GetShape(), xfA,
|
||||||
|
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CIRCLE_CONTACT_H
|
||||||
|
#define B2_CIRCLE_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2CircleContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
~b2CircleContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,247 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2CircleContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2PolygonContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ContactSolver.h"
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Collision/b2TimeOfImpact.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2Shape.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2World.h"
|
||||||
|
|
||||||
|
b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
|
||||||
|
bool b2Contact::s_initialized = false;
|
||||||
|
|
||||||
|
void b2Contact::InitializeRegisters()
|
||||||
|
{
|
||||||
|
AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
|
||||||
|
AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
|
||||||
|
AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
|
||||||
|
AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
|
||||||
|
AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
|
||||||
|
AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);
|
||||||
|
AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
|
||||||
|
b2Shape::Type type1, b2Shape::Type type2)
|
||||||
|
{
|
||||||
|
b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
|
||||||
|
b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
|
||||||
|
|
||||||
|
s_registers[type1][type2].createFcn = createFcn;
|
||||||
|
s_registers[type1][type2].destroyFcn = destoryFcn;
|
||||||
|
s_registers[type1][type2].primary = true;
|
||||||
|
|
||||||
|
if (type1 != type2)
|
||||||
|
{
|
||||||
|
s_registers[type2][type1].createFcn = createFcn;
|
||||||
|
s_registers[type2][type1].destroyFcn = destoryFcn;
|
||||||
|
s_registers[type2][type1].primary = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
if (s_initialized == false)
|
||||||
|
{
|
||||||
|
InitializeRegisters();
|
||||||
|
s_initialized = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Shape::Type type1 = fixtureA->GetType();
|
||||||
|
b2Shape::Type type2 = fixtureB->GetType();
|
||||||
|
|
||||||
|
b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
|
||||||
|
b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
|
||||||
|
|
||||||
|
b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
|
||||||
|
if (createFcn)
|
||||||
|
{
|
||||||
|
if (s_registers[type1][type2].primary)
|
||||||
|
{
|
||||||
|
return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
b2Assert(s_initialized == true);
|
||||||
|
|
||||||
|
b2Fixture* fixtureA = contact->m_fixtureA;
|
||||||
|
b2Fixture* fixtureB = contact->m_fixtureB;
|
||||||
|
|
||||||
|
if (contact->m_manifold.pointCount > 0 &&
|
||||||
|
fixtureA->IsSensor() == false &&
|
||||||
|
fixtureB->IsSensor() == false)
|
||||||
|
{
|
||||||
|
fixtureA->GetBody()->SetAwake(true);
|
||||||
|
fixtureB->GetBody()->SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Shape::Type typeA = fixtureA->GetType();
|
||||||
|
b2Shape::Type typeB = fixtureB->GetType();
|
||||||
|
|
||||||
|
b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
|
||||||
|
b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
|
||||||
|
|
||||||
|
b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
|
||||||
|
destroyFcn(contact, allocator);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
|
||||||
|
{
|
||||||
|
m_flags = e_enabledFlag;
|
||||||
|
|
||||||
|
m_fixtureA = fA;
|
||||||
|
m_fixtureB = fB;
|
||||||
|
|
||||||
|
m_indexA = indexA;
|
||||||
|
m_indexB = indexB;
|
||||||
|
|
||||||
|
m_manifold.pointCount = 0;
|
||||||
|
|
||||||
|
m_prev = nullptr;
|
||||||
|
m_next = nullptr;
|
||||||
|
|
||||||
|
m_nodeA.contact = nullptr;
|
||||||
|
m_nodeA.prev = nullptr;
|
||||||
|
m_nodeA.next = nullptr;
|
||||||
|
m_nodeA.other = nullptr;
|
||||||
|
|
||||||
|
m_nodeB.contact = nullptr;
|
||||||
|
m_nodeB.prev = nullptr;
|
||||||
|
m_nodeB.next = nullptr;
|
||||||
|
m_nodeB.other = nullptr;
|
||||||
|
|
||||||
|
m_toiCount = 0;
|
||||||
|
|
||||||
|
m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
|
||||||
|
m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
|
||||||
|
|
||||||
|
m_tangentSpeed = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Update the contact manifold and touching status.
|
||||||
|
// Note: do not assume the fixture AABBs are overlapping or are valid.
|
||||||
|
void b2Contact::Update(b2ContactListener* listener)
|
||||||
|
{
|
||||||
|
b2Manifold oldManifold = m_manifold;
|
||||||
|
|
||||||
|
// Re-enable this contact.
|
||||||
|
m_flags |= e_enabledFlag;
|
||||||
|
|
||||||
|
bool touching = false;
|
||||||
|
bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
|
||||||
|
|
||||||
|
bool sensorA = m_fixtureA->IsSensor();
|
||||||
|
bool sensorB = m_fixtureB->IsSensor();
|
||||||
|
bool sensor = sensorA || sensorB;
|
||||||
|
|
||||||
|
b2Body* bodyA = m_fixtureA->GetBody();
|
||||||
|
b2Body* bodyB = m_fixtureB->GetBody();
|
||||||
|
const b2Transform& xfA = bodyA->GetTransform();
|
||||||
|
const b2Transform& xfB = bodyB->GetTransform();
|
||||||
|
|
||||||
|
// Is this contact a sensor?
|
||||||
|
if (sensor)
|
||||||
|
{
|
||||||
|
const b2Shape* shapeA = m_fixtureA->GetShape();
|
||||||
|
const b2Shape* shapeB = m_fixtureB->GetShape();
|
||||||
|
touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
|
||||||
|
|
||||||
|
// Sensors don't generate manifolds.
|
||||||
|
m_manifold.pointCount = 0;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
Evaluate(&m_manifold, xfA, xfB);
|
||||||
|
touching = m_manifold.pointCount > 0;
|
||||||
|
|
||||||
|
// Match old contact ids to new contact ids and copy the
|
||||||
|
// stored impulses to warm start the solver.
|
||||||
|
for (int32 i = 0; i < m_manifold.pointCount; ++i)
|
||||||
|
{
|
||||||
|
b2ManifoldPoint* mp2 = m_manifold.points + i;
|
||||||
|
mp2->normalImpulse = 0.0f;
|
||||||
|
mp2->tangentImpulse = 0.0f;
|
||||||
|
b2ContactID id2 = mp2->id;
|
||||||
|
|
||||||
|
for (int32 j = 0; j < oldManifold.pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2ManifoldPoint* mp1 = oldManifold.points + j;
|
||||||
|
|
||||||
|
if (mp1->id.key == id2.key)
|
||||||
|
{
|
||||||
|
mp2->normalImpulse = mp1->normalImpulse;
|
||||||
|
mp2->tangentImpulse = mp1->tangentImpulse;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (touching != wasTouching)
|
||||||
|
{
|
||||||
|
bodyA->SetAwake(true);
|
||||||
|
bodyB->SetAwake(true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (touching)
|
||||||
|
{
|
||||||
|
m_flags |= e_touchingFlag;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_touchingFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wasTouching == false && touching == true && listener)
|
||||||
|
{
|
||||||
|
listener->BeginContact(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wasTouching == true && touching == false && listener)
|
||||||
|
{
|
||||||
|
listener->EndContact(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (sensor == false && touching && listener)
|
||||||
|
{
|
||||||
|
listener->PreSolve(this, &oldManifold);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,349 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CONTACT_H
|
||||||
|
#define B2_CONTACT_H
|
||||||
|
|
||||||
|
#include "../../Common/b2Math.h"
|
||||||
|
#include "../../Collision/b2Collision.h"
|
||||||
|
#include "../../Collision/Shapes/b2Shape.h"
|
||||||
|
#include "../../Dynamics/b2Fixture.h"
|
||||||
|
|
||||||
|
class b2Body;
|
||||||
|
class b2Contact;
|
||||||
|
class b2Fixture;
|
||||||
|
class b2World;
|
||||||
|
class b2BlockAllocator;
|
||||||
|
class b2StackAllocator;
|
||||||
|
class b2ContactListener;
|
||||||
|
|
||||||
|
/// Friction mixing law. The idea is to allow either fixture to drive the friction to zero.
|
||||||
|
/// For example, anything slides on ice.
|
||||||
|
inline float32 b2MixFriction(float32 friction1, float32 friction2)
|
||||||
|
{
|
||||||
|
return b2Sqrt(friction1 * friction2);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
|
||||||
|
/// For example, a superball bounces on anything.
|
||||||
|
inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
|
||||||
|
{
|
||||||
|
return restitution1 > restitution2 ? restitution1 : restitution2;
|
||||||
|
}
|
||||||
|
|
||||||
|
typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB,
|
||||||
|
b2BlockAllocator* allocator);
|
||||||
|
typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
struct b2ContactRegister
|
||||||
|
{
|
||||||
|
b2ContactCreateFcn* createFcn;
|
||||||
|
b2ContactDestroyFcn* destroyFcn;
|
||||||
|
bool primary;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A contact edge is used to connect bodies and contacts together
|
||||||
|
/// in a contact graph where each body is a node and each contact
|
||||||
|
/// is an edge. A contact edge belongs to a doubly linked list
|
||||||
|
/// maintained in each attached body. Each contact has two contact
|
||||||
|
/// nodes, one for each attached body.
|
||||||
|
struct b2ContactEdge
|
||||||
|
{
|
||||||
|
b2Body* other; ///< provides quick access to the other body attached.
|
||||||
|
b2Contact* contact; ///< the contact
|
||||||
|
b2ContactEdge* prev; ///< the previous contact edge in the body's contact list
|
||||||
|
b2ContactEdge* next; ///< the next contact edge in the body's contact list
|
||||||
|
};
|
||||||
|
|
||||||
|
/// The class manages contact between two shapes. A contact exists for each overlapping
|
||||||
|
/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
|
||||||
|
/// that has no contact points.
|
||||||
|
class b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Get the contact manifold. Do not modify the manifold unless you understand the
|
||||||
|
/// internals of Box2D.
|
||||||
|
b2Manifold* GetManifold();
|
||||||
|
const b2Manifold* GetManifold() const;
|
||||||
|
|
||||||
|
/// Get the world manifold.
|
||||||
|
void GetWorldManifold(b2WorldManifold* worldManifold) const;
|
||||||
|
|
||||||
|
/// Is this contact touching?
|
||||||
|
bool IsTouching() const;
|
||||||
|
|
||||||
|
/// Enable/disable this contact. This can be used inside the pre-solve
|
||||||
|
/// contact listener. The contact is only disabled for the current
|
||||||
|
/// time step (or sub-step in continuous collisions).
|
||||||
|
void SetEnabled(bool flag);
|
||||||
|
|
||||||
|
/// Has this contact been disabled?
|
||||||
|
bool IsEnabled() const;
|
||||||
|
|
||||||
|
/// Get the next contact in the world's contact list.
|
||||||
|
b2Contact* GetNext();
|
||||||
|
const b2Contact* GetNext() const;
|
||||||
|
|
||||||
|
/// Get fixture A in this contact.
|
||||||
|
b2Fixture* GetFixtureA();
|
||||||
|
const b2Fixture* GetFixtureA() const;
|
||||||
|
|
||||||
|
/// Get the child primitive index for fixture A.
|
||||||
|
int32 GetChildIndexA() const;
|
||||||
|
|
||||||
|
/// Get fixture B in this contact.
|
||||||
|
b2Fixture* GetFixtureB();
|
||||||
|
const b2Fixture* GetFixtureB() const;
|
||||||
|
|
||||||
|
/// Get the child primitive index for fixture B.
|
||||||
|
int32 GetChildIndexB() const;
|
||||||
|
|
||||||
|
/// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
|
||||||
|
/// This value persists until set or reset.
|
||||||
|
void SetFriction(float32 friction);
|
||||||
|
|
||||||
|
/// Get the friction.
|
||||||
|
float32 GetFriction() const;
|
||||||
|
|
||||||
|
/// Reset the friction mixture to the default value.
|
||||||
|
void ResetFriction();
|
||||||
|
|
||||||
|
/// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
|
||||||
|
/// The value persists until you set or reset.
|
||||||
|
void SetRestitution(float32 restitution);
|
||||||
|
|
||||||
|
/// Get the restitution.
|
||||||
|
float32 GetRestitution() const;
|
||||||
|
|
||||||
|
/// Reset the restitution to the default value.
|
||||||
|
void ResetRestitution();
|
||||||
|
|
||||||
|
/// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
|
||||||
|
void SetTangentSpeed(float32 speed);
|
||||||
|
|
||||||
|
/// Get the desired tangent speed. In meters per second.
|
||||||
|
float32 GetTangentSpeed() const;
|
||||||
|
|
||||||
|
/// Evaluate this contact with your own manifold and transforms.
|
||||||
|
virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
friend class b2ContactManager;
|
||||||
|
friend class b2World;
|
||||||
|
friend class b2ContactSolver;
|
||||||
|
friend class b2Body;
|
||||||
|
friend class b2Fixture;
|
||||||
|
|
||||||
|
// Flags stored in m_flags
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
// Used when crawling contact graph when forming islands.
|
||||||
|
e_islandFlag = 0x0001,
|
||||||
|
|
||||||
|
// Set when the shapes are touching.
|
||||||
|
e_touchingFlag = 0x0002,
|
||||||
|
|
||||||
|
// This contact can be disabled (by user)
|
||||||
|
e_enabledFlag = 0x0004,
|
||||||
|
|
||||||
|
// This contact needs filtering because a fixture filter was changed.
|
||||||
|
e_filterFlag = 0x0008,
|
||||||
|
|
||||||
|
// This bullet contact had a TOI event
|
||||||
|
e_bulletHitFlag = 0x0010,
|
||||||
|
|
||||||
|
// This contact has a valid TOI in m_toi
|
||||||
|
e_toiFlag = 0x0020
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Flag this contact for filtering. Filtering will occur the next time step.
|
||||||
|
void FlagForFiltering();
|
||||||
|
|
||||||
|
static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
|
||||||
|
b2Shape::Type typeA, b2Shape::Type typeB);
|
||||||
|
static void InitializeRegisters();
|
||||||
|
static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2Contact() : m_fixtureA(nullptr), m_fixtureB(nullptr) {}
|
||||||
|
b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
|
||||||
|
virtual ~b2Contact() {}
|
||||||
|
|
||||||
|
void Update(b2ContactListener* listener);
|
||||||
|
|
||||||
|
static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
|
||||||
|
static bool s_initialized;
|
||||||
|
|
||||||
|
uint32 m_flags;
|
||||||
|
|
||||||
|
// World pool and list pointers.
|
||||||
|
b2Contact* m_prev;
|
||||||
|
b2Contact* m_next;
|
||||||
|
|
||||||
|
// Nodes for connecting bodies.
|
||||||
|
b2ContactEdge m_nodeA;
|
||||||
|
b2ContactEdge m_nodeB;
|
||||||
|
|
||||||
|
b2Fixture* m_fixtureA;
|
||||||
|
b2Fixture* m_fixtureB;
|
||||||
|
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
|
||||||
|
b2Manifold m_manifold;
|
||||||
|
|
||||||
|
int32 m_toiCount;
|
||||||
|
float32 m_toi;
|
||||||
|
|
||||||
|
float32 m_friction;
|
||||||
|
float32 m_restitution;
|
||||||
|
|
||||||
|
float32 m_tangentSpeed;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2Manifold* b2Contact::GetManifold()
|
||||||
|
{
|
||||||
|
return &m_manifold;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Manifold* b2Contact::GetManifold() const
|
||||||
|
{
|
||||||
|
return &m_manifold;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const
|
||||||
|
{
|
||||||
|
const b2Body* bodyA = m_fixtureA->GetBody();
|
||||||
|
const b2Body* bodyB = m_fixtureB->GetBody();
|
||||||
|
const b2Shape* shapeA = m_fixtureA->GetShape();
|
||||||
|
const b2Shape* shapeB = m_fixtureB->GetShape();
|
||||||
|
|
||||||
|
worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::SetEnabled(bool flag)
|
||||||
|
{
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_enabledFlag;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_enabledFlag;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Contact::IsEnabled() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_enabledFlag) == e_enabledFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Contact::IsTouching() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_touchingFlag) == e_touchingFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Contact* b2Contact::GetNext()
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Contact* b2Contact::GetNext() const
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Fixture* b2Contact::GetFixtureA()
|
||||||
|
{
|
||||||
|
return m_fixtureA;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Fixture* b2Contact::GetFixtureA() const
|
||||||
|
{
|
||||||
|
return m_fixtureA;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Fixture* b2Contact::GetFixtureB()
|
||||||
|
{
|
||||||
|
return m_fixtureB;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2Contact::GetChildIndexA() const
|
||||||
|
{
|
||||||
|
return m_indexA;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Fixture* b2Contact::GetFixtureB() const
|
||||||
|
{
|
||||||
|
return m_fixtureB;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2Contact::GetChildIndexB() const
|
||||||
|
{
|
||||||
|
return m_indexB;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::FlagForFiltering()
|
||||||
|
{
|
||||||
|
m_flags |= e_filterFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::SetFriction(float32 friction)
|
||||||
|
{
|
||||||
|
m_friction = friction;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Contact::GetFriction() const
|
||||||
|
{
|
||||||
|
return m_friction;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::ResetFriction()
|
||||||
|
{
|
||||||
|
m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::SetRestitution(float32 restitution)
|
||||||
|
{
|
||||||
|
m_restitution = restitution;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Contact::GetRestitution() const
|
||||||
|
{
|
||||||
|
return m_restitution;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::ResetRestitution()
|
||||||
|
{
|
||||||
|
m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Contact::SetTangentSpeed(float32 speed)
|
||||||
|
{
|
||||||
|
m_tangentSpeed = speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Contact::GetTangentSpeed() const
|
||||||
|
{
|
||||||
|
return m_tangentSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,838 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ContactSolver.h"
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2World.h"
|
||||||
|
#include "Box2D/Common/b2StackAllocator.h"
|
||||||
|
|
||||||
|
// Solver debugging is normally disabled because the block solver sometimes has to deal with a poorly conditioned effective mass matrix.
|
||||||
|
#define B2_DEBUG_SOLVER 0
|
||||||
|
|
||||||
|
bool g_blockSolve = true;
|
||||||
|
|
||||||
|
struct b2ContactPositionConstraint
|
||||||
|
{
|
||||||
|
b2Vec2 localPoints[b2_maxManifoldPoints];
|
||||||
|
b2Vec2 localNormal;
|
||||||
|
b2Vec2 localPoint;
|
||||||
|
int32 indexA;
|
||||||
|
int32 indexB;
|
||||||
|
float32 invMassA, invMassB;
|
||||||
|
b2Vec2 localCenterA, localCenterB;
|
||||||
|
float32 invIA, invIB;
|
||||||
|
b2Manifold::Type type;
|
||||||
|
float32 radiusA, radiusB;
|
||||||
|
int32 pointCount;
|
||||||
|
};
|
||||||
|
|
||||||
|
b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
|
||||||
|
{
|
||||||
|
m_step = def->step;
|
||||||
|
m_allocator = def->allocator;
|
||||||
|
m_count = def->count;
|
||||||
|
m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
|
||||||
|
m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
|
||||||
|
m_positions = def->positions;
|
||||||
|
m_velocities = def->velocities;
|
||||||
|
m_contacts = def->contacts;
|
||||||
|
|
||||||
|
// Initialize position independent portions of the constraints.
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2Contact* contact = m_contacts[i];
|
||||||
|
|
||||||
|
b2Fixture* fixtureA = contact->m_fixtureA;
|
||||||
|
b2Fixture* fixtureB = contact->m_fixtureB;
|
||||||
|
b2Shape* shapeA = fixtureA->GetShape();
|
||||||
|
b2Shape* shapeB = fixtureB->GetShape();
|
||||||
|
float32 radiusA = shapeA->m_radius;
|
||||||
|
float32 radiusB = shapeB->m_radius;
|
||||||
|
b2Body* bodyA = fixtureA->GetBody();
|
||||||
|
b2Body* bodyB = fixtureB->GetBody();
|
||||||
|
b2Manifold* manifold = contact->GetManifold();
|
||||||
|
|
||||||
|
int32 pointCount = manifold->pointCount;
|
||||||
|
b2Assert(pointCount > 0);
|
||||||
|
|
||||||
|
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||||
|
vc->friction = contact->m_friction;
|
||||||
|
vc->restitution = contact->m_restitution;
|
||||||
|
vc->tangentSpeed = contact->m_tangentSpeed;
|
||||||
|
vc->indexA = bodyA->m_islandIndex;
|
||||||
|
vc->indexB = bodyB->m_islandIndex;
|
||||||
|
vc->invMassA = bodyA->m_invMass;
|
||||||
|
vc->invMassB = bodyB->m_invMass;
|
||||||
|
vc->invIA = bodyA->m_invI;
|
||||||
|
vc->invIB = bodyB->m_invI;
|
||||||
|
vc->contactIndex = i;
|
||||||
|
vc->pointCount = pointCount;
|
||||||
|
vc->K.SetZero();
|
||||||
|
vc->normalMass.SetZero();
|
||||||
|
|
||||||
|
b2ContactPositionConstraint* pc = m_positionConstraints + i;
|
||||||
|
pc->indexA = bodyA->m_islandIndex;
|
||||||
|
pc->indexB = bodyB->m_islandIndex;
|
||||||
|
pc->invMassA = bodyA->m_invMass;
|
||||||
|
pc->invMassB = bodyB->m_invMass;
|
||||||
|
pc->localCenterA = bodyA->m_sweep.localCenter;
|
||||||
|
pc->localCenterB = bodyB->m_sweep.localCenter;
|
||||||
|
pc->invIA = bodyA->m_invI;
|
||||||
|
pc->invIB = bodyB->m_invI;
|
||||||
|
pc->localNormal = manifold->localNormal;
|
||||||
|
pc->localPoint = manifold->localPoint;
|
||||||
|
pc->pointCount = pointCount;
|
||||||
|
pc->radiusA = radiusA;
|
||||||
|
pc->radiusB = radiusB;
|
||||||
|
pc->type = manifold->type;
|
||||||
|
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2ManifoldPoint* cp = manifold->points + j;
|
||||||
|
b2VelocityConstraintPoint* vcp = vc->points + j;
|
||||||
|
|
||||||
|
if (m_step.warmStarting)
|
||||||
|
{
|
||||||
|
vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
|
||||||
|
vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
vcp->normalImpulse = 0.0f;
|
||||||
|
vcp->tangentImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
vcp->rA.SetZero();
|
||||||
|
vcp->rB.SetZero();
|
||||||
|
vcp->normalMass = 0.0f;
|
||||||
|
vcp->tangentMass = 0.0f;
|
||||||
|
vcp->velocityBias = 0.0f;
|
||||||
|
|
||||||
|
pc->localPoints[j] = cp->localPoint;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ContactSolver::~b2ContactSolver()
|
||||||
|
{
|
||||||
|
m_allocator->Free(m_velocityConstraints);
|
||||||
|
m_allocator->Free(m_positionConstraints);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Initialize position dependent portions of the velocity constraints.
|
||||||
|
void b2ContactSolver::InitializeVelocityConstraints()
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||||
|
b2ContactPositionConstraint* pc = m_positionConstraints + i;
|
||||||
|
|
||||||
|
float32 radiusA = pc->radiusA;
|
||||||
|
float32 radiusB = pc->radiusB;
|
||||||
|
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
|
||||||
|
|
||||||
|
int32 indexA = vc->indexA;
|
||||||
|
int32 indexB = vc->indexB;
|
||||||
|
|
||||||
|
float32 mA = vc->invMassA;
|
||||||
|
float32 mB = vc->invMassB;
|
||||||
|
float32 iA = vc->invIA;
|
||||||
|
float32 iB = vc->invIB;
|
||||||
|
b2Vec2 localCenterA = pc->localCenterA;
|
||||||
|
b2Vec2 localCenterB = pc->localCenterB;
|
||||||
|
|
||||||
|
b2Vec2 cA = m_positions[indexA].c;
|
||||||
|
float32 aA = m_positions[indexA].a;
|
||||||
|
b2Vec2 vA = m_velocities[indexA].v;
|
||||||
|
float32 wA = m_velocities[indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = m_positions[indexB].c;
|
||||||
|
float32 aB = m_positions[indexB].a;
|
||||||
|
b2Vec2 vB = m_velocities[indexB].v;
|
||||||
|
float32 wB = m_velocities[indexB].w;
|
||||||
|
|
||||||
|
b2Assert(manifold->pointCount > 0);
|
||||||
|
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
xfA.q.Set(aA);
|
||||||
|
xfB.q.Set(aB);
|
||||||
|
xfA.p = cA - b2Mul(xfA.q, localCenterA);
|
||||||
|
xfB.p = cB - b2Mul(xfB.q, localCenterB);
|
||||||
|
|
||||||
|
b2WorldManifold worldManifold;
|
||||||
|
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
|
||||||
|
|
||||||
|
vc->normal = worldManifold.normal;
|
||||||
|
|
||||||
|
int32 pointCount = vc->pointCount;
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint* vcp = vc->points + j;
|
||||||
|
|
||||||
|
vcp->rA = worldManifold.points[j] - cA;
|
||||||
|
vcp->rB = worldManifold.points[j] - cB;
|
||||||
|
|
||||||
|
float32 rnA = b2Cross(vcp->rA, vc->normal);
|
||||||
|
float32 rnB = b2Cross(vcp->rB, vc->normal);
|
||||||
|
|
||||||
|
float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||||
|
|
||||||
|
vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
|
||||||
|
|
||||||
|
b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
|
||||||
|
|
||||||
|
float32 rtA = b2Cross(vcp->rA, tangent);
|
||||||
|
float32 rtB = b2Cross(vcp->rB, tangent);
|
||||||
|
|
||||||
|
float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
|
||||||
|
|
||||||
|
vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
|
||||||
|
|
||||||
|
// Setup a velocity bias for restitution.
|
||||||
|
vcp->velocityBias = 0.0f;
|
||||||
|
float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
|
||||||
|
if (vRel < -b2_velocityThreshold)
|
||||||
|
{
|
||||||
|
vcp->velocityBias = -vc->restitution * vRel;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// If we have two points, then prepare the block solver.
|
||||||
|
if (vc->pointCount == 2 && g_blockSolve)
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint* vcp1 = vc->points + 0;
|
||||||
|
b2VelocityConstraintPoint* vcp2 = vc->points + 1;
|
||||||
|
|
||||||
|
float32 rn1A = b2Cross(vcp1->rA, vc->normal);
|
||||||
|
float32 rn1B = b2Cross(vcp1->rB, vc->normal);
|
||||||
|
float32 rn2A = b2Cross(vcp2->rA, vc->normal);
|
||||||
|
float32 rn2B = b2Cross(vcp2->rB, vc->normal);
|
||||||
|
|
||||||
|
float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
|
||||||
|
float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
|
||||||
|
float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
|
||||||
|
|
||||||
|
// Ensure a reasonable condition number.
|
||||||
|
const float32 k_maxConditionNumber = 1000.0f;
|
||||||
|
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
|
||||||
|
{
|
||||||
|
// K is safe to invert.
|
||||||
|
vc->K.ex.Set(k11, k12);
|
||||||
|
vc->K.ey.Set(k12, k22);
|
||||||
|
vc->normalMass = vc->K.GetInverse();
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// The constraints are redundant, just use one.
|
||||||
|
// TODO_ERIN use deepest?
|
||||||
|
vc->pointCount = 1;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactSolver::WarmStart()
|
||||||
|
{
|
||||||
|
// Warm start.
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||||
|
|
||||||
|
int32 indexA = vc->indexA;
|
||||||
|
int32 indexB = vc->indexB;
|
||||||
|
float32 mA = vc->invMassA;
|
||||||
|
float32 iA = vc->invIA;
|
||||||
|
float32 mB = vc->invMassB;
|
||||||
|
float32 iB = vc->invIB;
|
||||||
|
int32 pointCount = vc->pointCount;
|
||||||
|
|
||||||
|
b2Vec2 vA = m_velocities[indexA].v;
|
||||||
|
float32 wA = m_velocities[indexA].w;
|
||||||
|
b2Vec2 vB = m_velocities[indexB].v;
|
||||||
|
float32 wB = m_velocities[indexB].w;
|
||||||
|
|
||||||
|
b2Vec2 normal = vc->normal;
|
||||||
|
b2Vec2 tangent = b2Cross(normal, 1.0f);
|
||||||
|
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint* vcp = vc->points + j;
|
||||||
|
b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
|
||||||
|
wA -= iA * b2Cross(vcp->rA, P);
|
||||||
|
vA -= mA * P;
|
||||||
|
wB += iB * b2Cross(vcp->rB, P);
|
||||||
|
vB += mB * P;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_velocities[indexA].v = vA;
|
||||||
|
m_velocities[indexA].w = wA;
|
||||||
|
m_velocities[indexB].v = vB;
|
||||||
|
m_velocities[indexB].w = wB;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactSolver::SolveVelocityConstraints()
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||||
|
|
||||||
|
int32 indexA = vc->indexA;
|
||||||
|
int32 indexB = vc->indexB;
|
||||||
|
float32 mA = vc->invMassA;
|
||||||
|
float32 iA = vc->invIA;
|
||||||
|
float32 mB = vc->invMassB;
|
||||||
|
float32 iB = vc->invIB;
|
||||||
|
int32 pointCount = vc->pointCount;
|
||||||
|
|
||||||
|
b2Vec2 vA = m_velocities[indexA].v;
|
||||||
|
float32 wA = m_velocities[indexA].w;
|
||||||
|
b2Vec2 vB = m_velocities[indexB].v;
|
||||||
|
float32 wB = m_velocities[indexB].w;
|
||||||
|
|
||||||
|
b2Vec2 normal = vc->normal;
|
||||||
|
b2Vec2 tangent = b2Cross(normal, 1.0f);
|
||||||
|
float32 friction = vc->friction;
|
||||||
|
|
||||||
|
b2Assert(pointCount == 1 || pointCount == 2);
|
||||||
|
|
||||||
|
// Solve tangent constraints first because non-penetration is more important
|
||||||
|
// than friction.
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint* vcp = vc->points + j;
|
||||||
|
|
||||||
|
// Relative velocity at contact
|
||||||
|
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
|
||||||
|
|
||||||
|
// Compute tangent force
|
||||||
|
float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
|
||||||
|
float32 lambda = vcp->tangentMass * (-vt);
|
||||||
|
|
||||||
|
// b2Clamp the accumulated force
|
||||||
|
float32 maxFriction = friction * vcp->normalImpulse;
|
||||||
|
float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
|
||||||
|
lambda = newImpulse - vcp->tangentImpulse;
|
||||||
|
vcp->tangentImpulse = newImpulse;
|
||||||
|
|
||||||
|
// Apply contact impulse
|
||||||
|
b2Vec2 P = lambda * tangent;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * b2Cross(vcp->rA, P);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * b2Cross(vcp->rB, P);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve normal constraints
|
||||||
|
if (pointCount == 1 || g_blockSolve == false)
|
||||||
|
{
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint* vcp = vc->points + j;
|
||||||
|
|
||||||
|
// Relative velocity at contact
|
||||||
|
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
|
||||||
|
|
||||||
|
// Compute normal impulse
|
||||||
|
float32 vn = b2Dot(dv, normal);
|
||||||
|
float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
|
||||||
|
|
||||||
|
// b2Clamp the accumulated impulse
|
||||||
|
float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
|
||||||
|
lambda = newImpulse - vcp->normalImpulse;
|
||||||
|
vcp->normalImpulse = newImpulse;
|
||||||
|
|
||||||
|
// Apply contact impulse
|
||||||
|
b2Vec2 P = lambda * normal;
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * b2Cross(vcp->rA, P);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * b2Cross(vcp->rB, P);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
|
||||||
|
// Build the mini LCP for this contact patch
|
||||||
|
//
|
||||||
|
// vn = A * x + b, vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
|
||||||
|
//
|
||||||
|
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
|
||||||
|
// b = vn0 - velocityBias
|
||||||
|
//
|
||||||
|
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
|
||||||
|
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
|
||||||
|
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
|
||||||
|
// solution that satisfies the problem is chosen.
|
||||||
|
//
|
||||||
|
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
|
||||||
|
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
|
||||||
|
//
|
||||||
|
// Substitute:
|
||||||
|
//
|
||||||
|
// x = a + d
|
||||||
|
//
|
||||||
|
// a := old total impulse
|
||||||
|
// x := new total impulse
|
||||||
|
// d := incremental impulse
|
||||||
|
//
|
||||||
|
// For the current iteration we extend the formula for the incremental impulse
|
||||||
|
// to compute the new total impulse:
|
||||||
|
//
|
||||||
|
// vn = A * d + b
|
||||||
|
// = A * (x - a) + b
|
||||||
|
// = A * x + b - A * a
|
||||||
|
// = A * x + b'
|
||||||
|
// b' = b - A * a;
|
||||||
|
|
||||||
|
b2VelocityConstraintPoint* cp1 = vc->points + 0;
|
||||||
|
b2VelocityConstraintPoint* cp2 = vc->points + 1;
|
||||||
|
|
||||||
|
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
|
||||||
|
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
|
||||||
|
|
||||||
|
// Relative velocity at contact
|
||||||
|
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
|
||||||
|
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
|
||||||
|
|
||||||
|
// Compute normal velocity
|
||||||
|
float32 vn1 = b2Dot(dv1, normal);
|
||||||
|
float32 vn2 = b2Dot(dv2, normal);
|
||||||
|
|
||||||
|
b2Vec2 b;
|
||||||
|
b.x = vn1 - cp1->velocityBias;
|
||||||
|
b.y = vn2 - cp2->velocityBias;
|
||||||
|
|
||||||
|
// Compute b'
|
||||||
|
b -= b2Mul(vc->K, a);
|
||||||
|
|
||||||
|
const float32 k_errorTol = 1e-3f;
|
||||||
|
B2_NOT_USED(k_errorTol);
|
||||||
|
|
||||||
|
for (;;)
|
||||||
|
{
|
||||||
|
//
|
||||||
|
// Case 1: vn = 0
|
||||||
|
//
|
||||||
|
// 0 = A * x + b'
|
||||||
|
//
|
||||||
|
// Solve for x:
|
||||||
|
//
|
||||||
|
// x = - inv(A) * b'
|
||||||
|
//
|
||||||
|
b2Vec2 x = - b2Mul(vc->normalMass, b);
|
||||||
|
|
||||||
|
if (x.x >= 0.0f && x.y >= 0.0f)
|
||||||
|
{
|
||||||
|
// Get the incremental impulse
|
||||||
|
b2Vec2 d = x - a;
|
||||||
|
|
||||||
|
// Apply incremental impulse
|
||||||
|
b2Vec2 P1 = d.x * normal;
|
||||||
|
b2Vec2 P2 = d.y * normal;
|
||||||
|
vA -= mA * (P1 + P2);
|
||||||
|
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
|
||||||
|
|
||||||
|
vB += mB * (P1 + P2);
|
||||||
|
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
|
||||||
|
|
||||||
|
// Accumulate
|
||||||
|
cp1->normalImpulse = x.x;
|
||||||
|
cp2->normalImpulse = x.y;
|
||||||
|
|
||||||
|
#if B2_DEBUG_SOLVER == 1
|
||||||
|
// Postconditions
|
||||||
|
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
|
||||||
|
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
|
||||||
|
|
||||||
|
// Compute normal velocity
|
||||||
|
vn1 = b2Dot(dv1, normal);
|
||||||
|
vn2 = b2Dot(dv2, normal);
|
||||||
|
|
||||||
|
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
|
||||||
|
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// Case 2: vn1 = 0 and x2 = 0
|
||||||
|
//
|
||||||
|
// 0 = a11 * x1 + a12 * 0 + b1'
|
||||||
|
// vn2 = a21 * x1 + a22 * 0 + b2'
|
||||||
|
//
|
||||||
|
x.x = - cp1->normalMass * b.x;
|
||||||
|
x.y = 0.0f;
|
||||||
|
vn1 = 0.0f;
|
||||||
|
vn2 = vc->K.ex.y * x.x + b.y;
|
||||||
|
if (x.x >= 0.0f && vn2 >= 0.0f)
|
||||||
|
{
|
||||||
|
// Get the incremental impulse
|
||||||
|
b2Vec2 d = x - a;
|
||||||
|
|
||||||
|
// Apply incremental impulse
|
||||||
|
b2Vec2 P1 = d.x * normal;
|
||||||
|
b2Vec2 P2 = d.y * normal;
|
||||||
|
vA -= mA * (P1 + P2);
|
||||||
|
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
|
||||||
|
|
||||||
|
vB += mB * (P1 + P2);
|
||||||
|
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
|
||||||
|
|
||||||
|
// Accumulate
|
||||||
|
cp1->normalImpulse = x.x;
|
||||||
|
cp2->normalImpulse = x.y;
|
||||||
|
|
||||||
|
#if B2_DEBUG_SOLVER == 1
|
||||||
|
// Postconditions
|
||||||
|
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
|
||||||
|
|
||||||
|
// Compute normal velocity
|
||||||
|
vn1 = b2Dot(dv1, normal);
|
||||||
|
|
||||||
|
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
//
|
||||||
|
// Case 3: vn2 = 0 and x1 = 0
|
||||||
|
//
|
||||||
|
// vn1 = a11 * 0 + a12 * x2 + b1'
|
||||||
|
// 0 = a21 * 0 + a22 * x2 + b2'
|
||||||
|
//
|
||||||
|
x.x = 0.0f;
|
||||||
|
x.y = - cp2->normalMass * b.y;
|
||||||
|
vn1 = vc->K.ey.x * x.y + b.x;
|
||||||
|
vn2 = 0.0f;
|
||||||
|
|
||||||
|
if (x.y >= 0.0f && vn1 >= 0.0f)
|
||||||
|
{
|
||||||
|
// Resubstitute for the incremental impulse
|
||||||
|
b2Vec2 d = x - a;
|
||||||
|
|
||||||
|
// Apply incremental impulse
|
||||||
|
b2Vec2 P1 = d.x * normal;
|
||||||
|
b2Vec2 P2 = d.y * normal;
|
||||||
|
vA -= mA * (P1 + P2);
|
||||||
|
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
|
||||||
|
|
||||||
|
vB += mB * (P1 + P2);
|
||||||
|
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
|
||||||
|
|
||||||
|
// Accumulate
|
||||||
|
cp1->normalImpulse = x.x;
|
||||||
|
cp2->normalImpulse = x.y;
|
||||||
|
|
||||||
|
#if B2_DEBUG_SOLVER == 1
|
||||||
|
// Postconditions
|
||||||
|
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
|
||||||
|
|
||||||
|
// Compute normal velocity
|
||||||
|
vn2 = b2Dot(dv2, normal);
|
||||||
|
|
||||||
|
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
|
||||||
|
#endif
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
//
|
||||||
|
// Case 4: x1 = 0 and x2 = 0
|
||||||
|
//
|
||||||
|
// vn1 = b1
|
||||||
|
// vn2 = b2;
|
||||||
|
x.x = 0.0f;
|
||||||
|
x.y = 0.0f;
|
||||||
|
vn1 = b.x;
|
||||||
|
vn2 = b.y;
|
||||||
|
|
||||||
|
if (vn1 >= 0.0f && vn2 >= 0.0f )
|
||||||
|
{
|
||||||
|
// Resubstitute for the incremental impulse
|
||||||
|
b2Vec2 d = x - a;
|
||||||
|
|
||||||
|
// Apply incremental impulse
|
||||||
|
b2Vec2 P1 = d.x * normal;
|
||||||
|
b2Vec2 P2 = d.y * normal;
|
||||||
|
vA -= mA * (P1 + P2);
|
||||||
|
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
|
||||||
|
|
||||||
|
vB += mB * (P1 + P2);
|
||||||
|
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
|
||||||
|
|
||||||
|
// Accumulate
|
||||||
|
cp1->normalImpulse = x.x;
|
||||||
|
cp2->normalImpulse = x.y;
|
||||||
|
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
m_velocities[indexA].v = vA;
|
||||||
|
m_velocities[indexA].w = wA;
|
||||||
|
m_velocities[indexB].v = vB;
|
||||||
|
m_velocities[indexB].w = wB;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactSolver::StoreImpulses()
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
|
||||||
|
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
|
||||||
|
|
||||||
|
for (int32 j = 0; j < vc->pointCount; ++j)
|
||||||
|
{
|
||||||
|
manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
|
||||||
|
manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
struct b2PositionSolverManifold
|
||||||
|
{
|
||||||
|
void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
|
||||||
|
{
|
||||||
|
b2Assert(pc->pointCount > 0);
|
||||||
|
|
||||||
|
switch (pc->type)
|
||||||
|
{
|
||||||
|
case b2Manifold::e_circles:
|
||||||
|
{
|
||||||
|
b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
|
||||||
|
b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
|
||||||
|
normal = pointB - pointA;
|
||||||
|
normal.Normalize();
|
||||||
|
point = 0.5f * (pointA + pointB);
|
||||||
|
separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Manifold::e_faceA:
|
||||||
|
{
|
||||||
|
normal = b2Mul(xfA.q, pc->localNormal);
|
||||||
|
b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
|
||||||
|
|
||||||
|
b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
|
||||||
|
separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
|
||||||
|
point = clipPoint;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Manifold::e_faceB:
|
||||||
|
{
|
||||||
|
normal = b2Mul(xfB.q, pc->localNormal);
|
||||||
|
b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
|
||||||
|
|
||||||
|
b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
|
||||||
|
separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
|
||||||
|
point = clipPoint;
|
||||||
|
|
||||||
|
// Ensure normal points from A to B
|
||||||
|
normal = -normal;
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 normal;
|
||||||
|
b2Vec2 point;
|
||||||
|
float32 separation;
|
||||||
|
};
|
||||||
|
|
||||||
|
// Sequential solver.
|
||||||
|
bool b2ContactSolver::SolvePositionConstraints()
|
||||||
|
{
|
||||||
|
float32 minSeparation = 0.0f;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactPositionConstraint* pc = m_positionConstraints + i;
|
||||||
|
|
||||||
|
int32 indexA = pc->indexA;
|
||||||
|
int32 indexB = pc->indexB;
|
||||||
|
b2Vec2 localCenterA = pc->localCenterA;
|
||||||
|
float32 mA = pc->invMassA;
|
||||||
|
float32 iA = pc->invIA;
|
||||||
|
b2Vec2 localCenterB = pc->localCenterB;
|
||||||
|
float32 mB = pc->invMassB;
|
||||||
|
float32 iB = pc->invIB;
|
||||||
|
int32 pointCount = pc->pointCount;
|
||||||
|
|
||||||
|
b2Vec2 cA = m_positions[indexA].c;
|
||||||
|
float32 aA = m_positions[indexA].a;
|
||||||
|
|
||||||
|
b2Vec2 cB = m_positions[indexB].c;
|
||||||
|
float32 aB = m_positions[indexB].a;
|
||||||
|
|
||||||
|
// Solve normal constraints
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
xfA.q.Set(aA);
|
||||||
|
xfB.q.Set(aB);
|
||||||
|
xfA.p = cA - b2Mul(xfA.q, localCenterA);
|
||||||
|
xfB.p = cB - b2Mul(xfB.q, localCenterB);
|
||||||
|
|
||||||
|
b2PositionSolverManifold psm;
|
||||||
|
psm.Initialize(pc, xfA, xfB, j);
|
||||||
|
b2Vec2 normal = psm.normal;
|
||||||
|
|
||||||
|
b2Vec2 point = psm.point;
|
||||||
|
float32 separation = psm.separation;
|
||||||
|
|
||||||
|
b2Vec2 rA = point - cA;
|
||||||
|
b2Vec2 rB = point - cB;
|
||||||
|
|
||||||
|
// Track max constraint error.
|
||||||
|
minSeparation = b2Min(minSeparation, separation);
|
||||||
|
|
||||||
|
// Prevent large corrections and allow slop.
|
||||||
|
float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
|
||||||
|
|
||||||
|
// Compute the effective mass.
|
||||||
|
float32 rnA = b2Cross(rA, normal);
|
||||||
|
float32 rnB = b2Cross(rB, normal);
|
||||||
|
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||||
|
|
||||||
|
// Compute normal impulse
|
||||||
|
float32 impulse = K > 0.0f ? - C / K : 0.0f;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * normal;
|
||||||
|
|
||||||
|
cA -= mA * P;
|
||||||
|
aA -= iA * b2Cross(rA, P);
|
||||||
|
|
||||||
|
cB += mB * P;
|
||||||
|
aB += iB * b2Cross(rB, P);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_positions[indexA].c = cA;
|
||||||
|
m_positions[indexA].a = aA;
|
||||||
|
|
||||||
|
m_positions[indexB].c = cB;
|
||||||
|
m_positions[indexB].a = aB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||||
|
// push the separation above -b2_linearSlop.
|
||||||
|
return minSeparation >= -3.0f * b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Sequential position solver for position constraints.
|
||||||
|
bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
|
||||||
|
{
|
||||||
|
float32 minSeparation = 0.0f;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_count; ++i)
|
||||||
|
{
|
||||||
|
b2ContactPositionConstraint* pc = m_positionConstraints + i;
|
||||||
|
|
||||||
|
int32 indexA = pc->indexA;
|
||||||
|
int32 indexB = pc->indexB;
|
||||||
|
b2Vec2 localCenterA = pc->localCenterA;
|
||||||
|
b2Vec2 localCenterB = pc->localCenterB;
|
||||||
|
int32 pointCount = pc->pointCount;
|
||||||
|
|
||||||
|
float32 mA = 0.0f;
|
||||||
|
float32 iA = 0.0f;
|
||||||
|
if (indexA == toiIndexA || indexA == toiIndexB)
|
||||||
|
{
|
||||||
|
mA = pc->invMassA;
|
||||||
|
iA = pc->invIA;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 mB = 0.0f;
|
||||||
|
float32 iB = 0.;
|
||||||
|
if (indexB == toiIndexA || indexB == toiIndexB)
|
||||||
|
{
|
||||||
|
mB = pc->invMassB;
|
||||||
|
iB = pc->invIB;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 cA = m_positions[indexA].c;
|
||||||
|
float32 aA = m_positions[indexA].a;
|
||||||
|
|
||||||
|
b2Vec2 cB = m_positions[indexB].c;
|
||||||
|
float32 aB = m_positions[indexB].a;
|
||||||
|
|
||||||
|
// Solve normal constraints
|
||||||
|
for (int32 j = 0; j < pointCount; ++j)
|
||||||
|
{
|
||||||
|
b2Transform xfA, xfB;
|
||||||
|
xfA.q.Set(aA);
|
||||||
|
xfB.q.Set(aB);
|
||||||
|
xfA.p = cA - b2Mul(xfA.q, localCenterA);
|
||||||
|
xfB.p = cB - b2Mul(xfB.q, localCenterB);
|
||||||
|
|
||||||
|
b2PositionSolverManifold psm;
|
||||||
|
psm.Initialize(pc, xfA, xfB, j);
|
||||||
|
b2Vec2 normal = psm.normal;
|
||||||
|
|
||||||
|
b2Vec2 point = psm.point;
|
||||||
|
float32 separation = psm.separation;
|
||||||
|
|
||||||
|
b2Vec2 rA = point - cA;
|
||||||
|
b2Vec2 rB = point - cB;
|
||||||
|
|
||||||
|
// Track max constraint error.
|
||||||
|
minSeparation = b2Min(minSeparation, separation);
|
||||||
|
|
||||||
|
// Prevent large corrections and allow slop.
|
||||||
|
float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
|
||||||
|
|
||||||
|
// Compute the effective mass.
|
||||||
|
float32 rnA = b2Cross(rA, normal);
|
||||||
|
float32 rnB = b2Cross(rB, normal);
|
||||||
|
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
|
||||||
|
|
||||||
|
// Compute normal impulse
|
||||||
|
float32 impulse = K > 0.0f ? - C / K : 0.0f;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * normal;
|
||||||
|
|
||||||
|
cA -= mA * P;
|
||||||
|
aA -= iA * b2Cross(rA, P);
|
||||||
|
|
||||||
|
cB += mB * P;
|
||||||
|
aB += iB * b2Cross(rB, P);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_positions[indexA].c = cA;
|
||||||
|
m_positions[indexA].a = aA;
|
||||||
|
|
||||||
|
m_positions[indexB].c = cB;
|
||||||
|
m_positions[indexB].a = aB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// We can't expect minSpeparation >= -b2_linearSlop because we don't
|
||||||
|
// push the separation above -b2_linearSlop.
|
||||||
|
return minSeparation >= -1.5f * b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,95 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CONTACT_SOLVER_H
|
||||||
|
#define B2_CONTACT_SOLVER_H
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Math.h"
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Dynamics/b2TimeStep.h"
|
||||||
|
|
||||||
|
class b2Contact;
|
||||||
|
class b2Body;
|
||||||
|
class b2StackAllocator;
|
||||||
|
struct b2ContactPositionConstraint;
|
||||||
|
|
||||||
|
struct b2VelocityConstraintPoint
|
||||||
|
{
|
||||||
|
b2Vec2 rA;
|
||||||
|
b2Vec2 rB;
|
||||||
|
float32 normalImpulse;
|
||||||
|
float32 tangentImpulse;
|
||||||
|
float32 normalMass;
|
||||||
|
float32 tangentMass;
|
||||||
|
float32 velocityBias;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct b2ContactVelocityConstraint
|
||||||
|
{
|
||||||
|
b2VelocityConstraintPoint points[b2_maxManifoldPoints];
|
||||||
|
b2Vec2 normal;
|
||||||
|
b2Mat22 normalMass;
|
||||||
|
b2Mat22 K;
|
||||||
|
int32 indexA;
|
||||||
|
int32 indexB;
|
||||||
|
float32 invMassA, invMassB;
|
||||||
|
float32 invIA, invIB;
|
||||||
|
float32 friction;
|
||||||
|
float32 restitution;
|
||||||
|
float32 tangentSpeed;
|
||||||
|
int32 pointCount;
|
||||||
|
int32 contactIndex;
|
||||||
|
};
|
||||||
|
|
||||||
|
struct b2ContactSolverDef
|
||||||
|
{
|
||||||
|
b2TimeStep step;
|
||||||
|
b2Contact** contacts;
|
||||||
|
int32 count;
|
||||||
|
b2Position* positions;
|
||||||
|
b2Velocity* velocities;
|
||||||
|
b2StackAllocator* allocator;
|
||||||
|
};
|
||||||
|
|
||||||
|
class b2ContactSolver
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2ContactSolver(b2ContactSolverDef* def);
|
||||||
|
~b2ContactSolver();
|
||||||
|
|
||||||
|
void InitializeVelocityConstraints();
|
||||||
|
|
||||||
|
void WarmStart();
|
||||||
|
void SolveVelocityConstraints();
|
||||||
|
void StoreImpulses();
|
||||||
|
|
||||||
|
bool SolvePositionConstraints();
|
||||||
|
bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
|
||||||
|
|
||||||
|
b2TimeStep m_step;
|
||||||
|
b2Position* m_positions;
|
||||||
|
b2Velocity* m_velocities;
|
||||||
|
b2StackAllocator* m_allocator;
|
||||||
|
b2ContactPositionConstraint* m_positionConstraints;
|
||||||
|
b2ContactVelocityConstraint* m_velocityConstraints;
|
||||||
|
b2Contact** m_contacts;
|
||||||
|
int m_count;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
||||||
|
|
@ -0,0 +1,49 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
|
||||||
|
return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
|
||||||
|
allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
: b2Contact(fixtureA, 0, fixtureB, 0)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2CollideEdgeAndCircle( manifold,
|
||||||
|
(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
|
||||||
|
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_EDGE_AND_CIRCLE_CONTACT_H
|
||||||
|
#define B2_EDGE_AND_CIRCLE_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2EdgeAndCircleContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
~b2EdgeAndCircleContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,49 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
|
||||||
|
return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
|
||||||
|
allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
: b2Contact(fixtureA, 0, fixtureB, 0)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2CollideEdgeAndPolygon( manifold,
|
||||||
|
(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
|
||||||
|
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_EDGE_AND_POLYGON_CONTACT_H
|
||||||
|
#define B2_EDGE_AND_POLYGON_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2EdgeAndPolygonContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
~b2EdgeAndPolygonContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,49 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
|
||||||
|
return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
|
||||||
|
allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
: b2Contact(fixtureA, 0, fixtureB, 0)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2CollidePolygonAndCircle( manifold,
|
||||||
|
(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
|
||||||
|
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,38 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_POLYGON_AND_CIRCLE_CONTACT_H
|
||||||
|
#define B2_POLYGON_AND_CIRCLE_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2PolygonAndCircleContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
~b2PolygonAndCircleContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2PolygonContact.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
#include "Box2D/Collision/b2TimeOfImpact.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2WorldCallbacks.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2PolygonContact));
|
||||||
|
return new (mem) b2PolygonContact(fixtureA, fixtureB);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
((b2PolygonContact*)contact)->~b2PolygonContact();
|
||||||
|
allocator->Free(contact, sizeof(b2PolygonContact));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
: b2Contact(fixtureA, 0, fixtureB, 0)
|
||||||
|
{
|
||||||
|
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
|
||||||
|
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
|
||||||
|
{
|
||||||
|
b2CollidePolygons( manifold,
|
||||||
|
(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
|
||||||
|
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,39 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_POLYGON_CONTACT_H
|
||||||
|
#define B2_POLYGON_CONTACT_H
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
class b2PolygonContact : public b2Contact
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
|
||||||
|
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
~b2PolygonContact() {}
|
||||||
|
|
||||||
|
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) override;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,260 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2DistanceJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// 1-D constrained system
|
||||||
|
// m (v2 - v1) = lambda
|
||||||
|
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
|
||||||
|
// x2 = x1 + h * v2
|
||||||
|
|
||||||
|
// 1-D mass-damper-spring system
|
||||||
|
// m (v2 - v1) + h * d * v2 + h * k *
|
||||||
|
|
||||||
|
// C = norm(p2 - p1) - L
|
||||||
|
// u = (p2 - p1) / norm(p2 - p1)
|
||||||
|
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
|
||||||
|
// J = [-u -cross(r1, u) u cross(r2, u)]
|
||||||
|
// K = J * invM * JT
|
||||||
|
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
|
||||||
|
|
||||||
|
void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
|
||||||
|
const b2Vec2& anchor1, const b2Vec2& anchor2)
|
||||||
|
{
|
||||||
|
bodyA = b1;
|
||||||
|
bodyB = b2;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor1);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor2);
|
||||||
|
b2Vec2 d = anchor2 - anchor1;
|
||||||
|
length = d.Length();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
m_length = def->length;
|
||||||
|
m_frequencyHz = def->frequencyHz;
|
||||||
|
m_dampingRatio = def->dampingRatio;
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
m_bias = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
m_u = cB + m_rB - cA - m_rA;
|
||||||
|
|
||||||
|
// Handle singularity.
|
||||||
|
float32 length = m_u.Length();
|
||||||
|
if (length > b2_linearSlop)
|
||||||
|
{
|
||||||
|
m_u *= 1.0f / length;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_u.Set(0.0f, 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 crAu = b2Cross(m_rA, m_u);
|
||||||
|
float32 crBu = b2Cross(m_rB, m_u);
|
||||||
|
float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
|
||||||
|
|
||||||
|
// Compute the effective mass matrix.
|
||||||
|
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
|
||||||
|
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
float32 C = length - m_length;
|
||||||
|
|
||||||
|
// Frequency
|
||||||
|
float32 omega = 2.0f * b2_pi * m_frequencyHz;
|
||||||
|
|
||||||
|
// Damping coefficient
|
||||||
|
float32 d = 2.0f * m_mass * m_dampingRatio * omega;
|
||||||
|
|
||||||
|
// Spring stiffness
|
||||||
|
float32 k = m_mass * omega * omega;
|
||||||
|
|
||||||
|
// magic formulas
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
m_gamma = h * (d + h * k);
|
||||||
|
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
|
||||||
|
m_bias = C * h * k * m_gamma;
|
||||||
|
|
||||||
|
invMass += m_gamma;
|
||||||
|
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
m_bias = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale the impulse to support a variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P = m_impulse * m_u;
|
||||||
|
vA -= m_invMassA * P;
|
||||||
|
wA -= m_invIA * b2Cross(m_rA, P);
|
||||||
|
vB += m_invMassB * P;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, P);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
// Cdot = dot(u, v + cross(w, r))
|
||||||
|
b2Vec2 vpA = vA + b2Cross(wA, m_rA);
|
||||||
|
b2Vec2 vpB = vB + b2Cross(wB, m_rB);
|
||||||
|
float32 Cdot = b2Dot(m_u, vpB - vpA);
|
||||||
|
|
||||||
|
float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
|
||||||
|
m_impulse += impulse;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * m_u;
|
||||||
|
vA -= m_invMassA * P;
|
||||||
|
wA -= m_invIA * b2Cross(m_rA, P);
|
||||||
|
vB += m_invMassB * P;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, P);
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
// There is no position correction for soft distance constraints.
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 u = cB + rB - cA - rA;
|
||||||
|
|
||||||
|
float32 length = u.Normalize();
|
||||||
|
float32 C = length - m_length;
|
||||||
|
C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
|
||||||
|
|
||||||
|
float32 impulse = -m_mass * C;
|
||||||
|
b2Vec2 P = impulse * u;
|
||||||
|
|
||||||
|
cA -= m_invMassA * P;
|
||||||
|
aA -= m_invIA * b2Cross(rA, P);
|
||||||
|
cB += m_invMassB * P;
|
||||||
|
aB += m_invIB * b2Cross(rB, P);
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return b2Abs(C) < b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2DistanceJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2DistanceJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 F = (inv_dt * m_impulse) * m_u;
|
||||||
|
return F;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(inv_dt);
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2DistanceJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2DistanceJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.length = %.15lef;\n", m_length);
|
||||||
|
b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
|
||||||
|
b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,169 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_DISTANCE_JOINT_H
|
||||||
|
#define B2_DISTANCE_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Distance joint definition. This requires defining an
|
||||||
|
/// anchor point on both bodies and the non-zero length of the
|
||||||
|
/// distance joint. The definition uses local anchor points
|
||||||
|
/// so that the initial configuration can violate the constraint
|
||||||
|
/// slightly. This helps when saving and loading a game.
|
||||||
|
/// @warning Do not use a zero or short length.
|
||||||
|
struct b2DistanceJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2DistanceJointDef()
|
||||||
|
{
|
||||||
|
type = e_distanceJoint;
|
||||||
|
localAnchorA.Set(0.0f, 0.0f);
|
||||||
|
localAnchorB.Set(0.0f, 0.0f);
|
||||||
|
length = 1.0f;
|
||||||
|
frequencyHz = 0.0f;
|
||||||
|
dampingRatio = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, and length using the world
|
||||||
|
/// anchors.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB,
|
||||||
|
const b2Vec2& anchorA, const b2Vec2& anchorB);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The natural length between the anchor points.
|
||||||
|
float32 length;
|
||||||
|
|
||||||
|
/// The mass-spring-damper frequency in Hertz. A value of 0
|
||||||
|
/// disables softness.
|
||||||
|
float32 frequencyHz;
|
||||||
|
|
||||||
|
/// The damping ratio. 0 = no damping, 1 = critical damping.
|
||||||
|
float32 dampingRatio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A distance joint constrains two points on two bodies
|
||||||
|
/// to remain at a fixed distance from each other. You can view
|
||||||
|
/// this as a massless, rigid rod.
|
||||||
|
class b2DistanceJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
/// Get the reaction force given the inverse time step.
|
||||||
|
/// Unit is N.
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Get the reaction torque given the inverse time step.
|
||||||
|
/// Unit is N*m. This is always zero for a distance joint.
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// Set/get the natural length.
|
||||||
|
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
|
||||||
|
void SetLength(float32 length);
|
||||||
|
float32 GetLength() const;
|
||||||
|
|
||||||
|
/// Set/get frequency in Hz.
|
||||||
|
void SetFrequency(float32 hz);
|
||||||
|
float32 GetFrequency() const;
|
||||||
|
|
||||||
|
/// Set/get damping ratio.
|
||||||
|
void SetDampingRatio(float32 ratio);
|
||||||
|
float32 GetDampingRatio() const;
|
||||||
|
|
||||||
|
/// Dump joint to dmLog
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
b2DistanceJoint(const b2DistanceJointDef* data);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
float32 m_frequencyHz;
|
||||||
|
float32 m_dampingRatio;
|
||||||
|
float32 m_bias;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
float32 m_gamma;
|
||||||
|
float32 m_impulse;
|
||||||
|
float32 m_length;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_u;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
float32 m_mass;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline void b2DistanceJoint::SetLength(float32 length)
|
||||||
|
{
|
||||||
|
m_length = length;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2DistanceJoint::GetLength() const
|
||||||
|
{
|
||||||
|
return m_length;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2DistanceJoint::SetFrequency(float32 hz)
|
||||||
|
{
|
||||||
|
m_frequencyHz = hz;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2DistanceJoint::GetFrequency() const
|
||||||
|
{
|
||||||
|
return m_frequencyHz;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
|
||||||
|
{
|
||||||
|
m_dampingRatio = ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2DistanceJoint::GetDampingRatio() const
|
||||||
|
{
|
||||||
|
return m_dampingRatio;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,251 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2FrictionJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Point-to-point constraint
|
||||||
|
// Cdot = v2 - v1
|
||||||
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
||||||
|
// J = [-I -r1_skew I r2_skew ]
|
||||||
|
// Identity used:
|
||||||
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||||
|
|
||||||
|
// Angle constraint
|
||||||
|
// Cdot = w2 - w1
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
// K = invI1 + invI2
|
||||||
|
|
||||||
|
void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
|
||||||
|
m_linearImpulse.SetZero();
|
||||||
|
m_angularImpulse = 0.0f;
|
||||||
|
|
||||||
|
m_maxForce = def->maxForce;
|
||||||
|
m_maxTorque = def->maxTorque;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
// Compute the effective mass matrix.
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// J = [-I -r1_skew I r2_skew]
|
||||||
|
// [ 0 -1 0 1]
|
||||||
|
// r_skew = [-ry; rx]
|
||||||
|
|
||||||
|
// Matlab
|
||||||
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
||||||
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
||||||
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Mat22 K;
|
||||||
|
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
|
||||||
|
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
|
||||||
|
K.ey.x = K.ex.y;
|
||||||
|
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
|
||||||
|
|
||||||
|
m_linearMass = K.GetInverse();
|
||||||
|
|
||||||
|
m_angularMass = iA + iB;
|
||||||
|
if (m_angularMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_angularMass = 1.0f / m_angularMass;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale impulses to support a variable time step.
|
||||||
|
m_linearImpulse *= data.step.dtRatio;
|
||||||
|
m_angularImpulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_linearImpulse.SetZero();
|
||||||
|
m_angularImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
|
||||||
|
// Solve angular friction
|
||||||
|
{
|
||||||
|
float32 Cdot = wB - wA;
|
||||||
|
float32 impulse = -m_angularMass * Cdot;
|
||||||
|
|
||||||
|
float32 oldImpulse = m_angularImpulse;
|
||||||
|
float32 maxImpulse = h * m_maxTorque;
|
||||||
|
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
|
||||||
|
impulse = m_angularImpulse - oldImpulse;
|
||||||
|
|
||||||
|
wA -= iA * impulse;
|
||||||
|
wB += iB * impulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve linear friction
|
||||||
|
{
|
||||||
|
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||||
|
|
||||||
|
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
|
||||||
|
b2Vec2 oldImpulse = m_linearImpulse;
|
||||||
|
m_linearImpulse += impulse;
|
||||||
|
|
||||||
|
float32 maxImpulse = h * m_maxForce;
|
||||||
|
|
||||||
|
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
|
||||||
|
{
|
||||||
|
m_linearImpulse.Normalize();
|
||||||
|
m_linearImpulse *= maxImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
impulse = m_linearImpulse - oldImpulse;
|
||||||
|
|
||||||
|
vA -= mA * impulse;
|
||||||
|
wA -= iA * b2Cross(m_rA, impulse);
|
||||||
|
|
||||||
|
vB += mB * impulse;
|
||||||
|
wB += iB * b2Cross(m_rB, impulse);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
B2_NOT_USED(data);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2FrictionJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2FrictionJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_linearImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_angularImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2FrictionJoint::SetMaxForce(float32 force)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(force) && force >= 0.0f);
|
||||||
|
m_maxForce = force;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2FrictionJoint::GetMaxForce() const
|
||||||
|
{
|
||||||
|
return m_maxForce;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2FrictionJoint::SetMaxTorque(float32 torque)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(torque) && torque >= 0.0f);
|
||||||
|
m_maxTorque = torque;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2FrictionJoint::GetMaxTorque() const
|
||||||
|
{
|
||||||
|
return m_maxTorque;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2FrictionJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2FrictionJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
|
||||||
|
b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,119 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_FRICTION_JOINT_H
|
||||||
|
#define B2_FRICTION_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Friction joint definition.
|
||||||
|
struct b2FrictionJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2FrictionJointDef()
|
||||||
|
{
|
||||||
|
type = e_frictionJoint;
|
||||||
|
localAnchorA.SetZero();
|
||||||
|
localAnchorB.SetZero();
|
||||||
|
maxForce = 0.0f;
|
||||||
|
maxTorque = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, axis, and reference angle using the world
|
||||||
|
/// anchor and world axis.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The maximum friction force in N.
|
||||||
|
float32 maxForce;
|
||||||
|
|
||||||
|
/// The maximum friction torque in N-m.
|
||||||
|
float32 maxTorque;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Friction joint. This is used for top-down friction.
|
||||||
|
/// It provides 2D translational friction and angular friction.
|
||||||
|
class b2FrictionJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// Set the maximum friction force in N.
|
||||||
|
void SetMaxForce(float32 force);
|
||||||
|
|
||||||
|
/// Get the maximum friction force in N.
|
||||||
|
float32 GetMaxForce() const;
|
||||||
|
|
||||||
|
/// Set the maximum friction torque in N*m.
|
||||||
|
void SetMaxTorque(float32 torque);
|
||||||
|
|
||||||
|
/// Get the maximum friction torque in N*m.
|
||||||
|
float32 GetMaxTorque() const;
|
||||||
|
|
||||||
|
/// Dump joint to dmLog
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
|
||||||
|
b2FrictionJoint(const b2FrictionJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_linearImpulse;
|
||||||
|
float32 m_angularImpulse;
|
||||||
|
float32 m_maxForce;
|
||||||
|
float32 m_maxTorque;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Mat22 m_linearMass;
|
||||||
|
float32 m_angularMass;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,419 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2GearJoint.h"
|
||||||
|
#include "b2RevoluteJoint.h"
|
||||||
|
#include "b2PrismaticJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Gear Joint:
|
||||||
|
// C0 = (coordinate1 + ratio * coordinate2)_initial
|
||||||
|
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
|
||||||
|
// J = [J1 ratio * J2]
|
||||||
|
// K = J * invM * JT
|
||||||
|
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
|
||||||
|
//
|
||||||
|
// Revolute:
|
||||||
|
// coordinate = rotation
|
||||||
|
// Cdot = angularVelocity
|
||||||
|
// J = [0 0 1]
|
||||||
|
// K = J * invM * JT = invI
|
||||||
|
//
|
||||||
|
// Prismatic:
|
||||||
|
// coordinate = dot(p - pg, ug)
|
||||||
|
// Cdot = dot(v + cross(w, r), ug)
|
||||||
|
// J = [ug cross(r, ug)]
|
||||||
|
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
|
||||||
|
|
||||||
|
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_joint1 = def->joint1;
|
||||||
|
m_joint2 = def->joint2;
|
||||||
|
|
||||||
|
m_typeA = m_joint1->GetType();
|
||||||
|
m_typeB = m_joint2->GetType();
|
||||||
|
|
||||||
|
b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
|
||||||
|
b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
|
||||||
|
|
||||||
|
float32 coordinateA, coordinateB;
|
||||||
|
|
||||||
|
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
|
||||||
|
|
||||||
|
m_bodyC = m_joint1->GetBodyA();
|
||||||
|
m_bodyA = m_joint1->GetBodyB();
|
||||||
|
|
||||||
|
// Get geometry of joint1
|
||||||
|
b2Transform xfA = m_bodyA->m_xf;
|
||||||
|
float32 aA = m_bodyA->m_sweep.a;
|
||||||
|
b2Transform xfC = m_bodyC->m_xf;
|
||||||
|
float32 aC = m_bodyC->m_sweep.a;
|
||||||
|
|
||||||
|
if (m_typeA == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
|
||||||
|
m_localAnchorC = revolute->m_localAnchorA;
|
||||||
|
m_localAnchorA = revolute->m_localAnchorB;
|
||||||
|
m_referenceAngleA = revolute->m_referenceAngle;
|
||||||
|
m_localAxisC.SetZero();
|
||||||
|
|
||||||
|
coordinateA = aA - aC - m_referenceAngleA;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
|
||||||
|
m_localAnchorC = prismatic->m_localAnchorA;
|
||||||
|
m_localAnchorA = prismatic->m_localAnchorB;
|
||||||
|
m_referenceAngleA = prismatic->m_referenceAngle;
|
||||||
|
m_localAxisC = prismatic->m_localXAxisA;
|
||||||
|
|
||||||
|
b2Vec2 pC = m_localAnchorC;
|
||||||
|
b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
|
||||||
|
coordinateA = b2Dot(pA - pC, m_localAxisC);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_bodyD = m_joint2->GetBodyA();
|
||||||
|
m_bodyB = m_joint2->GetBodyB();
|
||||||
|
|
||||||
|
// Get geometry of joint2
|
||||||
|
b2Transform xfB = m_bodyB->m_xf;
|
||||||
|
float32 aB = m_bodyB->m_sweep.a;
|
||||||
|
b2Transform xfD = m_bodyD->m_xf;
|
||||||
|
float32 aD = m_bodyD->m_sweep.a;
|
||||||
|
|
||||||
|
if (m_typeB == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
|
||||||
|
m_localAnchorD = revolute->m_localAnchorA;
|
||||||
|
m_localAnchorB = revolute->m_localAnchorB;
|
||||||
|
m_referenceAngleB = revolute->m_referenceAngle;
|
||||||
|
m_localAxisD.SetZero();
|
||||||
|
|
||||||
|
coordinateB = aB - aD - m_referenceAngleB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
|
||||||
|
m_localAnchorD = prismatic->m_localAnchorA;
|
||||||
|
m_localAnchorB = prismatic->m_localAnchorB;
|
||||||
|
m_referenceAngleB = prismatic->m_referenceAngle;
|
||||||
|
m_localAxisD = prismatic->m_localXAxisA;
|
||||||
|
|
||||||
|
b2Vec2 pD = m_localAnchorD;
|
||||||
|
b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
|
||||||
|
coordinateB = b2Dot(pB - pD, m_localAxisD);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_ratio = def->ratio;
|
||||||
|
|
||||||
|
m_constant = coordinateA + m_ratio * coordinateB;
|
||||||
|
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_indexC = m_bodyC->m_islandIndex;
|
||||||
|
m_indexD = m_bodyD->m_islandIndex;
|
||||||
|
m_lcA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_lcB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_lcC = m_bodyC->m_sweep.localCenter;
|
||||||
|
m_lcD = m_bodyD->m_sweep.localCenter;
|
||||||
|
m_mA = m_bodyA->m_invMass;
|
||||||
|
m_mB = m_bodyB->m_invMass;
|
||||||
|
m_mC = m_bodyC->m_invMass;
|
||||||
|
m_mD = m_bodyD->m_invMass;
|
||||||
|
m_iA = m_bodyA->m_invI;
|
||||||
|
m_iB = m_bodyB->m_invI;
|
||||||
|
m_iC = m_bodyC->m_invI;
|
||||||
|
m_iD = m_bodyD->m_invI;
|
||||||
|
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 aC = data.positions[m_indexC].a;
|
||||||
|
b2Vec2 vC = data.velocities[m_indexC].v;
|
||||||
|
float32 wC = data.velocities[m_indexC].w;
|
||||||
|
|
||||||
|
float32 aD = data.positions[m_indexD].a;
|
||||||
|
b2Vec2 vD = data.velocities[m_indexD].v;
|
||||||
|
float32 wD = data.velocities[m_indexD].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
|
||||||
|
|
||||||
|
m_mass = 0.0f;
|
||||||
|
|
||||||
|
if (m_typeA == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
m_JvAC.SetZero();
|
||||||
|
m_JwA = 1.0f;
|
||||||
|
m_JwC = 1.0f;
|
||||||
|
m_mass += m_iA + m_iC;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 u = b2Mul(qC, m_localAxisC);
|
||||||
|
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
|
||||||
|
m_JvAC = u;
|
||||||
|
m_JwC = b2Cross(rC, u);
|
||||||
|
m_JwA = b2Cross(rA, u);
|
||||||
|
m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_typeB == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
m_JvBD.SetZero();
|
||||||
|
m_JwB = m_ratio;
|
||||||
|
m_JwD = m_ratio;
|
||||||
|
m_mass += m_ratio * m_ratio * (m_iB + m_iD);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 u = b2Mul(qD, m_localAxisD);
|
||||||
|
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
|
||||||
|
m_JvBD = m_ratio * u;
|
||||||
|
m_JwD = m_ratio * b2Cross(rD, u);
|
||||||
|
m_JwB = m_ratio * b2Cross(rB, u);
|
||||||
|
m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute effective mass.
|
||||||
|
m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
vA += (m_mA * m_impulse) * m_JvAC;
|
||||||
|
wA += m_iA * m_impulse * m_JwA;
|
||||||
|
vB += (m_mB * m_impulse) * m_JvBD;
|
||||||
|
wB += m_iB * m_impulse * m_JwB;
|
||||||
|
vC -= (m_mC * m_impulse) * m_JvAC;
|
||||||
|
wC -= m_iC * m_impulse * m_JwC;
|
||||||
|
vD -= (m_mD * m_impulse) * m_JvBD;
|
||||||
|
wD -= m_iD * m_impulse * m_JwD;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
data.velocities[m_indexC].v = vC;
|
||||||
|
data.velocities[m_indexC].w = wC;
|
||||||
|
data.velocities[m_indexD].v = vD;
|
||||||
|
data.velocities[m_indexD].w = wD;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
b2Vec2 vC = data.velocities[m_indexC].v;
|
||||||
|
float32 wC = data.velocities[m_indexC].w;
|
||||||
|
b2Vec2 vD = data.velocities[m_indexD].v;
|
||||||
|
float32 wD = data.velocities[m_indexD].w;
|
||||||
|
|
||||||
|
float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
|
||||||
|
Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
|
||||||
|
|
||||||
|
float32 impulse = -m_mass * Cdot;
|
||||||
|
m_impulse += impulse;
|
||||||
|
|
||||||
|
vA += (m_mA * impulse) * m_JvAC;
|
||||||
|
wA += m_iA * impulse * m_JwA;
|
||||||
|
vB += (m_mB * impulse) * m_JvBD;
|
||||||
|
wB += m_iB * impulse * m_JwB;
|
||||||
|
vC -= (m_mC * impulse) * m_JvAC;
|
||||||
|
wC -= m_iC * impulse * m_JwC;
|
||||||
|
vD -= (m_mD * impulse) * m_JvBD;
|
||||||
|
wD -= m_iD * impulse * m_JwD;
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
data.velocities[m_indexC].v = vC;
|
||||||
|
data.velocities[m_indexC].w = wC;
|
||||||
|
data.velocities[m_indexD].v = vD;
|
||||||
|
data.velocities[m_indexD].w = wD;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 cC = data.positions[m_indexC].c;
|
||||||
|
float32 aC = data.positions[m_indexC].a;
|
||||||
|
b2Vec2 cD = data.positions[m_indexD].c;
|
||||||
|
float32 aD = data.positions[m_indexD].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
|
||||||
|
|
||||||
|
float32 linearError = 0.0f;
|
||||||
|
|
||||||
|
float32 coordinateA, coordinateB;
|
||||||
|
|
||||||
|
b2Vec2 JvAC, JvBD;
|
||||||
|
float32 JwA, JwB, JwC, JwD;
|
||||||
|
float32 mass = 0.0f;
|
||||||
|
|
||||||
|
if (m_typeA == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
JvAC.SetZero();
|
||||||
|
JwA = 1.0f;
|
||||||
|
JwC = 1.0f;
|
||||||
|
mass += m_iA + m_iC;
|
||||||
|
|
||||||
|
coordinateA = aA - aC - m_referenceAngleA;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 u = b2Mul(qC, m_localAxisC);
|
||||||
|
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
|
||||||
|
JvAC = u;
|
||||||
|
JwC = b2Cross(rC, u);
|
||||||
|
JwA = b2Cross(rA, u);
|
||||||
|
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
|
||||||
|
|
||||||
|
b2Vec2 pC = m_localAnchorC - m_lcC;
|
||||||
|
b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
|
||||||
|
coordinateA = b2Dot(pA - pC, m_localAxisC);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_typeB == e_revoluteJoint)
|
||||||
|
{
|
||||||
|
JvBD.SetZero();
|
||||||
|
JwB = m_ratio;
|
||||||
|
JwD = m_ratio;
|
||||||
|
mass += m_ratio * m_ratio * (m_iB + m_iD);
|
||||||
|
|
||||||
|
coordinateB = aB - aD - m_referenceAngleB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 u = b2Mul(qD, m_localAxisD);
|
||||||
|
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
|
||||||
|
JvBD = m_ratio * u;
|
||||||
|
JwD = m_ratio * b2Cross(rD, u);
|
||||||
|
JwB = m_ratio * b2Cross(rB, u);
|
||||||
|
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
|
||||||
|
|
||||||
|
b2Vec2 pD = m_localAnchorD - m_lcD;
|
||||||
|
b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
|
||||||
|
coordinateB = b2Dot(pB - pD, m_localAxisD);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
|
||||||
|
|
||||||
|
float32 impulse = 0.0f;
|
||||||
|
if (mass > 0.0f)
|
||||||
|
{
|
||||||
|
impulse = -C / mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
cA += m_mA * impulse * JvAC;
|
||||||
|
aA += m_iA * impulse * JwA;
|
||||||
|
cB += m_mB * impulse * JvBD;
|
||||||
|
aB += m_iB * impulse * JwB;
|
||||||
|
cC -= m_mC * impulse * JvAC;
|
||||||
|
aC -= m_iC * impulse * JwC;
|
||||||
|
cD -= m_mD * impulse * JvBD;
|
||||||
|
aD -= m_iD * impulse * JwD;
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
data.positions[m_indexC].c = cC;
|
||||||
|
data.positions[m_indexC].a = aC;
|
||||||
|
data.positions[m_indexD].c = cD;
|
||||||
|
data.positions[m_indexD].a = aD;
|
||||||
|
|
||||||
|
// TODO_ERIN not implemented
|
||||||
|
return linearError < b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2GearJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2GearJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 P = m_impulse * m_JvAC;
|
||||||
|
return inv_dt * P;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
float32 L = m_impulse * m_JwA;
|
||||||
|
return inv_dt * L;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2GearJoint::SetRatio(float32 ratio)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(ratio));
|
||||||
|
m_ratio = ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2GearJoint::GetRatio() const
|
||||||
|
{
|
||||||
|
return m_ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2GearJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
int32 index1 = m_joint1->m_index;
|
||||||
|
int32 index2 = m_joint2->m_index;
|
||||||
|
|
||||||
|
b2Log(" b2GearJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.joint1 = joints[%d];\n", index1);
|
||||||
|
b2Log(" jd.joint2 = joints[%d];\n", index2);
|
||||||
|
b2Log(" jd.ratio = %.15lef;\n", m_ratio);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,125 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_GEAR_JOINT_H
|
||||||
|
#define B2_GEAR_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Gear joint definition. This definition requires two existing
|
||||||
|
/// revolute or prismatic joints (any combination will work).
|
||||||
|
struct b2GearJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2GearJointDef()
|
||||||
|
{
|
||||||
|
type = e_gearJoint;
|
||||||
|
joint1 = nullptr;
|
||||||
|
joint2 = nullptr;
|
||||||
|
ratio = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The first revolute/prismatic joint attached to the gear joint.
|
||||||
|
b2Joint* joint1;
|
||||||
|
|
||||||
|
/// The second revolute/prismatic joint attached to the gear joint.
|
||||||
|
b2Joint* joint2;
|
||||||
|
|
||||||
|
/// The gear ratio.
|
||||||
|
/// @see b2GearJoint for explanation.
|
||||||
|
float32 ratio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A gear joint is used to connect two joints together. Either joint
|
||||||
|
/// can be a revolute or prismatic joint. You specify a gear ratio
|
||||||
|
/// to bind the motions together:
|
||||||
|
/// coordinate1 + ratio * coordinate2 = constant
|
||||||
|
/// The ratio can be negative or positive. If one joint is a revolute joint
|
||||||
|
/// and the other joint is a prismatic joint, then the ratio will have units
|
||||||
|
/// of length or units of 1/length.
|
||||||
|
/// @warning You have to manually destroy the gear joint if joint1 or joint2
|
||||||
|
/// is destroyed.
|
||||||
|
class b2GearJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Get the first joint.
|
||||||
|
b2Joint* GetJoint1() { return m_joint1; }
|
||||||
|
|
||||||
|
/// Get the second joint.
|
||||||
|
b2Joint* GetJoint2() { return m_joint2; }
|
||||||
|
|
||||||
|
/// Set/Get the gear ratio.
|
||||||
|
void SetRatio(float32 ratio);
|
||||||
|
float32 GetRatio() const;
|
||||||
|
|
||||||
|
/// Dump joint to dmLog
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
b2GearJoint(const b2GearJointDef* data);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
b2Joint* m_joint1;
|
||||||
|
b2Joint* m_joint2;
|
||||||
|
|
||||||
|
b2JointType m_typeA;
|
||||||
|
b2JointType m_typeB;
|
||||||
|
|
||||||
|
// Body A is connected to body C
|
||||||
|
// Body B is connected to body D
|
||||||
|
b2Body* m_bodyC;
|
||||||
|
b2Body* m_bodyD;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
b2Vec2 m_localAnchorC;
|
||||||
|
b2Vec2 m_localAnchorD;
|
||||||
|
|
||||||
|
b2Vec2 m_localAxisC;
|
||||||
|
b2Vec2 m_localAxisD;
|
||||||
|
|
||||||
|
float32 m_referenceAngleA;
|
||||||
|
float32 m_referenceAngleB;
|
||||||
|
|
||||||
|
float32 m_constant;
|
||||||
|
float32 m_ratio;
|
||||||
|
|
||||||
|
float32 m_impulse;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA, m_indexB, m_indexC, m_indexD;
|
||||||
|
b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
|
||||||
|
float32 m_mA, m_mB, m_mC, m_mD;
|
||||||
|
float32 m_iA, m_iB, m_iC, m_iD;
|
||||||
|
b2Vec2 m_JvAC, m_JvBD;
|
||||||
|
float32 m_JwA, m_JwB, m_JwC, m_JwD;
|
||||||
|
float32 m_mass;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,211 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
#include "b2DistanceJoint.h"
|
||||||
|
#include "b2WheelJoint.h"
|
||||||
|
#include "b2MouseJoint.h"
|
||||||
|
#include "b2RevoluteJoint.h"
|
||||||
|
#include "b2PrismaticJoint.h"
|
||||||
|
#include "b2PulleyJoint.h"
|
||||||
|
#include "b2GearJoint.h"
|
||||||
|
#include "b2WeldJoint.h"
|
||||||
|
#include "b2FrictionJoint.h"
|
||||||
|
#include "b2RopeJoint.h"
|
||||||
|
#include "b2MotorJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2World.h"
|
||||||
|
#include "../../Common/b2BlockAllocator.h"
|
||||||
|
|
||||||
|
#include <new>
|
||||||
|
|
||||||
|
b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
b2Joint* joint = nullptr;
|
||||||
|
|
||||||
|
switch (def->type)
|
||||||
|
{
|
||||||
|
case e_distanceJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
|
||||||
|
joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_mouseJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2MouseJoint));
|
||||||
|
joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_prismaticJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
|
||||||
|
joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_revoluteJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
|
||||||
|
joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_pulleyJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
|
||||||
|
joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_gearJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2GearJoint));
|
||||||
|
joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_wheelJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2WheelJoint));
|
||||||
|
joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_weldJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2WeldJoint));
|
||||||
|
joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_frictionJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
|
||||||
|
joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_ropeJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2RopeJoint));
|
||||||
|
joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_motorJoint:
|
||||||
|
{
|
||||||
|
void* mem = allocator->Allocate(sizeof(b2MotorJoint));
|
||||||
|
joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return joint;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
joint->~b2Joint();
|
||||||
|
switch (joint->m_type)
|
||||||
|
{
|
||||||
|
case e_distanceJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2DistanceJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_mouseJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2MouseJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_prismaticJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2PrismaticJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_revoluteJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2RevoluteJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_pulleyJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2PulleyJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_gearJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2GearJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_wheelJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2WheelJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_weldJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2WeldJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_frictionJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2FrictionJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_ropeJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2RopeJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
case e_motorJoint:
|
||||||
|
allocator->Free(joint, sizeof(b2MotorJoint));
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Joint::b2Joint(const b2JointDef* def)
|
||||||
|
{
|
||||||
|
b2Assert(def->bodyA != def->bodyB);
|
||||||
|
|
||||||
|
m_type = def->type;
|
||||||
|
m_prev = nullptr;
|
||||||
|
m_next = nullptr;
|
||||||
|
m_bodyA = def->bodyA;
|
||||||
|
m_bodyB = def->bodyB;
|
||||||
|
m_index = 0;
|
||||||
|
m_collideConnected = def->collideConnected;
|
||||||
|
m_islandFlag = false;
|
||||||
|
m_userData = def->userData;
|
||||||
|
|
||||||
|
m_edgeA.joint = nullptr;
|
||||||
|
m_edgeA.other = nullptr;
|
||||||
|
m_edgeA.prev = nullptr;
|
||||||
|
m_edgeA.next = nullptr;
|
||||||
|
|
||||||
|
m_edgeB.joint = nullptr;
|
||||||
|
m_edgeB.other = nullptr;
|
||||||
|
m_edgeB.prev = nullptr;
|
||||||
|
m_edgeB.next = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2Joint::IsActive() const
|
||||||
|
{
|
||||||
|
return m_bodyA->IsActive() && m_bodyB->IsActive();
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,226 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_JOINT_H
|
||||||
|
#define B2_JOINT_H
|
||||||
|
|
||||||
|
#include "../../Common/b2Math.h"
|
||||||
|
|
||||||
|
class b2Body;
|
||||||
|
class b2Joint;
|
||||||
|
struct b2SolverData;
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
enum b2JointType
|
||||||
|
{
|
||||||
|
e_unknownJoint,
|
||||||
|
e_revoluteJoint,
|
||||||
|
e_prismaticJoint,
|
||||||
|
e_distanceJoint,
|
||||||
|
e_pulleyJoint,
|
||||||
|
e_mouseJoint,
|
||||||
|
e_gearJoint,
|
||||||
|
e_wheelJoint,
|
||||||
|
e_weldJoint,
|
||||||
|
e_frictionJoint,
|
||||||
|
e_ropeJoint,
|
||||||
|
e_motorJoint
|
||||||
|
};
|
||||||
|
|
||||||
|
enum b2LimitState
|
||||||
|
{
|
||||||
|
e_inactiveLimit,
|
||||||
|
e_atLowerLimit,
|
||||||
|
e_atUpperLimit,
|
||||||
|
e_equalLimits
|
||||||
|
};
|
||||||
|
|
||||||
|
struct b2Jacobian
|
||||||
|
{
|
||||||
|
b2Vec2 linear;
|
||||||
|
float32 angularA;
|
||||||
|
float32 angularB;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A joint edge is used to connect bodies and joints together
|
||||||
|
/// in a joint graph where each body is a node and each joint
|
||||||
|
/// is an edge. A joint edge belongs to a doubly linked list
|
||||||
|
/// maintained in each attached body. Each joint has two joint
|
||||||
|
/// nodes, one for each attached body.
|
||||||
|
struct b2JointEdge
|
||||||
|
{
|
||||||
|
b2Body* other; ///< provides quick access to the other body attached.
|
||||||
|
b2Joint* joint; ///< the joint
|
||||||
|
b2JointEdge* prev; ///< the previous joint edge in the body's joint list
|
||||||
|
b2JointEdge* next; ///< the next joint edge in the body's joint list
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Joint definitions are used to construct joints.
|
||||||
|
struct b2JointDef
|
||||||
|
{
|
||||||
|
b2JointDef()
|
||||||
|
{
|
||||||
|
type = e_unknownJoint;
|
||||||
|
userData = nullptr;
|
||||||
|
bodyA = nullptr;
|
||||||
|
bodyB = nullptr;
|
||||||
|
collideConnected = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The joint type is set automatically for concrete joint types.
|
||||||
|
b2JointType type;
|
||||||
|
|
||||||
|
/// Use this to attach application specific data to your joints.
|
||||||
|
void* userData;
|
||||||
|
|
||||||
|
/// The first attached body.
|
||||||
|
b2Body* bodyA;
|
||||||
|
|
||||||
|
/// The second attached body.
|
||||||
|
b2Body* bodyB;
|
||||||
|
|
||||||
|
/// Set this flag to true if the attached bodies should collide.
|
||||||
|
bool collideConnected;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// The base joint class. Joints are used to constraint two bodies together in
|
||||||
|
/// various fashions. Some joints also feature limits and motors.
|
||||||
|
class b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Get the type of the concrete joint.
|
||||||
|
b2JointType GetType() const;
|
||||||
|
|
||||||
|
/// Get the first body attached to this joint.
|
||||||
|
b2Body* GetBodyA();
|
||||||
|
|
||||||
|
/// Get the second body attached to this joint.
|
||||||
|
b2Body* GetBodyB();
|
||||||
|
|
||||||
|
/// Get the anchor point on bodyA in world coordinates.
|
||||||
|
virtual b2Vec2 GetAnchorA() const = 0;
|
||||||
|
|
||||||
|
/// Get the anchor point on bodyB in world coordinates.
|
||||||
|
virtual b2Vec2 GetAnchorB() const = 0;
|
||||||
|
|
||||||
|
/// Get the reaction force on bodyB at the joint anchor in Newtons.
|
||||||
|
virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
|
||||||
|
|
||||||
|
/// Get the reaction torque on bodyB in N*m.
|
||||||
|
virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
|
||||||
|
|
||||||
|
/// Get the next joint the world joint list.
|
||||||
|
b2Joint* GetNext();
|
||||||
|
const b2Joint* GetNext() const;
|
||||||
|
|
||||||
|
/// Get the user data pointer.
|
||||||
|
void* GetUserData() const;
|
||||||
|
|
||||||
|
/// Set the user data pointer.
|
||||||
|
void SetUserData(void* data);
|
||||||
|
|
||||||
|
/// Short-cut function to determine if either body is inactive.
|
||||||
|
bool IsActive() const;
|
||||||
|
|
||||||
|
/// Get collide connected.
|
||||||
|
/// Note: modifying the collide connect flag won't work correctly because
|
||||||
|
/// the flag is only checked when fixture AABBs begin to overlap.
|
||||||
|
bool GetCollideConnected() const;
|
||||||
|
|
||||||
|
/// Dump this joint to the log file.
|
||||||
|
virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }
|
||||||
|
|
||||||
|
/// Shift the origin for any points stored in world coordinates.
|
||||||
|
virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); }
|
||||||
|
|
||||||
|
protected:
|
||||||
|
friend class b2World;
|
||||||
|
friend class b2Body;
|
||||||
|
friend class b2Island;
|
||||||
|
friend class b2GearJoint;
|
||||||
|
|
||||||
|
static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
|
||||||
|
static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
b2Joint(const b2JointDef* def);
|
||||||
|
virtual ~b2Joint() {}
|
||||||
|
|
||||||
|
virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
|
||||||
|
virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
|
||||||
|
|
||||||
|
// This returns true if the position errors are within tolerance.
|
||||||
|
virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
|
||||||
|
|
||||||
|
b2JointType m_type;
|
||||||
|
b2Joint* m_prev;
|
||||||
|
b2Joint* m_next;
|
||||||
|
b2JointEdge m_edgeA;
|
||||||
|
b2JointEdge m_edgeB;
|
||||||
|
b2Body* m_bodyA;
|
||||||
|
b2Body* m_bodyB;
|
||||||
|
|
||||||
|
int32 m_index;
|
||||||
|
|
||||||
|
bool m_islandFlag;
|
||||||
|
bool m_collideConnected;
|
||||||
|
|
||||||
|
void* m_userData;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2JointType b2Joint::GetType() const
|
||||||
|
{
|
||||||
|
return m_type;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Body* b2Joint::GetBodyA()
|
||||||
|
{
|
||||||
|
return m_bodyA;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Body* b2Joint::GetBodyB()
|
||||||
|
{
|
||||||
|
return m_bodyB;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Joint* b2Joint::GetNext()
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Joint* b2Joint::GetNext() const
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void* b2Joint::GetUserData() const
|
||||||
|
{
|
||||||
|
return m_userData;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Joint::SetUserData(void* data)
|
||||||
|
{
|
||||||
|
m_userData = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Joint::GetCollideConnected() const
|
||||||
|
{
|
||||||
|
return m_collideConnected;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,309 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2MotorJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Point-to-point constraint
|
||||||
|
// Cdot = v2 - v1
|
||||||
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
||||||
|
// J = [-I -r1_skew I r2_skew ]
|
||||||
|
// Identity used:
|
||||||
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||||
|
//
|
||||||
|
// r1 = offset - c1
|
||||||
|
// r2 = -c2
|
||||||
|
|
||||||
|
// Angle constraint
|
||||||
|
// Cdot = w2 - w1
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
// K = invI1 + invI2
|
||||||
|
|
||||||
|
void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
b2Vec2 xB = bodyB->GetPosition();
|
||||||
|
linearOffset = bodyA->GetLocalPoint(xB);
|
||||||
|
|
||||||
|
float32 angleA = bodyA->GetAngle();
|
||||||
|
float32 angleB = bodyB->GetAngle();
|
||||||
|
angularOffset = angleB - angleA;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_linearOffset = def->linearOffset;
|
||||||
|
m_angularOffset = def->angularOffset;
|
||||||
|
|
||||||
|
m_linearImpulse.SetZero();
|
||||||
|
m_angularImpulse = 0.0f;
|
||||||
|
|
||||||
|
m_maxForce = def->maxForce;
|
||||||
|
m_maxTorque = def->maxTorque;
|
||||||
|
m_correctionFactor = def->correctionFactor;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
// Compute the effective mass matrix.
|
||||||
|
m_rA = b2Mul(qA, m_linearOffset - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, -m_localCenterB);
|
||||||
|
|
||||||
|
// J = [-I -r1_skew I r2_skew]
|
||||||
|
// r_skew = [-ry; rx]
|
||||||
|
|
||||||
|
// Matlab
|
||||||
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
||||||
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
||||||
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
// Upper 2 by 2 of K for point to point
|
||||||
|
b2Mat22 K;
|
||||||
|
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
|
||||||
|
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
|
||||||
|
K.ey.x = K.ex.y;
|
||||||
|
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
|
||||||
|
|
||||||
|
m_linearMass = K.GetInverse();
|
||||||
|
|
||||||
|
m_angularMass = iA + iB;
|
||||||
|
if (m_angularMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_angularMass = 1.0f / m_angularMass;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_linearError = cB + m_rB - cA - m_rA;
|
||||||
|
m_angularError = aB - aA - m_angularOffset;
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale impulses to support a variable time step.
|
||||||
|
m_linearImpulse *= data.step.dtRatio;
|
||||||
|
m_angularImpulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_linearImpulse.SetZero();
|
||||||
|
m_angularImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
float32 inv_h = data.step.inv_dt;
|
||||||
|
|
||||||
|
// Solve angular friction
|
||||||
|
{
|
||||||
|
float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
|
||||||
|
float32 impulse = -m_angularMass * Cdot;
|
||||||
|
|
||||||
|
float32 oldImpulse = m_angularImpulse;
|
||||||
|
float32 maxImpulse = h * m_maxTorque;
|
||||||
|
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
|
||||||
|
impulse = m_angularImpulse - oldImpulse;
|
||||||
|
|
||||||
|
wA -= iA * impulse;
|
||||||
|
wB += iB * impulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve linear friction
|
||||||
|
{
|
||||||
|
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
|
||||||
|
|
||||||
|
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
|
||||||
|
b2Vec2 oldImpulse = m_linearImpulse;
|
||||||
|
m_linearImpulse += impulse;
|
||||||
|
|
||||||
|
float32 maxImpulse = h * m_maxForce;
|
||||||
|
|
||||||
|
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
|
||||||
|
{
|
||||||
|
m_linearImpulse.Normalize();
|
||||||
|
m_linearImpulse *= maxImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
impulse = m_linearImpulse - oldImpulse;
|
||||||
|
|
||||||
|
vA -= mA * impulse;
|
||||||
|
wA -= iA * b2Cross(m_rA, impulse);
|
||||||
|
|
||||||
|
vB += mB * impulse;
|
||||||
|
wB += iB * b2Cross(m_rB, impulse);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
B2_NOT_USED(data);
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MotorJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetPosition();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MotorJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetPosition();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_linearImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_angularImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SetMaxForce(float32 force)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(force) && force >= 0.0f);
|
||||||
|
m_maxForce = force;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MotorJoint::GetMaxForce() const
|
||||||
|
{
|
||||||
|
return m_maxForce;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SetMaxTorque(float32 torque)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(torque) && torque >= 0.0f);
|
||||||
|
m_maxTorque = torque;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MotorJoint::GetMaxTorque() const
|
||||||
|
{
|
||||||
|
return m_maxTorque;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SetCorrectionFactor(float32 factor)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
|
||||||
|
m_correctionFactor = factor;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MotorJoint::GetCorrectionFactor() const
|
||||||
|
{
|
||||||
|
return m_correctionFactor;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset)
|
||||||
|
{
|
||||||
|
if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_linearOffset = linearOffset;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2Vec2& b2MotorJoint::GetLinearOffset() const
|
||||||
|
{
|
||||||
|
return m_linearOffset;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::SetAngularOffset(float32 angularOffset)
|
||||||
|
{
|
||||||
|
if (angularOffset != m_angularOffset)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_angularOffset = angularOffset;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MotorJoint::GetAngularOffset() const
|
||||||
|
{
|
||||||
|
return m_angularOffset;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MotorJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2MotorJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);
|
||||||
|
b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset);
|
||||||
|
b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
|
||||||
|
b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
|
||||||
|
b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,133 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2012 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_MOTOR_JOINT_H
|
||||||
|
#define B2_MOTOR_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Motor joint definition.
|
||||||
|
struct b2MotorJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2MotorJointDef()
|
||||||
|
{
|
||||||
|
type = e_motorJoint;
|
||||||
|
linearOffset.SetZero();
|
||||||
|
angularOffset = 0.0f;
|
||||||
|
maxForce = 1.0f;
|
||||||
|
maxTorque = 1.0f;
|
||||||
|
correctionFactor = 0.3f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies and offsets using the current transforms.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB);
|
||||||
|
|
||||||
|
/// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
|
||||||
|
b2Vec2 linearOffset;
|
||||||
|
|
||||||
|
/// The bodyB angle minus bodyA angle in radians.
|
||||||
|
float32 angularOffset;
|
||||||
|
|
||||||
|
/// The maximum motor force in N.
|
||||||
|
float32 maxForce;
|
||||||
|
|
||||||
|
/// The maximum motor torque in N-m.
|
||||||
|
float32 maxTorque;
|
||||||
|
|
||||||
|
/// Position correction factor in the range [0,1].
|
||||||
|
float32 correctionFactor;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A motor joint is used to control the relative motion
|
||||||
|
/// between two bodies. A typical usage is to control the movement
|
||||||
|
/// of a dynamic body with respect to the ground.
|
||||||
|
class b2MotorJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Set/get the target linear offset, in frame A, in meters.
|
||||||
|
void SetLinearOffset(const b2Vec2& linearOffset);
|
||||||
|
const b2Vec2& GetLinearOffset() const;
|
||||||
|
|
||||||
|
/// Set/get the target angular offset, in radians.
|
||||||
|
void SetAngularOffset(float32 angularOffset);
|
||||||
|
float32 GetAngularOffset() const;
|
||||||
|
|
||||||
|
/// Set the maximum friction force in N.
|
||||||
|
void SetMaxForce(float32 force);
|
||||||
|
|
||||||
|
/// Get the maximum friction force in N.
|
||||||
|
float32 GetMaxForce() const;
|
||||||
|
|
||||||
|
/// Set the maximum friction torque in N*m.
|
||||||
|
void SetMaxTorque(float32 torque);
|
||||||
|
|
||||||
|
/// Get the maximum friction torque in N*m.
|
||||||
|
float32 GetMaxTorque() const;
|
||||||
|
|
||||||
|
/// Set the position correction factor in the range [0,1].
|
||||||
|
void SetCorrectionFactor(float32 factor);
|
||||||
|
|
||||||
|
/// Get the position correction factor in the range [0,1].
|
||||||
|
float32 GetCorrectionFactor() const;
|
||||||
|
|
||||||
|
/// Dump to b2Log
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
|
||||||
|
b2MotorJoint(const b2MotorJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_linearOffset;
|
||||||
|
float32 m_angularOffset;
|
||||||
|
b2Vec2 m_linearImpulse;
|
||||||
|
float32 m_angularImpulse;
|
||||||
|
float32 m_maxForce;
|
||||||
|
float32 m_maxTorque;
|
||||||
|
float32 m_correctionFactor;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
b2Vec2 m_linearError;
|
||||||
|
float32 m_angularError;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Mat22 m_linearMass;
|
||||||
|
float32 m_angularMass;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,222 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2MouseJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// p = attached point, m = mouse point
|
||||||
|
// C = p - m
|
||||||
|
// Cdot = v
|
||||||
|
// = v + cross(w, r)
|
||||||
|
// J = [I r_skew]
|
||||||
|
// Identity used:
|
||||||
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||||
|
|
||||||
|
b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
b2Assert(def->target.IsValid());
|
||||||
|
b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
|
||||||
|
b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
|
||||||
|
b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
|
||||||
|
|
||||||
|
m_targetA = def->target;
|
||||||
|
m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
|
||||||
|
|
||||||
|
m_maxForce = def->maxForce;
|
||||||
|
m_impulse.SetZero();
|
||||||
|
|
||||||
|
m_frequencyHz = def->frequencyHz;
|
||||||
|
m_dampingRatio = def->dampingRatio;
|
||||||
|
|
||||||
|
m_beta = 0.0f;
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::SetTarget(const b2Vec2& target)
|
||||||
|
{
|
||||||
|
if (target != m_targetA)
|
||||||
|
{
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_targetA = target;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
const b2Vec2& b2MouseJoint::GetTarget() const
|
||||||
|
{
|
||||||
|
return m_targetA;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::SetMaxForce(float32 force)
|
||||||
|
{
|
||||||
|
m_maxForce = force;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MouseJoint::GetMaxForce() const
|
||||||
|
{
|
||||||
|
return m_maxForce;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::SetFrequency(float32 hz)
|
||||||
|
{
|
||||||
|
m_frequencyHz = hz;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MouseJoint::GetFrequency() const
|
||||||
|
{
|
||||||
|
return m_frequencyHz;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::SetDampingRatio(float32 ratio)
|
||||||
|
{
|
||||||
|
m_dampingRatio = ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MouseJoint::GetDampingRatio() const
|
||||||
|
{
|
||||||
|
return m_dampingRatio;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qB(aB);
|
||||||
|
|
||||||
|
float32 mass = m_bodyB->GetMass();
|
||||||
|
|
||||||
|
// Frequency
|
||||||
|
float32 omega = 2.0f * b2_pi * m_frequencyHz;
|
||||||
|
|
||||||
|
// Damping coefficient
|
||||||
|
float32 d = 2.0f * mass * m_dampingRatio * omega;
|
||||||
|
|
||||||
|
// Spring stiffness
|
||||||
|
float32 k = mass * (omega * omega);
|
||||||
|
|
||||||
|
// magic formulas
|
||||||
|
// gamma has units of inverse mass.
|
||||||
|
// beta has units of inverse time.
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
b2Assert(d + h * k > b2_epsilon);
|
||||||
|
m_gamma = h * (d + h * k);
|
||||||
|
if (m_gamma != 0.0f)
|
||||||
|
{
|
||||||
|
m_gamma = 1.0f / m_gamma;
|
||||||
|
}
|
||||||
|
m_beta = h * k * m_gamma;
|
||||||
|
|
||||||
|
// Compute the effective mass matrix.
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
|
||||||
|
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
|
||||||
|
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
|
||||||
|
b2Mat22 K;
|
||||||
|
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
|
||||||
|
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
|
||||||
|
K.ey.x = K.ex.y;
|
||||||
|
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
|
||||||
|
|
||||||
|
m_mass = K.GetInverse();
|
||||||
|
|
||||||
|
m_C = cB + m_rB - m_targetA;
|
||||||
|
m_C *= m_beta;
|
||||||
|
|
||||||
|
// Cheat with some damping
|
||||||
|
wB *= 0.98f;
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
vB += m_invMassB * m_impulse;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, m_impulse);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
// Cdot = v + cross(w, r)
|
||||||
|
b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
|
||||||
|
b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
|
||||||
|
|
||||||
|
b2Vec2 oldImpulse = m_impulse;
|
||||||
|
m_impulse += impulse;
|
||||||
|
float32 maxImpulse = data.step.dt * m_maxForce;
|
||||||
|
if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
|
||||||
|
{
|
||||||
|
m_impulse *= maxImpulse / m_impulse.Length();
|
||||||
|
}
|
||||||
|
impulse = m_impulse - oldImpulse;
|
||||||
|
|
||||||
|
vB += m_invMassB * impulse;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, impulse);
|
||||||
|
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
B2_NOT_USED(data);
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MouseJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_targetA;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MouseJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_impulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
|
||||||
|
{
|
||||||
|
m_targetA -= newOrigin;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,129 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_MOUSE_JOINT_H
|
||||||
|
#define B2_MOUSE_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Mouse joint definition. This requires a world target point,
|
||||||
|
/// tuning parameters, and the time step.
|
||||||
|
struct b2MouseJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2MouseJointDef()
|
||||||
|
{
|
||||||
|
type = e_mouseJoint;
|
||||||
|
target.Set(0.0f, 0.0f);
|
||||||
|
maxForce = 0.0f;
|
||||||
|
frequencyHz = 5.0f;
|
||||||
|
dampingRatio = 0.7f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The initial world target point. This is assumed
|
||||||
|
/// to coincide with the body anchor initially.
|
||||||
|
b2Vec2 target;
|
||||||
|
|
||||||
|
/// The maximum constraint force that can be exerted
|
||||||
|
/// to move the candidate body. Usually you will express
|
||||||
|
/// as some multiple of the weight (multiplier * mass * gravity).
|
||||||
|
float32 maxForce;
|
||||||
|
|
||||||
|
/// The response speed.
|
||||||
|
float32 frequencyHz;
|
||||||
|
|
||||||
|
/// The damping ratio. 0 = no damping, 1 = critical damping.
|
||||||
|
float32 dampingRatio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A mouse joint is used to make a point on a body track a
|
||||||
|
/// specified world point. This a soft constraint with a maximum
|
||||||
|
/// force. This allows the constraint to stretch and without
|
||||||
|
/// applying huge forces.
|
||||||
|
/// NOTE: this joint is not documented in the manual because it was
|
||||||
|
/// developed to be used in the testbed. If you want to learn how to
|
||||||
|
/// use the mouse joint, look at the testbed.
|
||||||
|
class b2MouseJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
|
||||||
|
/// Implements b2Joint.
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
|
||||||
|
/// Implements b2Joint.
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
/// Implements b2Joint.
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Implements b2Joint.
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Use this to update the target point.
|
||||||
|
void SetTarget(const b2Vec2& target);
|
||||||
|
const b2Vec2& GetTarget() const;
|
||||||
|
|
||||||
|
/// Set/get the maximum force in Newtons.
|
||||||
|
void SetMaxForce(float32 force);
|
||||||
|
float32 GetMaxForce() const;
|
||||||
|
|
||||||
|
/// Set/get the frequency in Hertz.
|
||||||
|
void SetFrequency(float32 hz);
|
||||||
|
float32 GetFrequency() const;
|
||||||
|
|
||||||
|
/// Set/get the damping ratio (dimensionless).
|
||||||
|
void SetDampingRatio(float32 ratio);
|
||||||
|
float32 GetDampingRatio() const;
|
||||||
|
|
||||||
|
/// The mouse joint does not support dumping.
|
||||||
|
void Dump() override { b2Log("Mouse joint dumping is not supported.\n"); }
|
||||||
|
|
||||||
|
/// Implement b2Joint::ShiftOrigin
|
||||||
|
void ShiftOrigin(const b2Vec2& newOrigin) override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
friend class b2Joint;
|
||||||
|
|
||||||
|
b2MouseJoint(const b2MouseJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
b2Vec2 m_targetA;
|
||||||
|
float32 m_frequencyHz;
|
||||||
|
float32 m_dampingRatio;
|
||||||
|
float32 m_beta;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_impulse;
|
||||||
|
float32 m_maxForce;
|
||||||
|
float32 m_gamma;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Mat22 m_mass;
|
||||||
|
b2Vec2 m_C;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,642 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2PrismaticJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Linear constraint (point-to-line)
|
||||||
|
// d = p2 - p1 = x2 + r2 - x1 - r1
|
||||||
|
// C = dot(perp, d)
|
||||||
|
// Cdot = dot(d, cross(w1, perp)) + dot(perp, v2 + cross(w2, r2) - v1 - cross(w1, r1))
|
||||||
|
// = -dot(perp, v1) - dot(cross(d + r1, perp), w1) + dot(perp, v2) + dot(cross(r2, perp), v2)
|
||||||
|
// J = [-perp, -cross(d + r1, perp), perp, cross(r2,perp)]
|
||||||
|
//
|
||||||
|
// Angular constraint
|
||||||
|
// C = a2 - a1 + a_initial
|
||||||
|
// Cdot = w2 - w1
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
//
|
||||||
|
// K = J * invM * JT
|
||||||
|
//
|
||||||
|
// J = [-a -s1 a s2]
|
||||||
|
// [0 -1 0 1]
|
||||||
|
// a = perp
|
||||||
|
// s1 = cross(d + r1, a) = cross(p2 - x1, a)
|
||||||
|
// s2 = cross(r2, a) = cross(p2 - x2, a)
|
||||||
|
|
||||||
|
|
||||||
|
// Motor/Limit linear constraint
|
||||||
|
// C = dot(ax1, d)
|
||||||
|
// Cdot = = -dot(ax1, v1) - dot(cross(d + r1, ax1), w1) + dot(ax1, v2) + dot(cross(r2, ax1), v2)
|
||||||
|
// J = [-ax1 -cross(d+r1,ax1) ax1 cross(r2,ax1)]
|
||||||
|
|
||||||
|
// Block Solver
|
||||||
|
// We develop a block solver that includes the joint limit. This makes the limit stiff (inelastic) even
|
||||||
|
// when the mass has poor distribution (leading to large torques about the joint anchor points).
|
||||||
|
//
|
||||||
|
// The Jacobian has 3 rows:
|
||||||
|
// J = [-uT -s1 uT s2] // linear
|
||||||
|
// [0 -1 0 1] // angular
|
||||||
|
// [-vT -a1 vT a2] // limit
|
||||||
|
//
|
||||||
|
// u = perp
|
||||||
|
// v = axis
|
||||||
|
// s1 = cross(d + r1, u), s2 = cross(r2, u)
|
||||||
|
// a1 = cross(d + r1, v), a2 = cross(r2, v)
|
||||||
|
|
||||||
|
// M * (v2 - v1) = JT * df
|
||||||
|
// J * v2 = bias
|
||||||
|
//
|
||||||
|
// v2 = v1 + invM * JT * df
|
||||||
|
// J * (v1 + invM * JT * df) = bias
|
||||||
|
// K * df = bias - J * v1 = -Cdot
|
||||||
|
// K = J * invM * JT
|
||||||
|
// Cdot = J * v1 - bias
|
||||||
|
//
|
||||||
|
// Now solve for f2.
|
||||||
|
// df = f2 - f1
|
||||||
|
// K * (f2 - f1) = -Cdot
|
||||||
|
// f2 = invK * (-Cdot) + f1
|
||||||
|
//
|
||||||
|
// Clamp accumulated limit impulse.
|
||||||
|
// lower: f2(3) = max(f2(3), 0)
|
||||||
|
// upper: f2(3) = min(f2(3), 0)
|
||||||
|
//
|
||||||
|
// Solve for correct f2(1:2)
|
||||||
|
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:3) * f1
|
||||||
|
// = -Cdot(1:2) - K(1:2,3) * f2(3) + K(1:2,1:2) * f1(1:2) + K(1:2,3) * f1(3)
|
||||||
|
// K(1:2, 1:2) * f2(1:2) = -Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3)) + K(1:2,1:2) * f1(1:2)
|
||||||
|
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
|
||||||
|
//
|
||||||
|
// Now compute impulse to be applied:
|
||||||
|
// df = f2 - f1
|
||||||
|
|
||||||
|
void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||||
|
localAxisA = bodyA->GetLocalVector(axis);
|
||||||
|
referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
m_localXAxisA = def->localAxisA;
|
||||||
|
m_localXAxisA.Normalize();
|
||||||
|
m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
|
||||||
|
m_referenceAngle = def->referenceAngle;
|
||||||
|
|
||||||
|
m_impulse.SetZero();
|
||||||
|
m_motorMass = 0.0f;
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
|
||||||
|
m_lowerTranslation = def->lowerTranslation;
|
||||||
|
m_upperTranslation = def->upperTranslation;
|
||||||
|
m_maxMotorForce = def->maxMotorForce;
|
||||||
|
m_motorSpeed = def->motorSpeed;
|
||||||
|
m_enableLimit = def->enableLimit;
|
||||||
|
m_enableMotor = def->enableMotor;
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
|
||||||
|
m_axis.SetZero();
|
||||||
|
m_perp.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
// Compute the effective masses.
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 d = (cB - cA) + rB - rA;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
// Compute motor Jacobian and effective mass.
|
||||||
|
{
|
||||||
|
m_axis = b2Mul(qA, m_localXAxisA);
|
||||||
|
m_a1 = b2Cross(d + rA, m_axis);
|
||||||
|
m_a2 = b2Cross(rB, m_axis);
|
||||||
|
|
||||||
|
m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
|
||||||
|
if (m_motorMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_motorMass = 1.0f / m_motorMass;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Prismatic constraint.
|
||||||
|
{
|
||||||
|
m_perp = b2Mul(qA, m_localYAxisA);
|
||||||
|
|
||||||
|
m_s1 = b2Cross(d + rA, m_perp);
|
||||||
|
m_s2 = b2Cross(rB, m_perp);
|
||||||
|
|
||||||
|
float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
|
||||||
|
float32 k12 = iA * m_s1 + iB * m_s2;
|
||||||
|
float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
|
||||||
|
float32 k22 = iA + iB;
|
||||||
|
if (k22 == 0.0f)
|
||||||
|
{
|
||||||
|
// For bodies with fixed rotation.
|
||||||
|
k22 = 1.0f;
|
||||||
|
}
|
||||||
|
float32 k23 = iA * m_a1 + iB * m_a2;
|
||||||
|
float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
|
||||||
|
|
||||||
|
m_K.ex.Set(k11, k12, k13);
|
||||||
|
m_K.ey.Set(k12, k22, k23);
|
||||||
|
m_K.ez.Set(k13, k23, k33);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute motor and limit terms.
|
||||||
|
if (m_enableLimit)
|
||||||
|
{
|
||||||
|
float32 jointTranslation = b2Dot(m_axis, d);
|
||||||
|
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
m_limitState = e_equalLimits;
|
||||||
|
}
|
||||||
|
else if (jointTranslation <= m_lowerTranslation)
|
||||||
|
{
|
||||||
|
if (m_limitState != e_atLowerLimit)
|
||||||
|
{
|
||||||
|
m_limitState = e_atLowerLimit;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (jointTranslation >= m_upperTranslation)
|
||||||
|
{
|
||||||
|
if (m_limitState != e_atUpperLimit)
|
||||||
|
{
|
||||||
|
m_limitState = e_atUpperLimit;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_enableMotor == false)
|
||||||
|
{
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Account for variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
m_motorImpulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
|
||||||
|
float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
|
||||||
|
float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse.SetZero();
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
// Solve linear motor constraint.
|
||||||
|
if (m_enableMotor && m_limitState != e_equalLimits)
|
||||||
|
{
|
||||||
|
float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
|
||||||
|
float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
|
||||||
|
float32 oldImpulse = m_motorImpulse;
|
||||||
|
float32 maxImpulse = data.step.dt * m_maxMotorForce;
|
||||||
|
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||||
|
impulse = m_motorImpulse - oldImpulse;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * m_axis;
|
||||||
|
float32 LA = impulse * m_a1;
|
||||||
|
float32 LB = impulse * m_a2;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 Cdot1;
|
||||||
|
Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
|
||||||
|
Cdot1.y = wB - wA;
|
||||||
|
|
||||||
|
if (m_enableLimit && m_limitState != e_inactiveLimit)
|
||||||
|
{
|
||||||
|
// Solve prismatic and limit constraint in block form.
|
||||||
|
float32 Cdot2;
|
||||||
|
Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
|
||||||
|
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
|
||||||
|
|
||||||
|
b2Vec3 f1 = m_impulse;
|
||||||
|
b2Vec3 df = m_K.Solve33(-Cdot);
|
||||||
|
m_impulse += df;
|
||||||
|
|
||||||
|
if (m_limitState == e_atLowerLimit)
|
||||||
|
{
|
||||||
|
m_impulse.z = b2Max(m_impulse.z, 0.0f);
|
||||||
|
}
|
||||||
|
else if (m_limitState == e_atUpperLimit)
|
||||||
|
{
|
||||||
|
m_impulse.z = b2Min(m_impulse.z, 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
|
||||||
|
b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
|
||||||
|
b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
|
||||||
|
m_impulse.x = f2r.x;
|
||||||
|
m_impulse.y = f2r.y;
|
||||||
|
|
||||||
|
df = m_impulse - f1;
|
||||||
|
|
||||||
|
b2Vec2 P = df.x * m_perp + df.z * m_axis;
|
||||||
|
float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
|
||||||
|
float32 LB = df.x * m_s2 + df.y + df.z * m_a2;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Limit is inactive, just solve the prismatic constraint in block form.
|
||||||
|
b2Vec2 df = m_K.Solve22(-Cdot1);
|
||||||
|
m_impulse.x += df.x;
|
||||||
|
m_impulse.y += df.y;
|
||||||
|
|
||||||
|
b2Vec2 P = df.x * m_perp;
|
||||||
|
float32 LA = df.x * m_s1 + df.y;
|
||||||
|
float32 LB = df.x * m_s2 + df.y;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// A velocity based solver computes reaction forces(impulses) using the velocity constraint solver.Under this context,
|
||||||
|
// the position solver is not there to resolve forces.It is only there to cope with integration error.
|
||||||
|
//
|
||||||
|
// Therefore, the pseudo impulses in the position solver do not have any physical meaning.Thus it is okay if they suck.
|
||||||
|
//
|
||||||
|
// We could take the active state from the velocity solver.However, the joint might push past the limit when the velocity
|
||||||
|
// solver indicates the limit is inactive.
|
||||||
|
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
// Compute fresh Jacobians
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 d = cB + rB - cA - rA;
|
||||||
|
|
||||||
|
b2Vec2 axis = b2Mul(qA, m_localXAxisA);
|
||||||
|
float32 a1 = b2Cross(d + rA, axis);
|
||||||
|
float32 a2 = b2Cross(rB, axis);
|
||||||
|
b2Vec2 perp = b2Mul(qA, m_localYAxisA);
|
||||||
|
|
||||||
|
float32 s1 = b2Cross(d + rA, perp);
|
||||||
|
float32 s2 = b2Cross(rB, perp);
|
||||||
|
|
||||||
|
b2Vec3 impulse;
|
||||||
|
b2Vec2 C1;
|
||||||
|
C1.x = b2Dot(perp, d);
|
||||||
|
C1.y = aB - aA - m_referenceAngle;
|
||||||
|
|
||||||
|
float32 linearError = b2Abs(C1.x);
|
||||||
|
float32 angularError = b2Abs(C1.y);
|
||||||
|
|
||||||
|
bool active = false;
|
||||||
|
float32 C2 = 0.0f;
|
||||||
|
if (m_enableLimit)
|
||||||
|
{
|
||||||
|
float32 translation = b2Dot(axis, d);
|
||||||
|
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
// Prevent large angular corrections
|
||||||
|
C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
|
||||||
|
linearError = b2Max(linearError, b2Abs(translation));
|
||||||
|
active = true;
|
||||||
|
}
|
||||||
|
else if (translation <= m_lowerTranslation)
|
||||||
|
{
|
||||||
|
// Prevent large linear corrections and allow some slop.
|
||||||
|
C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
|
||||||
|
linearError = b2Max(linearError, m_lowerTranslation - translation);
|
||||||
|
active = true;
|
||||||
|
}
|
||||||
|
else if (translation >= m_upperTranslation)
|
||||||
|
{
|
||||||
|
// Prevent large linear corrections and allow some slop.
|
||||||
|
C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
|
||||||
|
linearError = b2Max(linearError, translation - m_upperTranslation);
|
||||||
|
active = true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (active)
|
||||||
|
{
|
||||||
|
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
|
||||||
|
float32 k12 = iA * s1 + iB * s2;
|
||||||
|
float32 k13 = iA * s1 * a1 + iB * s2 * a2;
|
||||||
|
float32 k22 = iA + iB;
|
||||||
|
if (k22 == 0.0f)
|
||||||
|
{
|
||||||
|
// For fixed rotation
|
||||||
|
k22 = 1.0f;
|
||||||
|
}
|
||||||
|
float32 k23 = iA * a1 + iB * a2;
|
||||||
|
float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;
|
||||||
|
|
||||||
|
b2Mat33 K;
|
||||||
|
K.ex.Set(k11, k12, k13);
|
||||||
|
K.ey.Set(k12, k22, k23);
|
||||||
|
K.ez.Set(k13, k23, k33);
|
||||||
|
|
||||||
|
b2Vec3 C;
|
||||||
|
C.x = C1.x;
|
||||||
|
C.y = C1.y;
|
||||||
|
C.z = C2;
|
||||||
|
|
||||||
|
impulse = K.Solve33(-C);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
|
||||||
|
float32 k12 = iA * s1 + iB * s2;
|
||||||
|
float32 k22 = iA + iB;
|
||||||
|
if (k22 == 0.0f)
|
||||||
|
{
|
||||||
|
k22 = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Mat22 K;
|
||||||
|
K.ex.Set(k11, k12);
|
||||||
|
K.ey.Set(k12, k22);
|
||||||
|
|
||||||
|
b2Vec2 impulse1 = K.Solve(-C1);
|
||||||
|
impulse.x = impulse1.x;
|
||||||
|
impulse.y = impulse1.y;
|
||||||
|
impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 P = impulse.x * perp + impulse.z * axis;
|
||||||
|
float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
|
||||||
|
float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;
|
||||||
|
|
||||||
|
cA -= mA * P;
|
||||||
|
aA -= iA * LA;
|
||||||
|
cB += mB * P;
|
||||||
|
aB += iB * LB;
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PrismaticJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PrismaticJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_impulse.y;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetJointTranslation() const
|
||||||
|
{
|
||||||
|
b2Vec2 pA = m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
b2Vec2 pB = m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
b2Vec2 d = pB - pA;
|
||||||
|
b2Vec2 axis = m_bodyA->GetWorldVector(m_localXAxisA);
|
||||||
|
|
||||||
|
float32 translation = b2Dot(d, axis);
|
||||||
|
return translation;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetJointSpeed() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
|
||||||
|
b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
|
||||||
|
b2Vec2 p1 = bA->m_sweep.c + rA;
|
||||||
|
b2Vec2 p2 = bB->m_sweep.c + rB;
|
||||||
|
b2Vec2 d = p2 - p1;
|
||||||
|
b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
|
||||||
|
|
||||||
|
b2Vec2 vA = bA->m_linearVelocity;
|
||||||
|
b2Vec2 vB = bB->m_linearVelocity;
|
||||||
|
float32 wA = bA->m_angularVelocity;
|
||||||
|
float32 wB = bB->m_angularVelocity;
|
||||||
|
|
||||||
|
float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
|
||||||
|
return speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PrismaticJoint::IsLimitEnabled() const
|
||||||
|
{
|
||||||
|
return m_enableLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::EnableLimit(bool flag)
|
||||||
|
{
|
||||||
|
if (flag != m_enableLimit)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_enableLimit = flag;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetLowerLimit() const
|
||||||
|
{
|
||||||
|
return m_lowerTranslation;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetUpperLimit() const
|
||||||
|
{
|
||||||
|
return m_upperTranslation;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
|
||||||
|
{
|
||||||
|
b2Assert(lower <= upper);
|
||||||
|
if (lower != m_lowerTranslation || upper != m_upperTranslation)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_lowerTranslation = lower;
|
||||||
|
m_upperTranslation = upper;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PrismaticJoint::IsMotorEnabled() const
|
||||||
|
{
|
||||||
|
return m_enableMotor;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::EnableMotor(bool flag)
|
||||||
|
{
|
||||||
|
if (flag != m_enableMotor)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_enableMotor = flag;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::SetMotorSpeed(float32 speed)
|
||||||
|
{
|
||||||
|
if (speed != m_motorSpeed)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_motorSpeed = speed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::SetMaxMotorForce(float32 force)
|
||||||
|
{
|
||||||
|
if (force != m_maxMotorForce)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_maxMotorForce = force;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_motorImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PrismaticJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2PrismaticJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
|
||||||
|
b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
|
||||||
|
b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);
|
||||||
|
b2Log(" jd.lowerTranslation = %.15lef;\n", m_lowerTranslation);
|
||||||
|
b2Log(" jd.upperTranslation = %.15lef;\n", m_upperTranslation);
|
||||||
|
b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);
|
||||||
|
b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);
|
||||||
|
b2Log(" jd.maxMotorForce = %.15lef;\n", m_maxMotorForce);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,196 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_PRISMATIC_JOINT_H
|
||||||
|
#define B2_PRISMATIC_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Prismatic joint definition. This requires defining a line of
|
||||||
|
/// motion using an axis and an anchor point. The definition uses local
|
||||||
|
/// anchor points and a local axis so that the initial configuration
|
||||||
|
/// can violate the constraint slightly. The joint translation is zero
|
||||||
|
/// when the local anchor points coincide in world space. Using local
|
||||||
|
/// anchors and a local axis helps when saving and loading a game.
|
||||||
|
struct b2PrismaticJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2PrismaticJointDef()
|
||||||
|
{
|
||||||
|
type = e_prismaticJoint;
|
||||||
|
localAnchorA.SetZero();
|
||||||
|
localAnchorB.SetZero();
|
||||||
|
localAxisA.Set(1.0f, 0.0f);
|
||||||
|
referenceAngle = 0.0f;
|
||||||
|
enableLimit = false;
|
||||||
|
lowerTranslation = 0.0f;
|
||||||
|
upperTranslation = 0.0f;
|
||||||
|
enableMotor = false;
|
||||||
|
maxMotorForce = 0.0f;
|
||||||
|
motorSpeed = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, axis, and reference angle using the world
|
||||||
|
/// anchor and unit world axis.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The local translation unit axis in bodyA.
|
||||||
|
b2Vec2 localAxisA;
|
||||||
|
|
||||||
|
/// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
|
||||||
|
float32 referenceAngle;
|
||||||
|
|
||||||
|
/// Enable/disable the joint limit.
|
||||||
|
bool enableLimit;
|
||||||
|
|
||||||
|
/// The lower translation limit, usually in meters.
|
||||||
|
float32 lowerTranslation;
|
||||||
|
|
||||||
|
/// The upper translation limit, usually in meters.
|
||||||
|
float32 upperTranslation;
|
||||||
|
|
||||||
|
/// Enable/disable the joint motor.
|
||||||
|
bool enableMotor;
|
||||||
|
|
||||||
|
/// The maximum motor torque, usually in N-m.
|
||||||
|
float32 maxMotorForce;
|
||||||
|
|
||||||
|
/// The desired motor speed in radians per second.
|
||||||
|
float32 motorSpeed;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A prismatic joint. This joint provides one degree of freedom: translation
|
||||||
|
/// along an axis fixed in bodyA. Relative rotation is prevented. You can
|
||||||
|
/// use a joint limit to restrict the range of motion and a joint motor to
|
||||||
|
/// drive the motion or to model joint friction.
|
||||||
|
class b2PrismaticJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// The local joint axis relative to bodyA.
|
||||||
|
const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
|
||||||
|
|
||||||
|
/// Get the reference angle.
|
||||||
|
float32 GetReferenceAngle() const { return m_referenceAngle; }
|
||||||
|
|
||||||
|
/// Get the current joint translation, usually in meters.
|
||||||
|
float32 GetJointTranslation() const;
|
||||||
|
|
||||||
|
/// Get the current joint translation speed, usually in meters per second.
|
||||||
|
float32 GetJointSpeed() const;
|
||||||
|
|
||||||
|
/// Is the joint limit enabled?
|
||||||
|
bool IsLimitEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/disable the joint limit.
|
||||||
|
void EnableLimit(bool flag);
|
||||||
|
|
||||||
|
/// Get the lower joint limit, usually in meters.
|
||||||
|
float32 GetLowerLimit() const;
|
||||||
|
|
||||||
|
/// Get the upper joint limit, usually in meters.
|
||||||
|
float32 GetUpperLimit() const;
|
||||||
|
|
||||||
|
/// Set the joint limits, usually in meters.
|
||||||
|
void SetLimits(float32 lower, float32 upper);
|
||||||
|
|
||||||
|
/// Is the joint motor enabled?
|
||||||
|
bool IsMotorEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/disable the joint motor.
|
||||||
|
void EnableMotor(bool flag);
|
||||||
|
|
||||||
|
/// Set the motor speed, usually in meters per second.
|
||||||
|
void SetMotorSpeed(float32 speed);
|
||||||
|
|
||||||
|
/// Get the motor speed, usually in meters per second.
|
||||||
|
float32 GetMotorSpeed() const;
|
||||||
|
|
||||||
|
/// Set the maximum motor force, usually in N.
|
||||||
|
void SetMaxMotorForce(float32 force);
|
||||||
|
float32 GetMaxMotorForce() const { return m_maxMotorForce; }
|
||||||
|
|
||||||
|
/// Get the current motor force given the inverse time step, usually in N.
|
||||||
|
float32 GetMotorForce(float32 inv_dt) const;
|
||||||
|
|
||||||
|
/// Dump to b2Log
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
friend class b2Joint;
|
||||||
|
friend class b2GearJoint;
|
||||||
|
b2PrismaticJoint(const b2PrismaticJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
b2Vec2 m_localXAxisA;
|
||||||
|
b2Vec2 m_localYAxisA;
|
||||||
|
float32 m_referenceAngle;
|
||||||
|
b2Vec3 m_impulse;
|
||||||
|
float32 m_motorImpulse;
|
||||||
|
float32 m_lowerTranslation;
|
||||||
|
float32 m_upperTranslation;
|
||||||
|
float32 m_maxMotorForce;
|
||||||
|
float32 m_motorSpeed;
|
||||||
|
bool m_enableLimit;
|
||||||
|
bool m_enableMotor;
|
||||||
|
b2LimitState m_limitState;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Vec2 m_axis, m_perp;
|
||||||
|
float32 m_s1, m_s2;
|
||||||
|
float32 m_a1, m_a2;
|
||||||
|
b2Mat33 m_K;
|
||||||
|
float32 m_motorMass;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline float32 b2PrismaticJoint::GetMotorSpeed() const
|
||||||
|
{
|
||||||
|
return m_motorSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,348 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2PulleyJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Pulley:
|
||||||
|
// length1 = norm(p1 - s1)
|
||||||
|
// length2 = norm(p2 - s2)
|
||||||
|
// C0 = (length1 + ratio * length2)_initial
|
||||||
|
// C = C0 - (length1 + ratio * length2)
|
||||||
|
// u1 = (p1 - s1) / norm(p1 - s1)
|
||||||
|
// u2 = (p2 - s2) / norm(p2 - s2)
|
||||||
|
// Cdot = -dot(u1, v1 + cross(w1, r1)) - ratio * dot(u2, v2 + cross(w2, r2))
|
||||||
|
// J = -[u1 cross(r1, u1) ratio * u2 ratio * cross(r2, u2)]
|
||||||
|
// K = J * invM * JT
|
||||||
|
// = invMass1 + invI1 * cross(r1, u1)^2 + ratio^2 * (invMass2 + invI2 * cross(r2, u2)^2)
|
||||||
|
|
||||||
|
void b2PulleyJointDef::Initialize(b2Body* bA, b2Body* bB,
|
||||||
|
const b2Vec2& groundA, const b2Vec2& groundB,
|
||||||
|
const b2Vec2& anchorA, const b2Vec2& anchorB,
|
||||||
|
float32 r)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
groundAnchorA = groundA;
|
||||||
|
groundAnchorB = groundB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchorA);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchorB);
|
||||||
|
b2Vec2 dA = anchorA - groundA;
|
||||||
|
lengthA = dA.Length();
|
||||||
|
b2Vec2 dB = anchorB - groundB;
|
||||||
|
lengthB = dB.Length();
|
||||||
|
ratio = r;
|
||||||
|
b2Assert(ratio > b2_epsilon);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2PulleyJoint::b2PulleyJoint(const b2PulleyJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_groundAnchorA = def->groundAnchorA;
|
||||||
|
m_groundAnchorB = def->groundAnchorB;
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
|
||||||
|
m_lengthA = def->lengthA;
|
||||||
|
m_lengthB = def->lengthB;
|
||||||
|
|
||||||
|
b2Assert(def->ratio != 0.0f);
|
||||||
|
m_ratio = def->ratio;
|
||||||
|
|
||||||
|
m_constant = def->lengthA + m_ratio * def->lengthB;
|
||||||
|
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// Get the pulley axes.
|
||||||
|
m_uA = cA + m_rA - m_groundAnchorA;
|
||||||
|
m_uB = cB + m_rB - m_groundAnchorB;
|
||||||
|
|
||||||
|
float32 lengthA = m_uA.Length();
|
||||||
|
float32 lengthB = m_uB.Length();
|
||||||
|
|
||||||
|
if (lengthA > 10.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
m_uA *= 1.0f / lengthA;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_uA.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (lengthB > 10.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
m_uB *= 1.0f / lengthB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_uB.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute effective mass.
|
||||||
|
float32 ruA = b2Cross(m_rA, m_uA);
|
||||||
|
float32 ruB = b2Cross(m_rB, m_uB);
|
||||||
|
|
||||||
|
float32 mA = m_invMassA + m_invIA * ruA * ruA;
|
||||||
|
float32 mB = m_invMassB + m_invIB * ruB * ruB;
|
||||||
|
|
||||||
|
m_mass = mA + m_ratio * m_ratio * mB;
|
||||||
|
|
||||||
|
if (m_mass > 0.0f)
|
||||||
|
{
|
||||||
|
m_mass = 1.0f / m_mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale impulses to support variable time steps.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
// Warm starting.
|
||||||
|
b2Vec2 PA = -(m_impulse) * m_uA;
|
||||||
|
b2Vec2 PB = (-m_ratio * m_impulse) * m_uB;
|
||||||
|
|
||||||
|
vA += m_invMassA * PA;
|
||||||
|
wA += m_invIA * b2Cross(m_rA, PA);
|
||||||
|
vB += m_invMassB * PB;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, PB);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PulleyJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Vec2 vpA = vA + b2Cross(wA, m_rA);
|
||||||
|
b2Vec2 vpB = vB + b2Cross(wB, m_rB);
|
||||||
|
|
||||||
|
float32 Cdot = -b2Dot(m_uA, vpA) - m_ratio * b2Dot(m_uB, vpB);
|
||||||
|
float32 impulse = -m_mass * Cdot;
|
||||||
|
m_impulse += impulse;
|
||||||
|
|
||||||
|
b2Vec2 PA = -impulse * m_uA;
|
||||||
|
b2Vec2 PB = -m_ratio * impulse * m_uB;
|
||||||
|
vA += m_invMassA * PA;
|
||||||
|
wA += m_invIA * b2Cross(m_rA, PA);
|
||||||
|
vB += m_invMassB * PB;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, PB);
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// Get the pulley axes.
|
||||||
|
b2Vec2 uA = cA + rA - m_groundAnchorA;
|
||||||
|
b2Vec2 uB = cB + rB - m_groundAnchorB;
|
||||||
|
|
||||||
|
float32 lengthA = uA.Length();
|
||||||
|
float32 lengthB = uB.Length();
|
||||||
|
|
||||||
|
if (lengthA > 10.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
uA *= 1.0f / lengthA;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
uA.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
if (lengthB > 10.0f * b2_linearSlop)
|
||||||
|
{
|
||||||
|
uB *= 1.0f / lengthB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
uB.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute effective mass.
|
||||||
|
float32 ruA = b2Cross(rA, uA);
|
||||||
|
float32 ruB = b2Cross(rB, uB);
|
||||||
|
|
||||||
|
float32 mA = m_invMassA + m_invIA * ruA * ruA;
|
||||||
|
float32 mB = m_invMassB + m_invIB * ruB * ruB;
|
||||||
|
|
||||||
|
float32 mass = mA + m_ratio * m_ratio * mB;
|
||||||
|
|
||||||
|
if (mass > 0.0f)
|
||||||
|
{
|
||||||
|
mass = 1.0f / mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 C = m_constant - lengthA - m_ratio * lengthB;
|
||||||
|
float32 linearError = b2Abs(C);
|
||||||
|
|
||||||
|
float32 impulse = -mass * C;
|
||||||
|
|
||||||
|
b2Vec2 PA = -impulse * uA;
|
||||||
|
b2Vec2 PB = -m_ratio * impulse * uB;
|
||||||
|
|
||||||
|
cA += m_invMassA * PA;
|
||||||
|
aA += m_invIA * b2Cross(rA, PA);
|
||||||
|
cB += m_invMassB * PB;
|
||||||
|
aB += m_invIB * b2Cross(rB, PB);
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return linearError < b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PulleyJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PulleyJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 P = m_impulse * m_uB;
|
||||||
|
return inv_dt * P;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(inv_dt);
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
|
||||||
|
{
|
||||||
|
return m_groundAnchorA;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
|
||||||
|
{
|
||||||
|
return m_groundAnchorB;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetLengthA() const
|
||||||
|
{
|
||||||
|
return m_lengthA;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetLengthB() const
|
||||||
|
{
|
||||||
|
return m_lengthB;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetRatio() const
|
||||||
|
{
|
||||||
|
return m_ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetCurrentLengthA() const
|
||||||
|
{
|
||||||
|
b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
b2Vec2 s = m_groundAnchorA;
|
||||||
|
b2Vec2 d = p - s;
|
||||||
|
return d.Length();
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2PulleyJoint::GetCurrentLengthB() const
|
||||||
|
{
|
||||||
|
b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
b2Vec2 s = m_groundAnchorB;
|
||||||
|
b2Vec2 d = p - s;
|
||||||
|
return d.Length();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PulleyJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2PulleyJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.groundAnchorA.Set(%.15lef, %.15lef);\n", m_groundAnchorA.x, m_groundAnchorA.y);
|
||||||
|
b2Log(" jd.groundAnchorB.Set(%.15lef, %.15lef);\n", m_groundAnchorB.x, m_groundAnchorB.y);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.lengthA = %.15lef;\n", m_lengthA);
|
||||||
|
b2Log(" jd.lengthB = %.15lef;\n", m_lengthB);
|
||||||
|
b2Log(" jd.ratio = %.15lef;\n", m_ratio);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2PulleyJoint::ShiftOrigin(const b2Vec2& newOrigin)
|
||||||
|
{
|
||||||
|
m_groundAnchorA -= newOrigin;
|
||||||
|
m_groundAnchorB -= newOrigin;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,152 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_PULLEY_JOINT_H
|
||||||
|
#define B2_PULLEY_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
const float32 b2_minPulleyLength = 2.0f;
|
||||||
|
|
||||||
|
/// Pulley joint definition. This requires two ground anchors,
|
||||||
|
/// two dynamic body anchor points, and a pulley ratio.
|
||||||
|
struct b2PulleyJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2PulleyJointDef()
|
||||||
|
{
|
||||||
|
type = e_pulleyJoint;
|
||||||
|
groundAnchorA.Set(-1.0f, 1.0f);
|
||||||
|
groundAnchorB.Set(1.0f, 1.0f);
|
||||||
|
localAnchorA.Set(-1.0f, 0.0f);
|
||||||
|
localAnchorB.Set(1.0f, 0.0f);
|
||||||
|
lengthA = 0.0f;
|
||||||
|
lengthB = 0.0f;
|
||||||
|
ratio = 1.0f;
|
||||||
|
collideConnected = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, lengths, max lengths, and ratio using the world anchors.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB,
|
||||||
|
const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,
|
||||||
|
const b2Vec2& anchorA, const b2Vec2& anchorB,
|
||||||
|
float32 ratio);
|
||||||
|
|
||||||
|
/// The first ground anchor in world coordinates. This point never moves.
|
||||||
|
b2Vec2 groundAnchorA;
|
||||||
|
|
||||||
|
/// The second ground anchor in world coordinates. This point never moves.
|
||||||
|
b2Vec2 groundAnchorB;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The a reference length for the segment attached to bodyA.
|
||||||
|
float32 lengthA;
|
||||||
|
|
||||||
|
/// The a reference length for the segment attached to bodyB.
|
||||||
|
float32 lengthB;
|
||||||
|
|
||||||
|
/// The pulley ratio, used to simulate a block-and-tackle.
|
||||||
|
float32 ratio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// The pulley joint is connected to two bodies and two fixed ground points.
|
||||||
|
/// The pulley supports a ratio such that:
|
||||||
|
/// length1 + ratio * length2 <= constant
|
||||||
|
/// Yes, the force transmitted is scaled by the ratio.
|
||||||
|
/// Warning: the pulley joint can get a bit squirrelly by itself. They often
|
||||||
|
/// work better when combined with prismatic joints. You should also cover the
|
||||||
|
/// the anchor points with static shapes to prevent one side from going to
|
||||||
|
/// zero length.
|
||||||
|
class b2PulleyJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Get the first ground anchor.
|
||||||
|
b2Vec2 GetGroundAnchorA() const;
|
||||||
|
|
||||||
|
/// Get the second ground anchor.
|
||||||
|
b2Vec2 GetGroundAnchorB() const;
|
||||||
|
|
||||||
|
/// Get the current length of the segment attached to bodyA.
|
||||||
|
float32 GetLengthA() const;
|
||||||
|
|
||||||
|
/// Get the current length of the segment attached to bodyB.
|
||||||
|
float32 GetLengthB() const;
|
||||||
|
|
||||||
|
/// Get the pulley ratio.
|
||||||
|
float32 GetRatio() const;
|
||||||
|
|
||||||
|
/// Get the current length of the segment attached to bodyA.
|
||||||
|
float32 GetCurrentLengthA() const;
|
||||||
|
|
||||||
|
/// Get the current length of the segment attached to bodyB.
|
||||||
|
float32 GetCurrentLengthB() const;
|
||||||
|
|
||||||
|
/// Dump joint to dmLog
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
/// Implement b2Joint::ShiftOrigin
|
||||||
|
void ShiftOrigin(const b2Vec2& newOrigin) override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
b2PulleyJoint(const b2PulleyJointDef* data);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
b2Vec2 m_groundAnchorA;
|
||||||
|
b2Vec2 m_groundAnchorB;
|
||||||
|
float32 m_lengthA;
|
||||||
|
float32 m_lengthB;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
float32 m_constant;
|
||||||
|
float32 m_ratio;
|
||||||
|
float32 m_impulse;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_uA;
|
||||||
|
b2Vec2 m_uB;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
float32 m_mass;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,511 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2RevoluteJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Point-to-point constraint
|
||||||
|
// C = p2 - p1
|
||||||
|
// Cdot = v2 - v1
|
||||||
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
||||||
|
// J = [-I -r1_skew I r2_skew ]
|
||||||
|
// Identity used:
|
||||||
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||||
|
|
||||||
|
// Motor constraint
|
||||||
|
// Cdot = w2 - w1
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
// K = invI1 + invI2
|
||||||
|
|
||||||
|
void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||||
|
referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
m_referenceAngle = def->referenceAngle;
|
||||||
|
|
||||||
|
m_impulse.SetZero();
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
|
||||||
|
m_lowerAngle = def->lowerAngle;
|
||||||
|
m_upperAngle = def->upperAngle;
|
||||||
|
m_maxMotorTorque = def->maxMotorTorque;
|
||||||
|
m_motorSpeed = def->motorSpeed;
|
||||||
|
m_enableLimit = def->enableLimit;
|
||||||
|
m_enableMotor = def->enableMotor;
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// J = [-I -r1_skew I r2_skew]
|
||||||
|
// [ 0 -1 0 1]
|
||||||
|
// r_skew = [-ry; rx]
|
||||||
|
|
||||||
|
// Matlab
|
||||||
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
||||||
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
||||||
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
bool fixedRotation = (iA + iB == 0.0f);
|
||||||
|
|
||||||
|
m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
|
||||||
|
m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
|
||||||
|
m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
|
||||||
|
m_mass.ex.y = m_mass.ey.x;
|
||||||
|
m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
|
||||||
|
m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
|
||||||
|
m_mass.ex.z = m_mass.ez.x;
|
||||||
|
m_mass.ey.z = m_mass.ez.y;
|
||||||
|
m_mass.ez.z = iA + iB;
|
||||||
|
|
||||||
|
m_motorMass = iA + iB;
|
||||||
|
if (m_motorMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_motorMass = 1.0f / m_motorMass;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_enableMotor == false || fixedRotation)
|
||||||
|
{
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_enableLimit && fixedRotation == false)
|
||||||
|
{
|
||||||
|
float32 jointAngle = aB - aA - m_referenceAngle;
|
||||||
|
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
|
||||||
|
{
|
||||||
|
m_limitState = e_equalLimits;
|
||||||
|
}
|
||||||
|
else if (jointAngle <= m_lowerAngle)
|
||||||
|
{
|
||||||
|
if (m_limitState != e_atLowerLimit)
|
||||||
|
{
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
m_limitState = e_atLowerLimit;
|
||||||
|
}
|
||||||
|
else if (jointAngle >= m_upperAngle)
|
||||||
|
{
|
||||||
|
if (m_limitState != e_atUpperLimit)
|
||||||
|
{
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
m_limitState = e_atUpperLimit;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_limitState = e_inactiveLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale impulses to support a variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
m_motorImpulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse.SetZero();
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
bool fixedRotation = (iA + iB == 0.0f);
|
||||||
|
|
||||||
|
// Solve motor constraint.
|
||||||
|
if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
|
||||||
|
{
|
||||||
|
float32 Cdot = wB - wA - m_motorSpeed;
|
||||||
|
float32 impulse = -m_motorMass * Cdot;
|
||||||
|
float32 oldImpulse = m_motorImpulse;
|
||||||
|
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
|
||||||
|
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||||
|
impulse = m_motorImpulse - oldImpulse;
|
||||||
|
|
||||||
|
wA -= iA * impulse;
|
||||||
|
wB += iB * impulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve limit constraint.
|
||||||
|
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
|
||||||
|
{
|
||||||
|
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||||
|
float32 Cdot2 = wB - wA;
|
||||||
|
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
|
||||||
|
|
||||||
|
b2Vec3 impulse = -m_mass.Solve33(Cdot);
|
||||||
|
|
||||||
|
if (m_limitState == e_equalLimits)
|
||||||
|
{
|
||||||
|
m_impulse += impulse;
|
||||||
|
}
|
||||||
|
else if (m_limitState == e_atLowerLimit)
|
||||||
|
{
|
||||||
|
float32 newImpulse = m_impulse.z + impulse.z;
|
||||||
|
if (newImpulse < 0.0f)
|
||||||
|
{
|
||||||
|
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
|
||||||
|
b2Vec2 reduced = m_mass.Solve22(rhs);
|
||||||
|
impulse.x = reduced.x;
|
||||||
|
impulse.y = reduced.y;
|
||||||
|
impulse.z = -m_impulse.z;
|
||||||
|
m_impulse.x += reduced.x;
|
||||||
|
m_impulse.y += reduced.y;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse += impulse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else if (m_limitState == e_atUpperLimit)
|
||||||
|
{
|
||||||
|
float32 newImpulse = m_impulse.z + impulse.z;
|
||||||
|
if (newImpulse > 0.0f)
|
||||||
|
{
|
||||||
|
b2Vec2 rhs = -Cdot1 + m_impulse.z * b2Vec2(m_mass.ez.x, m_mass.ez.y);
|
||||||
|
b2Vec2 reduced = m_mass.Solve22(rhs);
|
||||||
|
impulse.x = reduced.x;
|
||||||
|
impulse.y = reduced.y;
|
||||||
|
impulse.z = -m_impulse.z;
|
||||||
|
m_impulse.x += reduced.x;
|
||||||
|
m_impulse.y += reduced.y;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse += impulse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 P(impulse.x, impulse.y);
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + impulse.z);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Solve point-to-point constraint
|
||||||
|
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||||
|
b2Vec2 impulse = m_mass.Solve22(-Cdot);
|
||||||
|
|
||||||
|
m_impulse.x += impulse.x;
|
||||||
|
m_impulse.y += impulse.y;
|
||||||
|
|
||||||
|
vA -= mA * impulse;
|
||||||
|
wA -= iA * b2Cross(m_rA, impulse);
|
||||||
|
|
||||||
|
vB += mB * impulse;
|
||||||
|
wB += iB * b2Cross(m_rB, impulse);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
float32 angularError = 0.0f;
|
||||||
|
float32 positionError = 0.0f;
|
||||||
|
|
||||||
|
bool fixedRotation = (m_invIA + m_invIB == 0.0f);
|
||||||
|
|
||||||
|
// Solve angular limit constraint.
|
||||||
|
if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
|
||||||
|
{
|
||||||
|
float32 angle = aB - aA - m_referenceAngle;
|
||||||
|
float32 limitImpulse = 0.0f;
|
||||||
|
|
||||||
|
if (m_limitState == e_equalLimits)
|
||||||
|
{
|
||||||
|
// Prevent large angular corrections
|
||||||
|
float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
|
||||||
|
limitImpulse = -m_motorMass * C;
|
||||||
|
angularError = b2Abs(C);
|
||||||
|
}
|
||||||
|
else if (m_limitState == e_atLowerLimit)
|
||||||
|
{
|
||||||
|
float32 C = angle - m_lowerAngle;
|
||||||
|
angularError = -C;
|
||||||
|
|
||||||
|
// Prevent large angular corrections and allow some slop.
|
||||||
|
C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
|
||||||
|
limitImpulse = -m_motorMass * C;
|
||||||
|
}
|
||||||
|
else if (m_limitState == e_atUpperLimit)
|
||||||
|
{
|
||||||
|
float32 C = angle - m_upperAngle;
|
||||||
|
angularError = C;
|
||||||
|
|
||||||
|
// Prevent large angular corrections and allow some slop.
|
||||||
|
C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
|
||||||
|
limitImpulse = -m_motorMass * C;
|
||||||
|
}
|
||||||
|
|
||||||
|
aA -= m_invIA * limitImpulse;
|
||||||
|
aB += m_invIB * limitImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve point-to-point constraint.
|
||||||
|
{
|
||||||
|
qA.Set(aA);
|
||||||
|
qB.Set(aB);
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
b2Vec2 C = cB + rB - cA - rA;
|
||||||
|
positionError = C.Length();
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Mat22 K;
|
||||||
|
K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
|
||||||
|
K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
|
||||||
|
K.ey.x = K.ex.y;
|
||||||
|
K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;
|
||||||
|
|
||||||
|
b2Vec2 impulse = -K.Solve(C);
|
||||||
|
|
||||||
|
cA -= mA * impulse;
|
||||||
|
aA -= iA * b2Cross(rA, impulse);
|
||||||
|
|
||||||
|
cB += mB * impulse;
|
||||||
|
aB += iB * b2Cross(rB, impulse);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RevoluteJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RevoluteJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||||
|
return inv_dt * P;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_impulse.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetJointAngle() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetJointSpeed() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
return bB->m_angularVelocity - bA->m_angularVelocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2RevoluteJoint::IsMotorEnabled() const
|
||||||
|
{
|
||||||
|
return m_enableMotor;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::EnableMotor(bool flag)
|
||||||
|
{
|
||||||
|
if (flag != m_enableMotor)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_enableMotor = flag;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_motorImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::SetMotorSpeed(float32 speed)
|
||||||
|
{
|
||||||
|
if (speed != m_motorSpeed)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_motorSpeed = speed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
|
||||||
|
{
|
||||||
|
if (torque != m_maxMotorTorque)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_maxMotorTorque = torque;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2RevoluteJoint::IsLimitEnabled() const
|
||||||
|
{
|
||||||
|
return m_enableLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::EnableLimit(bool flag)
|
||||||
|
{
|
||||||
|
if (flag != m_enableLimit)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_enableLimit = flag;
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetLowerLimit() const
|
||||||
|
{
|
||||||
|
return m_lowerAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RevoluteJoint::GetUpperLimit() const
|
||||||
|
{
|
||||||
|
return m_upperAngle;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
|
||||||
|
{
|
||||||
|
b2Assert(lower <= upper);
|
||||||
|
|
||||||
|
if (lower != m_lowerAngle || upper != m_upperAngle)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_impulse.z = 0.0f;
|
||||||
|
m_lowerAngle = lower;
|
||||||
|
m_upperAngle = upper;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RevoluteJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2RevoluteJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
|
||||||
|
b2Log(" jd.enableLimit = bool(%d);\n", m_enableLimit);
|
||||||
|
b2Log(" jd.lowerAngle = %.15lef;\n", m_lowerAngle);
|
||||||
|
b2Log(" jd.upperAngle = %.15lef;\n", m_upperAngle);
|
||||||
|
b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);
|
||||||
|
b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);
|
||||||
|
b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,204 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_REVOLUTE_JOINT_H
|
||||||
|
#define B2_REVOLUTE_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Revolute joint definition. This requires defining an
|
||||||
|
/// anchor point where the bodies are joined. The definition
|
||||||
|
/// uses local anchor points so that the initial configuration
|
||||||
|
/// can violate the constraint slightly. You also need to
|
||||||
|
/// specify the initial relative angle for joint limits. This
|
||||||
|
/// helps when saving and loading a game.
|
||||||
|
/// The local anchor points are measured from the body's origin
|
||||||
|
/// rather than the center of mass because:
|
||||||
|
/// 1. you might not know where the center of mass will be.
|
||||||
|
/// 2. if you add/remove shapes from a body and recompute the mass,
|
||||||
|
/// the joints will be broken.
|
||||||
|
struct b2RevoluteJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2RevoluteJointDef()
|
||||||
|
{
|
||||||
|
type = e_revoluteJoint;
|
||||||
|
localAnchorA.Set(0.0f, 0.0f);
|
||||||
|
localAnchorB.Set(0.0f, 0.0f);
|
||||||
|
referenceAngle = 0.0f;
|
||||||
|
lowerAngle = 0.0f;
|
||||||
|
upperAngle = 0.0f;
|
||||||
|
maxMotorTorque = 0.0f;
|
||||||
|
motorSpeed = 0.0f;
|
||||||
|
enableLimit = false;
|
||||||
|
enableMotor = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, and reference angle using a world
|
||||||
|
/// anchor point.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The bodyB angle minus bodyA angle in the reference state (radians).
|
||||||
|
float32 referenceAngle;
|
||||||
|
|
||||||
|
/// A flag to enable joint limits.
|
||||||
|
bool enableLimit;
|
||||||
|
|
||||||
|
/// The lower angle for the joint limit (radians).
|
||||||
|
float32 lowerAngle;
|
||||||
|
|
||||||
|
/// The upper angle for the joint limit (radians).
|
||||||
|
float32 upperAngle;
|
||||||
|
|
||||||
|
/// A flag to enable the joint motor.
|
||||||
|
bool enableMotor;
|
||||||
|
|
||||||
|
/// The desired motor speed. Usually in radians per second.
|
||||||
|
float32 motorSpeed;
|
||||||
|
|
||||||
|
/// The maximum motor torque used to achieve the desired motor speed.
|
||||||
|
/// Usually in N-m.
|
||||||
|
float32 maxMotorTorque;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A revolute joint constrains two bodies to share a common point while they
|
||||||
|
/// are free to rotate about the point. The relative rotation about the shared
|
||||||
|
/// point is the joint angle. You can limit the relative rotation with
|
||||||
|
/// a joint limit that specifies a lower and upper angle. You can use a motor
|
||||||
|
/// to drive the relative rotation about the shared point. A maximum motor torque
|
||||||
|
/// is provided so that infinite forces are not generated.
|
||||||
|
class b2RevoluteJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// Get the reference angle.
|
||||||
|
float32 GetReferenceAngle() const { return m_referenceAngle; }
|
||||||
|
|
||||||
|
/// Get the current joint angle in radians.
|
||||||
|
float32 GetJointAngle() const;
|
||||||
|
|
||||||
|
/// Get the current joint angle speed in radians per second.
|
||||||
|
float32 GetJointSpeed() const;
|
||||||
|
|
||||||
|
/// Is the joint limit enabled?
|
||||||
|
bool IsLimitEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/disable the joint limit.
|
||||||
|
void EnableLimit(bool flag);
|
||||||
|
|
||||||
|
/// Get the lower joint limit in radians.
|
||||||
|
float32 GetLowerLimit() const;
|
||||||
|
|
||||||
|
/// Get the upper joint limit in radians.
|
||||||
|
float32 GetUpperLimit() const;
|
||||||
|
|
||||||
|
/// Set the joint limits in radians.
|
||||||
|
void SetLimits(float32 lower, float32 upper);
|
||||||
|
|
||||||
|
/// Is the joint motor enabled?
|
||||||
|
bool IsMotorEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/disable the joint motor.
|
||||||
|
void EnableMotor(bool flag);
|
||||||
|
|
||||||
|
/// Set the motor speed in radians per second.
|
||||||
|
void SetMotorSpeed(float32 speed);
|
||||||
|
|
||||||
|
/// Get the motor speed in radians per second.
|
||||||
|
float32 GetMotorSpeed() const;
|
||||||
|
|
||||||
|
/// Set the maximum motor torque, usually in N-m.
|
||||||
|
void SetMaxMotorTorque(float32 torque);
|
||||||
|
float32 GetMaxMotorTorque() const { return m_maxMotorTorque; }
|
||||||
|
|
||||||
|
/// Get the reaction force given the inverse time step.
|
||||||
|
/// Unit is N.
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Get the reaction torque due to the joint limit given the inverse time step.
|
||||||
|
/// Unit is N*m.
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// Get the current motor torque given the inverse time step.
|
||||||
|
/// Unit is N*m.
|
||||||
|
float32 GetMotorTorque(float32 inv_dt) const;
|
||||||
|
|
||||||
|
/// Dump to b2Log.
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
friend class b2GearJoint;
|
||||||
|
|
||||||
|
b2RevoluteJoint(const b2RevoluteJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
b2Vec3 m_impulse;
|
||||||
|
float32 m_motorImpulse;
|
||||||
|
|
||||||
|
bool m_enableMotor;
|
||||||
|
float32 m_maxMotorTorque;
|
||||||
|
float32 m_motorSpeed;
|
||||||
|
|
||||||
|
bool m_enableLimit;
|
||||||
|
float32 m_referenceAngle;
|
||||||
|
float32 m_lowerAngle;
|
||||||
|
float32 m_upperAngle;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Mat33 m_mass; // effective mass for point-to-point constraint.
|
||||||
|
float32 m_motorMass; // effective mass for motor/limit angular constraint.
|
||||||
|
b2LimitState m_limitState;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline float32 b2RevoluteJoint::GetMotorSpeed() const
|
||||||
|
{
|
||||||
|
return m_motorSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,241 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2RopeJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
|
||||||
|
// Limit:
|
||||||
|
// C = norm(pB - pA) - L
|
||||||
|
// u = (pB - pA) / norm(pB - pA)
|
||||||
|
// Cdot = dot(u, vB + cross(wB, rB) - vA - cross(wA, rA))
|
||||||
|
// J = [-u -cross(rA, u) u cross(rB, u)]
|
||||||
|
// K = J * invM * JT
|
||||||
|
// = invMassA + invIA * cross(rA, u)^2 + invMassB + invIB * cross(rB, u)^2
|
||||||
|
|
||||||
|
b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
|
||||||
|
m_maxLength = def->maxLength;
|
||||||
|
|
||||||
|
m_mass = 0.0f;
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
m_state = e_inactiveLimit;
|
||||||
|
m_length = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
m_u = cB + m_rB - cA - m_rA;
|
||||||
|
|
||||||
|
m_length = m_u.Length();
|
||||||
|
|
||||||
|
float32 C = m_length - m_maxLength;
|
||||||
|
if (C > 0.0f)
|
||||||
|
{
|
||||||
|
m_state = e_atUpperLimit;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_state = e_inactiveLimit;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_length > b2_linearSlop)
|
||||||
|
{
|
||||||
|
m_u *= 1.0f / m_length;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_u.SetZero();
|
||||||
|
m_mass = 0.0f;
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute effective mass.
|
||||||
|
float32 crA = b2Cross(m_rA, m_u);
|
||||||
|
float32 crB = b2Cross(m_rB, m_u);
|
||||||
|
float32 invMass = m_invMassA + m_invIA * crA * crA + m_invMassB + m_invIB * crB * crB;
|
||||||
|
|
||||||
|
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale the impulse to support a variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P = m_impulse * m_u;
|
||||||
|
vA -= m_invMassA * P;
|
||||||
|
wA -= m_invIA * b2Cross(m_rA, P);
|
||||||
|
vB += m_invMassB * P;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, P);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RopeJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
// Cdot = dot(u, v + cross(w, r))
|
||||||
|
b2Vec2 vpA = vA + b2Cross(wA, m_rA);
|
||||||
|
b2Vec2 vpB = vB + b2Cross(wB, m_rB);
|
||||||
|
float32 C = m_length - m_maxLength;
|
||||||
|
float32 Cdot = b2Dot(m_u, vpB - vpA);
|
||||||
|
|
||||||
|
// Predictive constraint.
|
||||||
|
if (C < 0.0f)
|
||||||
|
{
|
||||||
|
Cdot += data.step.inv_dt * C;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 impulse = -m_mass * Cdot;
|
||||||
|
float32 oldImpulse = m_impulse;
|
||||||
|
m_impulse = b2Min(0.0f, m_impulse + impulse);
|
||||||
|
impulse = m_impulse - oldImpulse;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * m_u;
|
||||||
|
vA -= m_invMassA * P;
|
||||||
|
wA -= m_invIA * b2Cross(m_rA, P);
|
||||||
|
vB += m_invMassB * P;
|
||||||
|
wB += m_invIB * b2Cross(m_rB, P);
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 u = cB + rB - cA - rA;
|
||||||
|
|
||||||
|
float32 length = u.Normalize();
|
||||||
|
float32 C = length - m_maxLength;
|
||||||
|
|
||||||
|
C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);
|
||||||
|
|
||||||
|
float32 impulse = -m_mass * C;
|
||||||
|
b2Vec2 P = impulse * u;
|
||||||
|
|
||||||
|
cA -= m_invMassA * P;
|
||||||
|
aA -= m_invIA * b2Cross(rA, P);
|
||||||
|
cB += m_invMassB * P;
|
||||||
|
aB += m_invIB * b2Cross(rB, P);
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return length - m_maxLength < b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RopeJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RopeJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 F = (inv_dt * m_impulse) * m_u;
|
||||||
|
return F;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
B2_NOT_USED(inv_dt);
|
||||||
|
return 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2RopeJoint::GetMaxLength() const
|
||||||
|
{
|
||||||
|
return m_maxLength;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2LimitState b2RopeJoint::GetLimitState() const
|
||||||
|
{
|
||||||
|
return m_state;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2RopeJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2RopeJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.maxLength = %.15lef;\n", m_maxLength);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,114 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_ROPE_JOINT_H
|
||||||
|
#define B2_ROPE_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Rope joint definition. This requires two body anchor points and
|
||||||
|
/// a maximum lengths.
|
||||||
|
/// Note: by default the connected objects will not collide.
|
||||||
|
/// see collideConnected in b2JointDef.
|
||||||
|
struct b2RopeJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2RopeJointDef()
|
||||||
|
{
|
||||||
|
type = e_ropeJoint;
|
||||||
|
localAnchorA.Set(-1.0f, 0.0f);
|
||||||
|
localAnchorB.Set(1.0f, 0.0f);
|
||||||
|
maxLength = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The maximum length of the rope.
|
||||||
|
/// Warning: this must be larger than b2_linearSlop or
|
||||||
|
/// the joint will have no effect.
|
||||||
|
float32 maxLength;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A rope joint enforces a maximum distance between two points
|
||||||
|
/// on two bodies. It has no other effect.
|
||||||
|
/// Warning: if you attempt to change the maximum length during
|
||||||
|
/// the simulation you will get some non-physical behavior.
|
||||||
|
/// A model that would allow you to dynamically modify the length
|
||||||
|
/// would have some sponginess, so I chose not to implement it
|
||||||
|
/// that way. See b2DistanceJoint if you want to dynamically
|
||||||
|
/// control length.
|
||||||
|
class b2RopeJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// Set/Get the maximum length of the rope.
|
||||||
|
void SetMaxLength(float32 length) { m_maxLength = length; }
|
||||||
|
float32 GetMaxLength() const;
|
||||||
|
|
||||||
|
b2LimitState GetLimitState() const;
|
||||||
|
|
||||||
|
/// Dump joint to dmLog
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
b2RopeJoint(const b2RopeJointDef* data);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
float32 m_maxLength;
|
||||||
|
float32 m_length;
|
||||||
|
float32 m_impulse;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_u;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
float32 m_mass;
|
||||||
|
b2LimitState m_state;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,344 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2WeldJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Point-to-point constraint
|
||||||
|
// C = p2 - p1
|
||||||
|
// Cdot = v2 - v1
|
||||||
|
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
|
||||||
|
// J = [-I -r1_skew I r2_skew ]
|
||||||
|
// Identity used:
|
||||||
|
// w k % (rx i + ry j) = w * (-ry i + rx j)
|
||||||
|
|
||||||
|
// Angle constraint
|
||||||
|
// C = angle2 - angle1 - referenceAngle
|
||||||
|
// Cdot = w2 - w1
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
// K = invI1 + invI2
|
||||||
|
|
||||||
|
void b2WeldJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||||
|
referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
|
||||||
|
}
|
||||||
|
|
||||||
|
b2WeldJoint::b2WeldJoint(const b2WeldJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
m_referenceAngle = def->referenceAngle;
|
||||||
|
m_frequencyHz = def->frequencyHz;
|
||||||
|
m_dampingRatio = def->dampingRatio;
|
||||||
|
|
||||||
|
m_impulse.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
// J = [-I -r1_skew I r2_skew]
|
||||||
|
// [ 0 -1 0 1]
|
||||||
|
// r_skew = [-ry; rx]
|
||||||
|
|
||||||
|
// Matlab
|
||||||
|
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
|
||||||
|
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
|
||||||
|
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Mat33 K;
|
||||||
|
K.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
|
||||||
|
K.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
|
||||||
|
K.ez.x = -m_rA.y * iA - m_rB.y * iB;
|
||||||
|
K.ex.y = K.ey.x;
|
||||||
|
K.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
|
||||||
|
K.ez.y = m_rA.x * iA + m_rB.x * iB;
|
||||||
|
K.ex.z = K.ez.x;
|
||||||
|
K.ey.z = K.ez.y;
|
||||||
|
K.ez.z = iA + iB;
|
||||||
|
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
K.GetInverse22(&m_mass);
|
||||||
|
|
||||||
|
float32 invM = iA + iB;
|
||||||
|
float32 m = invM > 0.0f ? 1.0f / invM : 0.0f;
|
||||||
|
|
||||||
|
float32 C = aB - aA - m_referenceAngle;
|
||||||
|
|
||||||
|
// Frequency
|
||||||
|
float32 omega = 2.0f * b2_pi * m_frequencyHz;
|
||||||
|
|
||||||
|
// Damping coefficient
|
||||||
|
float32 d = 2.0f * m * m_dampingRatio * omega;
|
||||||
|
|
||||||
|
// Spring stiffness
|
||||||
|
float32 k = m * omega * omega;
|
||||||
|
|
||||||
|
// magic formulas
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
m_gamma = h * (d + h * k);
|
||||||
|
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
|
||||||
|
m_bias = C * h * k * m_gamma;
|
||||||
|
|
||||||
|
invM += m_gamma;
|
||||||
|
m_mass.ez.z = invM != 0.0f ? 1.0f / invM : 0.0f;
|
||||||
|
}
|
||||||
|
else if (K.ez.z == 0.0f)
|
||||||
|
{
|
||||||
|
K.GetInverse22(&m_mass);
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
m_bias = 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
K.GetSymInverse33(&m_mass);
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
m_bias = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Scale impulses to support a variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WeldJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
float32 Cdot2 = wB - wA;
|
||||||
|
|
||||||
|
float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
|
||||||
|
m_impulse.z += impulse2;
|
||||||
|
|
||||||
|
wA -= iA * impulse2;
|
||||||
|
wB += iB * impulse2;
|
||||||
|
|
||||||
|
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||||
|
|
||||||
|
b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
|
||||||
|
m_impulse.x += impulse1.x;
|
||||||
|
m_impulse.y += impulse1.y;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse1;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * b2Cross(m_rA, P);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * b2Cross(m_rB, P);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
|
||||||
|
float32 Cdot2 = wB - wA;
|
||||||
|
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
|
||||||
|
|
||||||
|
b2Vec3 impulse = -b2Mul(m_mass, Cdot);
|
||||||
|
m_impulse += impulse;
|
||||||
|
|
||||||
|
b2Vec2 P(impulse.x, impulse.y);
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * (b2Cross(m_rA, P) + impulse.z);
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * (b2Cross(m_rB, P) + impulse.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
|
||||||
|
float32 positionError, angularError;
|
||||||
|
|
||||||
|
b2Mat33 K;
|
||||||
|
K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
|
||||||
|
K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
|
||||||
|
K.ez.x = -rA.y * iA - rB.y * iB;
|
||||||
|
K.ex.y = K.ey.x;
|
||||||
|
K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
|
||||||
|
K.ez.y = rA.x * iA + rB.x * iB;
|
||||||
|
K.ex.z = K.ez.x;
|
||||||
|
K.ey.z = K.ez.y;
|
||||||
|
K.ez.z = iA + iB;
|
||||||
|
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
b2Vec2 C1 = cB + rB - cA - rA;
|
||||||
|
|
||||||
|
positionError = C1.Length();
|
||||||
|
angularError = 0.0f;
|
||||||
|
|
||||||
|
b2Vec2 P = -K.Solve22(C1);
|
||||||
|
|
||||||
|
cA -= mA * P;
|
||||||
|
aA -= iA * b2Cross(rA, P);
|
||||||
|
|
||||||
|
cB += mB * P;
|
||||||
|
aB += iB * b2Cross(rB, P);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 C1 = cB + rB - cA - rA;
|
||||||
|
float32 C2 = aB - aA - m_referenceAngle;
|
||||||
|
|
||||||
|
positionError = C1.Length();
|
||||||
|
angularError = b2Abs(C2);
|
||||||
|
|
||||||
|
b2Vec3 C(C1.x, C1.y, C2);
|
||||||
|
|
||||||
|
b2Vec3 impulse;
|
||||||
|
if (K.ez.z > 0.0f)
|
||||||
|
{
|
||||||
|
impulse = -K.Solve33(C);
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b2Vec2 impulse2 = -K.Solve22(C1);
|
||||||
|
impulse.Set(impulse2.x, impulse2.y, 0.0f);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 P(impulse.x, impulse.y);
|
||||||
|
|
||||||
|
cA -= mA * P;
|
||||||
|
aA -= iA * (b2Cross(rA, P) + impulse.z);
|
||||||
|
|
||||||
|
cB += mB * P;
|
||||||
|
aB += iB * (b2Cross(rB, P) + impulse.z);
|
||||||
|
}
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WeldJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WeldJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
b2Vec2 P(m_impulse.x, m_impulse.y);
|
||||||
|
return inv_dt * P;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_impulse.z;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WeldJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2WeldJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.referenceAngle = %.15lef;\n", m_referenceAngle);
|
||||||
|
b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
|
||||||
|
b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,126 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_WELD_JOINT_H
|
||||||
|
#define B2_WELD_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Weld joint definition. You need to specify local anchor points
|
||||||
|
/// where they are attached and the relative body angle. The position
|
||||||
|
/// of the anchor points is important for computing the reaction torque.
|
||||||
|
struct b2WeldJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2WeldJointDef()
|
||||||
|
{
|
||||||
|
type = e_weldJoint;
|
||||||
|
localAnchorA.Set(0.0f, 0.0f);
|
||||||
|
localAnchorB.Set(0.0f, 0.0f);
|
||||||
|
referenceAngle = 0.0f;
|
||||||
|
frequencyHz = 0.0f;
|
||||||
|
dampingRatio = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, and reference angle using a world
|
||||||
|
/// anchor point.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The bodyB angle minus bodyA angle in the reference state (radians).
|
||||||
|
float32 referenceAngle;
|
||||||
|
|
||||||
|
/// The mass-spring-damper frequency in Hertz. Rotation only.
|
||||||
|
/// Disable softness with a value of 0.
|
||||||
|
float32 frequencyHz;
|
||||||
|
|
||||||
|
/// The damping ratio. 0 = no damping, 1 = critical damping.
|
||||||
|
float32 dampingRatio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A weld joint essentially glues two bodies together. A weld joint may
|
||||||
|
/// distort somewhat because the island constraint solver is approximate.
|
||||||
|
class b2WeldJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// Get the reference angle.
|
||||||
|
float32 GetReferenceAngle() const { return m_referenceAngle; }
|
||||||
|
|
||||||
|
/// Set/get frequency in Hz.
|
||||||
|
void SetFrequency(float32 hz) { m_frequencyHz = hz; }
|
||||||
|
float32 GetFrequency() const { return m_frequencyHz; }
|
||||||
|
|
||||||
|
/// Set/get damping ratio.
|
||||||
|
void SetDampingRatio(float32 ratio) { m_dampingRatio = ratio; }
|
||||||
|
float32 GetDampingRatio() const { return m_dampingRatio; }
|
||||||
|
|
||||||
|
/// Dump to b2Log
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
|
||||||
|
b2WeldJoint(const b2WeldJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
float32 m_frequencyHz;
|
||||||
|
float32 m_dampingRatio;
|
||||||
|
float32 m_bias;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
float32 m_referenceAngle;
|
||||||
|
float32 m_gamma;
|
||||||
|
b2Vec3 m_impulse;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_rA;
|
||||||
|
b2Vec2 m_rB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
b2Mat33 m_mass;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,456 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "b2WheelJoint.h"
|
||||||
|
#include "../b2Body.h"
|
||||||
|
#include "../b2TimeStep.h"
|
||||||
|
|
||||||
|
// Linear constraint (point-to-line)
|
||||||
|
// d = pB - pA = xB + rB - xA - rA
|
||||||
|
// C = dot(ay, d)
|
||||||
|
// Cdot = dot(d, cross(wA, ay)) + dot(ay, vB + cross(wB, rB) - vA - cross(wA, rA))
|
||||||
|
// = -dot(ay, vA) - dot(cross(d + rA, ay), wA) + dot(ay, vB) + dot(cross(rB, ay), vB)
|
||||||
|
// J = [-ay, -cross(d + rA, ay), ay, cross(rB, ay)]
|
||||||
|
|
||||||
|
// Spring linear constraint
|
||||||
|
// C = dot(ax, d)
|
||||||
|
// Cdot = = -dot(ax, vA) - dot(cross(d + rA, ax), wA) + dot(ax, vB) + dot(cross(rB, ax), vB)
|
||||||
|
// J = [-ax -cross(d+rA, ax) ax cross(rB, ax)]
|
||||||
|
|
||||||
|
// Motor rotational constraint
|
||||||
|
// Cdot = wB - wA
|
||||||
|
// J = [0 0 -1 0 0 1]
|
||||||
|
|
||||||
|
void b2WheelJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
|
||||||
|
{
|
||||||
|
bodyA = bA;
|
||||||
|
bodyB = bB;
|
||||||
|
localAnchorA = bodyA->GetLocalPoint(anchor);
|
||||||
|
localAnchorB = bodyB->GetLocalPoint(anchor);
|
||||||
|
localAxisA = bodyA->GetLocalVector(axis);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2WheelJoint::b2WheelJoint(const b2WheelJointDef* def)
|
||||||
|
: b2Joint(def)
|
||||||
|
{
|
||||||
|
m_localAnchorA = def->localAnchorA;
|
||||||
|
m_localAnchorB = def->localAnchorB;
|
||||||
|
m_localXAxisA = def->localAxisA;
|
||||||
|
m_localYAxisA = b2Cross(1.0f, m_localXAxisA);
|
||||||
|
|
||||||
|
m_mass = 0.0f;
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
m_motorMass = 0.0f;
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
m_springMass = 0.0f;
|
||||||
|
m_springImpulse = 0.0f;
|
||||||
|
|
||||||
|
m_maxMotorTorque = def->maxMotorTorque;
|
||||||
|
m_motorSpeed = def->motorSpeed;
|
||||||
|
m_enableMotor = def->enableMotor;
|
||||||
|
|
||||||
|
m_frequencyHz = def->frequencyHz;
|
||||||
|
m_dampingRatio = def->dampingRatio;
|
||||||
|
|
||||||
|
m_bias = 0.0f;
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
|
||||||
|
m_ax.SetZero();
|
||||||
|
m_ay.SetZero();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::InitVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
m_indexA = m_bodyA->m_islandIndex;
|
||||||
|
m_indexB = m_bodyB->m_islandIndex;
|
||||||
|
m_localCenterA = m_bodyA->m_sweep.localCenter;
|
||||||
|
m_localCenterB = m_bodyB->m_sweep.localCenter;
|
||||||
|
m_invMassA = m_bodyA->m_invMass;
|
||||||
|
m_invMassB = m_bodyB->m_invMass;
|
||||||
|
m_invIA = m_bodyA->m_invI;
|
||||||
|
m_invIB = m_bodyB->m_invI;
|
||||||
|
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
// Compute the effective masses.
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 d = cB + rB - cA - rA;
|
||||||
|
|
||||||
|
// Point to line constraint
|
||||||
|
{
|
||||||
|
m_ay = b2Mul(qA, m_localYAxisA);
|
||||||
|
m_sAy = b2Cross(d + rA, m_ay);
|
||||||
|
m_sBy = b2Cross(rB, m_ay);
|
||||||
|
|
||||||
|
m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
|
||||||
|
|
||||||
|
if (m_mass > 0.0f)
|
||||||
|
{
|
||||||
|
m_mass = 1.0f / m_mass;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Spring constraint
|
||||||
|
m_springMass = 0.0f;
|
||||||
|
m_bias = 0.0f;
|
||||||
|
m_gamma = 0.0f;
|
||||||
|
if (m_frequencyHz > 0.0f)
|
||||||
|
{
|
||||||
|
m_ax = b2Mul(qA, m_localXAxisA);
|
||||||
|
m_sAx = b2Cross(d + rA, m_ax);
|
||||||
|
m_sBx = b2Cross(rB, m_ax);
|
||||||
|
|
||||||
|
float32 invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
|
||||||
|
|
||||||
|
if (invMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_springMass = 1.0f / invMass;
|
||||||
|
|
||||||
|
float32 C = b2Dot(d, m_ax);
|
||||||
|
|
||||||
|
// Frequency
|
||||||
|
float32 omega = 2.0f * b2_pi * m_frequencyHz;
|
||||||
|
|
||||||
|
// Damping coefficient
|
||||||
|
float32 damp = 2.0f * m_springMass * m_dampingRatio * omega;
|
||||||
|
|
||||||
|
// Spring stiffness
|
||||||
|
float32 k = m_springMass * omega * omega;
|
||||||
|
|
||||||
|
// magic formulas
|
||||||
|
float32 h = data.step.dt;
|
||||||
|
m_gamma = h * (damp + h * k);
|
||||||
|
if (m_gamma > 0.0f)
|
||||||
|
{
|
||||||
|
m_gamma = 1.0f / m_gamma;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_bias = C * h * k * m_gamma;
|
||||||
|
|
||||||
|
m_springMass = invMass + m_gamma;
|
||||||
|
if (m_springMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_springMass = 1.0f / m_springMass;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_springImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Rotational motor
|
||||||
|
if (m_enableMotor)
|
||||||
|
{
|
||||||
|
m_motorMass = iA + iB;
|
||||||
|
if (m_motorMass > 0.0f)
|
||||||
|
{
|
||||||
|
m_motorMass = 1.0f / m_motorMass;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_motorMass = 0.0f;
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (data.step.warmStarting)
|
||||||
|
{
|
||||||
|
// Account for variable time step.
|
||||||
|
m_impulse *= data.step.dtRatio;
|
||||||
|
m_springImpulse *= data.step.dtRatio;
|
||||||
|
m_motorImpulse *= data.step.dtRatio;
|
||||||
|
|
||||||
|
b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
|
||||||
|
float32 LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
|
||||||
|
float32 LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
|
||||||
|
|
||||||
|
vA -= m_invMassA * P;
|
||||||
|
wA -= m_invIA * LA;
|
||||||
|
|
||||||
|
vB += m_invMassB * P;
|
||||||
|
wB += m_invIB * LB;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_impulse = 0.0f;
|
||||||
|
m_springImpulse = 0.0f;
|
||||||
|
m_motorImpulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::SolveVelocityConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
float32 mA = m_invMassA, mB = m_invMassB;
|
||||||
|
float32 iA = m_invIA, iB = m_invIB;
|
||||||
|
|
||||||
|
b2Vec2 vA = data.velocities[m_indexA].v;
|
||||||
|
float32 wA = data.velocities[m_indexA].w;
|
||||||
|
b2Vec2 vB = data.velocities[m_indexB].v;
|
||||||
|
float32 wB = data.velocities[m_indexB].w;
|
||||||
|
|
||||||
|
// Solve spring constraint
|
||||||
|
{
|
||||||
|
float32 Cdot = b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
|
||||||
|
float32 impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
|
||||||
|
m_springImpulse += impulse;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * m_ax;
|
||||||
|
float32 LA = impulse * m_sAx;
|
||||||
|
float32 LB = impulse * m_sBx;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve rotational motor constraint
|
||||||
|
{
|
||||||
|
float32 Cdot = wB - wA - m_motorSpeed;
|
||||||
|
float32 impulse = -m_motorMass * Cdot;
|
||||||
|
|
||||||
|
float32 oldImpulse = m_motorImpulse;
|
||||||
|
float32 maxImpulse = data.step.dt * m_maxMotorTorque;
|
||||||
|
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
|
||||||
|
impulse = m_motorImpulse - oldImpulse;
|
||||||
|
|
||||||
|
wA -= iA * impulse;
|
||||||
|
wB += iB * impulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve point to line constraint
|
||||||
|
{
|
||||||
|
float32 Cdot = b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
|
||||||
|
float32 impulse = -m_mass * Cdot;
|
||||||
|
m_impulse += impulse;
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * m_ay;
|
||||||
|
float32 LA = impulse * m_sAy;
|
||||||
|
float32 LB = impulse * m_sBy;
|
||||||
|
|
||||||
|
vA -= mA * P;
|
||||||
|
wA -= iA * LA;
|
||||||
|
|
||||||
|
vB += mB * P;
|
||||||
|
wB += iB * LB;
|
||||||
|
}
|
||||||
|
|
||||||
|
data.velocities[m_indexA].v = vA;
|
||||||
|
data.velocities[m_indexA].w = wA;
|
||||||
|
data.velocities[m_indexB].v = vB;
|
||||||
|
data.velocities[m_indexB].w = wB;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
|
||||||
|
{
|
||||||
|
b2Vec2 cA = data.positions[m_indexA].c;
|
||||||
|
float32 aA = data.positions[m_indexA].a;
|
||||||
|
b2Vec2 cB = data.positions[m_indexB].c;
|
||||||
|
float32 aB = data.positions[m_indexB].a;
|
||||||
|
|
||||||
|
b2Rot qA(aA), qB(aB);
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
|
||||||
|
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
|
||||||
|
b2Vec2 d = (cB - cA) + rB - rA;
|
||||||
|
|
||||||
|
b2Vec2 ay = b2Mul(qA, m_localYAxisA);
|
||||||
|
|
||||||
|
float32 sAy = b2Cross(d + rA, ay);
|
||||||
|
float32 sBy = b2Cross(rB, ay);
|
||||||
|
|
||||||
|
float32 C = b2Dot(d, ay);
|
||||||
|
|
||||||
|
float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;
|
||||||
|
|
||||||
|
float32 impulse;
|
||||||
|
if (k != 0.0f)
|
||||||
|
{
|
||||||
|
impulse = - C / k;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
impulse = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 P = impulse * ay;
|
||||||
|
float32 LA = impulse * sAy;
|
||||||
|
float32 LB = impulse * sBy;
|
||||||
|
|
||||||
|
cA -= m_invMassA * P;
|
||||||
|
aA -= m_invIA * LA;
|
||||||
|
cB += m_invMassB * P;
|
||||||
|
aB += m_invIB * LB;
|
||||||
|
|
||||||
|
data.positions[m_indexA].c = cA;
|
||||||
|
data.positions[m_indexA].a = aA;
|
||||||
|
data.positions[m_indexB].c = cB;
|
||||||
|
data.positions[m_indexB].a = aB;
|
||||||
|
|
||||||
|
return b2Abs(C) <= b2_linearSlop;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WheelJoint::GetAnchorA() const
|
||||||
|
{
|
||||||
|
return m_bodyA->GetWorldPoint(m_localAnchorA);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WheelJoint::GetAnchorB() const
|
||||||
|
{
|
||||||
|
return m_bodyB->GetWorldPoint(m_localAnchorB);
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Vec2 b2WheelJoint::GetReactionForce(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * (m_impulse * m_ay + m_springImpulse * m_ax);
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetReactionTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_motorImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetJointTranslation() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
|
||||||
|
b2Vec2 pA = bA->GetWorldPoint(m_localAnchorA);
|
||||||
|
b2Vec2 pB = bB->GetWorldPoint(m_localAnchorB);
|
||||||
|
b2Vec2 d = pB - pA;
|
||||||
|
b2Vec2 axis = bA->GetWorldVector(m_localXAxisA);
|
||||||
|
|
||||||
|
float32 translation = b2Dot(d, axis);
|
||||||
|
return translation;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetJointLinearSpeed() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
|
||||||
|
b2Vec2 rA = b2Mul(bA->m_xf.q, m_localAnchorA - bA->m_sweep.localCenter);
|
||||||
|
b2Vec2 rB = b2Mul(bB->m_xf.q, m_localAnchorB - bB->m_sweep.localCenter);
|
||||||
|
b2Vec2 p1 = bA->m_sweep.c + rA;
|
||||||
|
b2Vec2 p2 = bB->m_sweep.c + rB;
|
||||||
|
b2Vec2 d = p2 - p1;
|
||||||
|
b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
|
||||||
|
|
||||||
|
b2Vec2 vA = bA->m_linearVelocity;
|
||||||
|
b2Vec2 vB = bB->m_linearVelocity;
|
||||||
|
float32 wA = bA->m_angularVelocity;
|
||||||
|
float32 wB = bB->m_angularVelocity;
|
||||||
|
|
||||||
|
float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
|
||||||
|
return speed;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetJointAngle() const
|
||||||
|
{
|
||||||
|
b2Body* bA = m_bodyA;
|
||||||
|
b2Body* bB = m_bodyB;
|
||||||
|
return bB->m_sweep.a - bA->m_sweep.a;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetJointAngularSpeed() const
|
||||||
|
{
|
||||||
|
float32 wA = m_bodyA->m_angularVelocity;
|
||||||
|
float32 wB = m_bodyB->m_angularVelocity;
|
||||||
|
return wB - wA;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2WheelJoint::IsMotorEnabled() const
|
||||||
|
{
|
||||||
|
return m_enableMotor;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::EnableMotor(bool flag)
|
||||||
|
{
|
||||||
|
if (flag != m_enableMotor)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_enableMotor = flag;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::SetMotorSpeed(float32 speed)
|
||||||
|
{
|
||||||
|
if (speed != m_motorSpeed)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_motorSpeed = speed;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::SetMaxMotorTorque(float32 torque)
|
||||||
|
{
|
||||||
|
if (torque != m_maxMotorTorque)
|
||||||
|
{
|
||||||
|
m_bodyA->SetAwake(true);
|
||||||
|
m_bodyB->SetAwake(true);
|
||||||
|
m_maxMotorTorque = torque;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 b2WheelJoint::GetMotorTorque(float32 inv_dt) const
|
||||||
|
{
|
||||||
|
return inv_dt * m_motorImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2WheelJoint::Dump()
|
||||||
|
{
|
||||||
|
int32 indexA = m_bodyA->m_islandIndex;
|
||||||
|
int32 indexB = m_bodyB->m_islandIndex;
|
||||||
|
|
||||||
|
b2Log(" b2WheelJointDef jd;\n");
|
||||||
|
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
|
||||||
|
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
|
||||||
|
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
|
||||||
|
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
|
||||||
|
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
|
||||||
|
b2Log(" jd.localAxisA.Set(%.15lef, %.15lef);\n", m_localXAxisA.x, m_localXAxisA.y);
|
||||||
|
b2Log(" jd.enableMotor = bool(%d);\n", m_enableMotor);
|
||||||
|
b2Log(" jd.motorSpeed = %.15lef;\n", m_motorSpeed);
|
||||||
|
b2Log(" jd.maxMotorTorque = %.15lef;\n", m_maxMotorTorque);
|
||||||
|
b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
|
||||||
|
b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
|
||||||
|
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,216 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_WHEEL_JOINT_H
|
||||||
|
#define B2_WHEEL_JOINT_H
|
||||||
|
|
||||||
|
#include "b2Joint.h"
|
||||||
|
|
||||||
|
/// Wheel joint definition. This requires defining a line of
|
||||||
|
/// motion using an axis and an anchor point. The definition uses local
|
||||||
|
/// anchor points and a local axis so that the initial configuration
|
||||||
|
/// can violate the constraint slightly. The joint translation is zero
|
||||||
|
/// when the local anchor points coincide in world space. Using local
|
||||||
|
/// anchors and a local axis helps when saving and loading a game.
|
||||||
|
struct b2WheelJointDef : public b2JointDef
|
||||||
|
{
|
||||||
|
b2WheelJointDef()
|
||||||
|
{
|
||||||
|
type = e_wheelJoint;
|
||||||
|
localAnchorA.SetZero();
|
||||||
|
localAnchorB.SetZero();
|
||||||
|
localAxisA.Set(1.0f, 0.0f);
|
||||||
|
enableMotor = false;
|
||||||
|
maxMotorTorque = 0.0f;
|
||||||
|
motorSpeed = 0.0f;
|
||||||
|
frequencyHz = 2.0f;
|
||||||
|
dampingRatio = 0.7f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Initialize the bodies, anchors, axis, and reference angle using the world
|
||||||
|
/// anchor and world axis.
|
||||||
|
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
b2Vec2 localAnchorA;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
b2Vec2 localAnchorB;
|
||||||
|
|
||||||
|
/// The local translation axis in bodyA.
|
||||||
|
b2Vec2 localAxisA;
|
||||||
|
|
||||||
|
/// Enable/disable the joint motor.
|
||||||
|
bool enableMotor;
|
||||||
|
|
||||||
|
/// The maximum motor torque, usually in N-m.
|
||||||
|
float32 maxMotorTorque;
|
||||||
|
|
||||||
|
/// The desired motor speed in radians per second.
|
||||||
|
float32 motorSpeed;
|
||||||
|
|
||||||
|
/// Suspension frequency, zero indicates no suspension
|
||||||
|
float32 frequencyHz;
|
||||||
|
|
||||||
|
/// Suspension damping ratio, one indicates critical damping
|
||||||
|
float32 dampingRatio;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A wheel joint. This joint provides two degrees of freedom: translation
|
||||||
|
/// along an axis fixed in bodyA and rotation in the plane. In other words, it is a point to
|
||||||
|
/// line constraint with a rotational motor and a linear spring/damper.
|
||||||
|
/// This joint is designed for vehicle suspensions.
|
||||||
|
class b2WheelJoint : public b2Joint
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Vec2 GetAnchorA() const override;
|
||||||
|
b2Vec2 GetAnchorB() const override;
|
||||||
|
|
||||||
|
b2Vec2 GetReactionForce(float32 inv_dt) const override;
|
||||||
|
float32 GetReactionTorque(float32 inv_dt) const override;
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyA's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
|
||||||
|
|
||||||
|
/// The local anchor point relative to bodyB's origin.
|
||||||
|
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
|
||||||
|
|
||||||
|
/// The local joint axis relative to bodyA.
|
||||||
|
const b2Vec2& GetLocalAxisA() const { return m_localXAxisA; }
|
||||||
|
|
||||||
|
/// Get the current joint translation, usually in meters.
|
||||||
|
float32 GetJointTranslation() const;
|
||||||
|
|
||||||
|
/// Get the current joint linear speed, usually in meters per second.
|
||||||
|
float32 GetJointLinearSpeed() const;
|
||||||
|
|
||||||
|
/// Get the current joint angle in radians.
|
||||||
|
float32 GetJointAngle() const;
|
||||||
|
|
||||||
|
/// Get the current joint angular speed in radians per second.
|
||||||
|
float32 GetJointAngularSpeed() const;
|
||||||
|
|
||||||
|
/// Is the joint motor enabled?
|
||||||
|
bool IsMotorEnabled() const;
|
||||||
|
|
||||||
|
/// Enable/disable the joint motor.
|
||||||
|
void EnableMotor(bool flag);
|
||||||
|
|
||||||
|
/// Set the motor speed, usually in radians per second.
|
||||||
|
void SetMotorSpeed(float32 speed);
|
||||||
|
|
||||||
|
/// Get the motor speed, usually in radians per second.
|
||||||
|
float32 GetMotorSpeed() const;
|
||||||
|
|
||||||
|
/// Set/Get the maximum motor force, usually in N-m.
|
||||||
|
void SetMaxMotorTorque(float32 torque);
|
||||||
|
float32 GetMaxMotorTorque() const;
|
||||||
|
|
||||||
|
/// Get the current motor torque given the inverse time step, usually in N-m.
|
||||||
|
float32 GetMotorTorque(float32 inv_dt) const;
|
||||||
|
|
||||||
|
/// Set/Get the spring frequency in hertz. Setting the frequency to zero disables the spring.
|
||||||
|
void SetSpringFrequencyHz(float32 hz);
|
||||||
|
float32 GetSpringFrequencyHz() const;
|
||||||
|
|
||||||
|
/// Set/Get the spring damping ratio
|
||||||
|
void SetSpringDampingRatio(float32 ratio);
|
||||||
|
float32 GetSpringDampingRatio() const;
|
||||||
|
|
||||||
|
/// Dump to b2Log
|
||||||
|
void Dump() override;
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Joint;
|
||||||
|
b2WheelJoint(const b2WheelJointDef* def);
|
||||||
|
|
||||||
|
void InitVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
void SolveVelocityConstraints(const b2SolverData& data) override;
|
||||||
|
bool SolvePositionConstraints(const b2SolverData& data) override;
|
||||||
|
|
||||||
|
float32 m_frequencyHz;
|
||||||
|
float32 m_dampingRatio;
|
||||||
|
|
||||||
|
// Solver shared
|
||||||
|
b2Vec2 m_localAnchorA;
|
||||||
|
b2Vec2 m_localAnchorB;
|
||||||
|
b2Vec2 m_localXAxisA;
|
||||||
|
b2Vec2 m_localYAxisA;
|
||||||
|
|
||||||
|
float32 m_impulse;
|
||||||
|
float32 m_motorImpulse;
|
||||||
|
float32 m_springImpulse;
|
||||||
|
|
||||||
|
float32 m_maxMotorTorque;
|
||||||
|
float32 m_motorSpeed;
|
||||||
|
bool m_enableMotor;
|
||||||
|
|
||||||
|
// Solver temp
|
||||||
|
int32 m_indexA;
|
||||||
|
int32 m_indexB;
|
||||||
|
b2Vec2 m_localCenterA;
|
||||||
|
b2Vec2 m_localCenterB;
|
||||||
|
float32 m_invMassA;
|
||||||
|
float32 m_invMassB;
|
||||||
|
float32 m_invIA;
|
||||||
|
float32 m_invIB;
|
||||||
|
|
||||||
|
b2Vec2 m_ax, m_ay;
|
||||||
|
float32 m_sAx, m_sBx;
|
||||||
|
float32 m_sAy, m_sBy;
|
||||||
|
|
||||||
|
float32 m_mass;
|
||||||
|
float32 m_motorMass;
|
||||||
|
float32 m_springMass;
|
||||||
|
|
||||||
|
float32 m_bias;
|
||||||
|
float32 m_gamma;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline float32 b2WheelJoint::GetMotorSpeed() const
|
||||||
|
{
|
||||||
|
return m_motorSpeed;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2WheelJoint::GetMaxMotorTorque() const
|
||||||
|
{
|
||||||
|
return m_maxMotorTorque;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2WheelJoint::SetSpringFrequencyHz(float32 hz)
|
||||||
|
{
|
||||||
|
m_frequencyHz = hz;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2WheelJoint::GetSpringFrequencyHz() const
|
||||||
|
{
|
||||||
|
return m_frequencyHz;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2WheelJoint::SetSpringDampingRatio(float32 ratio)
|
||||||
|
{
|
||||||
|
m_dampingRatio = ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2WheelJoint::GetSpringDampingRatio() const
|
||||||
|
{
|
||||||
|
return m_dampingRatio;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,554 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2World.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
#include "Box2D/Dynamics/Joints/b2Joint.h"
|
||||||
|
|
||||||
|
b2Body::b2Body(const b2BodyDef* bd, b2World* world)
|
||||||
|
{
|
||||||
|
b2Assert(bd->position.IsValid());
|
||||||
|
b2Assert(bd->linearVelocity.IsValid());
|
||||||
|
b2Assert(b2IsValid(bd->angle));
|
||||||
|
b2Assert(b2IsValid(bd->angularVelocity));
|
||||||
|
b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
|
||||||
|
b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
|
||||||
|
|
||||||
|
m_flags = 0;
|
||||||
|
|
||||||
|
if (bd->bullet)
|
||||||
|
{
|
||||||
|
m_flags |= e_bulletFlag;
|
||||||
|
}
|
||||||
|
if (bd->fixedRotation)
|
||||||
|
{
|
||||||
|
m_flags |= e_fixedRotationFlag;
|
||||||
|
}
|
||||||
|
if (bd->allowSleep)
|
||||||
|
{
|
||||||
|
m_flags |= e_autoSleepFlag;
|
||||||
|
}
|
||||||
|
if (bd->awake)
|
||||||
|
{
|
||||||
|
m_flags |= e_awakeFlag;
|
||||||
|
}
|
||||||
|
if (bd->active)
|
||||||
|
{
|
||||||
|
m_flags |= e_activeFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_world = world;
|
||||||
|
|
||||||
|
m_xf.p = bd->position;
|
||||||
|
m_xf.q.Set(bd->angle);
|
||||||
|
|
||||||
|
m_sweep.localCenter.SetZero();
|
||||||
|
m_sweep.c0 = m_xf.p;
|
||||||
|
m_sweep.c = m_xf.p;
|
||||||
|
m_sweep.a0 = bd->angle;
|
||||||
|
m_sweep.a = bd->angle;
|
||||||
|
m_sweep.alpha0 = 0.0f;
|
||||||
|
|
||||||
|
m_jointList = nullptr;
|
||||||
|
m_contactList = nullptr;
|
||||||
|
m_prev = nullptr;
|
||||||
|
m_next = nullptr;
|
||||||
|
|
||||||
|
m_linearVelocity = bd->linearVelocity;
|
||||||
|
m_angularVelocity = bd->angularVelocity;
|
||||||
|
|
||||||
|
m_linearDamping = bd->linearDamping;
|
||||||
|
m_angularDamping = bd->angularDamping;
|
||||||
|
m_gravityScale = bd->gravityScale;
|
||||||
|
|
||||||
|
m_force.SetZero();
|
||||||
|
m_torque = 0.0f;
|
||||||
|
|
||||||
|
m_sleepTime = 0.0f;
|
||||||
|
|
||||||
|
m_type = bd->type;
|
||||||
|
|
||||||
|
if (m_type == b2_dynamicBody)
|
||||||
|
{
|
||||||
|
m_mass = 1.0f;
|
||||||
|
m_invMass = 1.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_mass = 0.0f;
|
||||||
|
m_invMass = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_I = 0.0f;
|
||||||
|
m_invI = 0.0f;
|
||||||
|
|
||||||
|
m_userData = bd->userData;
|
||||||
|
|
||||||
|
m_fixtureList = nullptr;
|
||||||
|
m_fixtureCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Body::~b2Body()
|
||||||
|
{
|
||||||
|
// shapes and joints are destroyed in b2World::Destroy
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SetType(b2BodyType type)
|
||||||
|
{
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
if (m_world->IsLocked() == true)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_type == type)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_type = type;
|
||||||
|
|
||||||
|
ResetMassData();
|
||||||
|
|
||||||
|
if (m_type == b2_staticBody)
|
||||||
|
{
|
||||||
|
m_linearVelocity.SetZero();
|
||||||
|
m_angularVelocity = 0.0f;
|
||||||
|
m_sweep.a0 = m_sweep.a;
|
||||||
|
m_sweep.c0 = m_sweep.c;
|
||||||
|
SynchronizeFixtures();
|
||||||
|
}
|
||||||
|
|
||||||
|
SetAwake(true);
|
||||||
|
|
||||||
|
m_force.SetZero();
|
||||||
|
m_torque = 0.0f;
|
||||||
|
|
||||||
|
// Delete the attached contacts.
|
||||||
|
b2ContactEdge* ce = m_contactList;
|
||||||
|
while (ce)
|
||||||
|
{
|
||||||
|
b2ContactEdge* ce0 = ce;
|
||||||
|
ce = ce->next;
|
||||||
|
m_world->m_contactManager.Destroy(ce0->contact);
|
||||||
|
}
|
||||||
|
m_contactList = nullptr;
|
||||||
|
|
||||||
|
// Touch the proxies so that new contacts will be created (when appropriate)
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
int32 proxyCount = f->m_proxyCount;
|
||||||
|
for (int32 i = 0; i < proxyCount; ++i)
|
||||||
|
{
|
||||||
|
broadPhase->TouchProxy(f->m_proxies[i].proxyId);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
|
||||||
|
{
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
if (m_world->IsLocked() == true)
|
||||||
|
{
|
||||||
|
return nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2BlockAllocator* allocator = &m_world->m_blockAllocator;
|
||||||
|
|
||||||
|
void* memory = allocator->Allocate(sizeof(b2Fixture));
|
||||||
|
b2Fixture* fixture = new (memory) b2Fixture;
|
||||||
|
fixture->Create(allocator, this, def);
|
||||||
|
|
||||||
|
if (m_flags & e_activeFlag)
|
||||||
|
{
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
fixture->CreateProxies(broadPhase, m_xf);
|
||||||
|
}
|
||||||
|
|
||||||
|
fixture->m_next = m_fixtureList;
|
||||||
|
m_fixtureList = fixture;
|
||||||
|
++m_fixtureCount;
|
||||||
|
|
||||||
|
fixture->m_body = this;
|
||||||
|
|
||||||
|
// Adjust mass properties if needed.
|
||||||
|
if (fixture->m_density > 0.0f)
|
||||||
|
{
|
||||||
|
ResetMassData();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Let the world know we have a new fixture. This will cause new contacts
|
||||||
|
// to be created at the beginning of the next time step.
|
||||||
|
m_world->m_flags |= b2World::e_newFixture;
|
||||||
|
|
||||||
|
return fixture;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
|
||||||
|
{
|
||||||
|
b2FixtureDef def;
|
||||||
|
def.shape = shape;
|
||||||
|
def.density = density;
|
||||||
|
|
||||||
|
return CreateFixture(&def);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::DestroyFixture(b2Fixture* fixture)
|
||||||
|
{
|
||||||
|
if (fixture == NULL)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
if (m_world->IsLocked() == true)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(fixture->m_body == this);
|
||||||
|
|
||||||
|
// Remove the fixture from this body's singly linked list.
|
||||||
|
b2Assert(m_fixtureCount > 0);
|
||||||
|
b2Fixture** node = &m_fixtureList;
|
||||||
|
bool found = false;
|
||||||
|
while (*node != nullptr)
|
||||||
|
{
|
||||||
|
if (*node == fixture)
|
||||||
|
{
|
||||||
|
*node = fixture->m_next;
|
||||||
|
found = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
node = &(*node)->m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
// You tried to remove a shape that is not attached to this body.
|
||||||
|
b2Assert(found);
|
||||||
|
|
||||||
|
// Destroy any contacts associated with the fixture.
|
||||||
|
b2ContactEdge* edge = m_contactList;
|
||||||
|
while (edge)
|
||||||
|
{
|
||||||
|
b2Contact* c = edge->contact;
|
||||||
|
edge = edge->next;
|
||||||
|
|
||||||
|
b2Fixture* fixtureA = c->GetFixtureA();
|
||||||
|
b2Fixture* fixtureB = c->GetFixtureB();
|
||||||
|
|
||||||
|
if (fixture == fixtureA || fixture == fixtureB)
|
||||||
|
{
|
||||||
|
// This destroys the contact and removes it from
|
||||||
|
// this body's contact list.
|
||||||
|
m_world->m_contactManager.Destroy(c);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
b2BlockAllocator* allocator = &m_world->m_blockAllocator;
|
||||||
|
|
||||||
|
if (m_flags & e_activeFlag)
|
||||||
|
{
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
fixture->DestroyProxies(broadPhase);
|
||||||
|
}
|
||||||
|
|
||||||
|
fixture->m_body = nullptr;
|
||||||
|
fixture->m_next = nullptr;
|
||||||
|
fixture->Destroy(allocator);
|
||||||
|
fixture->~b2Fixture();
|
||||||
|
allocator->Free(fixture, sizeof(b2Fixture));
|
||||||
|
|
||||||
|
--m_fixtureCount;
|
||||||
|
|
||||||
|
// Reset the mass data.
|
||||||
|
ResetMassData();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::ResetMassData()
|
||||||
|
{
|
||||||
|
// Compute mass data from shapes. Each shape has its own density.
|
||||||
|
m_mass = 0.0f;
|
||||||
|
m_invMass = 0.0f;
|
||||||
|
m_I = 0.0f;
|
||||||
|
m_invI = 0.0f;
|
||||||
|
m_sweep.localCenter.SetZero();
|
||||||
|
|
||||||
|
// Static and kinematic bodies have zero mass.
|
||||||
|
if (m_type == b2_staticBody || m_type == b2_kinematicBody)
|
||||||
|
{
|
||||||
|
m_sweep.c0 = m_xf.p;
|
||||||
|
m_sweep.c = m_xf.p;
|
||||||
|
m_sweep.a0 = m_sweep.a;
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Assert(m_type == b2_dynamicBody);
|
||||||
|
|
||||||
|
// Accumulate mass over all fixtures.
|
||||||
|
b2Vec2 localCenter = b2Vec2_zero;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
if (f->m_density == 0.0f)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2MassData massData;
|
||||||
|
f->GetMassData(&massData);
|
||||||
|
m_mass += massData.mass;
|
||||||
|
localCenter += massData.mass * massData.center;
|
||||||
|
m_I += massData.I;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Compute center of mass.
|
||||||
|
if (m_mass > 0.0f)
|
||||||
|
{
|
||||||
|
m_invMass = 1.0f / m_mass;
|
||||||
|
localCenter *= m_invMass;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
// Force all dynamic bodies to have a positive mass.
|
||||||
|
m_mass = 1.0f;
|
||||||
|
m_invMass = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_I > 0.0f && (m_flags & e_fixedRotationFlag) == 0)
|
||||||
|
{
|
||||||
|
// Center the inertia about the center of mass.
|
||||||
|
m_I -= m_mass * b2Dot(localCenter, localCenter);
|
||||||
|
b2Assert(m_I > 0.0f);
|
||||||
|
m_invI = 1.0f / m_I;
|
||||||
|
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_I = 0.0f;
|
||||||
|
m_invI = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Move center of mass.
|
||||||
|
b2Vec2 oldCenter = m_sweep.c;
|
||||||
|
m_sweep.localCenter = localCenter;
|
||||||
|
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
|
||||||
|
|
||||||
|
// Update center of mass velocity.
|
||||||
|
m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SetMassData(const b2MassData* massData)
|
||||||
|
{
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
if (m_world->IsLocked() == true)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_invMass = 0.0f;
|
||||||
|
m_I = 0.0f;
|
||||||
|
m_invI = 0.0f;
|
||||||
|
|
||||||
|
m_mass = massData->mass;
|
||||||
|
if (m_mass <= 0.0f)
|
||||||
|
{
|
||||||
|
m_mass = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_invMass = 1.0f / m_mass;
|
||||||
|
|
||||||
|
if (massData->I > 0.0f && (m_flags & b2Body::e_fixedRotationFlag) == 0)
|
||||||
|
{
|
||||||
|
m_I = massData->I - m_mass * b2Dot(massData->center, massData->center);
|
||||||
|
b2Assert(m_I > 0.0f);
|
||||||
|
m_invI = 1.0f / m_I;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Move center of mass.
|
||||||
|
b2Vec2 oldCenter = m_sweep.c;
|
||||||
|
m_sweep.localCenter = massData->center;
|
||||||
|
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
|
||||||
|
|
||||||
|
// Update center of mass velocity.
|
||||||
|
m_linearVelocity += b2Cross(m_angularVelocity, m_sweep.c - oldCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
bool b2Body::ShouldCollide(const b2Body* other) const
|
||||||
|
{
|
||||||
|
// At least one body should be dynamic.
|
||||||
|
if (m_type != b2_dynamicBody && other->m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Does a joint prevent collision?
|
||||||
|
for (b2JointEdge* jn = m_jointList; jn; jn = jn->next)
|
||||||
|
{
|
||||||
|
if (jn->other == other)
|
||||||
|
{
|
||||||
|
if (jn->joint->m_collideConnected == false)
|
||||||
|
{
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SetTransform(const b2Vec2& position, float32 angle)
|
||||||
|
{
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
if (m_world->IsLocked() == true)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_xf.q.Set(angle);
|
||||||
|
m_xf.p = position;
|
||||||
|
|
||||||
|
m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
|
||||||
|
m_sweep.a = angle;
|
||||||
|
|
||||||
|
m_sweep.c0 = m_sweep.c;
|
||||||
|
m_sweep.a0 = angle;
|
||||||
|
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
f->Synchronize(broadPhase, m_xf, m_xf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SynchronizeFixtures()
|
||||||
|
{
|
||||||
|
b2Transform xf1;
|
||||||
|
xf1.q.Set(m_sweep.a0);
|
||||||
|
xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
|
||||||
|
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
f->Synchronize(broadPhase, xf1, m_xf);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SetActive(bool flag)
|
||||||
|
{
|
||||||
|
b2Assert(m_world->IsLocked() == false);
|
||||||
|
|
||||||
|
if (flag == IsActive())
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_activeFlag;
|
||||||
|
|
||||||
|
// Create all proxies.
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
f->CreateProxies(broadPhase, m_xf);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Contacts are created the next time step.
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_activeFlag;
|
||||||
|
|
||||||
|
// Destroy all proxies.
|
||||||
|
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
f->DestroyProxies(broadPhase);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Destroy the attached contacts.
|
||||||
|
b2ContactEdge* ce = m_contactList;
|
||||||
|
while (ce)
|
||||||
|
{
|
||||||
|
b2ContactEdge* ce0 = ce;
|
||||||
|
ce = ce->next;
|
||||||
|
m_world->m_contactManager.Destroy(ce0->contact);
|
||||||
|
}
|
||||||
|
m_contactList = nullptr;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::SetFixedRotation(bool flag)
|
||||||
|
{
|
||||||
|
bool status = (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
|
||||||
|
if (status == flag)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_fixedRotationFlag;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_fixedRotationFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_angularVelocity = 0.0f;
|
||||||
|
|
||||||
|
ResetMassData();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Body::Dump()
|
||||||
|
{
|
||||||
|
int32 bodyIndex = m_islandIndex;
|
||||||
|
|
||||||
|
b2Log("{\n");
|
||||||
|
b2Log(" b2BodyDef bd;\n");
|
||||||
|
b2Log(" bd.type = b2BodyType(%d);\n", m_type);
|
||||||
|
b2Log(" bd.position.Set(%.15lef, %.15lef);\n", m_xf.p.x, m_xf.p.y);
|
||||||
|
b2Log(" bd.angle = %.15lef;\n", m_sweep.a);
|
||||||
|
b2Log(" bd.linearVelocity.Set(%.15lef, %.15lef);\n", m_linearVelocity.x, m_linearVelocity.y);
|
||||||
|
b2Log(" bd.angularVelocity = %.15lef;\n", m_angularVelocity);
|
||||||
|
b2Log(" bd.linearDamping = %.15lef;\n", m_linearDamping);
|
||||||
|
b2Log(" bd.angularDamping = %.15lef;\n", m_angularDamping);
|
||||||
|
b2Log(" bd.allowSleep = bool(%d);\n", m_flags & e_autoSleepFlag);
|
||||||
|
b2Log(" bd.awake = bool(%d);\n", m_flags & e_awakeFlag);
|
||||||
|
b2Log(" bd.fixedRotation = bool(%d);\n", m_flags & e_fixedRotationFlag);
|
||||||
|
b2Log(" bd.bullet = bool(%d);\n", m_flags & e_bulletFlag);
|
||||||
|
b2Log(" bd.active = bool(%d);\n", m_flags & e_activeFlag);
|
||||||
|
b2Log(" bd.gravityScale = %.15lef;\n", m_gravityScale);
|
||||||
|
b2Log(" bodies[%d] = m_world->CreateBody(&bd);\n", m_islandIndex);
|
||||||
|
b2Log("\n");
|
||||||
|
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
|
||||||
|
{
|
||||||
|
b2Log(" {\n");
|
||||||
|
f->Dump(bodyIndex);
|
||||||
|
b2Log(" }\n");
|
||||||
|
}
|
||||||
|
b2Log("}\n");
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,882 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_BODY_H
|
||||||
|
#define B2_BODY_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
#include "../Collision/Shapes/b2Shape.h"
|
||||||
|
#include <memory>
|
||||||
|
|
||||||
|
class b2Fixture;
|
||||||
|
class b2Joint;
|
||||||
|
class b2Contact;
|
||||||
|
class b2Controller;
|
||||||
|
class b2World;
|
||||||
|
struct b2FixtureDef;
|
||||||
|
struct b2JointEdge;
|
||||||
|
struct b2ContactEdge;
|
||||||
|
|
||||||
|
/// The body type.
|
||||||
|
/// static: zero mass, zero velocity, may be manually moved
|
||||||
|
/// kinematic: zero mass, non-zero velocity set by user, moved by solver
|
||||||
|
/// dynamic: positive mass, non-zero velocity determined by forces, moved by solver
|
||||||
|
enum b2BodyType
|
||||||
|
{
|
||||||
|
b2_staticBody = 0,
|
||||||
|
b2_kinematicBody,
|
||||||
|
b2_dynamicBody
|
||||||
|
|
||||||
|
// TODO_ERIN
|
||||||
|
//b2_bulletBody,
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A body definition holds all the data needed to construct a rigid body.
|
||||||
|
/// You can safely re-use body definitions. Shapes are added to a body after construction.
|
||||||
|
struct b2BodyDef
|
||||||
|
{
|
||||||
|
/// This constructor sets the body definition default values.
|
||||||
|
b2BodyDef()
|
||||||
|
{
|
||||||
|
userData = nullptr;
|
||||||
|
position.Set(0.0f, 0.0f);
|
||||||
|
angle = 0.0f;
|
||||||
|
linearVelocity.Set(0.0f, 0.0f);
|
||||||
|
angularVelocity = 0.0f;
|
||||||
|
linearDamping = 0.0f;
|
||||||
|
angularDamping = 0.0f;
|
||||||
|
allowSleep = true;
|
||||||
|
awake = true;
|
||||||
|
fixedRotation = false;
|
||||||
|
bullet = false;
|
||||||
|
type = b2_staticBody;
|
||||||
|
active = true;
|
||||||
|
gravityScale = 1.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The body type: static, kinematic, or dynamic.
|
||||||
|
/// Note: if a dynamic body would have zero mass, the mass is set to one.
|
||||||
|
b2BodyType type;
|
||||||
|
|
||||||
|
/// The world position of the body. Avoid creating bodies at the origin
|
||||||
|
/// since this can lead to many overlapping shapes.
|
||||||
|
b2Vec2 position;
|
||||||
|
|
||||||
|
/// The world angle of the body in radians.
|
||||||
|
float32 angle;
|
||||||
|
|
||||||
|
/// The linear velocity of the body's origin in world co-ordinates.
|
||||||
|
b2Vec2 linearVelocity;
|
||||||
|
|
||||||
|
/// The angular velocity of the body.
|
||||||
|
float32 angularVelocity;
|
||||||
|
|
||||||
|
/// Linear damping is use to reduce the linear velocity. The damping parameter
|
||||||
|
/// can be larger than 1.0f but the damping effect becomes sensitive to the
|
||||||
|
/// time step when the damping parameter is large.
|
||||||
|
/// Units are 1/time
|
||||||
|
float32 linearDamping;
|
||||||
|
|
||||||
|
/// Angular damping is use to reduce the angular velocity. The damping parameter
|
||||||
|
/// can be larger than 1.0f but the damping effect becomes sensitive to the
|
||||||
|
/// time step when the damping parameter is large.
|
||||||
|
/// Units are 1/time
|
||||||
|
float32 angularDamping;
|
||||||
|
|
||||||
|
/// Set this flag to false if this body should never fall asleep. Note that
|
||||||
|
/// this increases CPU usage.
|
||||||
|
bool allowSleep;
|
||||||
|
|
||||||
|
/// Is this body initially awake or sleeping?
|
||||||
|
bool awake;
|
||||||
|
|
||||||
|
/// Should this body be prevented from rotating? Useful for characters.
|
||||||
|
bool fixedRotation;
|
||||||
|
|
||||||
|
/// Is this a fast moving body that should be prevented from tunneling through
|
||||||
|
/// other moving bodies? Note that all bodies are prevented from tunneling through
|
||||||
|
/// kinematic and static bodies. This setting is only considered on dynamic bodies.
|
||||||
|
/// @warning You should use this flag sparingly since it increases processing time.
|
||||||
|
bool bullet;
|
||||||
|
|
||||||
|
/// Does this body start out active?
|
||||||
|
bool active;
|
||||||
|
|
||||||
|
/// Use this to store application specific body data.
|
||||||
|
void* userData;
|
||||||
|
|
||||||
|
/// Scale the gravity applied to this body.
|
||||||
|
float32 gravityScale;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A rigid body. These are created via b2World::CreateBody.
|
||||||
|
class b2Body
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/// Creates a fixture and attach it to this body. Use this function if you need
|
||||||
|
/// to set some fixture parameters, like friction. Otherwise you can create the
|
||||||
|
/// fixture directly from a shape.
|
||||||
|
/// If the density is non-zero, this function automatically updates the mass of the body.
|
||||||
|
/// Contacts are not created until the next time step.
|
||||||
|
/// @param def the fixture definition.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
b2Fixture* CreateFixture(const b2FixtureDef* def);
|
||||||
|
|
||||||
|
/// Creates a fixture from a shape and attach it to this body.
|
||||||
|
/// This is a convenience function. Use b2FixtureDef if you need to set parameters
|
||||||
|
/// like friction, restitution, user data, or filtering.
|
||||||
|
/// If the density is non-zero, this function automatically updates the mass of the body.
|
||||||
|
/// @param shape the shape to be cloned.
|
||||||
|
/// @param density the shape density (set to zero for static bodies).
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
b2Fixture* CreateFixture(const b2Shape* shape, float32 density);
|
||||||
|
|
||||||
|
/// Destroy a fixture. This removes the fixture from the broad-phase and
|
||||||
|
/// destroys all contacts associated with this fixture. This will
|
||||||
|
/// automatically adjust the mass of the body if the body is dynamic and the
|
||||||
|
/// fixture has positive density.
|
||||||
|
/// All fixtures attached to a body are implicitly destroyed when the body is destroyed.
|
||||||
|
/// @param fixture the fixture to be removed.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
void DestroyFixture(b2Fixture* fixture);
|
||||||
|
|
||||||
|
/// Set the position of the body's origin and rotation.
|
||||||
|
/// Manipulating a body's transform may cause non-physical behavior.
|
||||||
|
/// Note: contacts are updated on the next call to b2World::Step.
|
||||||
|
/// @param position the world position of the body's local origin.
|
||||||
|
/// @param angle the world rotation in radians.
|
||||||
|
void SetTransform(const b2Vec2& position, float32 angle);
|
||||||
|
|
||||||
|
/// Get the body transform for the body's origin.
|
||||||
|
/// @return the world transform of the body's origin.
|
||||||
|
const b2Transform& GetTransform() const;
|
||||||
|
|
||||||
|
/// Get the world body origin position.
|
||||||
|
/// @return the world position of the body's origin.
|
||||||
|
const b2Vec2& GetPosition() const;
|
||||||
|
|
||||||
|
/// Get the angle in radians.
|
||||||
|
/// @return the current world rotation angle in radians.
|
||||||
|
float32 GetAngle() const;
|
||||||
|
|
||||||
|
/// Get the world position of the center of mass.
|
||||||
|
const b2Vec2& GetWorldCenter() const;
|
||||||
|
|
||||||
|
/// Get the local position of the center of mass.
|
||||||
|
const b2Vec2& GetLocalCenter() const;
|
||||||
|
|
||||||
|
/// Set the linear velocity of the center of mass.
|
||||||
|
/// @param v the new linear velocity of the center of mass.
|
||||||
|
void SetLinearVelocity(const b2Vec2& v);
|
||||||
|
|
||||||
|
/// Get the linear velocity of the center of mass.
|
||||||
|
/// @return the linear velocity of the center of mass.
|
||||||
|
const b2Vec2& GetLinearVelocity() const;
|
||||||
|
|
||||||
|
/// Set the angular velocity.
|
||||||
|
/// @param omega the new angular velocity in radians/second.
|
||||||
|
void SetAngularVelocity(float32 omega);
|
||||||
|
|
||||||
|
/// Get the angular velocity.
|
||||||
|
/// @return the angular velocity in radians/second.
|
||||||
|
float32 GetAngularVelocity() const;
|
||||||
|
|
||||||
|
/// Apply a force at a world point. If the force is not
|
||||||
|
/// applied at the center of mass, it will generate a torque and
|
||||||
|
/// affect the angular velocity. This wakes up the body.
|
||||||
|
/// @param force the world force vector, usually in Newtons (N).
|
||||||
|
/// @param point the world position of the point of application.
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake);
|
||||||
|
|
||||||
|
/// Apply a force to the center of mass. This wakes up the body.
|
||||||
|
/// @param force the world force vector, usually in Newtons (N).
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyForceToCenter(const b2Vec2& force, bool wake);
|
||||||
|
|
||||||
|
/// Apply a torque. This affects the angular velocity
|
||||||
|
/// without affecting the linear velocity of the center of mass.
|
||||||
|
/// @param torque about the z-axis (out of the screen), usually in N-m.
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyTorque(float32 torque, bool wake);
|
||||||
|
|
||||||
|
/// Apply an impulse at a point. This immediately modifies the velocity.
|
||||||
|
/// It also modifies the angular velocity if the point of application
|
||||||
|
/// is not at the center of mass. This wakes up the body.
|
||||||
|
/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
|
||||||
|
/// @param point the world position of the point of application.
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake);
|
||||||
|
|
||||||
|
/// Apply an impulse to the center of mass. This immediately modifies the velocity.
|
||||||
|
/// @param impulse the world impulse vector, usually in N-seconds or kg-m/s.
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake);
|
||||||
|
|
||||||
|
/// Apply an angular impulse.
|
||||||
|
/// @param impulse the angular impulse in units of kg*m*m/s
|
||||||
|
/// @param wake also wake up the body
|
||||||
|
void ApplyAngularImpulse(float32 impulse, bool wake);
|
||||||
|
|
||||||
|
/// Get the total mass of the body.
|
||||||
|
/// @return the mass, usually in kilograms (kg).
|
||||||
|
float32 GetMass() const;
|
||||||
|
|
||||||
|
/// Get the rotational inertia of the body about the local origin.
|
||||||
|
/// @return the rotational inertia, usually in kg-m^2.
|
||||||
|
float32 GetInertia() const;
|
||||||
|
|
||||||
|
/// Get the mass data of the body.
|
||||||
|
/// @return a struct containing the mass, inertia and center of the body.
|
||||||
|
void GetMassData(b2MassData* data) const;
|
||||||
|
|
||||||
|
/// Set the mass properties to override the mass properties of the fixtures.
|
||||||
|
/// Note that this changes the center of mass position.
|
||||||
|
/// Note that creating or destroying fixtures can also alter the mass.
|
||||||
|
/// This function has no effect if the body isn't dynamic.
|
||||||
|
/// @param massData the mass properties.
|
||||||
|
void SetMassData(const b2MassData* data);
|
||||||
|
|
||||||
|
/// This resets the mass properties to the sum of the mass properties of the fixtures.
|
||||||
|
/// This normally does not need to be called unless you called SetMassData to override
|
||||||
|
/// the mass and you later want to reset the mass.
|
||||||
|
void ResetMassData();
|
||||||
|
|
||||||
|
/// Get the world coordinates of a point given the local coordinates.
|
||||||
|
/// @param localPoint a point on the body measured relative the the body's origin.
|
||||||
|
/// @return the same point expressed in world coordinates.
|
||||||
|
b2Vec2 GetWorldPoint(const b2Vec2& localPoint) const;
|
||||||
|
|
||||||
|
/// Get the world coordinates of a vector given the local coordinates.
|
||||||
|
/// @param localVector a vector fixed in the body.
|
||||||
|
/// @return the same vector expressed in world coordinates.
|
||||||
|
b2Vec2 GetWorldVector(const b2Vec2& localVector) const;
|
||||||
|
|
||||||
|
/// Gets a local point relative to the body's origin given a world point.
|
||||||
|
/// @param a point in world coordinates.
|
||||||
|
/// @return the corresponding local point relative to the body's origin.
|
||||||
|
b2Vec2 GetLocalPoint(const b2Vec2& worldPoint) const;
|
||||||
|
|
||||||
|
/// Gets a local vector given a world vector.
|
||||||
|
/// @param a vector in world coordinates.
|
||||||
|
/// @return the corresponding local vector.
|
||||||
|
b2Vec2 GetLocalVector(const b2Vec2& worldVector) const;
|
||||||
|
|
||||||
|
/// Get the world linear velocity of a world point attached to this body.
|
||||||
|
/// @param a point in world coordinates.
|
||||||
|
/// @return the world velocity of a point.
|
||||||
|
b2Vec2 GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const;
|
||||||
|
|
||||||
|
/// Get the world velocity of a local point.
|
||||||
|
/// @param a point in local coordinates.
|
||||||
|
/// @return the world velocity of a point.
|
||||||
|
b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;
|
||||||
|
|
||||||
|
/// Get the linear damping of the body.
|
||||||
|
float32 GetLinearDamping() const;
|
||||||
|
|
||||||
|
/// Set the linear damping of the body.
|
||||||
|
void SetLinearDamping(float32 linearDamping);
|
||||||
|
|
||||||
|
/// Get the angular damping of the body.
|
||||||
|
float32 GetAngularDamping() const;
|
||||||
|
|
||||||
|
/// Set the angular damping of the body.
|
||||||
|
void SetAngularDamping(float32 angularDamping);
|
||||||
|
|
||||||
|
/// Get the gravity scale of the body.
|
||||||
|
float32 GetGravityScale() const;
|
||||||
|
|
||||||
|
/// Set the gravity scale of the body.
|
||||||
|
void SetGravityScale(float32 scale);
|
||||||
|
|
||||||
|
/// Set the type of this body. This may alter the mass and velocity.
|
||||||
|
void SetType(b2BodyType type);
|
||||||
|
|
||||||
|
/// Get the type of this body.
|
||||||
|
b2BodyType GetType() const;
|
||||||
|
|
||||||
|
/// Should this body be treated like a bullet for continuous collision detection?
|
||||||
|
void SetBullet(bool flag);
|
||||||
|
|
||||||
|
/// Is this body treated like a bullet for continuous collision detection?
|
||||||
|
bool IsBullet() const;
|
||||||
|
|
||||||
|
/// You can disable sleeping on this body. If you disable sleeping, the
|
||||||
|
/// body will be woken.
|
||||||
|
void SetSleepingAllowed(bool flag);
|
||||||
|
|
||||||
|
/// Is this body allowed to sleep
|
||||||
|
bool IsSleepingAllowed() const;
|
||||||
|
|
||||||
|
/// Set the sleep state of the body. A sleeping body has very
|
||||||
|
/// low CPU cost.
|
||||||
|
/// @param flag set to true to wake the body, false to put it to sleep.
|
||||||
|
void SetAwake(bool flag);
|
||||||
|
|
||||||
|
/// Get the sleeping state of this body.
|
||||||
|
/// @return true if the body is awake.
|
||||||
|
bool IsAwake() const;
|
||||||
|
|
||||||
|
/// Set the active state of the body. An inactive body is not
|
||||||
|
/// simulated and cannot be collided with or woken up.
|
||||||
|
/// If you pass a flag of true, all fixtures will be added to the
|
||||||
|
/// broad-phase.
|
||||||
|
/// If you pass a flag of false, all fixtures will be removed from
|
||||||
|
/// the broad-phase and all contacts will be destroyed.
|
||||||
|
/// Fixtures and joints are otherwise unaffected. You may continue
|
||||||
|
/// to create/destroy fixtures and joints on inactive bodies.
|
||||||
|
/// Fixtures on an inactive body are implicitly inactive and will
|
||||||
|
/// not participate in collisions, ray-casts, or queries.
|
||||||
|
/// Joints connected to an inactive body are implicitly inactive.
|
||||||
|
/// An inactive body is still owned by a b2World object and remains
|
||||||
|
/// in the body list.
|
||||||
|
void SetActive(bool flag);
|
||||||
|
|
||||||
|
/// Get the active state of the body.
|
||||||
|
bool IsActive() const;
|
||||||
|
|
||||||
|
/// Set this body to have fixed rotation. This causes the mass
|
||||||
|
/// to be reset.
|
||||||
|
void SetFixedRotation(bool flag);
|
||||||
|
|
||||||
|
/// Does this body have fixed rotation?
|
||||||
|
bool IsFixedRotation() const;
|
||||||
|
|
||||||
|
/// Get the list of all fixtures attached to this body.
|
||||||
|
b2Fixture* GetFixtureList();
|
||||||
|
const b2Fixture* GetFixtureList() const;
|
||||||
|
|
||||||
|
/// Get the list of all joints attached to this body.
|
||||||
|
b2JointEdge* GetJointList();
|
||||||
|
const b2JointEdge* GetJointList() const;
|
||||||
|
|
||||||
|
/// Get the list of all contacts attached to this body.
|
||||||
|
/// @warning this list changes during the time step and you may
|
||||||
|
/// miss some collisions if you don't use b2ContactListener.
|
||||||
|
b2ContactEdge* GetContactList();
|
||||||
|
const b2ContactEdge* GetContactList() const;
|
||||||
|
|
||||||
|
/// Get the next body in the world's body list.
|
||||||
|
b2Body* GetNext();
|
||||||
|
const b2Body* GetNext() const;
|
||||||
|
|
||||||
|
/// Get the user data pointer that was provided in the body definition.
|
||||||
|
void* GetUserData() const;
|
||||||
|
|
||||||
|
/// Set the user data. Use this to store your application specific data.
|
||||||
|
void SetUserData(void* data);
|
||||||
|
|
||||||
|
/// Get the parent world of this body.
|
||||||
|
b2World* GetWorld();
|
||||||
|
const b2World* GetWorld() const;
|
||||||
|
|
||||||
|
/// Dump this body to a log file
|
||||||
|
void Dump();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
friend class b2World;
|
||||||
|
friend class b2Island;
|
||||||
|
friend class b2ContactManager;
|
||||||
|
friend class b2ContactSolver;
|
||||||
|
friend class b2Contact;
|
||||||
|
|
||||||
|
friend class b2DistanceJoint;
|
||||||
|
friend class b2FrictionJoint;
|
||||||
|
friend class b2GearJoint;
|
||||||
|
friend class b2MotorJoint;
|
||||||
|
friend class b2MouseJoint;
|
||||||
|
friend class b2PrismaticJoint;
|
||||||
|
friend class b2PulleyJoint;
|
||||||
|
friend class b2RevoluteJoint;
|
||||||
|
friend class b2RopeJoint;
|
||||||
|
friend class b2WeldJoint;
|
||||||
|
friend class b2WheelJoint;
|
||||||
|
|
||||||
|
// m_flags
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
e_islandFlag = 0x0001,
|
||||||
|
e_awakeFlag = 0x0002,
|
||||||
|
e_autoSleepFlag = 0x0004,
|
||||||
|
e_bulletFlag = 0x0008,
|
||||||
|
e_fixedRotationFlag = 0x0010,
|
||||||
|
e_activeFlag = 0x0020,
|
||||||
|
e_toiFlag = 0x0040
|
||||||
|
};
|
||||||
|
|
||||||
|
b2Body(const b2BodyDef* bd, b2World* world);
|
||||||
|
~b2Body();
|
||||||
|
|
||||||
|
void SynchronizeFixtures();
|
||||||
|
void SynchronizeTransform();
|
||||||
|
|
||||||
|
// This is used to prevent connected bodies from colliding.
|
||||||
|
// It may lie, depending on the collideConnected flag.
|
||||||
|
bool ShouldCollide(const b2Body* other) const;
|
||||||
|
|
||||||
|
void Advance(float32 t);
|
||||||
|
|
||||||
|
b2BodyType m_type;
|
||||||
|
|
||||||
|
uint16 m_flags;
|
||||||
|
|
||||||
|
int32 m_islandIndex;
|
||||||
|
|
||||||
|
b2Transform m_xf; // the body origin transform
|
||||||
|
b2Sweep m_sweep; // the swept motion for CCD
|
||||||
|
|
||||||
|
b2Vec2 m_linearVelocity;
|
||||||
|
float32 m_angularVelocity;
|
||||||
|
|
||||||
|
b2Vec2 m_force;
|
||||||
|
float32 m_torque;
|
||||||
|
|
||||||
|
b2World* m_world;
|
||||||
|
b2Body* m_prev;
|
||||||
|
b2Body* m_next;
|
||||||
|
|
||||||
|
b2Fixture* m_fixtureList;
|
||||||
|
int32 m_fixtureCount;
|
||||||
|
|
||||||
|
b2JointEdge* m_jointList;
|
||||||
|
b2ContactEdge* m_contactList;
|
||||||
|
|
||||||
|
float32 m_mass, m_invMass;
|
||||||
|
|
||||||
|
// Rotational inertia about the center of mass.
|
||||||
|
float32 m_I, m_invI;
|
||||||
|
|
||||||
|
float32 m_linearDamping;
|
||||||
|
float32 m_angularDamping;
|
||||||
|
float32 m_gravityScale;
|
||||||
|
|
||||||
|
float32 m_sleepTime;
|
||||||
|
|
||||||
|
void* m_userData;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2BodyType b2Body::GetType() const
|
||||||
|
{
|
||||||
|
return m_type;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Transform& b2Body::GetTransform() const
|
||||||
|
{
|
||||||
|
return m_xf;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2Body::GetPosition() const
|
||||||
|
{
|
||||||
|
return m_xf.p;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetAngle() const
|
||||||
|
{
|
||||||
|
return m_sweep.a;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2Body::GetWorldCenter() const
|
||||||
|
{
|
||||||
|
return m_sweep.c;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2Body::GetLocalCenter() const
|
||||||
|
{
|
||||||
|
return m_sweep.localCenter;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetLinearVelocity(const b2Vec2& v)
|
||||||
|
{
|
||||||
|
if (m_type == b2_staticBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (b2Dot(v,v) > 0.0f)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_linearVelocity = v;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Vec2& b2Body::GetLinearVelocity() const
|
||||||
|
{
|
||||||
|
return m_linearVelocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetAngularVelocity(float32 w)
|
||||||
|
{
|
||||||
|
if (m_type == b2_staticBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (w * w > 0.0f)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_angularVelocity = w;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetAngularVelocity() const
|
||||||
|
{
|
||||||
|
return m_angularVelocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetMass() const
|
||||||
|
{
|
||||||
|
return m_mass;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetInertia() const
|
||||||
|
{
|
||||||
|
return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::GetMassData(b2MassData* data) const
|
||||||
|
{
|
||||||
|
data->mass = m_mass;
|
||||||
|
data->I = m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
|
||||||
|
data->center = m_sweep.localCenter;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const
|
||||||
|
{
|
||||||
|
return b2Mul(m_xf, localPoint);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const
|
||||||
|
{
|
||||||
|
return b2Mul(m_xf.q, localVector);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
|
||||||
|
{
|
||||||
|
return b2MulT(m_xf, worldPoint);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const
|
||||||
|
{
|
||||||
|
return b2MulT(m_xf.q, worldVector);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
|
||||||
|
{
|
||||||
|
return m_linearVelocity + b2Cross(m_angularVelocity, worldPoint - m_sweep.c);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const
|
||||||
|
{
|
||||||
|
return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetLinearDamping() const
|
||||||
|
{
|
||||||
|
return m_linearDamping;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetLinearDamping(float32 linearDamping)
|
||||||
|
{
|
||||||
|
m_linearDamping = linearDamping;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetAngularDamping() const
|
||||||
|
{
|
||||||
|
return m_angularDamping;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetAngularDamping(float32 angularDamping)
|
||||||
|
{
|
||||||
|
m_angularDamping = angularDamping;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Body::GetGravityScale() const
|
||||||
|
{
|
||||||
|
return m_gravityScale;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetGravityScale(float32 scale)
|
||||||
|
{
|
||||||
|
m_gravityScale = scale;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetBullet(bool flag)
|
||||||
|
{
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_bulletFlag;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_bulletFlag;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Body::IsBullet() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_bulletFlag) == e_bulletFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetAwake(bool flag)
|
||||||
|
{
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_awakeFlag;
|
||||||
|
m_sleepTime = 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_awakeFlag;
|
||||||
|
m_sleepTime = 0.0f;
|
||||||
|
m_linearVelocity.SetZero();
|
||||||
|
m_angularVelocity = 0.0f;
|
||||||
|
m_force.SetZero();
|
||||||
|
m_torque = 0.0f;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Body::IsAwake() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_awakeFlag) == e_awakeFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Body::IsActive() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_activeFlag) == e_activeFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Body::IsFixedRotation() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetSleepingAllowed(bool flag)
|
||||||
|
{
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_autoSleepFlag;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_autoSleepFlag;
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Body::IsSleepingAllowed() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_autoSleepFlag) == e_autoSleepFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Fixture* b2Body::GetFixtureList()
|
||||||
|
{
|
||||||
|
return m_fixtureList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Fixture* b2Body::GetFixtureList() const
|
||||||
|
{
|
||||||
|
return m_fixtureList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2JointEdge* b2Body::GetJointList()
|
||||||
|
{
|
||||||
|
return m_jointList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2JointEdge* b2Body::GetJointList() const
|
||||||
|
{
|
||||||
|
return m_jointList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2ContactEdge* b2Body::GetContactList()
|
||||||
|
{
|
||||||
|
return m_contactList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2ContactEdge* b2Body::GetContactList() const
|
||||||
|
{
|
||||||
|
return m_contactList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Body* b2Body::GetNext()
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Body* b2Body::GetNext() const
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SetUserData(void* data)
|
||||||
|
{
|
||||||
|
m_userData = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void* b2Body::GetUserData() const
|
||||||
|
{
|
||||||
|
return m_userData;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate a force if the body is sleeping.
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_force += force;
|
||||||
|
m_torque += b2Cross(point - m_sweep.c, force);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyForceToCenter(const b2Vec2& force, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate a force if the body is sleeping
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_force += force;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyTorque(float32 torque, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate a force if the body is sleeping
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_torque += torque;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate velocity if the body is sleeping
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_linearVelocity += m_invMass * impulse;
|
||||||
|
m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyLinearImpulseToCenter(const b2Vec2& impulse, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate velocity if the body is sleeping
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_linearVelocity += m_invMass * impulse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake)
|
||||||
|
{
|
||||||
|
if (m_type != b2_dynamicBody)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (wake && (m_flags & e_awakeFlag) == 0)
|
||||||
|
{
|
||||||
|
SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't accumulate velocity if the body is sleeping
|
||||||
|
if (m_flags & e_awakeFlag)
|
||||||
|
{
|
||||||
|
m_angularVelocity += m_invI * impulse;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::SynchronizeTransform()
|
||||||
|
{
|
||||||
|
m_xf.q.Set(m_sweep.a);
|
||||||
|
m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Body::Advance(float32 alpha)
|
||||||
|
{
|
||||||
|
// Advance to the new safe time. This doesn't sync the broad-phase.
|
||||||
|
m_sweep.Advance(alpha);
|
||||||
|
m_sweep.c = m_sweep.c0;
|
||||||
|
m_sweep.a = m_sweep.a0;
|
||||||
|
m_xf.q.Set(m_sweep.a);
|
||||||
|
m_xf.p = m_sweep.c - b2Mul(m_xf.q, m_sweep.localCenter);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2World* b2Body::GetWorld()
|
||||||
|
{
|
||||||
|
return m_world;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2World* b2Body::GetWorld() const
|
||||||
|
{
|
||||||
|
return m_world;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,296 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/b2ContactManager.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2WorldCallbacks.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
|
||||||
|
b2ContactFilter b2_defaultFilter;
|
||||||
|
b2ContactListener b2_defaultListener;
|
||||||
|
|
||||||
|
b2ContactManager::b2ContactManager()
|
||||||
|
{
|
||||||
|
m_contactList = nullptr;
|
||||||
|
m_contactCount = 0;
|
||||||
|
m_contactFilter = &b2_defaultFilter;
|
||||||
|
m_contactListener = &b2_defaultListener;
|
||||||
|
m_allocator = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactManager::Destroy(b2Contact* c)
|
||||||
|
{
|
||||||
|
b2Fixture* fixtureA = c->GetFixtureA();
|
||||||
|
b2Fixture* fixtureB = c->GetFixtureB();
|
||||||
|
b2Body* bodyA = fixtureA->GetBody();
|
||||||
|
b2Body* bodyB = fixtureB->GetBody();
|
||||||
|
|
||||||
|
if (m_contactListener && c->IsTouching())
|
||||||
|
{
|
||||||
|
m_contactListener->EndContact(c);
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove from the world.
|
||||||
|
if (c->m_prev)
|
||||||
|
{
|
||||||
|
c->m_prev->m_next = c->m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c->m_next)
|
||||||
|
{
|
||||||
|
c->m_next->m_prev = c->m_prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c == m_contactList)
|
||||||
|
{
|
||||||
|
m_contactList = c->m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove from body 1
|
||||||
|
if (c->m_nodeA.prev)
|
||||||
|
{
|
||||||
|
c->m_nodeA.prev->next = c->m_nodeA.next;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c->m_nodeA.next)
|
||||||
|
{
|
||||||
|
c->m_nodeA.next->prev = c->m_nodeA.prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (&c->m_nodeA == bodyA->m_contactList)
|
||||||
|
{
|
||||||
|
bodyA->m_contactList = c->m_nodeA.next;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Remove from body 2
|
||||||
|
if (c->m_nodeB.prev)
|
||||||
|
{
|
||||||
|
c->m_nodeB.prev->next = c->m_nodeB.next;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c->m_nodeB.next)
|
||||||
|
{
|
||||||
|
c->m_nodeB.next->prev = c->m_nodeB.prev;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (&c->m_nodeB == bodyB->m_contactList)
|
||||||
|
{
|
||||||
|
bodyB->m_contactList = c->m_nodeB.next;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Call the factory.
|
||||||
|
b2Contact::Destroy(c, m_allocator);
|
||||||
|
--m_contactCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
// This is the top level collision call for the time step. Here
|
||||||
|
// all the narrow phase collision is processed for the world
|
||||||
|
// contact list.
|
||||||
|
void b2ContactManager::Collide()
|
||||||
|
{
|
||||||
|
// Update awake contacts.
|
||||||
|
b2Contact* c = m_contactList;
|
||||||
|
while (c)
|
||||||
|
{
|
||||||
|
b2Fixture* fixtureA = c->GetFixtureA();
|
||||||
|
b2Fixture* fixtureB = c->GetFixtureB();
|
||||||
|
int32 indexA = c->GetChildIndexA();
|
||||||
|
int32 indexB = c->GetChildIndexB();
|
||||||
|
b2Body* bodyA = fixtureA->GetBody();
|
||||||
|
b2Body* bodyB = fixtureB->GetBody();
|
||||||
|
|
||||||
|
// Is this contact flagged for filtering?
|
||||||
|
if (c->m_flags & b2Contact::e_filterFlag)
|
||||||
|
{
|
||||||
|
// Should these bodies collide?
|
||||||
|
if (bodyB->ShouldCollide(bodyA) == false)
|
||||||
|
{
|
||||||
|
b2Contact* cNuke = c;
|
||||||
|
c = cNuke->GetNext();
|
||||||
|
Destroy(cNuke);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check user filtering.
|
||||||
|
if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
|
||||||
|
{
|
||||||
|
b2Contact* cNuke = c;
|
||||||
|
c = cNuke->GetNext();
|
||||||
|
Destroy(cNuke);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Clear the filtering flag.
|
||||||
|
c->m_flags &= ~b2Contact::e_filterFlag;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool activeA = bodyA->IsAwake() && bodyA->m_type != b2_staticBody;
|
||||||
|
bool activeB = bodyB->IsAwake() && bodyB->m_type != b2_staticBody;
|
||||||
|
|
||||||
|
// At least one body must be awake and it must be dynamic or kinematic.
|
||||||
|
if (activeA == false && activeB == false)
|
||||||
|
{
|
||||||
|
c = c->GetNext();
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
int32 proxyIdA = fixtureA->m_proxies[indexA].proxyId;
|
||||||
|
int32 proxyIdB = fixtureB->m_proxies[indexB].proxyId;
|
||||||
|
bool overlap = m_broadPhase.TestOverlap(proxyIdA, proxyIdB);
|
||||||
|
|
||||||
|
// Here we destroy contacts that cease to overlap in the broad-phase.
|
||||||
|
if (overlap == false)
|
||||||
|
{
|
||||||
|
b2Contact* cNuke = c;
|
||||||
|
c = cNuke->GetNext();
|
||||||
|
Destroy(cNuke);
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
// The contact persists.
|
||||||
|
c->Update(m_contactListener);
|
||||||
|
c = c->GetNext();
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactManager::FindNewContacts()
|
||||||
|
{
|
||||||
|
m_broadPhase.UpdatePairs(this);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
|
||||||
|
{
|
||||||
|
b2FixtureProxy* proxyA = (b2FixtureProxy*)proxyUserDataA;
|
||||||
|
b2FixtureProxy* proxyB = (b2FixtureProxy*)proxyUserDataB;
|
||||||
|
|
||||||
|
b2Fixture* fixtureA = proxyA->fixture;
|
||||||
|
b2Fixture* fixtureB = proxyB->fixture;
|
||||||
|
|
||||||
|
int32 indexA = proxyA->childIndex;
|
||||||
|
int32 indexB = proxyB->childIndex;
|
||||||
|
|
||||||
|
b2Body* bodyA = fixtureA->GetBody();
|
||||||
|
b2Body* bodyB = fixtureB->GetBody();
|
||||||
|
|
||||||
|
// Are the fixtures on the same body?
|
||||||
|
if (bodyA == bodyB)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// TODO_ERIN use a hash table to remove a potential bottleneck when both
|
||||||
|
// bodies have a lot of contacts.
|
||||||
|
// Does a contact already exist?
|
||||||
|
b2ContactEdge* edge = bodyB->GetContactList();
|
||||||
|
while (edge)
|
||||||
|
{
|
||||||
|
if (edge->other == bodyA)
|
||||||
|
{
|
||||||
|
b2Fixture* fA = edge->contact->GetFixtureA();
|
||||||
|
b2Fixture* fB = edge->contact->GetFixtureB();
|
||||||
|
int32 iA = edge->contact->GetChildIndexA();
|
||||||
|
int32 iB = edge->contact->GetChildIndexB();
|
||||||
|
|
||||||
|
if (fA == fixtureA && fB == fixtureB && iA == indexA && iB == indexB)
|
||||||
|
{
|
||||||
|
// A contact already exists.
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (fA == fixtureB && fB == fixtureA && iA == indexB && iB == indexA)
|
||||||
|
{
|
||||||
|
// A contact already exists.
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
edge = edge->next;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Does a joint override collision? Is at least one body dynamic?
|
||||||
|
if (bodyB->ShouldCollide(bodyA) == false)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check user filtering.
|
||||||
|
if (m_contactFilter && m_contactFilter->ShouldCollide(fixtureA, fixtureB) == false)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Call the factory.
|
||||||
|
b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
|
||||||
|
if (c == nullptr)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Contact creation may swap fixtures.
|
||||||
|
fixtureA = c->GetFixtureA();
|
||||||
|
fixtureB = c->GetFixtureB();
|
||||||
|
indexA = c->GetChildIndexA();
|
||||||
|
indexB = c->GetChildIndexB();
|
||||||
|
bodyA = fixtureA->GetBody();
|
||||||
|
bodyB = fixtureB->GetBody();
|
||||||
|
|
||||||
|
// Insert into the world.
|
||||||
|
c->m_prev = nullptr;
|
||||||
|
c->m_next = m_contactList;
|
||||||
|
if (m_contactList != nullptr)
|
||||||
|
{
|
||||||
|
m_contactList->m_prev = c;
|
||||||
|
}
|
||||||
|
m_contactList = c;
|
||||||
|
|
||||||
|
// Connect to island graph.
|
||||||
|
|
||||||
|
// Connect to body A
|
||||||
|
c->m_nodeA.contact = c;
|
||||||
|
c->m_nodeA.other = bodyB;
|
||||||
|
|
||||||
|
c->m_nodeA.prev = nullptr;
|
||||||
|
c->m_nodeA.next = bodyA->m_contactList;
|
||||||
|
if (bodyA->m_contactList != nullptr)
|
||||||
|
{
|
||||||
|
bodyA->m_contactList->prev = &c->m_nodeA;
|
||||||
|
}
|
||||||
|
bodyA->m_contactList = &c->m_nodeA;
|
||||||
|
|
||||||
|
// Connect to body B
|
||||||
|
c->m_nodeB.contact = c;
|
||||||
|
c->m_nodeB.other = bodyA;
|
||||||
|
|
||||||
|
c->m_nodeB.prev = nullptr;
|
||||||
|
c->m_nodeB.next = bodyB->m_contactList;
|
||||||
|
if (bodyB->m_contactList != nullptr)
|
||||||
|
{
|
||||||
|
bodyB->m_contactList->prev = &c->m_nodeB;
|
||||||
|
}
|
||||||
|
bodyB->m_contactList = &c->m_nodeB;
|
||||||
|
|
||||||
|
// Wake up the bodies
|
||||||
|
if (fixtureA->IsSensor() == false && fixtureB->IsSensor() == false)
|
||||||
|
{
|
||||||
|
bodyA->SetAwake(true);
|
||||||
|
bodyB->SetAwake(true);
|
||||||
|
}
|
||||||
|
|
||||||
|
++m_contactCount;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,52 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_CONTACT_MANAGER_H
|
||||||
|
#define B2_CONTACT_MANAGER_H
|
||||||
|
|
||||||
|
#include "../Collision/b2BroadPhase.h"
|
||||||
|
|
||||||
|
class b2Contact;
|
||||||
|
class b2ContactFilter;
|
||||||
|
class b2ContactListener;
|
||||||
|
class b2BlockAllocator;
|
||||||
|
|
||||||
|
// Delegate of b2World.
|
||||||
|
class b2ContactManager
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2ContactManager();
|
||||||
|
|
||||||
|
// Broad-phase callback.
|
||||||
|
void AddPair(void* proxyUserDataA, void* proxyUserDataB);
|
||||||
|
|
||||||
|
void FindNewContacts();
|
||||||
|
|
||||||
|
void Destroy(b2Contact* c);
|
||||||
|
|
||||||
|
void Collide();
|
||||||
|
|
||||||
|
b2BroadPhase m_broadPhase;
|
||||||
|
b2Contact* m_contactList;
|
||||||
|
int32 m_contactCount;
|
||||||
|
b2ContactFilter* m_contactFilter;
|
||||||
|
b2ContactListener* m_contactListener;
|
||||||
|
b2BlockAllocator* m_allocator;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,303 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
#include "Box2D/Dynamics/b2World.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2CircleShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2EdgeShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2PolygonShape.h"
|
||||||
|
#include "Box2D/Collision/Shapes/b2ChainShape.h"
|
||||||
|
#include "Box2D/Collision/b2BroadPhase.h"
|
||||||
|
#include "Box2D/Collision/b2Collision.h"
|
||||||
|
#include "Box2D/Common/b2BlockAllocator.h"
|
||||||
|
|
||||||
|
b2Fixture::b2Fixture()
|
||||||
|
{
|
||||||
|
m_userData = nullptr;
|
||||||
|
m_body = nullptr;
|
||||||
|
m_next = nullptr;
|
||||||
|
m_proxies = nullptr;
|
||||||
|
m_proxyCount = 0;
|
||||||
|
m_shape = nullptr;
|
||||||
|
m_density = 0.0f;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def)
|
||||||
|
{
|
||||||
|
m_userData = def->userData;
|
||||||
|
m_friction = def->friction;
|
||||||
|
m_restitution = def->restitution;
|
||||||
|
|
||||||
|
m_body = body;
|
||||||
|
m_next = nullptr;
|
||||||
|
|
||||||
|
m_filter = def->filter;
|
||||||
|
|
||||||
|
m_isSensor = def->isSensor;
|
||||||
|
|
||||||
|
m_shape = def->shape->Clone(allocator);
|
||||||
|
|
||||||
|
// Reserve proxy space
|
||||||
|
int32 childCount = m_shape->GetChildCount();
|
||||||
|
m_proxies = (b2FixtureProxy*)allocator->Allocate(childCount * sizeof(b2FixtureProxy));
|
||||||
|
for (int32 i = 0; i < childCount; ++i)
|
||||||
|
{
|
||||||
|
m_proxies[i].fixture = nullptr;
|
||||||
|
m_proxies[i].proxyId = b2BroadPhase::e_nullProxy;
|
||||||
|
}
|
||||||
|
m_proxyCount = 0;
|
||||||
|
|
||||||
|
m_density = def->density;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::Destroy(b2BlockAllocator* allocator)
|
||||||
|
{
|
||||||
|
// The proxies must be destroyed before calling this.
|
||||||
|
b2Assert(m_proxyCount == 0);
|
||||||
|
|
||||||
|
// Free the proxy array.
|
||||||
|
int32 childCount = m_shape->GetChildCount();
|
||||||
|
allocator->Free(m_proxies, childCount * sizeof(b2FixtureProxy));
|
||||||
|
m_proxies = nullptr;
|
||||||
|
|
||||||
|
// Free the child shape.
|
||||||
|
switch (m_shape->m_type)
|
||||||
|
{
|
||||||
|
case b2Shape::e_circle:
|
||||||
|
{
|
||||||
|
b2CircleShape* s = (b2CircleShape*)m_shape;
|
||||||
|
s->~b2CircleShape();
|
||||||
|
allocator->Free(s, sizeof(b2CircleShape));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_edge:
|
||||||
|
{
|
||||||
|
b2EdgeShape* s = (b2EdgeShape*)m_shape;
|
||||||
|
s->~b2EdgeShape();
|
||||||
|
allocator->Free(s, sizeof(b2EdgeShape));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_polygon:
|
||||||
|
{
|
||||||
|
b2PolygonShape* s = (b2PolygonShape*)m_shape;
|
||||||
|
s->~b2PolygonShape();
|
||||||
|
allocator->Free(s, sizeof(b2PolygonShape));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_chain:
|
||||||
|
{
|
||||||
|
b2ChainShape* s = (b2ChainShape*)m_shape;
|
||||||
|
s->~b2ChainShape();
|
||||||
|
allocator->Free(s, sizeof(b2ChainShape));
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
b2Assert(false);
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_shape = nullptr;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf)
|
||||||
|
{
|
||||||
|
b2Assert(m_proxyCount == 0);
|
||||||
|
|
||||||
|
// Create proxies in the broad-phase.
|
||||||
|
m_proxyCount = m_shape->GetChildCount();
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_proxyCount; ++i)
|
||||||
|
{
|
||||||
|
b2FixtureProxy* proxy = m_proxies + i;
|
||||||
|
m_shape->ComputeAABB(&proxy->aabb, xf, i);
|
||||||
|
proxy->proxyId = broadPhase->CreateProxy(proxy->aabb, proxy);
|
||||||
|
proxy->fixture = this;
|
||||||
|
proxy->childIndex = i;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::DestroyProxies(b2BroadPhase* broadPhase)
|
||||||
|
{
|
||||||
|
// Destroy proxies in the broad-phase.
|
||||||
|
for (int32 i = 0; i < m_proxyCount; ++i)
|
||||||
|
{
|
||||||
|
b2FixtureProxy* proxy = m_proxies + i;
|
||||||
|
broadPhase->DestroyProxy(proxy->proxyId);
|
||||||
|
proxy->proxyId = b2BroadPhase::e_nullProxy;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_proxyCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transform1, const b2Transform& transform2)
|
||||||
|
{
|
||||||
|
if (m_proxyCount == 0)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_proxyCount; ++i)
|
||||||
|
{
|
||||||
|
b2FixtureProxy* proxy = m_proxies + i;
|
||||||
|
|
||||||
|
// Compute an AABB that covers the swept shape (may miss some rotation effect).
|
||||||
|
b2AABB aabb1, aabb2;
|
||||||
|
m_shape->ComputeAABB(&aabb1, transform1, proxy->childIndex);
|
||||||
|
m_shape->ComputeAABB(&aabb2, transform2, proxy->childIndex);
|
||||||
|
|
||||||
|
proxy->aabb.Combine(aabb1, aabb2);
|
||||||
|
|
||||||
|
b2Vec2 displacement = transform2.p - transform1.p;
|
||||||
|
|
||||||
|
broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::SetFilterData(const b2Filter& filter)
|
||||||
|
{
|
||||||
|
m_filter = filter;
|
||||||
|
|
||||||
|
Refilter();
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::Refilter()
|
||||||
|
{
|
||||||
|
if (m_body == nullptr)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Flag associated contacts for filtering.
|
||||||
|
b2ContactEdge* edge = m_body->GetContactList();
|
||||||
|
while (edge)
|
||||||
|
{
|
||||||
|
b2Contact* contact = edge->contact;
|
||||||
|
b2Fixture* fixtureA = contact->GetFixtureA();
|
||||||
|
b2Fixture* fixtureB = contact->GetFixtureB();
|
||||||
|
if (fixtureA == this || fixtureB == this)
|
||||||
|
{
|
||||||
|
contact->FlagForFiltering();
|
||||||
|
}
|
||||||
|
|
||||||
|
edge = edge->next;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2World* world = m_body->GetWorld();
|
||||||
|
|
||||||
|
if (world == nullptr)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Touch each proxy so that new pairs may be created
|
||||||
|
b2BroadPhase* broadPhase = &world->m_contactManager.m_broadPhase;
|
||||||
|
for (int32 i = 0; i < m_proxyCount; ++i)
|
||||||
|
{
|
||||||
|
broadPhase->TouchProxy(m_proxies[i].proxyId);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::SetSensor(bool sensor)
|
||||||
|
{
|
||||||
|
if (sensor != m_isSensor)
|
||||||
|
{
|
||||||
|
m_body->SetAwake(true);
|
||||||
|
m_isSensor = sensor;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Fixture::Dump(int32 bodyIndex)
|
||||||
|
{
|
||||||
|
b2Log(" b2FixtureDef fd;\n");
|
||||||
|
b2Log(" fd.friction = %.15lef;\n", m_friction);
|
||||||
|
b2Log(" fd.restitution = %.15lef;\n", m_restitution);
|
||||||
|
b2Log(" fd.density = %.15lef;\n", m_density);
|
||||||
|
b2Log(" fd.isSensor = bool(%d);\n", m_isSensor);
|
||||||
|
b2Log(" fd.filter.categoryBits = uint16(%d);\n", m_filter.categoryBits);
|
||||||
|
b2Log(" fd.filter.maskBits = uint16(%d);\n", m_filter.maskBits);
|
||||||
|
b2Log(" fd.filter.groupIndex = int16(%d);\n", m_filter.groupIndex);
|
||||||
|
|
||||||
|
switch (m_shape->m_type)
|
||||||
|
{
|
||||||
|
case b2Shape::e_circle:
|
||||||
|
{
|
||||||
|
b2CircleShape* s = (b2CircleShape*)m_shape;
|
||||||
|
b2Log(" b2CircleShape shape;\n");
|
||||||
|
b2Log(" shape.m_radius = %.15lef;\n", s->m_radius);
|
||||||
|
b2Log(" shape.m_p.Set(%.15lef, %.15lef);\n", s->m_p.x, s->m_p.y);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_edge:
|
||||||
|
{
|
||||||
|
b2EdgeShape* s = (b2EdgeShape*)m_shape;
|
||||||
|
b2Log(" b2EdgeShape shape;\n");
|
||||||
|
b2Log(" shape.m_radius = %.15lef;\n", s->m_radius);
|
||||||
|
b2Log(" shape.m_vertex0.Set(%.15lef, %.15lef);\n", s->m_vertex0.x, s->m_vertex0.y);
|
||||||
|
b2Log(" shape.m_vertex1.Set(%.15lef, %.15lef);\n", s->m_vertex1.x, s->m_vertex1.y);
|
||||||
|
b2Log(" shape.m_vertex2.Set(%.15lef, %.15lef);\n", s->m_vertex2.x, s->m_vertex2.y);
|
||||||
|
b2Log(" shape.m_vertex3.Set(%.15lef, %.15lef);\n", s->m_vertex3.x, s->m_vertex3.y);
|
||||||
|
b2Log(" shape.m_hasVertex0 = bool(%d);\n", s->m_hasVertex0);
|
||||||
|
b2Log(" shape.m_hasVertex3 = bool(%d);\n", s->m_hasVertex3);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_polygon:
|
||||||
|
{
|
||||||
|
b2PolygonShape* s = (b2PolygonShape*)m_shape;
|
||||||
|
b2Log(" b2PolygonShape shape;\n");
|
||||||
|
b2Log(" b2Vec2 vs[%d];\n", b2_maxPolygonVertices);
|
||||||
|
for (int32 i = 0; i < s->m_count; ++i)
|
||||||
|
{
|
||||||
|
b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
|
||||||
|
}
|
||||||
|
b2Log(" shape.Set(vs, %d);\n", s->m_count);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
case b2Shape::e_chain:
|
||||||
|
{
|
||||||
|
b2ChainShape* s = (b2ChainShape*)m_shape;
|
||||||
|
b2Log(" b2ChainShape shape;\n");
|
||||||
|
b2Log(" b2Vec2 vs[%d];\n", s->m_count);
|
||||||
|
for (int32 i = 0; i < s->m_count; ++i)
|
||||||
|
{
|
||||||
|
b2Log(" vs[%d].Set(%.15lef, %.15lef);\n", i, s->m_vertices[i].x, s->m_vertices[i].y);
|
||||||
|
}
|
||||||
|
b2Log(" shape.CreateChain(vs, %d);\n", s->m_count);
|
||||||
|
b2Log(" shape.m_prevVertex.Set(%.15lef, %.15lef);\n", s->m_prevVertex.x, s->m_prevVertex.y);
|
||||||
|
b2Log(" shape.m_nextVertex.Set(%.15lef, %.15lef);\n", s->m_nextVertex.x, s->m_nextVertex.y);
|
||||||
|
b2Log(" shape.m_hasPrevVertex = bool(%d);\n", s->m_hasPrevVertex);
|
||||||
|
b2Log(" shape.m_hasNextVertex = bool(%d);\n", s->m_hasNextVertex);
|
||||||
|
}
|
||||||
|
break;
|
||||||
|
|
||||||
|
default:
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Log("\n");
|
||||||
|
b2Log(" fd.shape = &shape;\n");
|
||||||
|
b2Log("\n");
|
||||||
|
b2Log(" bodies[%d]->CreateFixture(&fd);\n", bodyIndex);
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,345 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_FIXTURE_H
|
||||||
|
#define B2_FIXTURE_H
|
||||||
|
|
||||||
|
#include "../Dynamics/b2Body.h"
|
||||||
|
#include "../Collision/b2Collision.h"
|
||||||
|
#include "../Collision/Shapes/b2Shape.h"
|
||||||
|
|
||||||
|
class b2BlockAllocator;
|
||||||
|
class b2Body;
|
||||||
|
class b2BroadPhase;
|
||||||
|
class b2Fixture;
|
||||||
|
|
||||||
|
/// This holds contact filtering data.
|
||||||
|
struct b2Filter
|
||||||
|
{
|
||||||
|
b2Filter()
|
||||||
|
{
|
||||||
|
categoryBits = 0x0001;
|
||||||
|
maskBits = 0xFFFF;
|
||||||
|
groupIndex = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The collision category bits. Normally you would just set one bit.
|
||||||
|
uint16 categoryBits;
|
||||||
|
|
||||||
|
/// The collision mask bits. This states the categories that this
|
||||||
|
/// shape would accept for collision.
|
||||||
|
uint16 maskBits;
|
||||||
|
|
||||||
|
/// Collision groups allow a certain group of objects to never collide (negative)
|
||||||
|
/// or always collide (positive). Zero means no collision group. Non-zero group
|
||||||
|
/// filtering always wins against the mask bits.
|
||||||
|
int16 groupIndex;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A fixture definition is used to create a fixture. This class defines an
|
||||||
|
/// abstract fixture definition. You can reuse fixture definitions safely.
|
||||||
|
struct b2FixtureDef
|
||||||
|
{
|
||||||
|
/// The constructor sets the default fixture definition values.
|
||||||
|
b2FixtureDef()
|
||||||
|
{
|
||||||
|
shape = nullptr;
|
||||||
|
userData = nullptr;
|
||||||
|
friction = 0.2f;
|
||||||
|
restitution = 0.0f;
|
||||||
|
density = 0.0f;
|
||||||
|
isSensor = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
/// The shape, this must be set. The shape will be cloned, so you
|
||||||
|
/// can create the shape on the stack.
|
||||||
|
const b2Shape* shape;
|
||||||
|
|
||||||
|
/// Use this to store application specific fixture data.
|
||||||
|
void* userData;
|
||||||
|
|
||||||
|
/// The friction coefficient, usually in the range [0,1].
|
||||||
|
float32 friction;
|
||||||
|
|
||||||
|
/// The restitution (elasticity) usually in the range [0,1].
|
||||||
|
float32 restitution;
|
||||||
|
|
||||||
|
/// The density, usually in kg/m^2.
|
||||||
|
float32 density;
|
||||||
|
|
||||||
|
/// A sensor shape collects contact information but never generates a collision
|
||||||
|
/// response.
|
||||||
|
bool isSensor;
|
||||||
|
|
||||||
|
/// Contact filtering data.
|
||||||
|
b2Filter filter;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This proxy is used internally to connect fixtures to the broad-phase.
|
||||||
|
struct b2FixtureProxy
|
||||||
|
{
|
||||||
|
b2AABB aabb;
|
||||||
|
b2Fixture* fixture;
|
||||||
|
int32 childIndex;
|
||||||
|
int32 proxyId;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// A fixture is used to attach a shape to a body for collision detection. A fixture
|
||||||
|
/// inherits its transform from its parent. Fixtures hold additional non-geometric data
|
||||||
|
/// such as friction, collision filters, etc.
|
||||||
|
/// Fixtures are created via b2Body::CreateFixture.
|
||||||
|
/// @warning you cannot reuse fixtures.
|
||||||
|
class b2Fixture
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/// Get the type of the child shape. You can use this to down cast to the concrete shape.
|
||||||
|
/// @return the shape type.
|
||||||
|
b2Shape::Type GetType() const;
|
||||||
|
|
||||||
|
/// Get the child shape. You can modify the child shape, however you should not change the
|
||||||
|
/// number of vertices because this will crash some collision caching mechanisms.
|
||||||
|
/// Manipulating the shape may lead to non-physical behavior.
|
||||||
|
b2Shape* GetShape();
|
||||||
|
const b2Shape* GetShape() const;
|
||||||
|
|
||||||
|
/// Set if this fixture is a sensor.
|
||||||
|
void SetSensor(bool sensor);
|
||||||
|
|
||||||
|
/// Is this fixture a sensor (non-solid)?
|
||||||
|
/// @return the true if the shape is a sensor.
|
||||||
|
bool IsSensor() const;
|
||||||
|
|
||||||
|
/// Set the contact filtering data. This will not update contacts until the next time
|
||||||
|
/// step when either parent body is active and awake.
|
||||||
|
/// This automatically calls Refilter.
|
||||||
|
void SetFilterData(const b2Filter& filter);
|
||||||
|
|
||||||
|
/// Get the contact filtering data.
|
||||||
|
const b2Filter& GetFilterData() const;
|
||||||
|
|
||||||
|
/// Call this if you want to establish collision that was previously disabled by b2ContactFilter::ShouldCollide.
|
||||||
|
void Refilter();
|
||||||
|
|
||||||
|
/// Get the parent body of this fixture. This is nullptr if the fixture is not attached.
|
||||||
|
/// @return the parent body.
|
||||||
|
b2Body* GetBody();
|
||||||
|
const b2Body* GetBody() const;
|
||||||
|
|
||||||
|
/// Get the next fixture in the parent body's fixture list.
|
||||||
|
/// @return the next shape.
|
||||||
|
b2Fixture* GetNext();
|
||||||
|
const b2Fixture* GetNext() const;
|
||||||
|
|
||||||
|
/// Get the user data that was assigned in the fixture definition. Use this to
|
||||||
|
/// store your application specific data.
|
||||||
|
void* GetUserData() const;
|
||||||
|
|
||||||
|
/// Set the user data. Use this to store your application specific data.
|
||||||
|
void SetUserData(void* data);
|
||||||
|
|
||||||
|
/// Test a point for containment in this fixture.
|
||||||
|
/// @param p a point in world coordinates.
|
||||||
|
bool TestPoint(const b2Vec2& p) const;
|
||||||
|
|
||||||
|
/// Cast a ray against this shape.
|
||||||
|
/// @param output the ray-cast results.
|
||||||
|
/// @param input the ray-cast input parameters.
|
||||||
|
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const;
|
||||||
|
|
||||||
|
/// Get the mass data for this fixture. The mass data is based on the density and
|
||||||
|
/// the shape. The rotational inertia is about the shape's origin. This operation
|
||||||
|
/// may be expensive.
|
||||||
|
void GetMassData(b2MassData* massData) const;
|
||||||
|
|
||||||
|
/// Set the density of this fixture. This will _not_ automatically adjust the mass
|
||||||
|
/// of the body. You must call b2Body::ResetMassData to update the body's mass.
|
||||||
|
void SetDensity(float32 density);
|
||||||
|
|
||||||
|
/// Get the density of this fixture.
|
||||||
|
float32 GetDensity() const;
|
||||||
|
|
||||||
|
/// Get the coefficient of friction.
|
||||||
|
float32 GetFriction() const;
|
||||||
|
|
||||||
|
/// Set the coefficient of friction. This will _not_ change the friction of
|
||||||
|
/// existing contacts.
|
||||||
|
void SetFriction(float32 friction);
|
||||||
|
|
||||||
|
/// Get the coefficient of restitution.
|
||||||
|
float32 GetRestitution() const;
|
||||||
|
|
||||||
|
/// Set the coefficient of restitution. This will _not_ change the restitution of
|
||||||
|
/// existing contacts.
|
||||||
|
void SetRestitution(float32 restitution);
|
||||||
|
|
||||||
|
/// Get the fixture's AABB. This AABB may be enlarge and/or stale.
|
||||||
|
/// If you need a more accurate AABB, compute it using the shape and
|
||||||
|
/// the body transform.
|
||||||
|
const b2AABB& GetAABB(int32 childIndex) const;
|
||||||
|
|
||||||
|
/// Dump this fixture to the log file.
|
||||||
|
void Dump(int32 bodyIndex);
|
||||||
|
|
||||||
|
protected:
|
||||||
|
|
||||||
|
friend class b2Body;
|
||||||
|
friend class b2World;
|
||||||
|
friend class b2Contact;
|
||||||
|
friend class b2ContactManager;
|
||||||
|
|
||||||
|
b2Fixture();
|
||||||
|
|
||||||
|
// We need separation create/destroy functions from the constructor/destructor because
|
||||||
|
// the destructor cannot access the allocator (no destructor arguments allowed by C++).
|
||||||
|
void Create(b2BlockAllocator* allocator, b2Body* body, const b2FixtureDef* def);
|
||||||
|
void Destroy(b2BlockAllocator* allocator);
|
||||||
|
|
||||||
|
// These support body activation/deactivation.
|
||||||
|
void CreateProxies(b2BroadPhase* broadPhase, const b2Transform& xf);
|
||||||
|
void DestroyProxies(b2BroadPhase* broadPhase);
|
||||||
|
|
||||||
|
void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
|
||||||
|
|
||||||
|
float32 m_density;
|
||||||
|
|
||||||
|
b2Fixture* m_next;
|
||||||
|
b2Body* m_body;
|
||||||
|
|
||||||
|
b2Shape* m_shape;
|
||||||
|
|
||||||
|
float32 m_friction;
|
||||||
|
float32 m_restitution;
|
||||||
|
|
||||||
|
b2FixtureProxy* m_proxies;
|
||||||
|
int32 m_proxyCount;
|
||||||
|
|
||||||
|
b2Filter m_filter;
|
||||||
|
|
||||||
|
bool m_isSensor;
|
||||||
|
|
||||||
|
void* m_userData;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2Shape::Type b2Fixture::GetType() const
|
||||||
|
{
|
||||||
|
return m_shape->GetType();
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Shape* b2Fixture::GetShape()
|
||||||
|
{
|
||||||
|
return m_shape;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Shape* b2Fixture::GetShape() const
|
||||||
|
{
|
||||||
|
return m_shape;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Fixture::IsSensor() const
|
||||||
|
{
|
||||||
|
return m_isSensor;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Filter& b2Fixture::GetFilterData() const
|
||||||
|
{
|
||||||
|
return m_filter;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void* b2Fixture::GetUserData() const
|
||||||
|
{
|
||||||
|
return m_userData;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Fixture::SetUserData(void* data)
|
||||||
|
{
|
||||||
|
m_userData = data;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Body* b2Fixture::GetBody()
|
||||||
|
{
|
||||||
|
return m_body;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Body* b2Fixture::GetBody() const
|
||||||
|
{
|
||||||
|
return m_body;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Fixture* b2Fixture::GetNext()
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Fixture* b2Fixture::GetNext() const
|
||||||
|
{
|
||||||
|
return m_next;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Fixture::SetDensity(float32 density)
|
||||||
|
{
|
||||||
|
b2Assert(b2IsValid(density) && density >= 0.0f);
|
||||||
|
m_density = density;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Fixture::GetDensity() const
|
||||||
|
{
|
||||||
|
return m_density;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Fixture::GetFriction() const
|
||||||
|
{
|
||||||
|
return m_friction;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Fixture::SetFriction(float32 friction)
|
||||||
|
{
|
||||||
|
m_friction = friction;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline float32 b2Fixture::GetRestitution() const
|
||||||
|
{
|
||||||
|
return m_restitution;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Fixture::SetRestitution(float32 restitution)
|
||||||
|
{
|
||||||
|
m_restitution = restitution;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Fixture::TestPoint(const b2Vec2& p) const
|
||||||
|
{
|
||||||
|
return m_shape->TestPoint(m_body->GetTransform(), p);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2Fixture::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, int32 childIndex) const
|
||||||
|
{
|
||||||
|
return m_shape->RayCast(output, input, m_body->GetTransform(), childIndex);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2Fixture::GetMassData(b2MassData* massData) const
|
||||||
|
{
|
||||||
|
m_shape->ComputeMass(massData, m_density);
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2AABB& b2Fixture::GetAABB(int32 childIndex) const
|
||||||
|
{
|
||||||
|
b2Assert(0 <= childIndex && childIndex < m_proxyCount);
|
||||||
|
return m_proxies[childIndex].aabb;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,539 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Collision/b2Distance.h"
|
||||||
|
#include "Box2D/Dynamics/b2Island.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
#include "Box2D/Dynamics/b2World.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2Contact.h"
|
||||||
|
#include "Box2D/Dynamics/Contacts/b2ContactSolver.h"
|
||||||
|
#include "Box2D/Dynamics/Joints/b2Joint.h"
|
||||||
|
#include "Box2D/Common/b2StackAllocator.h"
|
||||||
|
#include "Box2D/Common/b2Timer.h"
|
||||||
|
|
||||||
|
/*
|
||||||
|
Position Correction Notes
|
||||||
|
=========================
|
||||||
|
I tried the several algorithms for position correction of the 2D revolute joint.
|
||||||
|
I looked at these systems:
|
||||||
|
- simple pendulum (1m diameter sphere on massless 5m stick) with initial angular velocity of 100 rad/s.
|
||||||
|
- suspension bridge with 30 1m long planks of length 1m.
|
||||||
|
- multi-link chain with 30 1m long links.
|
||||||
|
|
||||||
|
Here are the algorithms:
|
||||||
|
|
||||||
|
Baumgarte - A fraction of the position error is added to the velocity error. There is no
|
||||||
|
separate position solver.
|
||||||
|
|
||||||
|
Pseudo Velocities - After the velocity solver and position integration,
|
||||||
|
the position error, Jacobian, and effective mass are recomputed. Then
|
||||||
|
the velocity constraints are solved with pseudo velocities and a fraction
|
||||||
|
of the position error is added to the pseudo velocity error. The pseudo
|
||||||
|
velocities are initialized to zero and there is no warm-starting. After
|
||||||
|
the position solver, the pseudo velocities are added to the positions.
|
||||||
|
This is also called the First Order World method or the Position LCP method.
|
||||||
|
|
||||||
|
Modified Nonlinear Gauss-Seidel (NGS) - Like Pseudo Velocities except the
|
||||||
|
position error is re-computed for each constraint and the positions are updated
|
||||||
|
after the constraint is solved. The radius vectors (aka Jacobians) are
|
||||||
|
re-computed too (otherwise the algorithm has horrible instability). The pseudo
|
||||||
|
velocity states are not needed because they are effectively zero at the beginning
|
||||||
|
of each iteration. Since we have the current position error, we allow the
|
||||||
|
iterations to terminate early if the error becomes smaller than b2_linearSlop.
|
||||||
|
|
||||||
|
Full NGS or just NGS - Like Modified NGS except the effective mass are re-computed
|
||||||
|
each time a constraint is solved.
|
||||||
|
|
||||||
|
Here are the results:
|
||||||
|
Baumgarte - this is the cheapest algorithm but it has some stability problems,
|
||||||
|
especially with the bridge. The chain links separate easily close to the root
|
||||||
|
and they jitter as they struggle to pull together. This is one of the most common
|
||||||
|
methods in the field. The big drawback is that the position correction artificially
|
||||||
|
affects the momentum, thus leading to instabilities and false bounce. I used a
|
||||||
|
bias factor of 0.2. A larger bias factor makes the bridge less stable, a smaller
|
||||||
|
factor makes joints and contacts more spongy.
|
||||||
|
|
||||||
|
Pseudo Velocities - the is more stable than the Baumgarte method. The bridge is
|
||||||
|
stable. However, joints still separate with large angular velocities. Drag the
|
||||||
|
simple pendulum in a circle quickly and the joint will separate. The chain separates
|
||||||
|
easily and does not recover. I used a bias factor of 0.2. A larger value lead to
|
||||||
|
the bridge collapsing when a heavy cube drops on it.
|
||||||
|
|
||||||
|
Modified NGS - this algorithm is better in some ways than Baumgarte and Pseudo
|
||||||
|
Velocities, but in other ways it is worse. The bridge and chain are much more
|
||||||
|
stable, but the simple pendulum goes unstable at high angular velocities.
|
||||||
|
|
||||||
|
Full NGS - stable in all tests. The joints display good stiffness. The bridge
|
||||||
|
still sags, but this is better than infinite forces.
|
||||||
|
|
||||||
|
Recommendations
|
||||||
|
Pseudo Velocities are not really worthwhile because the bridge and chain cannot
|
||||||
|
recover from joint separation. In other cases the benefit over Baumgarte is small.
|
||||||
|
|
||||||
|
Modified NGS is not a robust method for the revolute joint due to the violent
|
||||||
|
instability seen in the simple pendulum. Perhaps it is viable with other constraint
|
||||||
|
types, especially scalar constraints where the effective mass is a scalar.
|
||||||
|
|
||||||
|
This leaves Baumgarte and Full NGS. Baumgarte has small, but manageable instabilities
|
||||||
|
and is very fast. I don't think we can escape Baumgarte, especially in highly
|
||||||
|
demanding cases where high constraint fidelity is not needed.
|
||||||
|
|
||||||
|
Full NGS is robust and easy on the eyes. I recommend this as an option for
|
||||||
|
higher fidelity simulation and certainly for suspension bridges and long chains.
|
||||||
|
Full NGS might be a good choice for ragdolls, especially motorized ragdolls where
|
||||||
|
joint separation can be problematic. The number of NGS iterations can be reduced
|
||||||
|
for better performance without harming robustness much.
|
||||||
|
|
||||||
|
Each joint in a can be handled differently in the position solver. So I recommend
|
||||||
|
a system where the user can select the algorithm on a per joint basis. I would
|
||||||
|
probably default to the slower Full NGS and let the user select the faster
|
||||||
|
Baumgarte method in performance critical scenarios.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
Cache Performance
|
||||||
|
|
||||||
|
The Box2D solvers are dominated by cache misses. Data structures are designed
|
||||||
|
to increase the number of cache hits. Much of misses are due to random access
|
||||||
|
to body data. The constraint structures are iterated over linearly, which leads
|
||||||
|
to few cache misses.
|
||||||
|
|
||||||
|
The bodies are not accessed during iteration. Instead read only data, such as
|
||||||
|
the mass values are stored with the constraints. The mutable data are the constraint
|
||||||
|
impulses and the bodies velocities/positions. The impulses are held inside the
|
||||||
|
constraint structures. The body velocities/positions are held in compact, temporary
|
||||||
|
arrays to increase the number of cache hits. Linear and angular velocity are
|
||||||
|
stored in a single array since multiple arrays lead to multiple misses.
|
||||||
|
*/
|
||||||
|
|
||||||
|
/*
|
||||||
|
2D Rotation
|
||||||
|
|
||||||
|
R = [cos(theta) -sin(theta)]
|
||||||
|
[sin(theta) cos(theta) ]
|
||||||
|
|
||||||
|
thetaDot = omega
|
||||||
|
|
||||||
|
Let q1 = cos(theta), q2 = sin(theta).
|
||||||
|
R = [q1 -q2]
|
||||||
|
[q2 q1]
|
||||||
|
|
||||||
|
q1Dot = -thetaDot * q2
|
||||||
|
q2Dot = thetaDot * q1
|
||||||
|
|
||||||
|
q1_new = q1_old - dt * w * q2
|
||||||
|
q2_new = q2_old + dt * w * q1
|
||||||
|
then normalize.
|
||||||
|
|
||||||
|
This might be faster than computing sin+cos.
|
||||||
|
However, we can compute sin+cos of the same angle fast.
|
||||||
|
*/
|
||||||
|
|
||||||
|
b2Island::b2Island(
|
||||||
|
int32 bodyCapacity,
|
||||||
|
int32 contactCapacity,
|
||||||
|
int32 jointCapacity,
|
||||||
|
b2StackAllocator* allocator,
|
||||||
|
b2ContactListener* listener)
|
||||||
|
{
|
||||||
|
m_bodyCapacity = bodyCapacity;
|
||||||
|
m_contactCapacity = contactCapacity;
|
||||||
|
m_jointCapacity = jointCapacity;
|
||||||
|
m_bodyCount = 0;
|
||||||
|
m_contactCount = 0;
|
||||||
|
m_jointCount = 0;
|
||||||
|
|
||||||
|
m_allocator = allocator;
|
||||||
|
m_listener = listener;
|
||||||
|
|
||||||
|
m_bodies = (b2Body**)m_allocator->Allocate(bodyCapacity * sizeof(b2Body*));
|
||||||
|
m_contacts = (b2Contact**)m_allocator->Allocate(contactCapacity * sizeof(b2Contact*));
|
||||||
|
m_joints = (b2Joint**)m_allocator->Allocate(jointCapacity * sizeof(b2Joint*));
|
||||||
|
|
||||||
|
m_velocities = (b2Velocity*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Velocity));
|
||||||
|
m_positions = (b2Position*)m_allocator->Allocate(m_bodyCapacity * sizeof(b2Position));
|
||||||
|
}
|
||||||
|
|
||||||
|
b2Island::~b2Island()
|
||||||
|
{
|
||||||
|
// Warning: the order should reverse the constructor order.
|
||||||
|
m_allocator->Free(m_positions);
|
||||||
|
m_allocator->Free(m_velocities);
|
||||||
|
m_allocator->Free(m_joints);
|
||||||
|
m_allocator->Free(m_contacts);
|
||||||
|
m_allocator->Free(m_bodies);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
|
||||||
|
{
|
||||||
|
b2Timer timer;
|
||||||
|
|
||||||
|
float32 h = step.dt;
|
||||||
|
|
||||||
|
// Integrate velocities and apply damping. Initialize the body state.
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Body* b = m_bodies[i];
|
||||||
|
|
||||||
|
b2Vec2 c = b->m_sweep.c;
|
||||||
|
float32 a = b->m_sweep.a;
|
||||||
|
b2Vec2 v = b->m_linearVelocity;
|
||||||
|
float32 w = b->m_angularVelocity;
|
||||||
|
|
||||||
|
// Store positions for continuous collision.
|
||||||
|
b->m_sweep.c0 = b->m_sweep.c;
|
||||||
|
b->m_sweep.a0 = b->m_sweep.a;
|
||||||
|
|
||||||
|
if (b->m_type == b2_dynamicBody)
|
||||||
|
{
|
||||||
|
// Integrate velocities.
|
||||||
|
v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
|
||||||
|
w += h * b->m_invI * b->m_torque;
|
||||||
|
|
||||||
|
// Apply damping.
|
||||||
|
// ODE: dv/dt + c * v = 0
|
||||||
|
// Solution: v(t) = v0 * exp(-c * t)
|
||||||
|
// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
|
||||||
|
// v2 = exp(-c * dt) * v1
|
||||||
|
// Pade approximation:
|
||||||
|
// v2 = v1 * 1 / (1 + c * dt)
|
||||||
|
v *= 1.0f / (1.0f + h * b->m_linearDamping);
|
||||||
|
w *= 1.0f / (1.0f + h * b->m_angularDamping);
|
||||||
|
}
|
||||||
|
|
||||||
|
m_positions[i].c = c;
|
||||||
|
m_positions[i].a = a;
|
||||||
|
m_velocities[i].v = v;
|
||||||
|
m_velocities[i].w = w;
|
||||||
|
}
|
||||||
|
|
||||||
|
timer.Reset();
|
||||||
|
|
||||||
|
// Solver data
|
||||||
|
b2SolverData solverData;
|
||||||
|
solverData.step = step;
|
||||||
|
solverData.positions = m_positions;
|
||||||
|
solverData.velocities = m_velocities;
|
||||||
|
|
||||||
|
// Initialize velocity constraints.
|
||||||
|
b2ContactSolverDef contactSolverDef;
|
||||||
|
contactSolverDef.step = step;
|
||||||
|
contactSolverDef.contacts = m_contacts;
|
||||||
|
contactSolverDef.count = m_contactCount;
|
||||||
|
contactSolverDef.positions = m_positions;
|
||||||
|
contactSolverDef.velocities = m_velocities;
|
||||||
|
contactSolverDef.allocator = m_allocator;
|
||||||
|
|
||||||
|
b2ContactSolver contactSolver(&contactSolverDef);
|
||||||
|
contactSolver.InitializeVelocityConstraints();
|
||||||
|
|
||||||
|
if (step.warmStarting)
|
||||||
|
{
|
||||||
|
contactSolver.WarmStart();
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_jointCount; ++i)
|
||||||
|
{
|
||||||
|
m_joints[i]->InitVelocityConstraints(solverData);
|
||||||
|
}
|
||||||
|
|
||||||
|
profile->solveInit = timer.GetMilliseconds();
|
||||||
|
|
||||||
|
// Solve velocity constraints
|
||||||
|
timer.Reset();
|
||||||
|
for (int32 i = 0; i < step.velocityIterations; ++i)
|
||||||
|
{
|
||||||
|
for (int32 j = 0; j < m_jointCount; ++j)
|
||||||
|
{
|
||||||
|
m_joints[j]->SolveVelocityConstraints(solverData);
|
||||||
|
}
|
||||||
|
|
||||||
|
contactSolver.SolveVelocityConstraints();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Store impulses for warm starting
|
||||||
|
contactSolver.StoreImpulses();
|
||||||
|
profile->solveVelocity = timer.GetMilliseconds();
|
||||||
|
|
||||||
|
// Integrate positions
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 c = m_positions[i].c;
|
||||||
|
float32 a = m_positions[i].a;
|
||||||
|
b2Vec2 v = m_velocities[i].v;
|
||||||
|
float32 w = m_velocities[i].w;
|
||||||
|
|
||||||
|
// Check for large velocities
|
||||||
|
b2Vec2 translation = h * v;
|
||||||
|
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
|
||||||
|
{
|
||||||
|
float32 ratio = b2_maxTranslation / translation.Length();
|
||||||
|
v *= ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 rotation = h * w;
|
||||||
|
if (rotation * rotation > b2_maxRotationSquared)
|
||||||
|
{
|
||||||
|
float32 ratio = b2_maxRotation / b2Abs(rotation);
|
||||||
|
w *= ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Integrate
|
||||||
|
c += h * v;
|
||||||
|
a += h * w;
|
||||||
|
|
||||||
|
m_positions[i].c = c;
|
||||||
|
m_positions[i].a = a;
|
||||||
|
m_velocities[i].v = v;
|
||||||
|
m_velocities[i].w = w;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Solve position constraints
|
||||||
|
timer.Reset();
|
||||||
|
bool positionSolved = false;
|
||||||
|
for (int32 i = 0; i < step.positionIterations; ++i)
|
||||||
|
{
|
||||||
|
bool contactsOkay = contactSolver.SolvePositionConstraints();
|
||||||
|
|
||||||
|
bool jointsOkay = true;
|
||||||
|
for (int32 j = 0; j < m_jointCount; ++j)
|
||||||
|
{
|
||||||
|
bool jointOkay = m_joints[j]->SolvePositionConstraints(solverData);
|
||||||
|
jointsOkay = jointsOkay && jointOkay;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (contactsOkay && jointsOkay)
|
||||||
|
{
|
||||||
|
// Exit early if the position errors are small.
|
||||||
|
positionSolved = true;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
// Copy state buffers back to the bodies
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Body* body = m_bodies[i];
|
||||||
|
body->m_sweep.c = m_positions[i].c;
|
||||||
|
body->m_sweep.a = m_positions[i].a;
|
||||||
|
body->m_linearVelocity = m_velocities[i].v;
|
||||||
|
body->m_angularVelocity = m_velocities[i].w;
|
||||||
|
body->SynchronizeTransform();
|
||||||
|
}
|
||||||
|
|
||||||
|
profile->solvePosition = timer.GetMilliseconds();
|
||||||
|
|
||||||
|
Report(contactSolver.m_velocityConstraints);
|
||||||
|
|
||||||
|
if (allowSleep)
|
||||||
|
{
|
||||||
|
float32 minSleepTime = b2_maxFloat;
|
||||||
|
|
||||||
|
const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
|
||||||
|
const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Body* b = m_bodies[i];
|
||||||
|
if (b->GetType() == b2_staticBody)
|
||||||
|
{
|
||||||
|
continue;
|
||||||
|
}
|
||||||
|
|
||||||
|
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
|
||||||
|
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
|
||||||
|
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
|
||||||
|
{
|
||||||
|
b->m_sleepTime = 0.0f;
|
||||||
|
minSleepTime = 0.0f;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
b->m_sleepTime += h;
|
||||||
|
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (minSleepTime >= b2_timeToSleep && positionSolved)
|
||||||
|
{
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Body* b = m_bodies[i];
|
||||||
|
b->SetAwake(false);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
|
||||||
|
{
|
||||||
|
b2Assert(toiIndexA < m_bodyCount);
|
||||||
|
b2Assert(toiIndexB < m_bodyCount);
|
||||||
|
|
||||||
|
// Initialize the body state.
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Body* b = m_bodies[i];
|
||||||
|
m_positions[i].c = b->m_sweep.c;
|
||||||
|
m_positions[i].a = b->m_sweep.a;
|
||||||
|
m_velocities[i].v = b->m_linearVelocity;
|
||||||
|
m_velocities[i].w = b->m_angularVelocity;
|
||||||
|
}
|
||||||
|
|
||||||
|
b2ContactSolverDef contactSolverDef;
|
||||||
|
contactSolverDef.contacts = m_contacts;
|
||||||
|
contactSolverDef.count = m_contactCount;
|
||||||
|
contactSolverDef.allocator = m_allocator;
|
||||||
|
contactSolverDef.step = subStep;
|
||||||
|
contactSolverDef.positions = m_positions;
|
||||||
|
contactSolverDef.velocities = m_velocities;
|
||||||
|
b2ContactSolver contactSolver(&contactSolverDef);
|
||||||
|
|
||||||
|
// Solve position constraints.
|
||||||
|
for (int32 i = 0; i < subStep.positionIterations; ++i)
|
||||||
|
{
|
||||||
|
bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
|
||||||
|
if (contactsOkay)
|
||||||
|
{
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
#if 0
|
||||||
|
// Is the new position really safe?
|
||||||
|
for (int32 i = 0; i < m_contactCount; ++i)
|
||||||
|
{
|
||||||
|
b2Contact* c = m_contacts[i];
|
||||||
|
b2Fixture* fA = c->GetFixtureA();
|
||||||
|
b2Fixture* fB = c->GetFixtureB();
|
||||||
|
|
||||||
|
b2Body* bA = fA->GetBody();
|
||||||
|
b2Body* bB = fB->GetBody();
|
||||||
|
|
||||||
|
int32 indexA = c->GetChildIndexA();
|
||||||
|
int32 indexB = c->GetChildIndexB();
|
||||||
|
|
||||||
|
b2DistanceInput input;
|
||||||
|
input.proxyA.Set(fA->GetShape(), indexA);
|
||||||
|
input.proxyB.Set(fB->GetShape(), indexB);
|
||||||
|
input.transformA = bA->GetTransform();
|
||||||
|
input.transformB = bB->GetTransform();
|
||||||
|
input.useRadii = false;
|
||||||
|
|
||||||
|
b2DistanceOutput output;
|
||||||
|
b2SimplexCache cache;
|
||||||
|
cache.count = 0;
|
||||||
|
b2Distance(&output, &cache, &input);
|
||||||
|
|
||||||
|
if (output.distance == 0 || cache.count == 3)
|
||||||
|
{
|
||||||
|
cache.count += 0;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Leap of faith to new safe state.
|
||||||
|
m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
|
||||||
|
m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
|
||||||
|
m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
|
||||||
|
m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;
|
||||||
|
|
||||||
|
// No warm starting is needed for TOI events because warm
|
||||||
|
// starting impulses were applied in the discrete solver.
|
||||||
|
contactSolver.InitializeVelocityConstraints();
|
||||||
|
|
||||||
|
// Solve velocity constraints.
|
||||||
|
for (int32 i = 0; i < subStep.velocityIterations; ++i)
|
||||||
|
{
|
||||||
|
contactSolver.SolveVelocityConstraints();
|
||||||
|
}
|
||||||
|
|
||||||
|
// Don't store the TOI contact forces for warm starting
|
||||||
|
// because they can be quite large.
|
||||||
|
|
||||||
|
float32 h = subStep.dt;
|
||||||
|
|
||||||
|
// Integrate positions
|
||||||
|
for (int32 i = 0; i < m_bodyCount; ++i)
|
||||||
|
{
|
||||||
|
b2Vec2 c = m_positions[i].c;
|
||||||
|
float32 a = m_positions[i].a;
|
||||||
|
b2Vec2 v = m_velocities[i].v;
|
||||||
|
float32 w = m_velocities[i].w;
|
||||||
|
|
||||||
|
// Check for large velocities
|
||||||
|
b2Vec2 translation = h * v;
|
||||||
|
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
|
||||||
|
{
|
||||||
|
float32 ratio = b2_maxTranslation / translation.Length();
|
||||||
|
v *= ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
float32 rotation = h * w;
|
||||||
|
if (rotation * rotation > b2_maxRotationSquared)
|
||||||
|
{
|
||||||
|
float32 ratio = b2_maxRotation / b2Abs(rotation);
|
||||||
|
w *= ratio;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Integrate
|
||||||
|
c += h * v;
|
||||||
|
a += h * w;
|
||||||
|
|
||||||
|
m_positions[i].c = c;
|
||||||
|
m_positions[i].a = a;
|
||||||
|
m_velocities[i].v = v;
|
||||||
|
m_velocities[i].w = w;
|
||||||
|
|
||||||
|
// Sync bodies
|
||||||
|
b2Body* body = m_bodies[i];
|
||||||
|
body->m_sweep.c = c;
|
||||||
|
body->m_sweep.a = a;
|
||||||
|
body->m_linearVelocity = v;
|
||||||
|
body->m_angularVelocity = w;
|
||||||
|
body->SynchronizeTransform();
|
||||||
|
}
|
||||||
|
|
||||||
|
Report(contactSolver.m_velocityConstraints);
|
||||||
|
}
|
||||||
|
|
||||||
|
void b2Island::Report(const b2ContactVelocityConstraint* constraints)
|
||||||
|
{
|
||||||
|
if (m_listener == nullptr)
|
||||||
|
{
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
for (int32 i = 0; i < m_contactCount; ++i)
|
||||||
|
{
|
||||||
|
b2Contact* c = m_contacts[i];
|
||||||
|
|
||||||
|
const b2ContactVelocityConstraint* vc = constraints + i;
|
||||||
|
|
||||||
|
b2ContactImpulse impulse;
|
||||||
|
impulse.count = vc->pointCount;
|
||||||
|
for (int32 j = 0; j < vc->pointCount; ++j)
|
||||||
|
{
|
||||||
|
impulse.normalImpulses[j] = vc->points[j].normalImpulse;
|
||||||
|
impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
|
||||||
|
}
|
||||||
|
|
||||||
|
m_listener->PostSolve(c, &impulse);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,93 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_ISLAND_H
|
||||||
|
#define B2_ISLAND_H
|
||||||
|
|
||||||
|
#include "Box2D/Common/b2Math.h"
|
||||||
|
#include "Box2D/Dynamics/b2Body.h"
|
||||||
|
#include "Box2D/Dynamics/b2TimeStep.h"
|
||||||
|
|
||||||
|
class b2Contact;
|
||||||
|
class b2Joint;
|
||||||
|
class b2StackAllocator;
|
||||||
|
class b2ContactListener;
|
||||||
|
struct b2ContactVelocityConstraint;
|
||||||
|
struct b2Profile;
|
||||||
|
|
||||||
|
/// This is an internal class.
|
||||||
|
class b2Island
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
b2Island(int32 bodyCapacity, int32 contactCapacity, int32 jointCapacity,
|
||||||
|
b2StackAllocator* allocator, b2ContactListener* listener);
|
||||||
|
~b2Island();
|
||||||
|
|
||||||
|
void Clear()
|
||||||
|
{
|
||||||
|
m_bodyCount = 0;
|
||||||
|
m_contactCount = 0;
|
||||||
|
m_jointCount = 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
|
||||||
|
|
||||||
|
void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB);
|
||||||
|
|
||||||
|
void Add(b2Body* body)
|
||||||
|
{
|
||||||
|
b2Assert(m_bodyCount < m_bodyCapacity);
|
||||||
|
body->m_islandIndex = m_bodyCount;
|
||||||
|
m_bodies[m_bodyCount] = body;
|
||||||
|
++m_bodyCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Add(b2Contact* contact)
|
||||||
|
{
|
||||||
|
b2Assert(m_contactCount < m_contactCapacity);
|
||||||
|
m_contacts[m_contactCount++] = contact;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Add(b2Joint* joint)
|
||||||
|
{
|
||||||
|
b2Assert(m_jointCount < m_jointCapacity);
|
||||||
|
m_joints[m_jointCount++] = joint;
|
||||||
|
}
|
||||||
|
|
||||||
|
void Report(const b2ContactVelocityConstraint* constraints);
|
||||||
|
|
||||||
|
b2StackAllocator* m_allocator;
|
||||||
|
b2ContactListener* m_listener;
|
||||||
|
|
||||||
|
b2Body** m_bodies;
|
||||||
|
b2Contact** m_contacts;
|
||||||
|
b2Joint** m_joints;
|
||||||
|
|
||||||
|
b2Position* m_positions;
|
||||||
|
b2Velocity* m_velocities;
|
||||||
|
|
||||||
|
int32 m_bodyCount;
|
||||||
|
int32 m_jointCount;
|
||||||
|
int32 m_contactCount;
|
||||||
|
|
||||||
|
int32 m_bodyCapacity;
|
||||||
|
int32 m_contactCapacity;
|
||||||
|
int32 m_jointCapacity;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,70 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_TIME_STEP_H
|
||||||
|
#define B2_TIME_STEP_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
|
||||||
|
/// Profiling data. Times are in milliseconds.
|
||||||
|
struct b2Profile
|
||||||
|
{
|
||||||
|
float32 step;
|
||||||
|
float32 collide;
|
||||||
|
float32 solve;
|
||||||
|
float32 solveInit;
|
||||||
|
float32 solveVelocity;
|
||||||
|
float32 solvePosition;
|
||||||
|
float32 broadphase;
|
||||||
|
float32 solveTOI;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is an internal structure.
|
||||||
|
struct b2TimeStep
|
||||||
|
{
|
||||||
|
float32 dt; // time step
|
||||||
|
float32 inv_dt; // inverse time step (0 if dt == 0).
|
||||||
|
float32 dtRatio; // dt * inv_dt0
|
||||||
|
int32 velocityIterations;
|
||||||
|
int32 positionIterations;
|
||||||
|
bool warmStarting;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is an internal structure.
|
||||||
|
struct b2Position
|
||||||
|
{
|
||||||
|
b2Vec2 c;
|
||||||
|
float32 a;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// This is an internal structure.
|
||||||
|
struct b2Velocity
|
||||||
|
{
|
||||||
|
b2Vec2 v;
|
||||||
|
float32 w;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Solver Data
|
||||||
|
struct b2SolverData
|
||||||
|
{
|
||||||
|
b2TimeStep step;
|
||||||
|
b2Position* positions;
|
||||||
|
b2Velocity* velocities;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
File diff suppressed because it is too large
Load Diff
|
|
@ -0,0 +1,354 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_WORLD_H
|
||||||
|
#define B2_WORLD_H
|
||||||
|
|
||||||
|
#include "../Common/b2Math.h"
|
||||||
|
#include "../Common/b2BlockAllocator.h"
|
||||||
|
#include "../Common/b2StackAllocator.h"
|
||||||
|
#include "b2ContactManager.h"
|
||||||
|
#include "b2WorldCallbacks.h"
|
||||||
|
#include "b2TimeStep.h"
|
||||||
|
|
||||||
|
struct b2AABB;
|
||||||
|
struct b2BodyDef;
|
||||||
|
struct b2Color;
|
||||||
|
struct b2JointDef;
|
||||||
|
class b2Body;
|
||||||
|
class b2Draw;
|
||||||
|
class b2Fixture;
|
||||||
|
class b2Joint;
|
||||||
|
|
||||||
|
/// The world class manages all physics entities, dynamic simulation,
|
||||||
|
/// and asynchronous queries. The world also contains efficient memory
|
||||||
|
/// management facilities.
|
||||||
|
class b2World
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
/// Construct a world object.
|
||||||
|
/// @param gravity the world gravity vector.
|
||||||
|
b2World(const b2Vec2& gravity);
|
||||||
|
|
||||||
|
/// Destruct the world. All physics entities are destroyed and all heap memory is released.
|
||||||
|
~b2World();
|
||||||
|
|
||||||
|
/// Register a destruction listener. The listener is owned by you and must
|
||||||
|
/// remain in scope.
|
||||||
|
void SetDestructionListener(b2DestructionListener* listener);
|
||||||
|
|
||||||
|
/// Register a contact filter to provide specific control over collision.
|
||||||
|
/// Otherwise the default filter is used (b2_defaultFilter). The listener is
|
||||||
|
/// owned by you and must remain in scope.
|
||||||
|
void SetContactFilter(b2ContactFilter* filter);
|
||||||
|
|
||||||
|
/// Register a contact event listener. The listener is owned by you and must
|
||||||
|
/// remain in scope.
|
||||||
|
void SetContactListener(b2ContactListener* listener);
|
||||||
|
|
||||||
|
/// Register a routine for debug drawing. The debug draw functions are called
|
||||||
|
/// inside with b2World::DrawDebugData method. The debug draw object is owned
|
||||||
|
/// by you and must remain in scope.
|
||||||
|
void SetDebugDraw(b2Draw* debugDraw);
|
||||||
|
|
||||||
|
/// Create a rigid body given a definition. No reference to the definition
|
||||||
|
/// is retained.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
b2Body* CreateBody(const b2BodyDef* def);
|
||||||
|
|
||||||
|
/// Destroy a rigid body given a definition. No reference to the definition
|
||||||
|
/// is retained. This function is locked during callbacks.
|
||||||
|
/// @warning This automatically deletes all associated shapes and joints.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
void DestroyBody(b2Body* body);
|
||||||
|
|
||||||
|
/// Create a joint to constrain bodies together. No reference to the definition
|
||||||
|
/// is retained. This may cause the connected bodies to cease colliding.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
b2Joint* CreateJoint(const b2JointDef* def);
|
||||||
|
|
||||||
|
/// Destroy a joint. This may cause the connected bodies to begin colliding.
|
||||||
|
/// @warning This function is locked during callbacks.
|
||||||
|
void DestroyJoint(b2Joint* joint);
|
||||||
|
|
||||||
|
/// Take a time step. This performs collision detection, integration,
|
||||||
|
/// and constraint solution.
|
||||||
|
/// @param timeStep the amount of time to simulate, this should not vary.
|
||||||
|
/// @param velocityIterations for the velocity constraint solver.
|
||||||
|
/// @param positionIterations for the position constraint solver.
|
||||||
|
void Step( float32 timeStep,
|
||||||
|
int32 velocityIterations,
|
||||||
|
int32 positionIterations);
|
||||||
|
|
||||||
|
/// Manually clear the force buffer on all bodies. By default, forces are cleared automatically
|
||||||
|
/// after each call to Step. The default behavior is modified by calling SetAutoClearForces.
|
||||||
|
/// The purpose of this function is to support sub-stepping. Sub-stepping is often used to maintain
|
||||||
|
/// a fixed sized time step under a variable frame-rate.
|
||||||
|
/// When you perform sub-stepping you will disable auto clearing of forces and instead call
|
||||||
|
/// ClearForces after all sub-steps are complete in one pass of your game loop.
|
||||||
|
/// @see SetAutoClearForces
|
||||||
|
void ClearForces();
|
||||||
|
|
||||||
|
/// Call this to draw shapes and other debug draw data. This is intentionally non-const.
|
||||||
|
void DrawDebugData();
|
||||||
|
|
||||||
|
/// Query the world for all fixtures that potentially overlap the
|
||||||
|
/// provided AABB.
|
||||||
|
/// @param callback a user implemented callback class.
|
||||||
|
/// @param aabb the query box.
|
||||||
|
void QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const;
|
||||||
|
|
||||||
|
/// Ray-cast the world for all fixtures in the path of the ray. Your callback
|
||||||
|
/// controls whether you get the closest point, any point, or n-points.
|
||||||
|
/// The ray-cast ignores shapes that contain the starting point.
|
||||||
|
/// @param callback a user implemented callback class.
|
||||||
|
/// @param point1 the ray starting point
|
||||||
|
/// @param point2 the ray ending point
|
||||||
|
void RayCast(b2RayCastCallback* callback, const b2Vec2& point1, const b2Vec2& point2) const;
|
||||||
|
|
||||||
|
/// Get the world body list. With the returned body, use b2Body::GetNext to get
|
||||||
|
/// the next body in the world list. A nullptr body indicates the end of the list.
|
||||||
|
/// @return the head of the world body list.
|
||||||
|
b2Body* GetBodyList();
|
||||||
|
const b2Body* GetBodyList() const;
|
||||||
|
|
||||||
|
/// Get the world joint list. With the returned joint, use b2Joint::GetNext to get
|
||||||
|
/// the next joint in the world list. A nullptr joint indicates the end of the list.
|
||||||
|
/// @return the head of the world joint list.
|
||||||
|
b2Joint* GetJointList();
|
||||||
|
const b2Joint* GetJointList() const;
|
||||||
|
|
||||||
|
/// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
|
||||||
|
/// the next contact in the world list. A nullptr contact indicates the end of the list.
|
||||||
|
/// @return the head of the world contact list.
|
||||||
|
/// @warning contacts are created and destroyed in the middle of a time step.
|
||||||
|
/// Use b2ContactListener to avoid missing contacts.
|
||||||
|
b2Contact* GetContactList();
|
||||||
|
const b2Contact* GetContactList() const;
|
||||||
|
|
||||||
|
/// Enable/disable sleep.
|
||||||
|
void SetAllowSleeping(bool flag);
|
||||||
|
bool GetAllowSleeping() const { return m_allowSleep; }
|
||||||
|
|
||||||
|
/// Enable/disable warm starting. For testing.
|
||||||
|
void SetWarmStarting(bool flag) { m_warmStarting = flag; }
|
||||||
|
bool GetWarmStarting() const { return m_warmStarting; }
|
||||||
|
|
||||||
|
/// Enable/disable continuous physics. For testing.
|
||||||
|
void SetContinuousPhysics(bool flag) { m_continuousPhysics = flag; }
|
||||||
|
bool GetContinuousPhysics() const { return m_continuousPhysics; }
|
||||||
|
|
||||||
|
/// Enable/disable single stepped continuous physics. For testing.
|
||||||
|
void SetSubStepping(bool flag) { m_subStepping = flag; }
|
||||||
|
bool GetSubStepping() const { return m_subStepping; }
|
||||||
|
|
||||||
|
/// Get the number of broad-phase proxies.
|
||||||
|
int32 GetProxyCount() const;
|
||||||
|
|
||||||
|
/// Get the number of bodies.
|
||||||
|
int32 GetBodyCount() const;
|
||||||
|
|
||||||
|
/// Get the number of joints.
|
||||||
|
int32 GetJointCount() const;
|
||||||
|
|
||||||
|
/// Get the number of contacts (each may have 0 or more contact points).
|
||||||
|
int32 GetContactCount() const;
|
||||||
|
|
||||||
|
/// Get the height of the dynamic tree.
|
||||||
|
int32 GetTreeHeight() const;
|
||||||
|
|
||||||
|
/// Get the balance of the dynamic tree.
|
||||||
|
int32 GetTreeBalance() const;
|
||||||
|
|
||||||
|
/// Get the quality metric of the dynamic tree. The smaller the better.
|
||||||
|
/// The minimum is 1.
|
||||||
|
float32 GetTreeQuality() const;
|
||||||
|
|
||||||
|
/// Change the global gravity vector.
|
||||||
|
void SetGravity(const b2Vec2& gravity);
|
||||||
|
|
||||||
|
/// Get the global gravity vector.
|
||||||
|
b2Vec2 GetGravity() const;
|
||||||
|
|
||||||
|
/// Is the world locked (in the middle of a time step).
|
||||||
|
bool IsLocked() const;
|
||||||
|
|
||||||
|
/// Set flag to control automatic clearing of forces after each time step.
|
||||||
|
void SetAutoClearForces(bool flag);
|
||||||
|
|
||||||
|
/// Get the flag that controls automatic clearing of forces after each time step.
|
||||||
|
bool GetAutoClearForces() const;
|
||||||
|
|
||||||
|
/// Shift the world origin. Useful for large worlds.
|
||||||
|
/// The body shift formula is: position -= newOrigin
|
||||||
|
/// @param newOrigin the new origin with respect to the old origin
|
||||||
|
void ShiftOrigin(const b2Vec2& newOrigin);
|
||||||
|
|
||||||
|
/// Get the contact manager for testing.
|
||||||
|
const b2ContactManager& GetContactManager() const;
|
||||||
|
|
||||||
|
/// Get the current profile.
|
||||||
|
const b2Profile& GetProfile() const;
|
||||||
|
|
||||||
|
/// Dump the world into the log file.
|
||||||
|
/// @warning this should be called outside of a time step.
|
||||||
|
void Dump();
|
||||||
|
|
||||||
|
private:
|
||||||
|
|
||||||
|
// m_flags
|
||||||
|
enum
|
||||||
|
{
|
||||||
|
e_newFixture = 0x0001,
|
||||||
|
e_locked = 0x0002,
|
||||||
|
e_clearForces = 0x0004
|
||||||
|
};
|
||||||
|
|
||||||
|
friend class b2Body;
|
||||||
|
friend class b2Fixture;
|
||||||
|
friend class b2ContactManager;
|
||||||
|
friend class b2Controller;
|
||||||
|
|
||||||
|
void Solve(const b2TimeStep& step);
|
||||||
|
void SolveTOI(const b2TimeStep& step);
|
||||||
|
|
||||||
|
void DrawJoint(b2Joint* joint);
|
||||||
|
void DrawShape(b2Fixture* shape, const b2Transform& xf, const b2Color& color);
|
||||||
|
|
||||||
|
b2BlockAllocator m_blockAllocator;
|
||||||
|
b2StackAllocator m_stackAllocator;
|
||||||
|
|
||||||
|
int32 m_flags;
|
||||||
|
|
||||||
|
b2ContactManager m_contactManager;
|
||||||
|
|
||||||
|
b2Body* m_bodyList;
|
||||||
|
b2Joint* m_jointList;
|
||||||
|
|
||||||
|
int32 m_bodyCount;
|
||||||
|
int32 m_jointCount;
|
||||||
|
|
||||||
|
b2Vec2 m_gravity;
|
||||||
|
bool m_allowSleep;
|
||||||
|
|
||||||
|
b2DestructionListener* m_destructionListener;
|
||||||
|
b2Draw* m_debugDraw;
|
||||||
|
|
||||||
|
// This is used to compute the time step ratio to
|
||||||
|
// support a variable time step.
|
||||||
|
float32 m_inv_dt0;
|
||||||
|
|
||||||
|
// These are for debugging the solver.
|
||||||
|
bool m_warmStarting;
|
||||||
|
bool m_continuousPhysics;
|
||||||
|
bool m_subStepping;
|
||||||
|
|
||||||
|
bool m_stepComplete;
|
||||||
|
|
||||||
|
b2Profile m_profile;
|
||||||
|
};
|
||||||
|
|
||||||
|
inline b2Body* b2World::GetBodyList()
|
||||||
|
{
|
||||||
|
return m_bodyList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Body* b2World::GetBodyList() const
|
||||||
|
{
|
||||||
|
return m_bodyList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Joint* b2World::GetJointList()
|
||||||
|
{
|
||||||
|
return m_jointList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Joint* b2World::GetJointList() const
|
||||||
|
{
|
||||||
|
return m_jointList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Contact* b2World::GetContactList()
|
||||||
|
{
|
||||||
|
return m_contactManager.m_contactList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Contact* b2World::GetContactList() const
|
||||||
|
{
|
||||||
|
return m_contactManager.m_contactList;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2World::GetBodyCount() const
|
||||||
|
{
|
||||||
|
return m_bodyCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2World::GetJointCount() const
|
||||||
|
{
|
||||||
|
return m_jointCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int32 b2World::GetContactCount() const
|
||||||
|
{
|
||||||
|
return m_contactManager.m_contactCount;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2World::SetGravity(const b2Vec2& gravity)
|
||||||
|
{
|
||||||
|
m_gravity = gravity;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline b2Vec2 b2World::GetGravity() const
|
||||||
|
{
|
||||||
|
return m_gravity;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline bool b2World::IsLocked() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_locked) == e_locked;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline void b2World::SetAutoClearForces(bool flag)
|
||||||
|
{
|
||||||
|
if (flag)
|
||||||
|
{
|
||||||
|
m_flags |= e_clearForces;
|
||||||
|
}
|
||||||
|
else
|
||||||
|
{
|
||||||
|
m_flags &= ~e_clearForces;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
/// Get the flag that controls automatic clearing of forces after each time step.
|
||||||
|
inline bool b2World::GetAutoClearForces() const
|
||||||
|
{
|
||||||
|
return (m_flags & e_clearForces) == e_clearForces;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2ContactManager& b2World::GetContactManager() const
|
||||||
|
{
|
||||||
|
return m_contactManager;
|
||||||
|
}
|
||||||
|
|
||||||
|
inline const b2Profile& b2World::GetProfile() const
|
||||||
|
{
|
||||||
|
return m_profile;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,36 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include "Box2D/Dynamics/b2WorldCallbacks.h"
|
||||||
|
#include "Box2D/Dynamics/b2Fixture.h"
|
||||||
|
|
||||||
|
// Return true if contact calculations should be performed between these two shapes.
|
||||||
|
// If you implement your own collision filter you may want to build from this implementation.
|
||||||
|
bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
|
||||||
|
{
|
||||||
|
const b2Filter& filterA = fixtureA->GetFilterData();
|
||||||
|
const b2Filter& filterB = fixtureB->GetFilterData();
|
||||||
|
|
||||||
|
if (filterA.groupIndex == filterB.groupIndex && filterA.groupIndex != 0)
|
||||||
|
{
|
||||||
|
return filterA.groupIndex > 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
|
||||||
|
return collide;
|
||||||
|
}
|
||||||
|
|
@ -0,0 +1,155 @@
|
||||||
|
/*
|
||||||
|
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
|
||||||
|
*
|
||||||
|
* This software is provided 'as-is', without any express or implied
|
||||||
|
* warranty. In no event will the authors be held liable for any damages
|
||||||
|
* arising from the use of this software.
|
||||||
|
* Permission is granted to anyone to use this software for any purpose,
|
||||||
|
* including commercial applications, and to alter it and redistribute it
|
||||||
|
* freely, subject to the following restrictions:
|
||||||
|
* 1. The origin of this software must not be misrepresented; you must not
|
||||||
|
* claim that you wrote the original software. If you use this software
|
||||||
|
* in a product, an acknowledgment in the product documentation would be
|
||||||
|
* appreciated but is not required.
|
||||||
|
* 2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
* misrepresented as being the original software.
|
||||||
|
* 3. This notice may not be removed or altered from any source distribution.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#ifndef B2_WORLD_CALLBACKS_H
|
||||||
|
#define B2_WORLD_CALLBACKS_H
|
||||||
|
|
||||||
|
#include "../Common/b2Settings.h"
|
||||||
|
|
||||||
|
struct b2Vec2;
|
||||||
|
struct b2Transform;
|
||||||
|
class b2Fixture;
|
||||||
|
class b2Body;
|
||||||
|
class b2Joint;
|
||||||
|
class b2Contact;
|
||||||
|
struct b2ContactResult;
|
||||||
|
struct b2Manifold;
|
||||||
|
|
||||||
|
/// Joints and fixtures are destroyed when their associated
|
||||||
|
/// body is destroyed. Implement this listener so that you
|
||||||
|
/// may nullify references to these joints and shapes.
|
||||||
|
class b2DestructionListener
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual ~b2DestructionListener() {}
|
||||||
|
|
||||||
|
/// Called when any joint is about to be destroyed due
|
||||||
|
/// to the destruction of one of its attached bodies.
|
||||||
|
virtual void SayGoodbye(b2Joint* joint) = 0;
|
||||||
|
|
||||||
|
/// Called when any fixture is about to be destroyed due
|
||||||
|
/// to the destruction of its parent body.
|
||||||
|
virtual void SayGoodbye(b2Fixture* fixture) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Implement this class to provide collision filtering. In other words, you can implement
|
||||||
|
/// this class if you want finer control over contact creation.
|
||||||
|
class b2ContactFilter
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual ~b2ContactFilter() {}
|
||||||
|
|
||||||
|
/// Return true if contact calculations should be performed between these two shapes.
|
||||||
|
/// @warning for performance reasons this is only called when the AABBs begin to overlap.
|
||||||
|
virtual bool ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB);
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Contact impulses for reporting. Impulses are used instead of forces because
|
||||||
|
/// sub-step forces may approach infinity for rigid body collisions. These
|
||||||
|
/// match up one-to-one with the contact points in b2Manifold.
|
||||||
|
struct b2ContactImpulse
|
||||||
|
{
|
||||||
|
float32 normalImpulses[b2_maxManifoldPoints];
|
||||||
|
float32 tangentImpulses[b2_maxManifoldPoints];
|
||||||
|
int32 count;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Implement this class to get contact information. You can use these results for
|
||||||
|
/// things like sounds and game logic. You can also get contact results by
|
||||||
|
/// traversing the contact lists after the time step. However, you might miss
|
||||||
|
/// some contacts because continuous physics leads to sub-stepping.
|
||||||
|
/// Additionally you may receive multiple callbacks for the same contact in a
|
||||||
|
/// single time step.
|
||||||
|
/// You should strive to make your callbacks efficient because there may be
|
||||||
|
/// many callbacks per time step.
|
||||||
|
/// @warning You cannot create/destroy Box2D entities inside these callbacks.
|
||||||
|
class b2ContactListener
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual ~b2ContactListener() {}
|
||||||
|
|
||||||
|
/// Called when two fixtures begin to touch.
|
||||||
|
virtual void BeginContact(b2Contact* contact) { B2_NOT_USED(contact); }
|
||||||
|
|
||||||
|
/// Called when two fixtures cease to touch.
|
||||||
|
virtual void EndContact(b2Contact* contact) { B2_NOT_USED(contact); }
|
||||||
|
|
||||||
|
/// This is called after a contact is updated. This allows you to inspect a
|
||||||
|
/// contact before it goes to the solver. If you are careful, you can modify the
|
||||||
|
/// contact manifold (e.g. disable contact).
|
||||||
|
/// A copy of the old manifold is provided so that you can detect changes.
|
||||||
|
/// Note: this is called only for awake bodies.
|
||||||
|
/// Note: this is called even when the number of contact points is zero.
|
||||||
|
/// Note: this is not called for sensors.
|
||||||
|
/// Note: if you set the number of contact points to zero, you will not
|
||||||
|
/// get an EndContact callback. However, you may get a BeginContact callback
|
||||||
|
/// the next step.
|
||||||
|
virtual void PreSolve(b2Contact* contact, const b2Manifold* oldManifold)
|
||||||
|
{
|
||||||
|
B2_NOT_USED(contact);
|
||||||
|
B2_NOT_USED(oldManifold);
|
||||||
|
}
|
||||||
|
|
||||||
|
/// This lets you inspect a contact after the solver is finished. This is useful
|
||||||
|
/// for inspecting impulses.
|
||||||
|
/// Note: the contact manifold does not include time of impact impulses, which can be
|
||||||
|
/// arbitrarily large if the sub-step is small. Hence the impulse is provided explicitly
|
||||||
|
/// in a separate data structure.
|
||||||
|
/// Note: this is only called for contacts that are touching, solid, and awake.
|
||||||
|
virtual void PostSolve(b2Contact* contact, const b2ContactImpulse* impulse)
|
||||||
|
{
|
||||||
|
B2_NOT_USED(contact);
|
||||||
|
B2_NOT_USED(impulse);
|
||||||
|
}
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Callback class for AABB queries.
|
||||||
|
/// See b2World::Query
|
||||||
|
class b2QueryCallback
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual ~b2QueryCallback() {}
|
||||||
|
|
||||||
|
/// Called for each fixture found in the query AABB.
|
||||||
|
/// @return false to terminate the query.
|
||||||
|
virtual bool ReportFixture(b2Fixture* fixture) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
/// Callback class for ray casts.
|
||||||
|
/// See b2World::RayCast
|
||||||
|
class b2RayCastCallback
|
||||||
|
{
|
||||||
|
public:
|
||||||
|
virtual ~b2RayCastCallback() {}
|
||||||
|
|
||||||
|
/// Called for each fixture found in the query. You control how the ray cast
|
||||||
|
/// proceeds by returning a float:
|
||||||
|
/// return -1: ignore this fixture and continue
|
||||||
|
/// return 0: terminate the ray cast
|
||||||
|
/// return fraction: clip the ray to this point
|
||||||
|
/// return 1: don't clip the ray and continue
|
||||||
|
/// @param fixture the fixture hit by the ray
|
||||||
|
/// @param point the point of initial intersection
|
||||||
|
/// @param normal the normal vector at the point of intersection
|
||||||
|
/// @return -1 to filter, 0 to terminate, fraction to clip the ray for
|
||||||
|
/// closest hit, 1 to continue
|
||||||
|
virtual float32 ReportFixture( b2Fixture* fixture, const b2Vec2& point,
|
||||||
|
const b2Vec2& normal, float32 fraction) = 0;
|
||||||
|
};
|
||||||
|
|
||||||
|
#endif
|
||||||
|
|
@ -0,0 +1,17 @@
|
||||||
|
Copyright (c) 2006-2017 Erin Catto http://www.box2d.org
|
||||||
|
|
||||||
|
This software is provided 'as-is', without any express or implied
|
||||||
|
warranty. In no event will the authors be held liable for any damages
|
||||||
|
arising from the use of this software.
|
||||||
|
|
||||||
|
Permission is granted to anyone to use this software for any purpose,
|
||||||
|
including commercial applications, and to alter it and redistribute it
|
||||||
|
freely, subject to the following restrictions:
|
||||||
|
|
||||||
|
1. The origin of this software must not be misrepresented; you must not
|
||||||
|
claim that you wrote the original software. If you use this software
|
||||||
|
in a product, an acknowledgment in the product documentation would be
|
||||||
|
appreciated but is not required.
|
||||||
|
2. Altered source versions must be plainly marked as such, and must not be
|
||||||
|
misrepresented as being the original software.
|
||||||
|
3. This notice may not be removed or altered from any source distribution.
|
||||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue