summaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorFrederik Schwarzer <[email protected]>2016-01-06 09:27:02 +0100
committerFrederik Schwarzer <[email protected]>2016-01-06 09:27:02 +0100
commitc59ff50799e5076e3f2c53478e12e456ef8cce33 (patch)
treecad4729776834dfea2b708dc9ce05ea5fc456cc1
parent174df03b094d760ca2d853d0ebe0b8dc6e6a869c (diff)
Revert "Pull in new Version of Box2D."
This was not meant to be here. This reverts commit a051772428f64b1f12f300bc4945e9edc831a80d.
-rw-r--r--external/Box2D/Box2D.h68
-rw-r--r--external/Box2D/Box2DConfig.cmake.in31
-rw-r--r--external/Box2D/CMakeLists.txt218
-rw-r--r--external/Box2D/Collision/Shapes/b2ChainShape.cpp193
-rw-r--r--external/Box2D/Collision/Shapes/b2CircleShape.cpp21
-rw-r--r--external/Box2D/Collision/Shapes/b2CircleShape.h4
-rw-r--r--external/Box2D/Collision/Shapes/b2EdgeShape.cpp25
-rw-r--r--external/Box2D/Collision/Shapes/b2EdgeShape.h8
-rw-r--r--external/Box2D/Collision/Shapes/b2LoopShape.cpp130
-rw-r--r--external/Box2D/Collision/Shapes/b2LoopShape.h (renamed from external/Box2D/Collision/Shapes/b2ChainShape.h)67
-rw-r--r--external/Box2D/Collision/Shapes/b2PolygonShape.cpp271
-rw-r--r--external/Box2D/Collision/Shapes/b2PolygonShape.h30
-rw-r--r--external/Box2D/Collision/Shapes/b2Shape.h14
-rw-r--r--external/Box2D/Collision/b2BroadPhase.cpp10
-rw-r--r--external/Box2D/Collision/b2BroadPhase.h42
-rw-r--r--external/Box2D/Collision/b2CollideCircle.cpp22
-rw-r--r--external/Box2D/Collision/b2CollideEdge.cpp843
-rw-r--r--external/Box2D/Collision/b2CollidePolygon.cpp170
-rw-r--r--external/Box2D/Collision/b2Collision.cpp35
-rw-r--r--external/Box2D/Collision/b2Collision.h30
-rw-r--r--external/Box2D/Collision/b2Distance.cpp100
-rw-r--r--external/Box2D/Collision/b2Distance.h16
-rw-r--r--external/Box2D/Collision/b2DynamicTree.cpp526
-rw-r--r--external/Box2D/Collision/b2DynamicTree.h63
-rw-r--r--external/Box2D/Collision/b2TimeOfImpact.cpp109
-rw-r--r--external/Box2D/Collision/b2TimeOfImpact.h6
-rw-r--r--external/Box2D/Common/b2BlockAllocator.cpp10
-rw-r--r--external/Box2D/Common/b2BlockAllocator.h4
-rw-r--r--external/Box2D/Common/b2Draw.cpp44
-rw-r--r--external/Box2D/Common/b2Draw.h86
-rw-r--r--external/Box2D/Common/b2GrowableStack.h6
-rw-r--r--external/Box2D/Common/b2Math.cpp57
-rw-r--r--external/Box2D/Common/b2Math.h401
-rw-r--r--external/Box2D/Common/b2Settings.cpp17
-rw-r--r--external/Box2D/Common/b2Settings.h54
-rw-r--r--external/Box2D/Common/b2StackAllocator.cpp2
-rw-r--r--external/Box2D/Common/b2StackAllocator.h2
-rw-r--r--external/Box2D/Common/b2Timer.cpp101
-rw-r--r--external/Box2D/Common/b2Timer.h50
-rw-r--r--external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp53
-rw-r--r--external/Box2D/Dynamics/Contacts/b2CircleContact.cpp3
-rw-r--r--external/Box2D/Dynamics/Contacts/b2CircleContact.h2
-rw-r--r--external/Box2D/Dynamics/Contacts/b2Contact.cpp50
-rw-r--r--external/Box2D/Dynamics/Contacts/b2Contact.h90
-rw-r--r--external/Box2D/Dynamics/Contacts/b2ContactSolver.cpp683
-rw-r--r--external/Box2D/Dynamics/Contacts/b2ContactSolver.h55
-rw-r--r--external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp3
-rw-r--r--external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h2
-rw-r--r--external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp3
-rw-r--r--external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h2
-rw-r--r--external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp54
-rw-r--r--external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h (renamed from external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h)12
-rw-r--r--external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp (renamed from external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp)35
-rw-r--r--external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h (renamed from external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h)12
-rw-r--r--external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp3
-rw-r--r--external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h2
-rw-r--r--external/Box2D/Dynamics/Contacts/b2PolygonContact.cpp3
-rw-r--r--external/Box2D/Dynamics/Contacts/b2PolygonContact.h2
-rw-r--r--external/Box2D/Dynamics/Joints/b2DistanceJoint.cpp187
-rw-r--r--external/Box2D/Dynamics/Joints/b2DistanceJoint.h94
-rw-r--r--external/Box2D/Dynamics/Joints/b2FrictionJoint.cpp148
-rw-r--r--external/Box2D/Dynamics/Joints/b2FrictionJoint.h58
-rw-r--r--external/Box2D/Dynamics/Joints/b2GearJoint.cpp400
-rw-r--r--external/Box2D/Dynamics/Joints/b2GearJoint.h82
-rw-r--r--external/Box2D/Dynamics/Joints/b2Joint.cpp45
-rw-r--r--external/Box2D/Dynamics/Joints/b2Joint.h76
-rw-r--r--external/Box2D/Dynamics/Joints/b2LineJoint.cpp591
-rw-r--r--external/Box2D/Dynamics/Joints/b2LineJoint.h175
-rw-r--r--external/Box2D/Dynamics/Joints/b2MotorJoint.cpp304
-rw-r--r--external/Box2D/Dynamics/Joints/b2MotorJoint.h133
-rw-r--r--external/Box2D/Dynamics/Joints/b2MouseJoint.cpp133
-rw-r--r--external/Box2D/Dynamics/Joints/b2MouseJoint.h67
-rw-r--r--external/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp451
-rw-r--r--external/Box2D/Dynamics/Joints/b2PrismaticJoint.h121
-rw-r--r--external/Box2D/Dynamics/Joints/b2PulleyJoint.cpp493
-rw-r--r--external/Box2D/Dynamics/Joints/b2PulleyJoint.h112
-rw-r--r--external/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp378
-rw-r--r--external/Box2D/Dynamics/Joints/b2RevoluteJoint.h96
-rw-r--r--external/Box2D/Dynamics/Joints/b2RopeJoint.cpp156
-rw-r--r--external/Box2D/Dynamics/Joints/b2RopeJoint.h59
-rw-r--r--external/Box2D/Dynamics/Joints/b2WeldJoint.cpp307
-rw-r--r--external/Box2D/Dynamics/Joints/b2WeldJoint.h70
-rw-r--r--external/Box2D/Dynamics/Joints/b2WheelJoint.cpp419
-rw-r--r--external/Box2D/Dynamics/Joints/b2WheelJoint.h210
-rw-r--r--external/Box2D/Dynamics/b2Body.cpp133
-rw-r--r--external/Box2D/Dynamics/b2Body.h224
-rw-r--r--external/Box2D/Dynamics/b2ContactManager.cpp33
-rw-r--r--external/Box2D/Dynamics/b2ContactManager.h2
-rw-r--r--external/Box2D/Dynamics/b2Fixture.cpp115
-rw-r--r--external/Box2D/Dynamics/b2Fixture.h65
-rw-r--r--external/Box2D/Dynamics/b2Island.cpp301
-rw-r--r--external/Box2D/Dynamics/b2Island.h28
-rw-r--r--external/Box2D/Dynamics/b2TimeStep.h45
-rw-r--r--external/Box2D/Dynamics/b2World.cpp301
-rw-r--r--external/Box2D/Dynamics/b2World.h59
-rw-r--r--external/Box2D/Dynamics/b2WorldCallbacks.cpp27
-rw-r--r--external/Box2D/Dynamics/b2WorldCallbacks.h74
-rw-r--r--external/Box2D/Rope/b2Rope.cpp259
-rw-r--r--external/Box2D/Rope/b2Rope.h115
-rw-r--r--external/Box2D/UseBox2D.cmake9
100 files changed, 4506 insertions, 7604 deletions
diff --git a/external/Box2D/Box2D.h b/external/Box2D/Box2D.h
deleted file mode 100644
index 1870461..0000000
--- a/external/Box2D/Box2D.h
+++ /dev/null
@@ -1,68 +0,0 @@
-/*
-* 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 <Box2D/Common/b2Settings.h>
-#include <Box2D/Common/b2Draw.h>
-#include <Box2D/Common/b2Timer.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>
-
-#include <Box2D/Collision/b2BroadPhase.h>
-#include <Box2D/Collision/b2Distance.h>
-#include <Box2D/Collision/b2DynamicTree.h>
-#include <Box2D/Collision/b2TimeOfImpact.h>
-
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
-#include <Box2D/Dynamics/b2World.h>
-
-#include <Box2D/Dynamics/Contacts/b2Contact.h>
-
-#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
-#include <Box2D/Dynamics/Joints/b2GearJoint.h>
-#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
-#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
-#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
-#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
-#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
-#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
-#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
-
-#endif
diff --git a/external/Box2D/Box2DConfig.cmake.in b/external/Box2D/Box2DConfig.cmake.in
deleted file mode 100644
index 55d62f6..0000000
--- a/external/Box2D/Box2DConfig.cmake.in
+++ /dev/null
@@ -1,31 +0,0 @@
-# -*- cmake -*-
-#
-# Box2dConfig.cmake(.in)
-#
-
-# Use the following variables to compile and link against Box2d:
-# BOX2D_FOUND - True if Box2d was found on your system
-# BOX2D_USE_FILE - The file making Box2d usable
-# BOX2D_DEFINITIONS - Definitions needed to build with Box2d
-# BOX2D_INCLUDE_DIR - Box2d headers location
-# BOX2D_INCLUDE_DIRS - List of directories where Box2d header file are
-# BOX2D_LIBRARY - Library name
-# BOX2D_LIBRARIES - List of libraries to link against
-# BOX2D_LIBRARY_DIRS - List of directories containing Box2d libraries
-# BOX2D_ROOT_DIR - The base directory of Box2d
-# BOX2D_VERSION_STRING - A human-readable string containing the version
-
-set ( BOX2D_FOUND 1 )
-set ( BOX2D_USE_FILE "@[email protected]" )
-
-set ( BOX2D_DEFINITIONS "@[email protected]" )
-set ( BOX2D_INCLUDE_DIR "@[email protected]" )
-set ( Box2D_INCLUDE_DIRS "@[email protected]" ) # deprecated
-set ( BOX2D_INCLUDE_DIRS "@[email protected]" )
-set ( BOX2D_LIBRARY "@[email protected]" )
-set ( BOX2D_LIBRARIES "@[email protected]" )
-set ( BOX2D_LIBRARY_DIRS "@[email protected]" )
-set ( BOX2D_ROOT_DIR "@[email protected]" )
-
-set ( BOX2D_VERSION_STRING "@[email protected]" )
-
diff --git a/external/Box2D/CMakeLists.txt b/external/Box2D/CMakeLists.txt
deleted file mode 100644
index 05087d3..0000000
--- a/external/Box2D/CMakeLists.txt
+++ /dev/null
@@ -1,218 +0,0 @@
-set(BOX2D_Collision_SRCS
- Collision/b2BroadPhase.cpp
- Collision/b2CollideCircle.cpp
- Collision/b2CollideEdge.cpp
- Collision/b2CollidePolygon.cpp
- Collision/b2Collision.cpp
- Collision/b2Distance.cpp
- Collision/b2DynamicTree.cpp
- Collision/b2TimeOfImpact.cpp
-)
-set(BOX2D_Collision_HDRS
- Collision/b2BroadPhase.h
- Collision/b2Collision.h
- Collision/b2Distance.h
- Collision/b2DynamicTree.h
- Collision/b2TimeOfImpact.h
-)
-set(BOX2D_Shapes_SRCS
- Collision/Shapes/b2CircleShape.cpp
- Collision/Shapes/b2EdgeShape.cpp
- Collision/Shapes/b2ChainShape.cpp
- Collision/Shapes/b2PolygonShape.cpp
-)
-set(BOX2D_Shapes_HDRS
- Collision/Shapes/b2CircleShape.h
- Collision/Shapes/b2EdgeShape.h
- Collision/Shapes/b2ChainShape.h
- Collision/Shapes/b2PolygonShape.h
- Collision/Shapes/b2Shape.h
-)
-set(BOX2D_Common_SRCS
- Common/b2BlockAllocator.cpp
- Common/b2Draw.cpp
- Common/b2Math.cpp
- Common/b2Settings.cpp
- Common/b2StackAllocator.cpp
- Common/b2Timer.cpp
-)
-set(BOX2D_Common_HDRS
- Common/b2BlockAllocator.h
- Common/b2Draw.h
- Common/b2GrowableStack.h
- Common/b2Math.h
- Common/b2Settings.h
- Common/b2StackAllocator.h
- Common/b2Timer.h
-)
-set(BOX2D_Dynamics_SRCS
- Dynamics/b2Body.cpp
- Dynamics/b2ContactManager.cpp
- Dynamics/b2Fixture.cpp
- Dynamics/b2Island.cpp
- Dynamics/b2World.cpp
- Dynamics/b2WorldCallbacks.cpp
-)
-set(BOX2D_Dynamics_HDRS
- Dynamics/b2Body.h
- Dynamics/b2ContactManager.h
- Dynamics/b2Fixture.h
- Dynamics/b2Island.h
- Dynamics/b2TimeStep.h
- Dynamics/b2World.h
- Dynamics/b2WorldCallbacks.h
-)
-set(BOX2D_Contacts_SRCS
- Dynamics/Contacts/b2CircleContact.cpp
- Dynamics/Contacts/b2Contact.cpp
- Dynamics/Contacts/b2ContactSolver.cpp
- Dynamics/Contacts/b2PolygonAndCircleContact.cpp
- Dynamics/Contacts/b2EdgeAndCircleContact.cpp
- Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
- Dynamics/Contacts/b2ChainAndCircleContact.cpp
- Dynamics/Contacts/b2ChainAndPolygonContact.cpp
- Dynamics/Contacts/b2PolygonContact.cpp
-)
-set(BOX2D_Contacts_HDRS
- Dynamics/Contacts/b2CircleContact.h
- Dynamics/Contacts/b2Contact.h
- Dynamics/Contacts/b2ContactSolver.h
- Dynamics/Contacts/b2PolygonAndCircleContact.h
- Dynamics/Contacts/b2EdgeAndCircleContact.h
- Dynamics/Contacts/b2EdgeAndPolygonContact.h
- Dynamics/Contacts/b2ChainAndCircleContact.h
- Dynamics/Contacts/b2ChainAndPolygonContact.h
- Dynamics/Contacts/b2PolygonContact.h
-)
-set(BOX2D_Joints_SRCS
- Dynamics/Joints/b2DistanceJoint.cpp
- Dynamics/Joints/b2FrictionJoint.cpp
- Dynamics/Joints/b2GearJoint.cpp
- Dynamics/Joints/b2Joint.cpp
- Dynamics/Joints/b2MotorJoint.cpp
- Dynamics/Joints/b2MouseJoint.cpp
- Dynamics/Joints/b2PrismaticJoint.cpp
- Dynamics/Joints/b2PulleyJoint.cpp
- Dynamics/Joints/b2RevoluteJoint.cpp
- Dynamics/Joints/b2RopeJoint.cpp
- Dynamics/Joints/b2WeldJoint.cpp
- Dynamics/Joints/b2WheelJoint.cpp
-)
-set(BOX2D_Joints_HDRS
- Dynamics/Joints/b2DistanceJoint.h
- Dynamics/Joints/b2FrictionJoint.h
- Dynamics/Joints/b2GearJoint.h
- Dynamics/Joints/b2Joint.h
- Dynamics/Joints/b2MotorJoint.h
- Dynamics/Joints/b2MouseJoint.h
- Dynamics/Joints/b2PrismaticJoint.h
- Dynamics/Joints/b2PulleyJoint.h
- Dynamics/Joints/b2RevoluteJoint.h
- Dynamics/Joints/b2RopeJoint.h
- Dynamics/Joints/b2WeldJoint.h
- Dynamics/Joints/b2WheelJoint.h
-)
-set(BOX2D_Rope_SRCS
- Rope/b2Rope.cpp
-)
-set(BOX2D_Rope_HDRS
- Rope/b2Rope.h
-)
-set(BOX2D_General_HDRS
- Box2D.h
-)
-include_directories( ../ )
-
-if(BOX2D_BUILD_SHARED)
- add_library(Box2D_shared SHARED
- ${BOX2D_General_HDRS}
- ${BOX2D_Joints_SRCS}
- ${BOX2D_Joints_HDRS}
- ${BOX2D_Contacts_SRCS}
- ${BOX2D_Contacts_HDRS}
- ${BOX2D_Dynamics_SRCS}
- ${BOX2D_Dynamics_HDRS}
- ${BOX2D_Common_SRCS}
- ${BOX2D_Common_HDRS}
- ${BOX2D_Shapes_SRCS}
- ${BOX2D_Shapes_HDRS}
- ${BOX2D_Collision_SRCS}
- ${BOX2D_Collision_HDRS}
- ${BOX2D_Rope_SRCS}
- ${BOX2D_Rope_HDRS}
- )
- set_target_properties(Box2D_shared PROPERTIES
- OUTPUT_NAME "Box2D"
- CLEAN_DIRECT_OUTPUT 1
- VERSION ${BOX2D_VERSION}
- )
-endif()
-
-if(BOX2D_BUILD_STATIC)
- add_library(Box2D STATIC
- ${BOX2D_General_HDRS}
- ${BOX2D_Joints_SRCS}
- ${BOX2D_Joints_HDRS}
- ${BOX2D_Contacts_SRCS}
- ${BOX2D_Contacts_HDRS}
- ${BOX2D_Dynamics_SRCS}
- ${BOX2D_Dynamics_HDRS}
- ${BOX2D_Common_SRCS}
- ${BOX2D_Common_HDRS}
- ${BOX2D_Shapes_SRCS}
- ${BOX2D_Shapes_HDRS}
- ${BOX2D_Collision_SRCS}
- ${BOX2D_Collision_HDRS}
- ${BOX2D_Rope_SRCS}
- ${BOX2D_Rope_HDRS}
- )
- set_target_properties(Box2D PROPERTIES
- CLEAN_DIRECT_OUTPUT 1
- VERSION ${BOX2D_VERSION}
- )
-endif()
-
-# These are used to create visual studio folders.
-source_group(Collision FILES ${BOX2D_Collision_SRCS} ${BOX2D_Collision_HDRS})
-source_group(Collision\\Shapes FILES ${BOX2D_Shapes_SRCS} ${BOX2D_Shapes_HDRS})
-source_group(Common FILES ${BOX2D_Common_SRCS} ${BOX2D_Common_HDRS})
-source_group(Dynamics FILES ${BOX2D_Dynamics_SRCS} ${BOX2D_Dynamics_HDRS})
-source_group(Dynamics\\Contacts FILES ${BOX2D_Contacts_SRCS} ${BOX2D_Contacts_HDRS})
-source_group(Dynamics\\Joints FILES ${BOX2D_Joints_SRCS} ${BOX2D_Joints_HDRS})
-source_group(Include FILES ${BOX2D_General_HDRS})
-source_group(Rope FILES ${BOX2D_Rope_SRCS} ${BOX2D_Rope_HDRS})
-
-if(BOX2D_INSTALL)
- # install headers
- install(FILES ${BOX2D_General_HDRS} DESTINATION include/Box2D)
- install(FILES ${BOX2D_Collision_HDRS} DESTINATION include/Box2D/Collision)
- install(FILES ${BOX2D_Shapes_HDRS} DESTINATION include/Box2D/Collision/Shapes)
- install(FILES ${BOX2D_Common_HDRS} DESTINATION include/Box2D/Common)
- install(FILES ${BOX2D_Dynamics_HDRS} DESTINATION include/Box2D/Dynamics)
- install(FILES ${BOX2D_Contacts_HDRS} DESTINATION include/Box2D/Dynamics/Contacts)
- install(FILES ${BOX2D_Joints_HDRS} DESTINATION include/Box2D/Dynamics/Joints)
- install(FILES ${BOX2D_Rope_HDRS} DESTINATION include/Box2D/Rope)
-
- # install libraries
- if(BOX2D_BUILD_SHARED)
- install(TARGETS Box2D_shared EXPORT Box2D-targets
- LIBRARY DESTINATION ${LIB_INSTALL_DIR}
- ARCHIVE DESTINATION ${LIB_INSTALL_DIR}
- RUNTIME DESTINATION bin)
- endif()
- if(BOX2D_BUILD_STATIC)
- install(TARGETS Box2D EXPORT Box2D-targets DESTINATION ${LIB_INSTALL_DIR})
- endif()
-
- # install build system hooks for third-party apps
- install(EXPORT Box2D-targets DESTINATION ${LIB_INSTALL_DIR}/Box2D)
-
- set (BOX2D_INCLUDE_DIR ${CMAKE_INSTALL_PREFIX}/include)
- set (BOX2D_INCLUDE_DIRS ${BOX2D_INCLUDE_DIR} )
- set (BOX2D_LIBRARY_DIRS ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR})
- set (BOX2D_LIBRARY Box2D)
- set (BOX2D_LIBRARIES ${BOX2D_LIBRARY})
- set (BOX2D_USE_FILE ${CMAKE_INSTALL_PREFIX}/${LIB_INSTALL_DIR}/cmake/Box2D/UseBox2D.cmake)
- configure_file(Box2DConfig.cmake.in ${CMAKE_CURRENT_BINARY_DIR}/Box2DConfig.cmake @ONLY ESCAPE_QUOTES)
- install(FILES ${CMAKE_CURRENT_BINARY_DIR}/Box2DConfig.cmake UseBox2D.cmake DESTINATION ${LIB_INSTALL_DIR}/cmake/Box2D)
-endif(BOX2D_INSTALL)
diff --git a/external/Box2D/Collision/Shapes/b2ChainShape.cpp b/external/Box2D/Collision/Shapes/b2ChainShape.cpp
deleted file mode 100644
index 0c7282c..0000000
--- a/external/Box2D/Collision/Shapes/b2ChainShape.cpp
+++ /dev/null
@@ -1,193 +0,0 @@
-/*
-* 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 = NULL;
- m_count = 0;
-}
-
-void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
-{
- b2Assert(m_vertices == NULL && m_count == 0);
- b2Assert(count >= 3);
- 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 == NULL && 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;
-}
diff --git a/external/Box2D/Collision/Shapes/b2CircleShape.cpp b/external/Box2D/Collision/Shapes/b2CircleShape.cpp
index 39d4ae9..b437954 100644
--- a/external/Box2D/Collision/Shapes/b2CircleShape.cpp
+++ b/external/Box2D/Collision/Shapes/b2CircleShape.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -18,6 +18,7 @@
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <new>
+using namespace std;
b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
{
@@ -34,7 +35,7 @@ int32 b2CircleShape::GetChildCount() const
bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
{
- b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
+ b2Vec2 center = transform.position + b2Mul(transform.R, m_p);
b2Vec2 d = p - center;
return b2Dot(d, d) <= m_radius * m_radius;
}
@@ -48,15 +49,15 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input
{
B2_NOT_USED(childIndex);
- b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
+ b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
b2Vec2 s = input.p1 - position;
- float32 b = b2Dot(s, s) - m_radius * m_radius;
+ qreal 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;
+ qreal c = b2Dot(s, r);
+ qreal rr = b2Dot(r, r);
+ qreal sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2_epsilon)
@@ -65,7 +66,7 @@ bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input
}
// Find the point of intersection of the line with the circle.
- float32 a = -(c + b2Sqrt(sigma));
+ qreal a = -(c + b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
@@ -84,12 +85,12 @@ void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int3
{
B2_NOT_USED(childIndex);
- b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
+ b2Vec2 p = transform.position + b2Mul(transform.R, 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
+void b2CircleShape::ComputeMass(b2MassData* massData, qreal density) const
{
massData->mass = density * b2_pi * m_radius * m_radius;
massData->center = m_p;
diff --git a/external/Box2D/Collision/Shapes/b2CircleShape.h b/external/Box2D/Collision/Shapes/b2CircleShape.h
index 3209f92..d87ca2a 100644
--- a/external/Box2D/Collision/Shapes/b2CircleShape.h
+++ b/external/Box2D/Collision/Shapes/b2CircleShape.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -44,7 +44,7 @@ public:
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
- void ComputeMass(b2MassData* massData, float32 density) const;
+ void ComputeMass(b2MassData* massData, qreal density) const;
/// Get the supporting vertex index in the given direction.
int32 GetSupport(const b2Vec2& d) const;
diff --git a/external/Box2D/Collision/Shapes/b2EdgeShape.cpp b/external/Box2D/Collision/Shapes/b2EdgeShape.cpp
index 8bcd12f..c01a496 100644
--- a/external/Box2D/Collision/Shapes/b2EdgeShape.cpp
+++ b/external/Box2D/Collision/Shapes/b2EdgeShape.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -18,6 +18,7 @@
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
+using namespace std;
void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
{
@@ -57,8 +58,8 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
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 p1 = b2MulT(xf.R, input.p1 - xf.position);
+ b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
@@ -70,16 +71,16 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
// 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);
+ qreal numerator = b2Dot(normal, v1 - p1);
+ qreal denominator = b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
- float32 t = numerator / denominator;
- if (t < 0.0f || input.maxFraction < t)
+ qreal t = numerator / denominator;
+ if (t < 0.0f || 1.0f < t)
{
return false;
}
@@ -89,13 +90,13 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
- float32 rr = b2Dot(r, r);
+ qreal rr = b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
- float32 s = b2Dot(q - v1, r) / rr;
+ qreal s = b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
@@ -104,11 +105,11 @@ bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
output->fraction = t;
if (numerator > 0.0f)
{
- output->normal = -b2Mul(xf.q, normal);
+ output->normal = -normal;
}
else
{
- output->normal = b2Mul(xf.q, normal);
+ output->normal = normal;
}
return true;
}
@@ -128,7 +129,7 @@ void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIn
aabb->upperBound = upper + r;
}
-void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2EdgeShape::ComputeMass(b2MassData* massData, qreal density) const
{
B2_NOT_USED(density);
diff --git a/external/Box2D/Collision/Shapes/b2EdgeShape.h b/external/Box2D/Collision/Shapes/b2EdgeShape.h
index 0acef85..bf38682 100644
--- a/external/Box2D/Collision/Shapes/b2EdgeShape.h
+++ b/external/Box2D/Collision/Shapes/b2EdgeShape.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -49,7 +49,7 @@ public:
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
- void ComputeMass(b2MassData* massData, float32 density) const;
+ void ComputeMass(b2MassData* massData, qreal density) const;
/// These are the edge vertices
b2Vec2 m_vertex1, m_vertex2;
@@ -63,10 +63,6 @@ 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;
}
diff --git a/external/Box2D/Collision/Shapes/b2LoopShape.cpp b/external/Box2D/Collision/Shapes/b2LoopShape.cpp
new file mode 100644
index 0000000..deb2207
--- /dev/null
+++ b/external/Box2D/Collision/Shapes/b2LoopShape.cpp
@@ -0,0 +1,130 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* 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/b2LoopShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+#include <new>
+#include <cstring>
+using namespace std;
+
+b2LoopShape::~b2LoopShape()
+{
+ b2Free(m_vertices);
+ m_vertices = NULL;
+ m_count = 0;
+}
+
+void b2LoopShape::Create(const b2Vec2* vertices, int32 count)
+{
+ b2Assert(m_vertices == NULL && m_count == 0);
+ b2Assert(count >= 2);
+ m_count = count;
+ m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
+ memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
+}
+
+b2Shape* b2LoopShape::Clone(b2BlockAllocator* allocator) const
+{
+ void* mem = allocator->Allocate(sizeof(b2LoopShape));
+ b2LoopShape* clone = new (mem) b2LoopShape;
+ clone->Create(m_vertices, m_count);
+ return clone;
+}
+
+int32 b2LoopShape::GetChildCount() const
+{
+ return m_count;
+}
+
+void b2LoopShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
+{
+ b2Assert(2 <= m_count);
+ b2Assert(0 <= index && index < m_count);
+ edge->m_type = b2Shape::e_edge;
+ edge->m_radius = m_radius;
+ edge->m_hasVertex0 = true;
+ edge->m_hasVertex3 = true;
+
+ int32 i0 = index - 1 >= 0 ? index - 1 : m_count - 1;
+ int32 i1 = index;
+ int32 i2 = index + 1 < m_count ? index + 1 : 0;
+ int32 i3 = index + 2;
+ while (i3 >= m_count)
+ {
+ i3 -= m_count;
+ }
+
+ edge->m_vertex0 = m_vertices[i0];
+ edge->m_vertex1 = m_vertices[i1];
+ edge->m_vertex2 = m_vertices[i2];
+ edge->m_vertex3 = m_vertices[i3];
+}
+
+bool b2LoopShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
+{
+ B2_NOT_USED(xf);
+ B2_NOT_USED(p);
+ return false;
+}
+
+bool b2LoopShape::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 b2LoopShape::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 b2LoopShape::ComputeMass(b2MassData* massData, qreal density) const
+{
+ B2_NOT_USED(density);
+
+ massData->mass = 0.0f;
+ massData->center.SetZero();
+ massData->I = 0.0f;
+}
diff --git a/external/Box2D/Collision/Shapes/b2ChainShape.h b/external/Box2D/Collision/Shapes/b2LoopShape.h
index 44493c4..11d2e10 100644
--- a/external/Box2D/Collision/Shapes/b2ChainShape.h
+++ b/external/Box2D/Collision/Shapes/b2LoopShape.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -16,47 +16,28 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
-#ifndef B2_CHAIN_SHAPE_H
-#define B2_CHAIN_SHAPE_H
+#ifndef B2_LOOP_SHAPE_H
+#define B2_LOOP_SHAPE_H
#include <Box2D/Collision/Shapes/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.
+/// A loop shape is a free form sequence of line segments that form a circular list.
+/// The loop may cross upon itself, but this is not recommended for smooth collision.
+/// The loop has double 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
+class b2LoopShape : public b2Shape
{
public:
- b2ChainShape();
+ b2LoopShape();
/// The destructor frees the vertices using b2Free.
- ~b2ChainShape();
+ ~b2LoopShape();
- /// 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);
+ /// Create the loop shape, copy all vertices.
+ void Create(const b2Vec2* vertices, int32 count);
/// Implement b2Shape. Vertices are cloned using b2Alloc.
b2Shape* Clone(b2BlockAllocator* allocator) const;
@@ -80,26 +61,36 @@ public:
/// Chains have zero mass.
/// @see b2Shape::ComputeMass
- void ComputeMass(b2MassData* massData, float32 density) const;
+ void ComputeMass(b2MassData* massData, qreal density) const;
+
+ /// Get the number of vertices.
+ int32 GetCount() const { return m_count; }
+
+ /// Get the vertices (read-only).
+ const b2Vec2& GetVertex(int32 index) const
+ {
+ b2Assert(0 <= index && index < m_count);
+ return m_vertices[index];
+ }
+
+ /// Get the vertices (read-only).
+ const b2Vec2* GetVertices() const { return m_vertices; }
+
+protected:
/// 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()
+inline b2LoopShape::b2LoopShape()
{
- m_type = e_chain;
+ m_type = e_loop;
m_radius = b2_polygonRadius;
m_vertices = NULL;
m_count = 0;
- m_hasPrevVertex = false;
- m_hasNextVertex = false;
}
#endif
diff --git a/external/Box2D/Collision/Shapes/b2PolygonShape.cpp b/external/Box2D/Collision/Shapes/b2PolygonShape.cpp
index 26ba7b3..af0b855 100644
--- a/external/Box2D/Collision/Shapes/b2PolygonShape.cpp
+++ b/external/Box2D/Collision/Shapes/b2PolygonShape.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -27,9 +27,9 @@ b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
return clone;
}
-void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
+void b2PolygonShape::SetAsBox(qreal hx, qreal hy)
{
- m_count = 4;
+ m_vertexCount = 4;
m_vertices[0].Set(-hx, -hy);
m_vertices[1].Set( hx, -hy);
m_vertices[2].Set( hx, hy);
@@ -41,9 +41,9 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
m_centroid.SetZero();
}
-void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
+void b2PolygonShape::SetAsBox(qreal hx, qreal hy, const b2Vec2& center, qreal angle)
{
- m_count = 4;
+ m_vertexCount = 4;
m_vertices[0].Set(-hx, -hy);
m_vertices[1].Set( hx, -hy);
m_vertices[2].Set( hx, hy);
@@ -55,14 +55,14 @@ void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, floa
m_centroid = center;
b2Transform xf;
- xf.p = center;
- xf.q.Set(angle);
+ xf.position = center;
+ xf.R.Set(angle);
// Transform vertices and normals.
- for (int32 i = 0; i < m_count; ++i)
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
m_vertices[i] = b2Mul(xf, m_vertices[i]);
- m_normals[i] = b2Mul(xf.q, m_normals[i]);
+ m_normals[i] = b2Mul(xf.R, m_normals[i]);
}
}
@@ -76,7 +76,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
b2Assert(count >= 3);
b2Vec2 c; c.Set(0.0f, 0.0f);
- float32 area = 0.0f;
+ qreal area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
@@ -90,7 +90,7 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
pRef *= 1.0f / count;
#endif
- const float32 inv3 = 1.0f / 3.0f;
+ const qreal inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < count; ++i)
{
@@ -102,9 +102,9 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
- float32 D = b2Cross(e1, e2);
+ qreal D = b2Cross(e1, e2);
- float32 triangleArea = 0.5f * D;
+ qreal triangleArea = qAbs(0.5f * D);
area += triangleArea;
// Area weighted centroid
@@ -120,141 +120,63 @@ static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
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];
+ m_vertexCount = count;
- 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)
+ // Copy vertices.
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
- // Polygon is degenerate.
- b2Assert(false);
- SetAsBox(1.0f, 1.0f);
- return;
+ m_vertices[i] = vertices[i];
}
- // 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)
+ // Compute normals. Ensure the edges have non-zero length.
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
- float32 x = ps[i].x;
- if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
- {
- i0 = i;
- x0 = x;
- }
+ int32 i1 = i;
+ int32 i2 = i + 1 < m_vertexCount ? 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();
}
- int32 hull[b2_maxPolygonVertices];
- int32 m = 0;
- int32 ih = i0;
-
- for (;;)
+#ifdef _DEBUG
+ // Ensure the polygon is convex and the interior
+ // is to the left of each edge.
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
- hull[m] = ih;
+ int32 i1 = i;
+ int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0;
+ b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
- int32 ie = 0;
- for (int32 j = 1; j < n; ++j)
+ for (int32 j = 0; j < m_vertexCount; ++j)
{
- if (ie == ih)
+ // Don't check vertices on the current edge.
+ if (j == i1 || j == i2)
{
- ie = j;
continue;
}
+
+ b2Vec2 r = m_vertices[j] - m_vertices[i1];
- 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;
+ // Your polygon is non-convex (it has an indentation) or
+ // has colinear edges.
+ qreal s = b2Cross(edge, r);
+ b2Assert(s > 0.0f);
}
}
-
- 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();
- }
+#endif
// Compute the polygon centroid.
- m_centroid = ComputeCentroid(m_vertices, m);
+ m_centroid = ComputeCentroid(m_vertices, m_vertexCount);
}
bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
{
- b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
+ b2Vec2 pLocal = b2MulT(xf.R, p - xf.position);
- for (int32 i = 0; i < m_count; ++i)
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
- float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
+ qreal dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
if (dot > 0.0f)
{
return false;
@@ -270,21 +192,21 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu
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 p1 = b2MulT(xf.R, input.p1 - xf.position);
+ b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
b2Vec2 d = p2 - p1;
- float32 lower = 0.0f, upper = input.maxFraction;
+ qreal lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
- for (int32 i = 0; i < m_count; ++i)
+ for (int32 i = 0; i < m_vertexCount; ++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);
+ qreal numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
+ qreal denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
@@ -329,7 +251,7 @@ bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& inpu
if (index >= 0)
{
output->fraction = lower;
- output->normal = b2Mul(xf.q, m_normals[index]);
+ output->normal = b2Mul(xf.R, m_normals[index]);
return true;
}
@@ -343,7 +265,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil
b2Vec2 lower = b2Mul(xf, m_vertices[0]);
b2Vec2 upper = lower;
- for (int32 i = 1; i < m_count; ++i)
+ for (int32 i = 1; i < m_vertexCount; ++i)
{
b2Vec2 v = b2Mul(xf, m_vertices[i]);
lower = b2Min(lower, v);
@@ -355,7 +277,7 @@ void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 chil
aabb->upperBound = upper + r;
}
-void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
+void b2PolygonShape::ComputeMass(b2MassData* massData, qreal density) const
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
@@ -381,46 +303,52 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
//
// The rest of the derivation is handled by computer algebra.
- b2Assert(m_count >= 3);
+ b2Assert(m_vertexCount >= 3);
b2Vec2 center; center.Set(0.0f, 0.0f);
- float32 area = 0.0f;
- float32 I = 0.0f;
+ qreal area = 0.0f;
+ qreal I = 0.0f;
- // s is the reference point for forming triangles.
+ // pRef 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);
-
+ b2Vec2 pRef(0.0f, 0.0f);
+#if 0
// This code would put the reference point inside the polygon.
- for (int32 i = 0; i < m_count; ++i)
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
- s += m_vertices[i];
+ pRef += m_vertices[i];
}
- s *= 1.0f / m_count;
+ pRef *= 1.0f / count;
+#endif
- const float32 k_inv3 = 1.0f / 3.0f;
+ const qreal k_inv3 = 1.0f / 3.0f;
- for (int32 i = 0; i < m_count; ++i)
+ for (int32 i = 0; i < m_vertexCount; ++i)
{
// Triangle vertices.
- b2Vec2 e1 = m_vertices[i] - s;
- b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
+ b2Vec2 p1 = pRef;
+ b2Vec2 p2 = m_vertices[i];
+ b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
- float32 D = b2Cross(e1, e2);
+ b2Vec2 e1 = p2 - p1;
+ b2Vec2 e2 = p3 - p1;
- float32 triangleArea = 0.5f * D;
+ qreal D = b2Cross(e1, e2);
+
+ qreal triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
- center += triangleArea * k_inv3 * (e1 + e2);
+ center += triangleArea * k_inv3 * (p1 + p2 + p3);
- float32 ex1 = e1.x, ey1 = e1.y;
- float32 ex2 = e2.x, ey2 = e2.y;
+ qreal px = p1.x, py = p1.y;
+ qreal ex1 = e1.x, ey1 = e1.y;
+ qreal ex2 = e2.x, ey2 = e2.y;
- float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
- float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
+ qreal intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
+ qreal inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
- I += (0.25f * k_inv3 * D) * (intx2 + inty2);
+ I += D * (intx2 + inty2);
}
// Total mass
@@ -429,39 +357,8 @@ void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
// Center of mass
b2Assert(area > b2_epsilon);
center *= 1.0f / area;
- massData->center = center + s;
+ massData->center = center;
- // Inertia tensor relative to the local origin (point s).
+ // Inertia tensor relative to the local origin.
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;
}
diff --git a/external/Box2D/Collision/Shapes/b2PolygonShape.h b/external/Box2D/Collision/Shapes/b2PolygonShape.h
index b192980..b699f2d 100644
--- a/external/Box2D/Collision/Shapes/b2PolygonShape.h
+++ b/external/Box2D/Collision/Shapes/b2PolygonShape.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -36,24 +36,22 @@ public:
/// @see b2Shape::GetChildCount
int32 GetChildCount() const;
- /// Create a convex hull from the given array of local points.
+ /// Copy vertices. This assumes the vertices define a convex polygon.
+ /// It is assumed that the exterior is the the right of each edge.
/// 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);
+ void Set(const b2Vec2* vertices, int32 vertexCount);
- /// Build vertices to represent an axis-aligned box centered on the local origin.
+ /// Build vertices to represent an axis-aligned box.
/// @param hx the half-width.
/// @param hy the half-height.
- void SetAsBox(float32 hx, float32 hy);
+ void SetAsBox(qreal hx, qreal 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);
+ void SetAsBox(qreal hx, qreal hy, const b2Vec2& center, qreal angle);
/// @see b2Shape::TestPoint
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
@@ -66,35 +64,31 @@ public:
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
- void ComputeMass(b2MassData* massData, float32 density) const;
+ void ComputeMass(b2MassData* massData, qreal density) const;
/// Get the vertex count.
- int32 GetVertexCount() const { return m_count; }
+ int32 GetVertexCount() const { return m_vertexCount; }
/// Get a vertex by index.
const b2Vec2& GetVertex(int32 index) const;
- /// 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;
+ int32 m_vertexCount;
};
inline b2PolygonShape::b2PolygonShape()
{
m_type = e_polygon;
m_radius = b2_polygonRadius;
- m_count = 0;
+ m_vertexCount = 0;
m_centroid.SetZero();
}
inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
{
- b2Assert(0 <= index && index < m_count);
+ b2Assert(0 <= index && index < m_vertexCount);
return m_vertices[index];
}
diff --git a/external/Box2D/Collision/Shapes/b2Shape.h b/external/Box2D/Collision/Shapes/b2Shape.h
index 170ea1a..59f1788 100644
--- a/external/Box2D/Collision/Shapes/b2Shape.h
+++ b/external/Box2D/Collision/Shapes/b2Shape.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -27,13 +27,13 @@
struct b2MassData
{
/// The mass of the shape, usually in kilograms.
- float32 mass;
+ qreal 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;
+ qreal I;
};
/// A shape is used for collision detection. You can create a shape however you like.
@@ -45,13 +45,15 @@ public:
enum Type
{
+ e_unknown= -1,
e_circle = 0,
e_edge = 1,
e_polygon = 2,
- e_chain = 3,
+ e_loop = 3,
e_typeCount = 4
};
+ b2Shape() { m_type = e_unknown; }
virtual ~b2Shape() {}
/// Clone the concrete shape using the provided allocator.
@@ -87,10 +89,10 @@ public:
/// 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;
+ virtual void ComputeMass(b2MassData* massData, qreal density) const = 0;
Type m_type;
- float32 m_radius;
+ qreal m_radius;
};
inline b2Shape::Type b2Shape::GetType() const
diff --git a/external/Box2D/Collision/b2BroadPhase.cpp b/external/Box2D/Collision/b2BroadPhase.cpp
index 04e955d..bd2a376 100644
--- a/external/Box2D/Collision/b2BroadPhase.cpp
+++ b/external/Box2D/Collision/b2BroadPhase.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -17,6 +17,8 @@
*/
#include <Box2D/Collision/b2BroadPhase.h>
+#include <cstring>
+using namespace std;
b2BroadPhase::b2BroadPhase()
{
@@ -61,11 +63,6 @@ void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& di
}
}
-void b2BroadPhase::TouchProxy(int32 proxyId)
-{
- BufferMove(proxyId);
-}
-
void b2BroadPhase::BufferMove(int32 proxyId)
{
if (m_moveCount == m_moveCapacity)
@@ -88,6 +85,7 @@ void b2BroadPhase::UnBufferMove(int32 proxyId)
if (m_moveBuffer[i] == proxyId)
{
m_moveBuffer[i] = e_nullProxy;
+ return;
}
}
}
diff --git a/external/Box2D/Collision/b2BroadPhase.h b/external/Box2D/Collision/b2BroadPhase.h
index 0c460ca..e14dcb2 100644
--- a/external/Box2D/Collision/b2BroadPhase.h
+++ b/external/Box2D/Collision/b2BroadPhase.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -28,6 +28,7 @@ struct b2Pair
{
int32 proxyIdA;
int32 proxyIdB;
+ int32 next;
};
/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
@@ -56,9 +57,6 @@ public:
/// 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;
@@ -90,19 +88,8 @@ public:
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);
+ /// Compute the height of the embedded tree.
+ int32 ComputeHeight() const;
private:
@@ -166,19 +153,9 @@ inline int32 b2BroadPhase::GetProxyCount() const
return m_proxyCount;
}
-inline int32 b2BroadPhase::GetTreeHeight() const
-{
- return m_tree.GetHeight();
-}
-
-inline int32 b2BroadPhase::GetTreeBalance() const
+inline int32 b2BroadPhase::ComputeHeight() const
{
- return m_tree.GetMaxBalance();
-}
-
-inline float32 b2BroadPhase::GetTreeQuality() const
-{
- return m_tree.GetAreaRatio();
+ return m_tree.ComputeHeight();
}
template <typename T>
@@ -234,7 +211,7 @@ void b2BroadPhase::UpdatePairs(T* callback)
}
// Try to keep the tree balanced.
- //m_tree.Rebalance(4);
+ m_tree.Rebalance(4);
}
template <typename T>
@@ -249,9 +226,4 @@ inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) cons
m_tree.RayCast(callback, input);
}
-inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
-{
- m_tree.ShiftOrigin(newOrigin);
-}
-
#endif
diff --git a/external/Box2D/Collision/b2CollideCircle.cpp b/external/Box2D/Collision/b2CollideCircle.cpp
index 4181fd4..e20edcf 100644
--- a/external/Box2D/Collision/b2CollideCircle.cpp
+++ b/external/Box2D/Collision/b2CollideCircle.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -31,9 +31,9 @@ void b2CollideCircles(
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;
+ qreal distSqr = b2Dot(d, d);
+ qreal rA = circleA->m_radius, rB = circleB->m_radius;
+ qreal radius = rA + rB;
if (distSqr > radius * radius)
{
return;
@@ -61,15 +61,15 @@ void b2CollidePolygonAndCircle(
// 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;
+ qreal separation = -b2_maxFloat;
+ qreal radius = polygonA->m_radius + circleB->m_radius;
+ int32 vertexCount = polygonA->m_vertexCount;
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]);
+ qreal s = b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
@@ -103,8 +103,8 @@ void b2CollidePolygonAndCircle(
}
// Compute barycentric coordinates
- float32 u1 = b2Dot(cLocal - v1, v2 - v1);
- float32 u2 = b2Dot(cLocal - v2, v1 - v2);
+ qreal u1 = b2Dot(cLocal - v1, v2 - v1);
+ qreal u2 = b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v1) > radius * radius)
@@ -138,7 +138,7 @@ void b2CollidePolygonAndCircle(
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
- float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
+ qreal separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
diff --git a/external/Box2D/Collision/b2CollideEdge.cpp b/external/Box2D/Collision/b2CollideEdge.cpp
index 88e311c..d2035a6 100644
--- a/external/Box2D/Collision/b2CollideEdge.cpp
+++ b/external/Box2D/Collision/b2CollideEdge.cpp
@@ -1,26 +1,33 @@
/*
- * 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.
- */
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
+*
+* 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>
+enum b2EdgeType
+{
+ b2_isolated,
+ b2_concave,
+ b2_flat,
+ b2_convex
+};
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
@@ -29,49 +36,49 @@ void b2CollideEdgeAndCircle(b2Manifold* manifold,
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;
-
+ qreal u = b2Dot(e, B - Q);
+ qreal v = b2Dot(e, Q - A);
+
+ qreal 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);
+ qreal 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);
-
+ qreal 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;
@@ -89,27 +96,27 @@ void b2CollideEdgeAndCircle(b2Manifold* manifold,
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
- float32 dd = b2Dot(d, d);
+ qreal 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);
-
+ qreal 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;
@@ -121,25 +128,25 @@ void b2CollideEdgeAndCircle(b2Manifold* manifold,
manifold->points[0].localPoint = circleB->m_p;
return;
}
-
+
// Region AB
- float32 den = b2Dot(e, e);
+ qreal 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);
+ qreal 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;
@@ -151,7 +158,6 @@ void b2CollideEdgeAndCircle(b2Manifold* manifold,
manifold->points[0].localPoint = circleB->m_p;
}
-// This structure is used to keep track of the best separating axis.
struct b2EPAxis
{
enum Type
@@ -160,305 +166,142 @@ struct b2EPAxis
e_edgeA,
e_edgeB
};
-
+
Type type;
int32 index;
- float32 separation;
+ qreal separation;
};
-// This holds polygon B expressed in frame A.
-struct b2TempPolygon
+// Edge shape plus more stuff.
+struct b2FatEdge
{
- b2Vec2 vertices[b2_maxPolygonVertices];
- b2Vec2 normals[b2_maxPolygonVertices];
- int32 count;
+ b2Vec2 v0, v1, v2, v3;
+ b2Vec2 normal;
+ bool hasVertex0, hasVertex3;
};
-// Reference face used for clipping
-struct b2ReferenceFace
+// This lets us treate and edge shape and a polygon in the same
+// way in the SAT collider.
+struct b2EPProxy
{
- int32 i1, i2;
-
- b2Vec2 v1, v2;
-
- b2Vec2 normal;
-
- b2Vec2 sideNormal1;
- float32 sideOffset1;
-
- b2Vec2 sideNormal2;
- float32 sideOffset2;
+ b2Vec2 vertices[b2_maxPolygonVertices];
+ b2Vec2 normals[b2_maxPolygonVertices];
+ b2Vec2 centroid;
+ int32 count;
};
// 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);
+ b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA,
+ const b2PolygonShape* polygonB_in, const b2Transform& xfB);
+
+ void Collide(b2Manifold* manifold);
+
+ void ComputeAdjacency();
b2EPAxis ComputeEdgeSeparation();
b2EPAxis ComputePolygonSeparation();
-
- enum VertexType
- {
- e_isolated,
- e_concave,
- e_convex
- };
-
- b2TempPolygon m_polygonB;
-
+ void FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2);
+
+ b2FatEdge m_edgeA;
+
+ b2EPProxy m_proxyA, m_proxyB;
+
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;
+ b2Vec2 m_normal0, m_normal2;
+ b2Vec2 m_limit11, m_limit12;
+ b2Vec2 m_limit21, m_limit22;
+ qreal m_radius;
};
-// 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)
+b2EPCollider::b2EPCollider(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)
+
+ // Edge geometry
+ m_edgeA.v0 = edgeA->m_vertex0;
+ m_edgeA.v1 = edgeA->m_vertex1;
+ m_edgeA.v2 = edgeA->m_vertex2;
+ m_edgeA.v3 = edgeA->m_vertex3;
+ b2Vec2 e = m_edgeA.v2 - m_edgeA.v1;
+
+ // Normal points outwards in CCW order.
+ m_edgeA.normal.Set(e.y, -e.x);
+ m_edgeA.normal.Normalize();
+ m_edgeA.hasVertex0 = edgeA->m_hasVertex0;
+ m_edgeA.hasVertex3 = edgeA->m_hasVertex3;
+
+ // Proxy for edge
+ m_proxyA.vertices[0] = m_edgeA.v1;
+ m_proxyA.vertices[1] = m_edgeA.v2;
+ m_proxyA.normals[0] = m_edgeA.normal;
+ m_proxyA.normals[1] = -m_edgeA.normal;
+ m_proxyA.centroid = 0.5f * (m_edgeA.v1 + m_edgeA.v2);
+ m_proxyA.count = 2;
+
+ // Proxy for polygon
+ m_proxyB.count = polygonB->m_vertexCount;
+ m_proxyB.centroid = b2Mul(m_xf, polygonB->m_centroid);
+ for (int32 i = 0; i < polygonB->m_vertexCount; ++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_proxyB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
+ m_proxyB.normals[i] = b2Mul(m_xf.R, polygonB->m_normals[i]);
}
-
+
m_radius = 2.0f * b2_polygonRadius;
-
+
+ m_limit11.SetZero();
+ m_limit12.SetZero();
+ m_limit21.SetZero();
+ m_limit22.SetZero();
+}
+
+// Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
+// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
+// to allow the polygon to slide smoothly over an edge chain.
+//
+// Algorithm
+// 1. Classify front-side or back-side collision with edge.
+// 2. Compute separation
+// 3. Process adjacent edges
+// 4. Classify adjacent edge as convex, flat, null, or concave
+// 5. Skip null or concave edges. Concave edges get a separate manifold.
+// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
+// 7. If the edge is convex, compute it's separation.
+// 8. Use the minimum separation of up to three edges. If the minimum separation
+// is not the primary edge, return.
+// 9. If the minimum separation is the primary edge, compute the contact points and return.
+void b2EPCollider::Collide(b2Manifold* manifold)
+{
manifold->pointCount = 0;
-
+
+ ComputeAdjacency();
+
b2EPAxis edgeAxis = ComputeEdgeSeparation();
-
+
// If no valid normal can be found than this edge should not collide.
+ // This can happen on the middle edge of a 3-edge zig-zag chain.
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;
-
+ const qreal k_relativeTol = 0.98f;
+ const qreal k_absoluteTol = 0.001f;
+
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxis::e_unknown)
{
@@ -472,130 +315,92 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const
{
primaryAxis = edgeAxis;
}
-
- b2ClipVertex ie[2];
- b2ReferenceFace rf;
+
+ b2EPProxy* proxy1;
+ b2EPProxy* proxy2;
+ b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
+ proxy1 = &m_proxyA;
+ proxy2 = &m_proxyB;
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
{
+ proxy1 = &m_proxyB;
+ proxy2 = &m_proxyA;
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];
}
+
+ int32 edge1 = primaryAxis.index;
+
+ FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);
+ int32 count1 = proxy1->count;
+ const b2Vec2* vertices1 = proxy1->vertices;
+
+ int32 iv1 = edge1;
+ int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
+
+ b2Vec2 v11 = vertices1[iv1];
+ b2Vec2 v12 = vertices1[iv2];
+
+ b2Vec2 tangent = v12 - v11;
+ tangent.Normalize();
- 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);
-
+ b2Vec2 normal = b2Cross(tangent, 1.0f);
+ b2Vec2 planePoint = 0.5f * (v11 + v12);
+
+ // Face offset.
+ qreal frontOffset = b2Dot(normal, v11);
+
+ // Side offsets, extended by polytope skin thickness.
+ qreal sideOffset1 = -b2Dot(tangent, v11) + m_radius;
+ qreal sideOffset2 = b2Dot(tangent, v12) + m_radius;
+
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
- int32 np;
-
+ int np;
+
// Clip to box side 1
- np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
-
+ np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
+
if (np < b2_maxManifoldPoints)
{
return;
}
-
+
// Clip to negative box side 1
- np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
-
+ np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
+
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;
+ manifold->localNormal = normal;
+ manifold->localPoint = planePoint;
}
else
{
- manifold->localNormal = polygonB->m_normals[rf.i1];
- manifold->localPoint = polygonB->m_vertices[rf.i1];
+ manifold->localNormal = b2MulT(m_xf.R, normal);
+ manifold->localPoint = b2MulT(m_xf, planePoint);
}
-
+
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
- float32 separation;
-
- separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
+ qreal separation;
+ separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
+
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
-
+
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
@@ -609,31 +414,169 @@ void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
-
+
++pointCount;
}
}
-
+
manifold->pointCount = pointCount;
}
+// Compute allowable normal ranges based on adjacency.
+// A normal n is allowable iff:
+// cross(n, n1) >= 0.0f && cross(n2, n) >= 0.0f
+// n points from A to B (edge to polygon)
+void b2EPCollider::ComputeAdjacency()
+{
+ b2Vec2 v0 = m_edgeA.v0;
+ b2Vec2 v1 = m_edgeA.v1;
+ b2Vec2 v2 = m_edgeA.v2;
+ b2Vec2 v3 = m_edgeA.v3;
+
+ // Determine allowable the normal regions based on adjacency.
+ // Note: it may be possible that no normal is admissable.
+ b2Vec2 centerB = m_proxyB.centroid;
+ if (m_edgeA.hasVertex0)
+ {
+ b2Vec2 e0 = v1 - v0;
+ b2Vec2 e1 = v2 - v1;
+ b2Vec2 n0(e0.y, -e0.x);
+ b2Vec2 n1(e1.y, -e1.x);
+ n0.Normalize();
+ n1.Normalize();
+
+ bool convex = b2Cross(n0, n1) >= 0.0f;
+ bool front0 = b2Dot(n0, centerB - v0) >= 0.0f;
+ bool front1 = b2Dot(n1, centerB - v1) >= 0.0f;
+
+ if (convex)
+ {
+ if (front0 || front1)
+ {
+ m_limit11 = n1;
+ m_limit12 = n0;
+ }
+ else
+ {
+ m_limit11 = -n1;
+ m_limit12 = -n0;
+ }
+ }
+ else
+ {
+ if (front0 && front1)
+ {
+ m_limit11 = n0;
+ m_limit12 = n1;
+ }
+ else
+ {
+ m_limit11 = -n0;
+ m_limit12 = -n1;
+ }
+ }
+ }
+ else
+ {
+ m_limit11.SetZero();
+ m_limit12.SetZero();
+ }
+
+ if (m_edgeA.hasVertex3)
+ {
+ b2Vec2 e1 = v2 - v1;
+ b2Vec2 e2 = v3 - v2;
+ b2Vec2 n1(e1.y, -e1.x);
+ b2Vec2 n2(e2.y, -e2.x);
+ n1.Normalize();
+ n2.Normalize();
+
+ bool convex = b2Cross(n1, n2) >= 0.0f;
+ bool front1 = b2Dot(n1, centerB - v1) >= 0.0f;
+ bool front2 = b2Dot(n2, centerB - v2) >= 0.0f;
+
+ if (convex)
+ {
+ if (front1 || front2)
+ {
+ m_limit21 = n2;
+ m_limit22 = n1;
+ }
+ else
+ {
+ m_limit21 = -n2;
+ m_limit22 = -n1;
+ }
+ }
+ else
+ {
+ if (front1 && front2)
+ {
+ m_limit21 = n1;
+ m_limit22 = n2;
+ }
+ else
+ {
+ m_limit21 = -n1;
+ m_limit22 = -n2;
+ }
+ }
+ }
+ else
+ {
+ m_limit21.SetZero();
+ m_limit22.SetZero();
+ }
+}
+
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)
+ // EdgeA separation
+ b2EPAxis bestAxis;
+ bestAxis.type = b2EPAxis::e_unknown;
+ bestAxis.index = -1;
+ bestAxis.separation = -FLT_MAX;
+ b2Vec2 normals[2] = {m_edgeA.normal, -m_edgeA.normal};
+
+ for (int32 i = 0; i < 2; ++i)
{
- float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
- if (s < axis.separation)
+ b2Vec2 n = normals[i];
+
+ // Adjacency
+ bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop;
+ bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop;
+
+ if (valid1 == false || valid2 == false)
{
- axis.separation = s;
+ continue;
+ }
+
+ b2EPAxis axis;
+ axis.type = b2EPAxis::e_edgeA;
+ axis.index = i;
+ axis.separation = FLT_MAX;
+
+ for (int32 j = 0; j < m_proxyB.count; ++j)
+ {
+ qreal s = b2Dot(n, m_proxyB.vertices[j] - m_edgeA.v1);
+ if (s < axis.separation)
+ {
+ axis.separation = s;
+ }
+ }
+
+ if (axis.separation > m_radius)
+ {
+ return axis;
+ }
+
+ if (axis.separation > bestAxis.separation)
+ {
+ bestAxis = axis;
}
}
-
- return axis;
+
+ return bestAxis;
}
b2EPAxis b2EPCollider::ComputePolygonSeparation()
@@ -642,42 +585,30 @@ b2EPAxis b2EPCollider::ComputePolygonSeparation()
axis.type = b2EPAxis::e_unknown;
axis.index = -1;
axis.separation = -FLT_MAX;
+ for (int32 i = 0; i < m_proxyB.count; ++i)
+ {
+ b2Vec2 n = -m_proxyB.normals[i];
- b2Vec2 perp(-m_normal.y, m_normal.x);
+ // Adjacency
+ bool valid1 = b2Cross(n, m_limit11) >= -b2_angularSlop && b2Cross(m_limit12, n) >= -b2_angularSlop;
+ bool valid2 = b2Cross(n, m_limit21) >= -b2_angularSlop && b2Cross(m_limit22, n) >= -b2_angularSlop;
+
+ if (valid1 == false && valid2 == false)
+ {
+ continue;
+ }
+
+ qreal s1 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v1);
+ qreal s2 = b2Dot(n, m_proxyB.vertices[i] - m_edgeA.v2);
+ qreal s = b2Min(s1, s2);
- 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;
@@ -685,14 +616,58 @@ b2EPAxis b2EPCollider::ComputePolygonSeparation()
axis.separation = s;
}
}
-
+
return axis;
}
+void b2EPCollider::FindIncidentEdge(b2ClipVertex c[2], const b2EPProxy* proxy1, int32 edge1, const b2EPProxy* proxy2)
+{
+ int32 count1 = proxy1->count;
+ const b2Vec2* normals1 = proxy1->normals;
+
+ int32 count2 = proxy2->count;
+ const b2Vec2* vertices2 = proxy2->vertices;
+ const b2Vec2* normals2 = proxy2->normals;
+
+ b2Assert(0 <= edge1 && edge1 < count1);
+
+ // Get the normal of the reference edge in proxy2's frame.
+ b2Vec2 normal1 = normals1[edge1];
+
+ // Find the incident edge on proxy2.
+ int32 index = 0;
+ qreal minDot = b2_maxFloat;
+ for (int32 i = 0; i < count2; ++i)
+ {
+ qreal 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 = 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 = 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;
+}
+
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
- const b2EdgeShape* edgeA, const b2Transform& xfA,
- const b2PolygonShape* polygonB, const b2Transform& xfB)
+ const b2EdgeShape* edgeA, const b2Transform& xfA,
+ const b2PolygonShape* polygonB, const b2Transform& xfB)
{
- b2EPCollider collider;
- collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
+ b2EPCollider collider(edgeA, xfA, polygonB, xfB);
+ collider.Collide(manifold);
}
diff --git a/external/Box2D/Collision/b2CollidePolygon.cpp b/external/Box2D/Collision/b2CollidePolygon.cpp
index 0aac38b..9974f68 100644
--- a/external/Box2D/Collision/b2CollidePolygon.cpp
+++ b/external/Box2D/Collision/b2CollidePolygon.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,69 +19,148 @@
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
+// Find the separation between poly1 and poly2 for a give edge normal on poly1.
+static qreal b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
+ const b2PolygonShape* poly2, const b2Transform& xf2)
+{
+ int32 count1 = poly1->m_vertexCount;
+ const b2Vec2* vertices1 = poly1->m_vertices;
+ const b2Vec2* normals1 = poly1->m_normals;
+
+ int32 count2 = poly2->m_vertexCount;
+ const b2Vec2* vertices2 = poly2->m_vertices;
+
+ b2Assert(0 <= edge1 && edge1 < count1);
+
+ // Convert normal from poly1's frame into poly2's frame.
+ b2Vec2 normal1World = b2Mul(xf1.R, normals1[edge1]);
+ b2Vec2 normal1 = b2MulT(xf2.R, normal1World);
+
+ // Find support vertex on poly2 for -normal.
+ int32 index = 0;
+ qreal minDot = b2_maxFloat;
+
+ for (int32 i = 0; i < count2; ++i)
+ {
+ qreal dot = b2Dot(vertices2[i], normal1);
+ if (dot < minDot)
+ {
+ minDot = dot;
+ index = i;
+ }
+ }
+
+ b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
+ b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
+ qreal separation = b2Dot(v2 - v1, normal1World);
+ return separation;
+}
+
// Find the max separation between poly1 and poly2 using edge normals from poly1.
-static float32 b2FindMaxSeparation(int32* edgeIndex,
+static qreal 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;
+ int32 count1 = poly1->m_vertexCount;
+ const b2Vec2* normals1 = poly1->m_normals;
+
+ // Vector pointing from the centroid of poly1 to the centroid of poly2.
+ b2Vec2 d = b2Mul(xf2, poly2->m_centroid) - b2Mul(xf1, poly1->m_centroid);
+ b2Vec2 dLocal1 = b2MulT(xf1.R, d);
+
+ // Find edge normal on poly1 that has the largest projection onto d.
+ int32 edge = 0;
+ qreal maxDot = -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)
+ qreal dot = b2Dot(normals1[i], dLocal1);
+ if (dot > maxDot)
{
- float32 sij = b2Dot(n, v2s[j] - v1);
- if (sij < si)
- {
- si = sij;
- }
+ maxDot = dot;
+ edge = i;
}
+ }
+
+ // Get the separation for the edge normal.
+ qreal s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+
+ // Check the separation for the previous edge normal.
+ int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
+ qreal sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
+
+ // Check the separation for the next edge normal.
+ int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
+ qreal sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
+
+ // Find the best edge and the search direction.
+ int32 bestEdge;
+ qreal bestSeparation;
+ int32 increment;
+ if (sPrev > s && sPrev > sNext)
+ {
+ increment = -1;
+ bestEdge = prevEdge;
+ bestSeparation = sPrev;
+ }
+ else if (sNext > s)
+ {
+ increment = 1;
+ bestEdge = nextEdge;
+ bestSeparation = sNext;
+ }
+ else
+ {
+ *edgeIndex = edge;
+ return s;
+ }
- if (si > maxSeparation)
+ // Perform a local search for the best edge normal.
+ for ( ; ; )
+ {
+ if (increment == -1)
+ edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
+ else
+ edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
+
+ s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
+
+ if (s > bestSeparation)
+ {
+ bestEdge = edge;
+ bestSeparation = s;
+ }
+ else
{
- maxSeparation = si;
- bestIndex = i;
+ break;
}
}
- *edgeIndex = bestIndex;
- return maxSeparation;
+ *edgeIndex = bestEdge;
+ return bestSeparation;
}
static void b2FindIncidentEdge(b2ClipVertex c[2],
const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
+ int32 count1 = poly1->m_vertexCount;
const b2Vec2* normals1 = poly1->m_normals;
- int32 count2 = poly2->m_count;
+ int32 count2 = poly2->m_vertexCount;
const b2Vec2* vertices2 = poly2->m_vertices;
const b2Vec2* normals2 = poly2->m_normals;
- b2Assert(0 <= edge1 && edge1 < poly1->m_count);
+ b2Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
- b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
+ b2Vec2 normal1 = b2MulT(xf2.R, b2Mul(xf1.R, normals1[edge1]));
// Find the incident edge on poly2.
int32 index = 0;
- float32 minDot = b2_maxFloat;
+ qreal minDot = b2_maxFloat;
for (int32 i = 0; i < count2; ++i)
{
- float32 dot = b2Dot(normal1, normals2[i]);
+ qreal dot = b2Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
@@ -118,26 +197,27 @@ void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyB, const b2Transform& xfB)
{
manifold->pointCount = 0;
- float32 totalRadius = polyA->m_radius + polyB->m_radius;
+ qreal totalRadius = polyA->m_radius + polyB->m_radius;
int32 edgeA = 0;
- float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
+ qreal separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int32 edgeB = 0;
- float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
+ qreal 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
+ int32 edge1; // reference edge
uint8 flip;
- const float32 k_tol = 0.1f * b2_linearSlop;
+ const qreal k_relativeTol = 0.98f;
+ const qreal k_absoluteTol = 0.001f;
- if (separationB > separationA + k_tol)
+ if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
@@ -161,7 +241,7 @@ void b2CollidePolygons(b2Manifold* manifold,
b2ClipVertex incidentEdge[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
- int32 count1 = poly1->m_count;
+ int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
@@ -176,18 +256,18 @@ void b2CollidePolygons(b2Manifold* manifold,
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
- b2Vec2 tangent = b2Mul(xf1.q, localTangent);
+ b2Vec2 tangent = b2Mul(xf1.R, localTangent);
b2Vec2 normal = b2Cross(tangent, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
- float32 frontOffset = b2Dot(normal, v11);
+ qreal frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
- float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
- float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
+ qreal sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
+ qreal sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
@@ -215,7 +295,7 @@ void b2CollidePolygons(b2Manifold* manifold,
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
- float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
+ qreal separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
diff --git a/external/Box2D/Collision/b2Collision.cpp b/external/Box2D/Collision/b2Collision.cpp
index 4ebf355..3428be9 100644
--- a/external/Box2D/Collision/b2Collision.cpp
+++ b/external/Box2D/Collision/b2Collision.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -20,8 +20,8 @@
#include <Box2D/Collision/b2Distance.h>
void b2WorldManifold::Initialize(const b2Manifold* manifold,
- const b2Transform& xfA, float32 radiusA,
- const b2Transform& xfB, float32 radiusB)
+ const b2Transform& xfA, qreal radiusA,
+ const b2Transform& xfB, qreal radiusB)
{
if (manifold->pointCount == 0)
{
@@ -44,13 +44,12 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
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);
+ normal = b2Mul(xfA.R, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
@@ -59,14 +58,13 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
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);
+ normal = b2Mul(xfB.R, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
@@ -75,7 +73,6 @@ void b2WorldManifold::Initialize(const b2Manifold* manifold,
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.
@@ -132,8 +129,8 @@ void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState st
// From Real-time Collision Detection, p179.
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
{
- float32 tmin = -b2_maxFloat;
- float32 tmax = b2_maxFloat;
+ qreal tmin = -b2_maxFloat;
+ qreal tmax = b2_maxFloat;
b2Vec2 p = input.p1;
b2Vec2 d = input.p2 - input.p1;
@@ -153,12 +150,12 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
}
else
{
- float32 inv_d = 1.0f / d(i);
- float32 t1 = (lowerBound(i) - p(i)) * inv_d;
- float32 t2 = (upperBound(i) - p(i)) * inv_d;
+ qreal inv_d = 1.0f / d(i);
+ qreal t1 = (lowerBound(i) - p(i)) * inv_d;
+ qreal t2 = (upperBound(i) - p(i)) * inv_d;
// Sign of the normal vector.
- float32 s = -1.0f;
+ qreal s = -1.0f;
if (t1 > t2)
{
@@ -199,14 +196,14 @@ bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
// Sutherland-Hodgman clipping.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
- const b2Vec2& normal, float32 offset, int32 vertexIndexA)
+ const b2Vec2& normal, qreal 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;
+ qreal distance0 = b2Dot(normal, vIn[0].v) - offset;
+ qreal distance1 = b2Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
@@ -216,11 +213,11 @@ int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
- float32 interp = distance0 / (distance0 - distance1);
+ qreal 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.indexA = 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;
diff --git a/external/Box2D/Collision/b2Collision.h b/external/Box2D/Collision/b2Collision.h
index ad023b3..1729ed9 100644
--- a/external/Box2D/Collision/b2Collision.h
+++ b/external/Box2D/Collision/b2Collision.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -20,7 +20,7 @@
#define B2_COLLISION_H
#include <Box2D/Common/b2Math.h>
-#include <limits.h>
+#include <climits>
/// @file
/// Structures and functions used for computing contact points, distance
@@ -69,9 +69,10 @@ union b2ContactID
struct b2ManifoldPoint
{
b2Vec2 localPoint; ///< usage depends on manifold type
- float32 normalImpulse; ///< the non-penetration impulse
- float32 tangentImpulse; ///< the friction impulse
+ qreal normalImpulse; ///< the non-penetration impulse
+ qreal tangentImpulse; ///< the friction impulse
b2ContactID id; ///< uniquely identifies a contact point between two shapes
+ bool isNew;
};
/// A manifold for two touching convex shapes.
@@ -114,12 +115,11 @@ struct b2WorldManifold
/// 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);
+ const b2Transform& xfA, qreal radiusA,
+ const b2Transform& xfB, qreal 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
+ b2Vec2 normal; ///< world vector pointing from A to B
+ b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection)
};
/// This is used for determining the state of contact points.
@@ -147,7 +147,7 @@ struct b2ClipVertex
struct b2RayCastInput
{
b2Vec2 p1, p2;
- float32 maxFraction;
+ qreal maxFraction;
};
/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
@@ -155,7 +155,7 @@ struct b2RayCastInput
struct b2RayCastOutput
{
b2Vec2 normal;
- float32 fraction;
+ qreal fraction;
};
/// An axis aligned bounding box.
@@ -177,10 +177,10 @@ struct b2AABB
}
/// Get the perimeter length
- float32 GetPerimeter() const
+ qreal GetPerimeter() const
{
- float32 wx = upperBound.x - lowerBound.x;
- float32 wy = upperBound.y - lowerBound.y;
+ qreal wx = upperBound.x - lowerBound.x;
+ qreal wy = upperBound.y - lowerBound.y;
return 2.0f * (wx + wy);
}
@@ -242,7 +242,7 @@ void b2CollideEdgeAndPolygon(b2Manifold* manifold,
/// Clipping for contact manifolds.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
- const b2Vec2& normal, float32 offset, int32 vertexIndexA);
+ const b2Vec2& normal, qreal offset, int32 vertexIndexA);
/// Determine if two generic shapes overlap.
bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,
diff --git a/external/Box2D/Collision/b2Distance.cpp b/external/Box2D/Collision/b2Distance.cpp
index 78daab3..90a7181 100644
--- a/external/Box2D/Collision/b2Distance.cpp
+++ b/external/Box2D/Collision/b2Distance.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,7 +19,7 @@
#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/b2LoopShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
@@ -31,7 +31,7 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
{
case b2Shape::e_circle:
{
- const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
+ const b2CircleShape* circle = (b2CircleShape*)shape;
m_vertices = &circle->m_p;
m_count = 1;
m_radius = circle->m_radius;
@@ -40,37 +40,37 @@ void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
case b2Shape::e_polygon:
{
- const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
+ const b2PolygonShape* polygon = (b2PolygonShape*)shape;
m_vertices = polygon->m_vertices;
- m_count = polygon->m_count;
+ m_count = polygon->m_vertexCount;
m_radius = polygon->m_radius;
}
break;
- case b2Shape::e_chain:
+ case b2Shape::e_loop:
{
- const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
- b2Assert(0 <= index && index < chain->m_count);
+ const b2LoopShape* loop = (b2LoopShape*)shape;
+ b2Assert(0 <= index && index < loop->GetCount());
- m_buffer[0] = chain->m_vertices[index];
- if (index + 1 < chain->m_count)
+ m_buffer[0] = loop->GetVertex(index);
+ if (index + 1 < loop->GetCount())
{
- m_buffer[1] = chain->m_vertices[index + 1];
+ m_buffer[1] = loop->GetVertex(index + 1);
}
else
{
- m_buffer[1] = chain->m_vertices[0];
+ m_buffer[1] = loop->GetVertex(0);
}
m_vertices = m_buffer;
m_count = 2;
- m_radius = chain->m_radius;
+ m_radius = loop->m_radius;
}
break;
case b2Shape::e_edge:
{
- const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
+ const b2EdgeShape* edge = (b2EdgeShape*)shape;
m_vertices = &edge->m_vertex1;
m_count = 2;
m_radius = edge->m_radius;
@@ -88,7 +88,7 @@ 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
+ qreal a; // barycentric coordinate for closest point
int32 indexA; // wA index
int32 indexB; // wB index
};
@@ -121,8 +121,8 @@ struct b2Simplex
// old metric then flush the simplex.
if (m_count > 1)
{
- float32 metric1 = cache->metric;
- float32 metric2 = GetMetric();
+ qreal metric1 = cache->metric;
+ qreal metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
{
// Reset the simplex.
@@ -141,7 +141,6 @@ struct b2Simplex
v->wA = b2Mul(transformA, wALocal);
v->wB = b2Mul(transformB, wBLocal);
v->w = v->wB - v->wA;
- v->a = 1.0f;
m_count = 1;
}
}
@@ -168,7 +167,7 @@ struct b2Simplex
case 2:
{
b2Vec2 e12 = m_v2.w - m_v1.w;
- float32 sgn = b2Cross(e12, -m_v1.w);
+ qreal sgn = b2Cross(e12, -m_v1.w);
if (sgn > 0.0f)
{
// Origin is left of e12.
@@ -239,13 +238,13 @@ struct b2Simplex
}
}
- float32 GetMetric() const
+ qreal GetMetric() const
{
switch (m_count)
{
case 0:
b2Assert(false);
- return 0.0f;
+ return 0.0;
case 1:
return 0.0f;
@@ -300,7 +299,7 @@ void b2Simplex::Solve2()
b2Vec2 e12 = w2 - w1;
// w1 region
- float32 d12_2 = -b2Dot(w1, e12);
+ qreal d12_2 = -b2Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
@@ -310,7 +309,7 @@ void b2Simplex::Solve2()
}
// w2 region
- float32 d12_1 = b2Dot(w2, e12);
+ qreal d12_1 = b2Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
@@ -321,7 +320,7 @@ void b2Simplex::Solve2()
}
// Must be in e12 region.
- float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+ qreal 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;
@@ -343,37 +342,37 @@ void b2Simplex::Solve3()
// [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;
+ qreal w1e12 = b2Dot(w1, e12);
+ qreal w2e12 = b2Dot(w2, e12);
+ qreal d12_1 = w2e12;
+ qreal 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;
+ qreal w1e13 = b2Dot(w1, e13);
+ qreal w3e13 = b2Dot(w3, e13);
+ qreal d13_1 = w3e13;
+ qreal 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;
+ qreal w2e23 = b2Dot(w2, e23);
+ qreal w3e23 = b2Dot(w3, e23);
+ qreal d23_1 = w3e23;
+ qreal d23_2 = -w2e23;
// Triangle123
- float32 n123 = b2Cross(e12, e13);
+ qreal 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);
+ qreal d123_1 = n123 * b2Cross(w2, w3);
+ qreal d123_2 = n123 * b2Cross(w3, w1);
+ qreal d123_3 = n123 * b2Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
@@ -386,7 +385,7 @@ void b2Simplex::Solve3()
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
- float32 inv_d12 = 1.0f / (d12_1 + d12_2);
+ qreal 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;
@@ -396,7 +395,7 @@ void b2Simplex::Solve3()
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
- float32 inv_d13 = 1.0f / (d13_1 + d13_2);
+ qreal 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;
@@ -425,7 +424,7 @@ void b2Simplex::Solve3()
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
- float32 inv_d23 = 1.0f / (d23_1 + d23_2);
+ qreal 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;
@@ -434,7 +433,7 @@ void b2Simplex::Solve3()
}
// Must be in triangle123
- float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
+ qreal 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;
@@ -466,8 +465,9 @@ void b2Distance(b2DistanceOutput* output,
int32 saveA[3], saveB[3];
int32 saveCount = 0;
- float32 distanceSqr1 = b2_maxFloat;
- float32 distanceSqr2 = distanceSqr1;
+ b2Vec2 closestPoint = simplex.GetClosestPoint();
+ qreal distanceSqr1 = closestPoint.LengthSquared();
+ qreal distanceSqr2 = distanceSqr1;
// Main iteration loop.
int32 iter = 0;
@@ -532,10 +532,10 @@ void b2Distance(b2DistanceOutput* output,
// Compute a tentative new simplex vertex using support points.
b2SimplexVertex* vertex = vertices + simplex.m_count;
- vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
+ vertex->indexA = proxyA->GetSupport(b2MulT(transformA.R, -d));
vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
b2Vec2 wBLocal;
- vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
+ vertex->indexB = proxyB->GetSupport(b2MulT(transformB.R, d));
vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
@@ -577,8 +577,8 @@ void b2Distance(b2DistanceOutput* output,
// Apply radii if requested.
if (input->useRadii)
{
- float32 rA = proxyA->m_radius;
- float32 rB = proxyB->m_radius;
+ qreal rA = proxyA->m_radius;
+ qreal rB = proxyB->m_radius;
if (output->distance > rA + rB && output->distance > b2_epsilon)
{
diff --git a/external/Box2D/Collision/b2Distance.h b/external/Box2D/Collision/b2Distance.h
index 7bae689..8b41111 100644
--- a/external/Box2D/Collision/b2Distance.h
+++ b/external/Box2D/Collision/b2Distance.h
@@ -1,6 +1,6 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -49,14 +49,14 @@ struct b2DistanceProxy
b2Vec2 m_buffer[2];
const b2Vec2* m_vertices;
int32 m_count;
- float32 m_radius;
+ qreal m_radius;
};
/// Used to warm start b2Distance.
/// Set count to zero on first call.
struct b2SimplexCache
{
- float32 metric; ///< length or area
+ qreal metric; ///< length or area
uint16 count;
uint8 indexA[3]; ///< vertices on shape A
uint8 indexB[3]; ///< vertices on shape B
@@ -79,7 +79,7 @@ struct b2DistanceOutput
{
b2Vec2 pointA; ///< closest point on shapeA
b2Vec2 pointB; ///< closest point on shapeB
- float32 distance;
+ qreal distance;
int32 iterations; ///< number of GJK iterations used
};
@@ -107,10 +107,10 @@ inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
{
int32 bestIndex = 0;
- float32 bestValue = b2Dot(m_vertices[0], d);
+ qreal bestValue = b2Dot(m_vertices[0], d);
for (int32 i = 1; i < m_count; ++i)
{
- float32 value = b2Dot(m_vertices[i], d);
+ qreal value = b2Dot(m_vertices[i], d);
if (value > bestValue)
{
bestIndex = i;
@@ -124,10 +124,10 @@ inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
{
int32 bestIndex = 0;
- float32 bestValue = b2Dot(m_vertices[0], d);
+ qreal bestValue = b2Dot(m_vertices[0], d);
for (int32 i = 1; i < m_count; ++i)
{
- float32 value = b2Dot(m_vertices[i], d);
+ qreal value = b2Dot(m_vertices[i], d);
if (value > bestValue)
{
bestIndex = i;
diff --git a/external/Box2D/Collision/b2DynamicTree.cpp b/external/Box2D/Collision/b2DynamicTree.cpp
index ef57b42..229f7a3 100644
--- a/external/Box2D/Collision/b2DynamicTree.cpp
+++ b/external/Box2D/Collision/b2DynamicTree.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -17,7 +17,9 @@
*/
#include <Box2D/Collision/b2DynamicTree.h>
-#include <string.h>
+#include <cstring>
+#include <cfloat>
+using namespace std;
b2DynamicTree::b2DynamicTree()
{
@@ -25,17 +27,15 @@ b2DynamicTree::b2DynamicTree()
m_nodeCapacity = 16;
m_nodeCount = 0;
- m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
- memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
+ m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+ memset(m_nodes, 0, m_nodeCapacity * sizeof(b2DynamicTreeNode));
// 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;
@@ -58,10 +58,10 @@ int32 b2DynamicTree::AllocateNode()
b2Assert(m_nodeCount == m_nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
- b2TreeNode* oldNodes = m_nodes;
+ b2DynamicTreeNode* oldNodes = m_nodes;
m_nodeCapacity *= 2;
- m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
- memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
+ m_nodes = (b2DynamicTreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2DynamicTreeNode));
+ memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2DynamicTreeNode));
b2Free(oldNodes);
// Build a linked list for the free list. The parent
@@ -69,10 +69,8 @@ int32 b2DynamicTree::AllocateNode()
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;
}
@@ -82,8 +80,7 @@ int32 b2DynamicTree::AllocateNode()
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 = NULL;
+ m_nodes[nodeId].leafCount = 0;
++m_nodeCount;
return nodeId;
}
@@ -94,7 +91,6 @@ 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;
}
@@ -111,7 +107,7 @@ int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
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;
+ m_nodes[proxyId].leafCount = 1;
InsertLeaf(proxyId);
@@ -186,84 +182,83 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
- int32 index = m_root;
- while (m_nodes[index].IsLeaf() == false)
+ int32 sibling = m_root;
+ while (m_nodes[sibling].IsLeaf() == false)
{
- int32 child1 = m_nodes[index].child1;
- int32 child2 = m_nodes[index].child2;
+ int32 child1 = m_nodes[sibling].child1;
+ int32 child2 = m_nodes[sibling].child2;
- float32 area = m_nodes[index].aabb.GetPerimeter();
+ // Expand the node's AABB.
+ m_nodes[sibling].aabb.Combine(leafAABB);
+ m_nodes[sibling].leafCount += 1;
- b2AABB combinedAABB;
- combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
- float32 combinedArea = combinedAABB.GetPerimeter();
+ qreal siblingArea = m_nodes[sibling].aabb.GetPerimeter();
+ b2AABB parentAABB;
+ parentAABB.Combine(m_nodes[sibling].aabb, leafAABB);
+ qreal parentArea = parentAABB.GetPerimeter();
+ qreal cost1 = 2.0f * parentArea;
- // Cost of creating a new parent for this node and the new leaf
- float32 cost = 2.0f * combinedArea;
+ qreal inheritanceCost = 2.0f * (parentArea - siblingArea);
- // Minimum cost of pushing the leaf further down the tree
- float32 inheritanceCost = 2.0f * (combinedArea - area);
-
- // Cost of descending into child1
- float32 cost1;
+ qreal cost2;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child1].aabb);
- cost1 = aabb.GetPerimeter() + inheritanceCost;
+ cost2 = 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;
+ qreal oldArea = m_nodes[child1].aabb.GetPerimeter();
+ qreal newArea = aabb.GetPerimeter();
+ cost2 = (newArea - oldArea) + inheritanceCost;
}
- // Cost of descending into child2
- float32 cost2;
+ qreal cost3;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child2].aabb);
- cost2 = aabb.GetPerimeter() + inheritanceCost;
+ cost3 = 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;
+ qreal oldArea = m_nodes[child2].aabb.GetPerimeter();
+ qreal newArea = aabb.GetPerimeter();
+ cost3 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
- if (cost < cost1 && cost < cost2)
+ if (cost1 < cost2 && cost1 < cost3)
{
break;
}
+ // Expand the node's AABB to account for the new leaf.
+ m_nodes[sibling].aabb.Combine(leafAABB);
+
// Descend
- if (cost1 < cost2)
+ if (cost2 < cost3)
{
- index = child1;
+ sibling = child1;
}
else
{
- index = child2;
+ sibling = child2;
}
}
- int32 sibling = index;
-
- // Create a new parent.
+ // Create a new parent for the siblings.
int32 oldParent = m_nodes[sibling].parent;
int32 newParent = AllocateNode();
m_nodes[newParent].parent = oldParent;
m_nodes[newParent].userData = NULL;
m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
- m_nodes[newParent].height = m_nodes[sibling].height + 1;
+ m_nodes[newParent].leafCount = m_nodes[sibling].leafCount + 1;
if (oldParent != b2_nullNode)
{
@@ -291,26 +286,6 @@ void b2DynamicTree::InsertLeaf(int32 leaf)
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)
@@ -348,18 +323,16 @@ void b2DynamicTree::RemoveLeaf(int32 leaf)
FreeNode(parent);
// Adjust ancestor bounds.
- int32 index = grandParent;
- while (index != b2_nullNode)
+ parent = grandParent;
+ while (parent != b2_nullNode)
{
- index = Balance(index);
-
- int32 child1 = m_nodes[index].child1;
- int32 child2 = m_nodes[index].child2;
+ b2AABB oldAABB = m_nodes[parent].aabb;
+ m_nodes[parent].aabb.Combine(m_nodes[m_nodes[parent].child1].aabb, m_nodes[m_nodes[parent].child2].aabb);
- 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);
+ b2Assert(m_nodes[parent].leafCount > 0);
+ m_nodes[parent].leafCount -= 1;
- index = m_nodes[index].parent;
+ parent = m_nodes[parent].parent;
}
}
else
@@ -368,203 +341,52 @@ void b2DynamicTree::RemoveLeaf(int32 leaf)
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)
+void b2DynamicTree::Rebalance(int32 iterations)
{
- b2Assert(iA != b2_nullNode);
-
- b2TreeNode* A = m_nodes + iA;
- if (A->IsLeaf() || A->height < 2)
+ if (m_root == b2_nullNode)
{
- return iA;
+ return;
}
- 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)
+ // Rebalance the tree by removing and re-inserting leaves.
+ for (int32 i = 0; i < iterations; ++i)
{
- 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;
- }
+ int32 node = m_root;
- // Rotate
- if (F->height > G->height)
+ uint32 bit = 0;
+ while (m_nodes[node].IsLeaf() == false)
{
- 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);
- }
+ int32* children = &m_nodes[node].child1;
+
+ // Child selector based on a bit in the path
+ int32 selector = (m_path >> bit) & 1;
- 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();
+ // Select the child nod
+ node = children[selector];
- 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;
+ // Keep bit between 0 and 31 because m_path has 32 bits
+ // bit = (bit + 1) % 31
+ bit = (bit + 1) & 0x1F;
}
+ ++m_path;
- totalArea += node->aabb.GetPerimeter();
+ RemoveLeaf(node);
+ InsertLeaf(node);
}
-
- 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())
+ if (nodeId == b2_nullNode)
{
return 0;
}
+ b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+ b2DynamicTreeNode* node = m_nodes + nodeId;
int32 height1 = ComputeHeight(node->child1);
int32 height2 = ComputeHeight(node->child2);
return 1 + b2Max(height1, height2);
@@ -572,207 +394,33 @@ int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
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);
+ return ComputeHeight(m_root);
}
-void b2DynamicTree::ValidateMetrics(int32 index) const
+int32 b2DynamicTree::CountLeaves(int32 nodeId) const
{
- if (index == b2_nullNode)
+ if (nodeId == b2_nullNode)
{
- return;
+ return 0;
}
- const b2TreeNode* node = m_nodes + index;
-
- int32 child1 = node->child1;
- int32 child2 = node->child2;
+ b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
+ b2DynamicTreeNode* node = m_nodes + nodeId;
if (node->IsLeaf())
{
- b2Assert(child1 == b2_nullNode);
- b2Assert(child2 == b2_nullNode);
- b2Assert(node->height == 0);
- return;
+ b2Assert(node->leafCount == 1);
+ return 1;
}
- 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);
+ int32 count1 = CountLeaves(node->child1);
+ int32 count2 = CountLeaves(node->child2);
+ int32 count = count1 + count2;
+ b2Assert(count == node->leafCount);
+ return count;
}
void b2DynamicTree::Validate() const
{
- 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);
-}
-
-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;
- }
+ CountLeaves(m_root);
}
diff --git a/external/Box2D/Collision/b2DynamicTree.h b/external/Box2D/Collision/b2DynamicTree.h
index bf80f73..79682e0 100644
--- a/external/Box2D/Collision/b2DynamicTree.h
+++ b/external/Box2D/Collision/b2DynamicTree.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -22,17 +22,19 @@
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Common/b2GrowableStack.h>
+/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
+
#define b2_nullNode (-1)
/// A node in the dynamic tree. The client does not interact with this directly.
-struct b2TreeNode
+struct b2DynamicTreeNode
{
bool IsLeaf() const
{
return child1 == b2_nullNode;
}
- /// Enlarged AABB
+ /// This is the fattened AABB.
b2AABB aabb;
void* userData;
@@ -45,12 +47,9 @@ struct b2TreeNode
int32 child1;
int32 child2;
-
- // leaf = 0, free node = -1
- int32 height;
+ int32 leafCount;
};
-/// 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
@@ -61,6 +60,7 @@ struct b2TreeNode
class b2DynamicTree
{
public:
+
/// Constructing the tree initializes the node pool.
b2DynamicTree();
@@ -79,6 +79,9 @@ public:
/// @return true if the proxy was re-inserted.
bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
+ /// Perform some iterations to re-balance the tree.
+ void Rebalance(int32 iterations);
+
/// Get proxy user data.
/// @return the proxy user data or 0 if the id is invalid.
void* GetUserData(int32 proxyId) const;
@@ -86,6 +89,10 @@ public:
/// Get the fat AABB for a proxy.
const b2AABB& GetFatAABB(int32 proxyId) const;
+ /// Compute the height of the binary tree in O(N) time. Should not be
+ /// called often.
+ int32 ComputeHeight() const;
+
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
template <typename T>
@@ -101,28 +108,8 @@ public:
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();
@@ -131,23 +118,19 @@ private:
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 CountLeaves(int32 nodeId) const;
int32 m_root;
- b2TreeNode* m_nodes;
+ b2DynamicTreeNode* m_nodes;
int32 m_nodeCount;
int32 m_nodeCapacity;
int32 m_freeList;
- /// This is used to incrementally traverse the tree for re-balancing.
+ /// This is used incrementally traverse the tree for re-balancing.
uint32 m_path;
int32 m_insertionCount;
@@ -179,7 +162,7 @@ inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
continue;
}
- const b2TreeNode* node = m_nodes + nodeId;
+ const b2DynamicTreeNode* node = m_nodes + nodeId;
if (b2TestOverlap(node->aabb, aabb))
{
@@ -216,7 +199,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
- float32 maxFraction = input.maxFraction;
+ qreal maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB;
@@ -237,7 +220,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
continue;
}
- const b2TreeNode* node = m_nodes + nodeId;
+ const b2DynamicTreeNode* node = m_nodes + nodeId;
if (b2TestOverlap(node->aabb, segmentAABB) == false)
{
@@ -248,7 +231,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
// |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);
+ qreal separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
@@ -261,7 +244,7 @@ inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) con
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
- float32 value = callback->RayCastCallback(subInput, nodeId);
+ qreal value = callback->RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
diff --git a/external/Box2D/Collision/b2TimeOfImpact.cpp b/external/Box2D/Collision/b2TimeOfImpact.cpp
index 60da95c..d68a9ab 100644
--- a/external/Box2D/Collision/b2TimeOfImpact.cpp
+++ b/external/Box2D/Collision/b2TimeOfImpact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,15 +21,13 @@
#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>
+#include <cstdio>
+using namespace std;
-float32 b2_toiTime, b2_toiMaxTime;
int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
int32 b2_toiRootIters, b2_toiMaxRootIters;
-//
struct b2SeparationFunction
{
enum Type
@@ -41,10 +39,10 @@ struct b2SeparationFunction
// TODO_ERIN might not need to return the separation
- float32 Initialize(const b2SimplexCache* cache,
+ qreal Initialize(const b2SimplexCache* cache,
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
- float32 t1)
+ qreal t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
@@ -66,7 +64,7 @@ struct b2SeparationFunction
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
- float32 s = m_axis.Normalize();
+ qreal s = m_axis.Normalize();
return s;
}
else if (cache->indexA[0] == cache->indexA[1])
@@ -78,7 +76,7 @@ struct b2SeparationFunction
m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
- b2Vec2 normal = b2Mul(xfB.q, m_axis);
+ b2Vec2 normal = b2Mul(xfB.R, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
@@ -86,7 +84,7 @@ struct b2SeparationFunction
b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 s = b2Dot(pointA - pointB, normal);
+ qreal s = b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
@@ -103,7 +101,7 @@ struct b2SeparationFunction
m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
- b2Vec2 normal = b2Mul(xfA.q, m_axis);
+ b2Vec2 normal = b2Mul(xfA.R, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
@@ -111,7 +109,7 @@ struct b2SeparationFunction
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 s = b2Dot(pointB - pointA, normal);
+ qreal s = b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
@@ -121,8 +119,7 @@ struct b2SeparationFunction
}
}
- //
- float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
+ qreal FindMinSeparation(int32* indexA, int32* indexB, qreal t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
@@ -132,8 +129,8 @@ struct b2SeparationFunction
{
case e_points:
{
- b2Vec2 axisA = b2MulT(xfA.q, m_axis);
- b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
+ b2Vec2 axisA = b2MulT(xfA.R, m_axis);
+ b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
*indexA = m_proxyA->GetSupport(axisA);
*indexB = m_proxyB->GetSupport(axisB);
@@ -144,16 +141,16 @@ struct b2SeparationFunction
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, m_axis);
+ qreal separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
- b2Vec2 normal = b2Mul(xfA.q, m_axis);
+ b2Vec2 normal = b2Mul(xfA.R, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
- b2Vec2 axisB = b2MulT(xfB.q, -normal);
+ b2Vec2 axisB = b2MulT(xfB.R, -normal);
*indexA = -1;
*indexB = m_proxyB->GetSupport(axisB);
@@ -161,16 +158,16 @@ struct b2SeparationFunction
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, normal);
+ qreal separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
- b2Vec2 normal = b2Mul(xfB.q, m_axis);
+ b2Vec2 normal = b2Mul(xfB.R, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
- b2Vec2 axisA = b2MulT(xfA.q, -normal);
+ b2Vec2 axisA = b2MulT(xfA.R, -normal);
*indexB = -1;
*indexA = m_proxyA->GetSupport(axisA);
@@ -178,7 +175,7 @@ struct b2SeparationFunction
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 separation = b2Dot(pointA - pointB, normal);
+ qreal separation = b2Dot(pointA - pointB, normal);
return separation;
}
@@ -190,8 +187,7 @@ struct b2SeparationFunction
}
}
- //
- float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
+ qreal Evaluate(int32 indexA, int32 indexB, qreal t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
@@ -201,37 +197,44 @@ struct b2SeparationFunction
{
case e_points:
{
+ b2Vec2 axisA = b2MulT(xfA.R, m_axis);
+ b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
+
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);
+ qreal separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
- b2Vec2 normal = b2Mul(xfA.q, m_axis);
+ b2Vec2 normal = b2Mul(xfA.R, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
+ b2Vec2 axisB = b2MulT(xfB.R, -normal);
+
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
- float32 separation = b2Dot(pointB - pointA, normal);
+ qreal separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
- b2Vec2 normal = b2Mul(xfB.q, m_axis);
+ b2Vec2 normal = b2Mul(xfB.R, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
+ b2Vec2 axisA = b2MulT(xfA.R, -normal);
+
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
- float32 separation = b2Dot(pointA - pointB, normal);
+ qreal separation = b2Dot(pointA - pointB, normal);
return separation;
}
@@ -253,8 +256,6 @@ struct b2SeparationFunction
// 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;
@@ -271,14 +272,14 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
sweepA.Normalize();
sweepB.Normalize();
- float32 tMax = input->tMax;
+ qreal 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;
+ qreal totalRadius = proxyA->m_radius + proxyB->m_radius;
+ qreal target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
+ qreal tolerance = 0.25f * b2_linearSlop;
b2Assert(target > tolerance);
- float32 t1 = 0.0f;
+ qreal t1 = 0.0f;
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
int32 iter = 0;
@@ -329,17 +330,17 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
// 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];
+ qreal dx = 1.0f / N;
+ qreal xs[N+1];
+ qreal fs[N+1];
- float32 x = 0.0f;
+ qreal 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;
+ qreal f = fcn.Evaluate(xfA, xfB) - target;
printf("%g %g\n", x, f);
@@ -354,13 +355,13 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
// 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;
+ qreal 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);
+ qreal s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
@@ -381,7 +382,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
}
// Compute the initial separation of the witness points.
- float32 s1 = fcn.Evaluate(indexA, indexB, t1);
+ qreal s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
@@ -405,11 +406,11 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
// Compute 1D root of: f(x) - target = 0
int32 rootIterCount = 0;
- float32 a1 = t1, a2 = t2;
+ qreal a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
- float32 t;
+ qreal t;
if (rootIterCount & 1)
{
// Secant rule to improve convergence.
@@ -421,10 +422,7 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
t = 0.5f * (a1 + a2);
}
- ++rootIterCount;
- ++b2_toiRootIters;
-
- float32 s = fcn.Evaluate(indexA, indexB, t);
+ qreal s = fcn.Evaluate(indexA, indexB, t);
if (b2Abs(s - target) < tolerance)
{
@@ -444,7 +442,10 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
a2 = t;
s2 = s;
}
-
+
+ ++rootIterCount;
+ ++b2_toiRootIters;
+
if (rootIterCount == 50)
{
break;
@@ -479,8 +480,4 @@ void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
}
b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
-
- float32 time = timer.GetMilliseconds();
- b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
- b2_toiTime += time;
}
diff --git a/external/Box2D/Collision/b2TimeOfImpact.h b/external/Box2D/Collision/b2TimeOfImpact.h
index 20620c1..2436187 100644
--- a/external/Box2D/Collision/b2TimeOfImpact.h
+++ b/external/Box2D/Collision/b2TimeOfImpact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -29,7 +29,7 @@ struct b2TOIInput
b2DistanceProxy proxyB;
b2Sweep sweepA;
b2Sweep sweepB;
- float32 tMax; // defines sweep interval [0, tMax]
+ qreal tMax; // defines sweep interval [0, tMax]
};
// Output parameters for b2TimeOfImpact.
@@ -45,7 +45,7 @@ struct b2TOIOutput
};
State state;
- float32 t;
+ qreal t;
};
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
diff --git a/external/Box2D/Common/b2BlockAllocator.cpp b/external/Box2D/Common/b2BlockAllocator.cpp
index 59674e8..3804977 100644
--- a/external/Box2D/Common/b2BlockAllocator.cpp
+++ b/external/Box2D/Common/b2BlockAllocator.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -17,9 +17,11 @@
*/
#include <Box2D/Common/b2BlockAllocator.h>
-#include <limits.h>
-#include <string.h>
-#include <stddef.h>
+#include <cstdlib>
+#include <climits>
+#include <cstring>
+#include <memory>
+using namespace std;
int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =
{
diff --git a/external/Box2D/Common/b2BlockAllocator.h b/external/Box2D/Common/b2BlockAllocator.h
index cea3cef..de32d4d 100644
--- a/external/Box2D/Common/b2BlockAllocator.h
+++ b/external/Box2D/Common/b2BlockAllocator.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,7 +21,7 @@
#include <Box2D/Common/b2Settings.h>
-const int32 b2_chunkSize = 16 * 1024;
+const int32 b2_chunkSize = 4096;
const int32 b2_maxBlockSize = 640;
const int32 b2_blockSizes = 14;
const int32 b2_chunkArrayIncrement = 128;
diff --git a/external/Box2D/Common/b2Draw.cpp b/external/Box2D/Common/b2Draw.cpp
deleted file mode 100644
index 2cd9b1a..0000000
--- a/external/Box2D/Common/b2Draw.cpp
+++ /dev/null
@@ -1,44 +0,0 @@
-/*
-* 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;
-}
diff --git a/external/Box2D/Common/b2Draw.h b/external/Box2D/Common/b2Draw.h
deleted file mode 100644
index be65fb0..0000000
--- a/external/Box2D/Common/b2Draw.h
+++ /dev/null
@@ -1,86 +0,0 @@
-/*
-* 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 <Box2D/Common/b2Math.h>
-
-/// Color for debug drawing. Each value has the range [0,1].
-struct b2Color
-{
- b2Color() {}
- b2Color(float32 r, float32 g, float32 b, float32 a = 1.0f) : r(r), g(g), b(b), a(a) {}
- void Set(float32 ri, float32 gi, float32 bi, float32 ai = 1.0f) { r = ri; g = gi; b = bi; a = ai; }
- 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;
-
-protected:
- uint32 m_drawFlags;
-};
-
-#endif
diff --git a/external/Box2D/Common/b2GrowableStack.h b/external/Box2D/Common/b2GrowableStack.h
index 9892477..e83392f 100644
--- a/external/Box2D/Common/b2GrowableStack.h
+++ b/external/Box2D/Common/b2GrowableStack.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,7 +19,7 @@
#ifndef B2_GROWABLE_STACK_H
#define B2_GROWABLE_STACK_H
#include <Box2D/Common/b2Settings.h>
-#include <string.h>
+#include <cstring>
/// This is a growable LIFO stack with an initial capacity of N.
/// If the stack size exceeds the initial capacity, the heap is used
@@ -51,7 +51,7 @@ public:
T* old = m_stack;
m_capacity *= 2;
m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
- memcpy(m_stack, old, m_count * sizeof(T));
+ std::memcpy(m_stack, old, m_count * sizeof(T));
if (old != m_array)
{
b2Free(old);
diff --git a/external/Box2D/Common/b2Math.cpp b/external/Box2D/Common/b2Math.cpp
index d6027be..e061fcd 100644
--- a/external/Box2D/Common/b2Math.cpp
+++ b/external/Box2D/Common/b2Math.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,20 +19,22 @@
#include <Box2D/Common/b2Math.h>
const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
+const b2Mat22 b2Mat22_identity(1.0f, 0.0f, 0.0f, 1.0f);
+const b2Transform b2Transform_identity(b2Vec2_zero, b2Mat22_identity);
/// 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));
+ qreal det = b2Dot(col1, b2Cross(col2, col3));
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));
+ x.x = det * b2Dot(b, b2Cross(col2, col3));
+ x.y = det * b2Dot(col1, b2Cross(b, col3));
+ x.z = det * b2Dot(col1, b2Cross(col2, b));
return x;
}
@@ -40,8 +42,8 @@ b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
/// 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;
+ qreal a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+ qreal det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
@@ -51,44 +53,3 @@ b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
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);
-}
diff --git a/external/Box2D/Common/b2Math.h b/external/Box2D/Common/b2Math.h
index 755c519..0ce5699 100644
--- a/external/Box2D/Common/b2Math.h
+++ b/external/Box2D/Common/b2Math.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -20,34 +20,54 @@
#define B2_MATH_H
#include <Box2D/Common/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)
+#include <cmath>
+#include <cfloat>
+#include <cstddef>
+#include <limits>
+
+/// This function is used to ensure that a floating point number is
+/// not a NaN or infinity.
+inline bool b2IsValid(qreal x)
{
- int32 ix = *reinterpret_cast<int32*>(&x);
- return (ix & 0x7f800000) != 0x7f800000;
+ if (x != x)
+ {
+ // NaN.
+ return false;
+ }
+
+ qreal infinity = std::numeric_limits<qreal>::infinity();
+ return -infinity < x && x < infinity;
}
+template<size_t size> struct b2InvSqrt_magic;
+template<> struct b2InvSqrt_magic<4> { static inline int32_t value() { return 0x5f3759df; } };
+template<> struct b2InvSqrt_magic<8> { static inline int64_t value() { return 0x5fe6eb50c7aa19f9ULL; } };
+
/// This is a approximate yet fast inverse square-root.
-inline float32 b2InvSqrt(float32 x)
+inline qreal b2InvSqrt(qreal x)
{
union
{
- float32 x;
+ qreal x;
int32 i;
} convert;
convert.x = x;
- float32 xhalf = 0.5f * x;
- convert.i = 0x5f3759df - (convert.i >> 1);
+ qreal xhalf = 0.5f * x;
+ convert.i = b2InvSqrt_magic<sizeof(qreal)>::value() - (convert.i >> 1);
x = convert.x;
x = x * (1.5f - xhalf * x * x);
return x;
}
-#define b2Sqrt(x) sqrtf(x)
-#define b2Atan2(y, x) atan2f(y, x)
+#define b2Sqrt(x) std::sqrt(x)
+#define b2Atan2(y, x) std::atan2(y, x)
+
+inline qreal b2Abs(qreal a)
+{
+ return a > 0.0f ? a : -a;
+}
/// A 2D column vector.
struct b2Vec2
@@ -56,25 +76,25 @@ struct b2Vec2
b2Vec2() {}
/// Construct using coordinates.
- b2Vec2(float32 x, float32 y) : x(x), y(y) {}
+ b2Vec2(qreal x, qreal y) : x(x), y(y) {}
/// 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_; }
+ void Set(qreal x_, qreal 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
+ qreal operator () (int32 i) const
{
return (&x)[i];
}
/// Write to an indexed element.
- float32& operator () (int32 i)
+ qreal& operator () (int32 i)
{
return (&x)[i];
}
@@ -92,33 +112,33 @@ struct b2Vec2
}
/// Multiply this vector by a scalar.
- void operator *= (float32 a)
+ void operator *= (qreal a)
{
x *= a; y *= a;
}
/// Get the length of this vector (the norm).
- float32 Length() const
+ qreal 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
+ qreal LengthSquared() const
{
return x * x + y * y;
}
/// Convert this vector into a unit vector. Returns the length.
- float32 Normalize()
+ qreal Normalize()
{
- float32 length = Length();
+ qreal length = Length();
if (length < b2_epsilon)
{
return 0.0f;
}
- float32 invLength = 1.0f / length;
+ qreal invLength = 1.0f / length;
x *= invLength;
y *= invLength;
@@ -131,13 +151,7 @@ struct b2Vec2
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;
+ qreal x, y;
};
/// A 2D column vector with 3 elements.
@@ -147,13 +161,13 @@ struct b2Vec3
b2Vec3() {}
/// Construct using coordinates.
- b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
+ b2Vec3(qreal x, qreal y, qreal z) : x(x), y(y), z(z) {}
/// 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_; }
+ void Set(qreal x_, qreal y_, qreal z_) { x = x_; y = y_; z = z_; }
/// Negate this vector.
b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
@@ -171,12 +185,12 @@ struct b2Vec3
}
/// Multiply this vector by a scalar.
- void operator *= (float32 s)
+ void operator *= (qreal s)
{
x *= s; y *= s; z *= s;
}
- float32 x, y, z;
+ qreal x, y, z;
};
/// A 2-by-2 matrix. Stored in column-major order.
@@ -188,49 +202,75 @@ struct b2Mat22
/// Construct this matrix using columns.
b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
{
- ex = c1;
- ey = c2;
+ col1 = c1;
+ col2 = c2;
}
/// Construct this matrix using scalars.
- b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
+ b2Mat22(qreal a11, qreal a12, qreal a21, qreal a22)
{
- ex.x = a11; ex.y = a21;
- ey.x = a12; ey.y = a22;
+ col1.x = a11; col1.y = a21;
+ col2.x = a12; col2.y = a22;
+ }
+
+ /// Construct this matrix using an angle. This matrix becomes
+ /// an orthonormal rotation matrix.
+ explicit b2Mat22(qreal angle)
+ {
+ // TODO_ERIN compute sin+cos together.
+ qreal c = cosf(angle), s = sinf(angle);
+ col1.x = c; col2.x = -s;
+ col1.y = s; col2.y = c;
}
/// Initialize this matrix using columns.
void Set(const b2Vec2& c1, const b2Vec2& c2)
{
- ex = c1;
- ey = c2;
+ col1 = c1;
+ col2 = c2;
+ }
+
+ /// Initialize this matrix using an angle. This matrix becomes
+ /// an orthonormal rotation matrix.
+ void Set(qreal angle)
+ {
+ qreal c = cosf(angle), s = sinf(angle);
+ col1.x = c; col2.x = -s;
+ col1.y = s; col2.y = c;
}
/// Set this to the identity matrix.
void SetIdentity()
{
- ex.x = 1.0f; ey.x = 0.0f;
- ex.y = 0.0f; ey.y = 1.0f;
+ col1.x = 1.0f; col2.x = 0.0f;
+ col1.y = 0.0f; col2.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;
+ col1.x = 0.0f; col2.x = 0.0f;
+ col1.y = 0.0f; col2.y = 0.0f;
+ }
+
+ /// Extract the angle from this matrix (assumed to be
+ /// a rotation matrix).
+ qreal GetAngle() const
+ {
+ return b2Atan2(col1.y, col1.x);
}
b2Mat22 GetInverse() const
{
- float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
+ qreal a = col1.x, b = col2.x, c = col1.y, d = col2.y;
b2Mat22 B;
- float32 det = a * d - b * c;
+ qreal 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;
+ B.col1.x = det * d; B.col2.x = -det * b;
+ B.col1.y = -det * c; B.col2.y = det * a;
return B;
}
@@ -238,8 +278,8 @@ struct b2Mat22
/// 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;
+ qreal a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
+ qreal det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
@@ -250,7 +290,7 @@ struct b2Mat22
return x;
}
- b2Vec2 ex, ey;
+ b2Vec2 col1, col2;
};
/// A 3-by-3 matrix. Stored in column-major order.
@@ -262,17 +302,17 @@ struct b2Mat33
/// Construct this matrix using columns.
b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
{
- ex = c1;
- ey = c2;
- ez = c3;
+ col1 = c1;
+ col2 = c2;
+ col3 = c3;
}
/// Set this matrix to all zeros.
void SetZero()
{
- ex.SetZero();
- ey.SetZero();
- ez.SetZero();
+ col1.SetZero();
+ col2.SetZero();
+ col3.SetZero();
}
/// Solve A * x = b, where b is a column vector. This is more efficient
@@ -284,93 +324,41 @@ struct b2Mat33
/// 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;
+ b2Vec3 col1, col2, col3;
};
/// 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.
+ /// The default constructor does nothing (for performance).
b2Transform() {}
- /// Initialize using a position vector and a rotation.
- b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
+ /// Initialize using a position vector and a rotation matrix.
+ b2Transform(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
/// Set this to the identity transform.
void SetIdentity()
{
- p.SetZero();
- q.SetIdentity();
+ position.SetZero();
+ R.SetIdentity();
}
/// Set this based on the position and angle.
- void Set(const b2Vec2& position, float32 angle)
+ void Set(const b2Vec2& p, qreal angle)
+ {
+ position = p;
+ R.Set(angle);
+ }
+
+ /// Calculate the angle that the rotation matrix represents.
+ qreal GetAngle() const
{
- p = position;
- q.Set(angle);
+ return b2Atan2(R.col1.y, R.col1.x);
}
- b2Vec2 p;
- b2Rot q;
+ b2Vec2 position;
+ b2Mat22 R;
};
/// This describes the motion of a body/shape for TOI computation.
@@ -381,49 +369,51 @@ 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;
+ void GetTransform(b2Transform* xf, qreal beta) const;
/// Advance the sweep forward, yielding a new initial state.
/// @param alpha the new initial time.
- void Advance(float32 alpha);
+ void Advance(qreal alpha);
/// Normalize the angles.
void Normalize();
b2Vec2 localCenter; ///< local center of mass position
b2Vec2 c0, c; ///< center world positions
- float32 a0, a; ///< world angles
+ qreal 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;
+ qreal alpha0;
};
-/// Useful constant
+
extern const b2Vec2 b2Vec2_zero;
+extern const b2Mat22 b2Mat22_identity;
+extern const b2Transform b2Transform_identity;
/// Perform the dot product on two vectors.
-inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
+inline qreal 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)
+inline qreal 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)
+inline b2Vec2 b2Cross(const b2Vec2& a, qreal 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)
+inline b2Vec2 b2Cross(qreal s, const b2Vec2& a)
{
return b2Vec2(-s * a.y, s * a.x);
}
@@ -432,14 +422,14 @@ inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
/// 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);
+ return b2Vec2(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.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));
+ return b2Vec2(b2Dot(v, A.col1), b2Dot(v, A.col2));
}
/// Add two vectors component-wise.
@@ -454,7 +444,7 @@ 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)
+inline b2Vec2 operator * (qreal s, const b2Vec2& a)
{
return b2Vec2(s * a.x, s * a.y);
}
@@ -464,19 +454,19 @@ 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)
+inline qreal b2Distance(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return c.Length();
}
-inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
+inline qreal b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return b2Dot(c, c);
}
-inline b2Vec3 operator * (float32 s, const b2Vec3& a)
+inline b2Vec3 operator * (qreal s, const b2Vec3& a)
{
return b2Vec3(s * a.x, s * a.y, s * a.z);
}
@@ -494,7 +484,7 @@ inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
}
/// Perform the dot product on two vectors.
-inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
+inline qreal b2Dot(const b2Vec3& a, const b2Vec3& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
@@ -507,117 +497,51 @@ inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
{
- return b2Mat22(A.ex + B.ex, A.ey + B.ey);
+ return b2Mat22(A.col1 + B.col1, A.col2 + B.col2);
}
// A * B
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
{
- return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
+ return b2Mat22(b2Mul(A, B.col1), b2Mul(A, B.col2));
}
// 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));
+ b2Vec2 c1(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
+ b2Vec2 c2(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
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);
+ return v.x * A.col1 + v.y * A.col2 + v.z * A.col3;
}
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;
+ qreal x = T.position.x + T.R.col1.x * v.x + T.R.col2.x * v.y;
+ qreal y = T.position.y + T.R.col1.y * v.x + T.R.col2.y * v.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;
+ return b2MulT(T.R, v - T.position);
}
-// v2 = A.q' * (B.q * v1 + B.p - A.p)
-// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
+// v2 = A.R' * (B.R * v1 + B.p - A.p) = (A.R' * B.R) * v1 + (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);
+ C.R = b2MulT(A.R, B.R);
+ C.position = B.position - A.position;
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));
@@ -625,7 +549,7 @@ inline b2Vec2 b2Abs(const b2Vec2& a)
inline b2Mat22 b2Abs(const b2Mat22& A)
{
- return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
+ return b2Mat22(b2Abs(A.col1), b2Abs(A.col2));
}
template <typename T>
@@ -634,6 +558,11 @@ inline T b2Min(T a, T b)
return a < b ? a : b;
}
+inline qreal b2Min(qreal a, qreal 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));
@@ -645,6 +574,11 @@ inline T b2Max(T a, T b)
return a > b ? a : b;
}
+inline qreal b2Max(qreal a, qreal 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));
@@ -656,6 +590,11 @@ inline T b2Clamp(T a, T low, T high)
return b2Max(low, b2Min(a, high));
}
+inline qreal b2Clamp(qreal a, qreal low, qreal 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));
@@ -689,30 +628,30 @@ inline bool b2IsPowerOfTwo(uint32 x)
return result;
}
-inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
+inline void b2Sweep::GetTransform(b2Transform* xf, qreal beta) const
{
- xf->p = (1.0f - beta) * c0 + beta * c;
- float32 angle = (1.0f - beta) * a0 + beta * a;
- xf->q.Set(angle);
+ xf->position = (1.0f - beta) * c0 + beta * c;
+ qreal angle = (1.0f - beta) * a0 + beta * a;
+ xf->R.Set(angle);
// Shift to origin
- xf->p -= b2Mul(xf->q, localCenter);
+ xf->position -= b2Mul(xf->R, localCenter);
}
-inline void b2Sweep::Advance(float32 alpha)
+inline void b2Sweep::Advance(qreal alpha)
{
b2Assert(alpha0 < 1.0f);
- float32 beta = (alpha - alpha0) / (1.0f - alpha0);
- c0 += beta * (c - c0);
- a0 += beta * (a - a0);
+ qreal beta = (alpha - alpha0) / (1.0f - alpha0);
+ c0 = (1.0f - beta) * c0 + beta * c;
+ a0 = (1.0f - beta) * a0 + beta * a;
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);
+ qreal twoPi = 2.0f * b2_pi;
+ qreal d = twoPi * floorf(a0 / twoPi);
a0 -= d;
a -= d;
}
diff --git a/external/Box2D/Common/b2Settings.cpp b/external/Box2D/Common/b2Settings.cpp
index 91e1477..8fef728 100644
--- a/external/Box2D/Common/b2Settings.cpp
+++ b/external/Box2D/Common/b2Settings.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -17,11 +17,9 @@
*/
#include <Box2D/Common/b2Settings.h>
-#include <stdio.h>
-#include <stdarg.h>
-#include <stdlib.h>
+#include <cstdlib>
-b2Version b2_version = {2, 3, 2};
+b2Version b2_version = {2, 2, 0};
// Memory allocators. Modify these to use your own allocator.
void* b2Alloc(int32 size)
@@ -33,12 +31,3 @@ 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);
-}
diff --git a/external/Box2D/Common/b2Settings.h b/external/Box2D/Common/b2Settings.h
index 86f76bd..4d50f72 100644
--- a/external/Box2D/Common/b2Settings.h
+++ b/external/Box2D/Common/b2Settings.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,9 +19,12 @@
#ifndef B2_SETTINGS_H
#define B2_SETTINGS_H
-#include <stddef.h>
-#include <assert.h>
-#include <float.h>
+#include <cassert>
+#include <cfloat>
+#include <cmath>
+#include <climits>
+#include <stdint.h>
+#include <QtCore/qmath.h>
#define B2_NOT_USED(x) ((void)(x))
#define b2Assert(A) assert(A)
@@ -32,12 +35,22 @@ 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
+template<size_t fltsize> struct b2_floatValues;
+template<> struct b2_floatValues<4> {
+ static inline float maxFloat() { return FLT_MAX; }
+ static inline float epsilon() { return FLT_EPSILON; }
+ static inline float pi() { return 3.14159265359f; }
+};
+template<> struct b2_floatValues<8> {
+ static inline double maxFloat() { return DBL_MAX; }
+ static inline double epsilon() { return DBL_EPSILON; }
+ static inline double pi() { return M_PI; }
+};
+
+#define b2_maxFloat b2_floatValues<sizeof(qreal)>::maxFloat()
+#define b2_epsilon b2_floatValues<sizeof(qreal)>::epsilon()
+#define b2_pi b2_floatValues<sizeof(qreal)>::pi()
/// @file
/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
@@ -45,8 +58,7 @@ typedef double float64;
// Collision
-/// The maximum number of contact points between two convex shapes. Do
-/// not change this value.
+/// The maximum number of contact points between two convex shapes.
#define b2_maxManifoldPoints 2
/// The maximum number of vertices on a convex polygon. You cannot increase
@@ -87,7 +99,7 @@ typedef double float64;
/// 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
+#define b2_velocityThreshold 0.0f
/// The maximum linear position correction used when solving constraints. This helps to
/// prevent overshoot.
@@ -110,8 +122,7 @@ typedef double float64;
/// 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
+#define b2_contactBaumgarte 0.2f
// Sleep
@@ -133,9 +144,6 @@ 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
@@ -148,4 +156,16 @@ struct b2Version
/// Current version.
extern b2Version b2_version;
+/// Friction mixing law. Feel free to customize this.
+inline qreal b2MixFriction(qreal friction1, qreal friction2)
+{
+ return std::sqrt(friction1 * friction2);
+}
+
+/// Restitution mixing law. Feel free to customize this.
+inline qreal b2MixRestitution(qreal restitution1, qreal restitution2)
+{
+ return restitution1 > restitution2 ? restitution1 : restitution2;
+}
+
#endif
diff --git a/external/Box2D/Common/b2StackAllocator.cpp b/external/Box2D/Common/b2StackAllocator.cpp
index 321cabb..c49e311 100644
--- a/external/Box2D/Common/b2StackAllocator.cpp
+++ b/external/Box2D/Common/b2StackAllocator.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Common/b2StackAllocator.h b/external/Box2D/Common/b2StackAllocator.h
index bb87a7e..590d04c 100644
--- a/external/Box2D/Common/b2StackAllocator.h
+++ b/external/Box2D/Common/b2StackAllocator.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Common/b2Timer.cpp b/external/Box2D/Common/b2Timer.cpp
deleted file mode 100644
index cbea530..0000000
--- a/external/Box2D/Common/b2Timer.cpp
+++ /dev/null
@@ -1,101 +0,0 @@
-/*
-* 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;
-
-#define WIN32_LEAN_AND_MEAN
-#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);
- return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);
-}
-
-#else
-
-b2Timer::b2Timer()
-{
-}
-
-void b2Timer::Reset()
-{
-}
-
-float32 b2Timer::GetMilliseconds() const
-{
- return 0.0f;
-}
-
-#endif
diff --git a/external/Box2D/Common/b2Timer.h b/external/Box2D/Common/b2Timer.h
deleted file mode 100644
index edf1ca4..0000000
--- a/external/Box2D/Common/b2Timer.h
+++ /dev/null
@@ -1,50 +0,0 @@
-/*
-* 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 <Box2D/Common/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 m_start_sec;
- unsigned long m_start_usec;
-#endif
-};
-
-#endif
diff --git a/external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp b/external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
deleted file mode 100644
index 4642fa4..0000000
--- a/external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.cpp
+++ /dev/null
@@ -1,53 +0,0 @@
-/*
-* 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);
-}
diff --git a/external/Box2D/Dynamics/Contacts/b2CircleContact.cpp b/external/Box2D/Dynamics/Contacts/b2CircleContact.cpp
index b47f35a..3203f09 100644
--- a/external/Box2D/Dynamics/Contacts/b2CircleContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2CircleContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,6 +24,7 @@
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <new>
+using namespace std;
b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
diff --git a/external/Box2D/Dynamics/Contacts/b2CircleContact.h b/external/Box2D/Dynamics/Contacts/b2CircleContact.h
index 079d3ab..59552eb 100644
--- a/external/Box2D/Dynamics/Contacts/b2CircleContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2CircleContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/Contacts/b2Contact.cpp b/external/Box2D/Dynamics/Contacts/b2Contact.cpp
index d9f1021..dce316f 100644
--- a/external/Box2D/Dynamics/Contacts/b2Contact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2Contact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -22,8 +22,8 @@
#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/b2LoopAndCircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h>
#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
#include <Box2D/Collision/b2Collision.h>
@@ -44,15 +44,15 @@ void b2Contact::InitializeRegisters()
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);
+ AddType(b2LoopAndCircleContact::Create, b2LoopAndCircleContact::Destroy, b2Shape::e_loop, b2Shape::e_circle);
+ AddType(b2LoopAndPolygonContact::Create, b2LoopAndPolygonContact::Destroy, b2Shape::e_loop, 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);
+ b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+ b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
s_registers[type1][type2].createFcn = createFcn;
s_registers[type1][type2].destroyFcn = destoryFcn;
@@ -77,8 +77,8 @@ b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtu
b2Shape::Type type1 = fixtureA->GetType();
b2Shape::Type type2 = fixtureB->GetType();
- b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
- b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
+ b2Assert(b2Shape::e_unknown < type1 && type1 < b2Shape::e_typeCount);
+ b2Assert(b2Shape::e_unknown < type2 && type2 < b2Shape::e_typeCount);
b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
if (createFcn)
@@ -102,22 +102,17 @@ 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)
+ if (contact->m_manifold.pointCount > 0)
{
- fixtureA->GetBody()->SetAwake(true);
- fixtureB->GetBody()->SetAwake(true);
+ contact->GetFixtureA()->GetBody()->SetAwake(true);
+ contact->GetFixtureB()->GetBody()->SetAwake(true);
}
- b2Shape::Type typeA = fixtureA->GetType();
- b2Shape::Type typeB = fixtureB->GetType();
+ b2Shape::Type typeA = contact->GetFixtureA()->GetType();
+ b2Shape::Type typeB = contact->GetFixtureB()->GetType();
- b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
- b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
+ b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
+ b2Assert(b2Shape::e_unknown < typeA && typeB < b2Shape::e_typeCount);
b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
destroyFcn(contact, allocator);
@@ -149,11 +144,6 @@ b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
m_nodeB.other = NULL;
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.
@@ -200,6 +190,7 @@ void b2Contact::Update(b2ContactListener* listener)
mp2->normalImpulse = 0.0f;
mp2->tangentImpulse = 0.0f;
b2ContactID id2 = mp2->id;
+ bool found = false;
for (int32 j = 0; j < oldManifold.pointCount; ++j)
{
@@ -209,9 +200,16 @@ void b2Contact::Update(b2ContactListener* listener)
{
mp2->normalImpulse = mp1->normalImpulse;
mp2->tangentImpulse = mp1->tangentImpulse;
+ found = true;
break;
}
}
+
+ if (found == false)
+ {
+ mp2->normalImpulse = 0.0f;
+ mp2->tangentImpulse = 0.0f;
+ }
}
if (touching != wasTouching)
diff --git a/external/Box2D/Dynamics/Contacts/b2Contact.h b/external/Box2D/Dynamics/Contacts/b2Contact.h
index 36e31e2..f3a0460 100644
--- a/external/Box2D/Dynamics/Contacts/b2Contact.h
+++ b/external/Box2D/Dynamics/Contacts/b2Contact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -29,23 +29,8 @@ 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 restitution 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);
@@ -115,32 +100,6 @@ public:
/// 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;
@@ -211,12 +170,7 @@ protected:
b2Manifold m_manifold;
int32 m_toiCount;
- float32 m_toi;
-
- float32 m_friction;
- float32 m_restitution;
-
- float32 m_tangentSpeed;
+ qreal m_toi;
};
inline b2Manifold* b2Contact::GetManifold()
@@ -306,44 +260,4 @@ 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
diff --git a/external/Box2D/Dynamics/Contacts/b2ContactSolver.cpp b/external/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
index e31b6c2..c4f16bf 100644
--- a/external/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2ContactSolver.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -18,6 +18,7 @@
#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
+
#include <Box2D/Dynamics/Contacts/b2Contact.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
@@ -26,114 +27,77 @@
#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;
+ m_constraints = (b2ContactConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactConstraint));
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
- b2Contact* contact = m_contacts[i];
+ b2Contact* contact = def->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;
+ qreal radiusA = shapeA->m_radius;
+ qreal 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)
+ b2Assert(manifold->pointCount > 0);
+
+ b2ContactConstraint* cc = m_constraints + i;
+ cc->friction = b2MixFriction(fixtureA->GetFriction(), fixtureB->GetFriction());
+ cc->restitution = b2MixRestitution(fixtureA->GetRestitution(), fixtureB->GetRestitution());
+ cc->bodyA = bodyA;
+ cc->bodyB = bodyB;
+ cc->manifold = manifold;
+ cc->normal.SetZero();
+ cc->pointCount = manifold->pointCount;
+
+ cc->localNormal = manifold->localNormal;
+ cc->localPoint = manifold->localPoint;
+ cc->radiusA = radiusA;
+ cc->radiusB = radiusB;
+ cc->type = manifold->type;
+
+ for (int32 j = 0; j < cc->pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->points + j;
- b2VelocityConstraintPoint* vcp = vc->points + j;
-
- if (m_step.warmStarting)
+ b2ContactConstraintPoint* ccp = cc->points + j;
+
+ if (def->warmStarting)
{
- vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
- vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
+ ccp->normalImpulse = def->impulseRatio * cp->normalImpulse;
+ ccp->tangentImpulse = def->impulseRatio * cp->tangentImpulse;
}
else
{
- vcp->normalImpulse = 0.0f;
- vcp->tangentImpulse = 0.0f;
+ ccp->normalImpulse = 0.0f;
+ ccp->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;
+ ccp->localPoint = cp->localPoint;
+ ccp->rA.SetZero();
+ ccp->rB.SetZero();
+ ccp->normalMass = 0.0f;
+ ccp->tangentMass = 0.0f;
+ ccp->velocityBias = 0.0f;
}
+
+ cc->K.SetZero();
+ cc->normalMass.SetZero();
}
}
b2ContactSolver::~b2ContactSolver()
{
- m_allocator->Free(m_velocityConstraints);
- m_allocator->Free(m_positionConstraints);
+ m_allocator->Free(m_constraints);
}
// Initialize position dependent portions of the velocity constraints.
@@ -141,108 +105,99 @@ void b2ContactSolver::InitializeVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
- b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
- b2ContactPositionConstraint* pc = m_positionConstraints + i;
+ b2ContactConstraint* cc = m_constraints + i;
- float32 radiusA = pc->radiusA;
- float32 radiusB = pc->radiusB;
- b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
+ qreal radiusA = cc->radiusA;
+ qreal radiusB = cc->radiusB;
+ b2Body* bodyA = cc->bodyA;
+ b2Body* bodyB = cc->bodyB;
+ b2Manifold* manifold = cc->manifold;
- 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;
+ b2Vec2 vA = bodyA->m_linearVelocity;
+ b2Vec2 vB = bodyB->m_linearVelocity;
+ qreal wA = bodyA->m_angularVelocity;
+ qreal wB = bodyB->m_angularVelocity;
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);
+ worldManifold.Initialize(manifold, bodyA->m_xf, radiusA, bodyB->m_xf, radiusB);
- vc->normal = worldManifold.normal;
+ cc->normal = worldManifold.normal;
- int32 pointCount = vc->pointCount;
- for (int32 j = 0; j < pointCount; ++j)
+ for (int32 j = 0; j < cc->pointCount; ++j)
{
- b2VelocityConstraintPoint* vcp = vc->points + j;
+ b2ManifoldPoint* cp = manifold->points + j;
+ b2ContactConstraintPoint* ccp = cc->points + j;
- vcp->rA = worldManifold.points[j] - cA;
- vcp->rB = worldManifold.points[j] - cB;
+ ccp->rA = worldManifold.points[j] - bodyA->m_sweep.c;
+ ccp->rB = worldManifold.points[j] - bodyB->m_sweep.c;
- float32 rnA = b2Cross(vcp->rA, vc->normal);
- float32 rnB = b2Cross(vcp->rB, vc->normal);
+ qreal rnA = b2Cross(ccp->rA, cc->normal);
+ qreal rnB = b2Cross(ccp->rB, cc->normal);
+ rnA *= rnA;
+ rnB *= rnB;
- float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
+ qreal kNormal = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rnA + bodyB->m_invI * rnB;
- vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
+ b2Assert(kNormal > b2_epsilon);
+ ccp->normalMass = 1.0f / kNormal;
- b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
+ b2Vec2 tangent = b2Cross(cc->normal, 1.0f);
- float32 rtA = b2Cross(vcp->rA, tangent);
- float32 rtB = b2Cross(vcp->rB, tangent);
+ qreal rtA = b2Cross(ccp->rA, tangent);
+ qreal rtB = b2Cross(ccp->rB, tangent);
+ rtA *= rtA;
+ rtB *= rtB;
- float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
+ qreal kTangent = bodyA->m_invMass + bodyB->m_invMass + bodyA->m_invI * rtA + bodyB->m_invI * rtB;
- vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
+ b2Assert(kTangent > b2_epsilon);
+ ccp->tangentMass = 1.0f / kTangent;
// 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));
+ ccp->velocityBias = 0.0f;
+ qreal vRel = b2Dot(cc->normal, vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA));
if (vRel < -b2_velocityThreshold)
{
- vcp->velocityBias = -vc->restitution * vRel;
+ ccp->velocityBias = -cc->restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
- if (vc->pointCount == 2 && g_blockSolve)
+ if (cc->pointCount == 2)
{
- b2VelocityConstraintPoint* vcp1 = vc->points + 0;
- b2VelocityConstraintPoint* vcp2 = vc->points + 1;
+ b2ContactConstraintPoint* ccp1 = cc->points + 0;
+ b2ContactConstraintPoint* ccp2 = cc->points + 1;
+
+ qreal invMassA = bodyA->m_invMass;
+ qreal invIA = bodyA->m_invI;
+ qreal invMassB = bodyB->m_invMass;
+ qreal invIB = bodyB->m_invI;
- 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);
+ qreal rn1A = b2Cross(ccp1->rA, cc->normal);
+ qreal rn1B = b2Cross(ccp1->rB, cc->normal);
+ qreal rn2A = b2Cross(ccp2->rA, cc->normal);
+ qreal rn2B = b2Cross(ccp2->rB, cc->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;
+ qreal k11 = invMassA + invMassB + invIA * rn1A * rn1A + invIB * rn1B * rn1B;
+ qreal k22 = invMassA + invMassB + invIA * rn2A * rn2A + invIB * rn2B * rn2B;
+ qreal k12 = invMassA + invMassB + invIA * rn1A * rn2A + invIB * rn1B * rn2B;
// Ensure a reasonable condition number.
- const float32 k_maxConditionNumber = 1000.0f;
+ const qreal k_maxConditionNumber = 100.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();
+ cc->K.col1.Set(k11, k12);
+ cc->K.col2.Set(k12, k22);
+ cc->normalMass = cc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
- vc->pointCount = 1;
+ cc->pointCount = 1;
}
}
}
@@ -253,38 +208,26 @@ 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;
+ b2ContactConstraint* c = m_constraints + i;
+
+ b2Body* bodyA = c->bodyA;
+ b2Body* bodyB = c->bodyB;
+ qreal invMassA = bodyA->m_invMass;
+ qreal invIA = bodyA->m_invI;
+ qreal invMassB = bodyB->m_invMass;
+ qreal invIB = bodyB->m_invI;
+ b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
- for (int32 j = 0; j < pointCount; ++j)
+ for (int32 j = 0; j < c->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;
+ b2ContactConstraintPoint* ccp = c->points + j;
+ b2Vec2 P = ccp->normalImpulse * normal + ccp->tangentImpulse * tangent;
+ bodyA->m_angularVelocity -= invIA * b2Cross(ccp->rA, P);
+ bodyA->m_linearVelocity -= invMassA * P;
+ bodyB->m_angularVelocity += invIB * b2Cross(ccp->rB, P);
+ bodyB->m_linearVelocity += invMassB * P;
}
-
- m_velocities[indexA].v = vA;
- m_velocities[indexA].w = wA;
- m_velocities[indexB].v = vB;
- m_velocities[indexB].w = wB;
}
}
@@ -292,83 +235,76 @@ 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;
+ b2ContactConstraint* c = m_constraints + i;
+ b2Body* bodyA = c->bodyA;
+ b2Body* bodyB = c->bodyB;
+ qreal wA = bodyA->m_angularVelocity;
+ qreal wB = bodyB->m_angularVelocity;
+ b2Vec2 vA = bodyA->m_linearVelocity;
+ b2Vec2 vB = bodyB->m_linearVelocity;
+ qreal invMassA = bodyA->m_invMass;
+ qreal invIA = bodyA->m_invI;
+ qreal invMassB = bodyB->m_invMass;
+ qreal invIB = bodyB->m_invI;
+ b2Vec2 normal = c->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
- float32 friction = vc->friction;
+ qreal friction = c->friction;
- b2Assert(pointCount == 1 || pointCount == 2);
+ b2Assert(c->pointCount == 1 || c->pointCount == 2);
- // Solve tangent constraints first because non-penetration is more important
- // than friction.
- for (int32 j = 0; j < pointCount; ++j)
+ // Solve tangent constraints
+ for (int32 j = 0; j < c->pointCount; ++j)
{
- b2VelocityConstraintPoint* vcp = vc->points + j;
+ b2ContactConstraintPoint* ccp = c->points + j;
// Relative velocity at contact
- b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
+ b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
// Compute tangent force
- float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
- float32 lambda = vcp->tangentMass * (-vt);
+ qreal vt = b2Dot(dv, tangent);
+ qreal lambda = ccp->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;
+ qreal maxFriction = friction * ccp->normalImpulse;
+ qreal newImpulse = b2Clamp(ccp->tangentImpulse + lambda, -maxFriction, maxFriction);
+ lambda = newImpulse - ccp->tangentImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
- vA -= mA * P;
- wA -= iA * b2Cross(vcp->rA, P);
+ vA -= invMassA * P;
+ wA -= invIA * b2Cross(ccp->rA, P);
+
+ vB += invMassB * P;
+ wB += invIB * b2Cross(ccp->rB, P);
- vB += mB * P;
- wB += iB * b2Cross(vcp->rB, P);
+ ccp->tangentImpulse = newImpulse;
}
// Solve normal constraints
- if (pointCount == 1 || g_blockSolve == false)
+ if (c->pointCount == 1)
{
- for (int32 i = 0; i < pointCount; ++i)
- {
- b2VelocityConstraintPoint* vcp = vc->points + i;
+ b2ContactConstraintPoint* ccp = c->points + 0;
- // Relative velocity at contact
- b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
+ // Relative velocity at contact
+ b2Vec2 dv = vB + b2Cross(wB, ccp->rB) - vA - b2Cross(wA, ccp->rA);
- // Compute normal impulse
- float32 vn = b2Dot(dv, normal);
- float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
+ // Compute normal impulse
+ qreal vn = b2Dot(dv, normal);
+ qreal lambda = -ccp->normalMass * (vn - ccp->velocityBias);
- // b2Clamp the accumulated impulse
- float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
- lambda = newImpulse - vcp->normalImpulse;
- vcp->normalImpulse = newImpulse;
+ // b2Clamp the accumulated impulse
+ qreal newImpulse = b2Max(ccp->normalImpulse + lambda, 0.0f);
+ lambda = newImpulse - ccp->normalImpulse;
- // Apply contact impulse
- b2Vec2 P = lambda * normal;
- vA -= mA * P;
- wA -= iA * b2Cross(vcp->rA, P);
+ // Apply contact impulse
+ b2Vec2 P = lambda * normal;
+ vA -= invMassA * P;
+ wA -= invIA * b2Cross(ccp->rA, P);
- vB += mB * P;
- wB += iB * b2Cross(vcp->rB, P);
- }
+ vB += invMassB * P;
+ wB += invIB * b2Cross(ccp->rB, P);
+ ccp->normalImpulse = newImpulse;
}
else
{
@@ -378,7 +314,7 @@ void b2ContactSolver::SolveVelocityConstraints()
// vn = A * x + b, vn >= 0, , 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
+ // b = vn_0 - 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
@@ -390,23 +326,18 @@ void b2ContactSolver::SolveVelocityConstraints()
//
// Substitute:
//
- // x = a + d
+ // x = x' - a
//
- // 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:
+ // Plug into above equation:
//
- // vn = A * d + b
- // = A * (x - a) + b
- // = A * x + b - A * a
- // = A * x + b'
+ // vn = A * x + 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;
+ b2ContactConstraintPoint* cp1 = c->points + 0;
+ b2ContactConstraintPoint* cp2 = c->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
@@ -416,17 +347,15 @@ void b2ContactSolver::SolveVelocityConstraints()
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);
+ qreal vn1 = b2Dot(dv1, normal);
+ qreal vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
+ b -= b2Mul(c->K, a);
- // Compute b'
- b -= b2Mul(vc->K, a);
-
- const float32 k_errorTol = 1e-3f;
+ const qreal k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
@@ -434,27 +363,27 @@ void b2ContactSolver::SolveVelocityConstraints()
//
// Case 1: vn = 0
//
- // 0 = A * x + b'
+ // 0 = A * x' + b'
//
- // Solve for x:
+ // Solve for x':
//
- // x = - inv(A) * b'
+ // x' = - inv(A) * b'
//
- b2Vec2 x = - b2Mul(vc->normalMass, b);
+ b2Vec2 x = - b2Mul(c->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
- // Get the incremental impulse
+ // 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));
+ vA -= invMassA * (P1 + P2);
+ wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
- vB += mB * (P1 + P2);
- wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+ vB += invMassB * (P1 + P2);
+ wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
@@ -478,27 +407,27 @@ void b2ContactSolver::SolveVelocityConstraints()
//
// Case 2: vn1 = 0 and x2 = 0
//
- // 0 = a11 * x1 + a12 * 0 + b1'
- // vn2 = a21 * x1 + a22 * 0 + b2'
+ // 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;
+ vn2 = c->K.col1.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
- // Get the incremental impulse
+ // 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));
+ vA -= invMassA * (P1 + P2);
+ wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
- vB += mB * (P1 + P2);
- wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+ vB += invMassB * (P1 + P2);
+ wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
@@ -520,12 +449,12 @@ void b2ContactSolver::SolveVelocityConstraints()
//
// Case 3: vn2 = 0 and x1 = 0
//
- // vn1 = a11 * 0 + a12 * x2 + b1'
- // 0 = a21 * 0 + a22 * x2 + b2'
+ // 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;
+ vn1 = c->K.col2.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
@@ -536,11 +465,11 @@ void b2ContactSolver::SolveVelocityConstraints()
// 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));
+ vA -= invMassA * (P1 + P2);
+ wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
- vB += mB * (P1 + P2);
- wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+ vB += invMassB * (P1 + P2);
+ wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
@@ -576,11 +505,11 @@ void b2ContactSolver::SolveVelocityConstraints()
// 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));
+ vA -= invMassA * (P1 + P2);
+ wA -= invIA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
- vB += mB * (P1 + P2);
- wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
+ vB += invMassB * (P1 + P2);
+ wB += invIB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
@@ -594,10 +523,10 @@ void b2ContactSolver::SolveVelocityConstraints()
}
}
- m_velocities[indexA].v = vA;
- m_velocities[indexA].w = wA;
- m_velocities[indexB].v = vB;
- m_velocities[indexB].w = wB;
+ bodyA->m_linearVelocity = vA;
+ bodyA->m_angularVelocity = wA;
+ bodyB->m_linearVelocity = vB;
+ bodyB->m_angularVelocity = wB;
}
}
@@ -605,54 +534,62 @@ void b2ContactSolver::StoreImpulses()
{
for (int32 i = 0; i < m_count; ++i)
{
- b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
- b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
+ b2ContactConstraint* c = m_constraints + i;
+ b2Manifold* m = c->manifold;
- for (int32 j = 0; j < vc->pointCount; ++j)
+ for (int32 j = 0; j < c->pointCount; ++j)
{
- manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
- manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
+ m->points[j].normalImpulse = c->points[j].normalImpulse;
+ m->points[j].tangentImpulse = c->points[j].tangentImpulse;
}
}
}
struct b2PositionSolverManifold
{
- void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
+ void Initialize(b2ContactConstraint* cc, int32 index)
{
- b2Assert(pc->pointCount > 0);
+ b2Assert(cc->pointCount > 0);
- switch (pc->type)
+ switch (cc->type)
{
case b2Manifold::e_circles:
{
- b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
- b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
- normal = pointB - pointA;
- normal.Normalize();
+ b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
+ b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint);
+ if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
+ {
+ normal = pointB - pointA;
+ normal.Normalize();
+ }
+ else
+ {
+ normal.Set(1.0f, 0.0f);
+ }
+
point = 0.5f * (pointA + pointB);
- separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
+ separation = b2Dot(pointB - pointA, normal) - cc->radiusA - cc->radiusB;
}
break;
case b2Manifold::e_faceA:
{
- normal = b2Mul(xfA.q, pc->localNormal);
- b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
+ normal = cc->bodyA->GetWorldVector(cc->localNormal);
+ b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
- b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
- separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
+ b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[index].localPoint);
+ separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB;
point = clipPoint;
}
break;
case b2Manifold::e_faceB:
{
- normal = b2Mul(xfB.q, pc->localNormal);
- b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
+ normal = cc->bodyB->GetWorldVector(cc->localNormal);
+ b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
- b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
- separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
+ b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[index].localPoint);
+ separation = b2Dot(clipPoint - planePoint, normal) - cc->radiusA - cc->radiusB;
point = clipPoint;
// Ensure normal points from A to B
@@ -664,172 +601,134 @@ struct b2PositionSolverManifold
b2Vec2 normal;
b2Vec2 point;
- float32 separation;
+ qreal separation;
};
// Sequential solver.
-bool b2ContactSolver::SolvePositionConstraints()
+bool b2ContactSolver::SolvePositionConstraints(qreal baumgarte)
{
- float32 minSeparation = 0.0f;
+ qreal minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
- b2ContactPositionConstraint* pc = m_positionConstraints + i;
+ b2ContactConstraint* c = m_constraints + i;
+ b2Body* bodyA = c->bodyA;
+ b2Body* bodyB = c->bodyB;
- 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;
+ qreal invMassA = bodyA->m_mass * bodyA->m_invMass;
+ qreal invIA = bodyA->m_mass * bodyA->m_invI;
+ qreal invMassB = bodyB->m_mass * bodyB->m_invMass;
+ qreal invIB = bodyB->m_mass * bodyB->m_invI;
// Solve normal constraints
- for (int32 j = 0; j < pointCount; ++j)
+ for (int32 j = 0; j < c->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);
+ psm.Initialize(c, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
- float32 separation = psm.separation;
+ qreal separation = psm.separation;
- b2Vec2 rA = point - cA;
- b2Vec2 rB = point - cB;
+ b2Vec2 rA = point - bodyA->m_sweep.c;
+ b2Vec2 rB = point - bodyB->m_sweep.c;
// 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);
+ qreal C = b2Clamp(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;
+ qreal rnA = b2Cross(rA, normal);
+ qreal rnB = b2Cross(rB, normal);
+ qreal K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
- float32 impulse = K > 0.0f ? - C / K : 0.0f;
+ qreal impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
- cA -= mA * P;
- aA -= iA * b2Cross(rA, P);
+ bodyA->m_sweep.c -= invMassA * P;
+ bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
+ bodyA->SynchronizeTransform();
- cB += mB * P;
- aB += iB * b2Cross(rB, P);
+ bodyB->m_sweep.c += invMassB * P;
+ bodyB->m_sweep.a += invIB * b2Cross(rB, P);
+ bodyB->SynchronizeTransform();
}
-
- 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;
+ return minSeparation >= -1.5f * b2_linearSlop;
}
// Sequential position solver for position constraints.
-bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
+bool b2ContactSolver::SolveTOIPositionConstraints(qreal baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB)
{
- float32 minSeparation = 0.0f;
+ qreal minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
- b2ContactPositionConstraint* pc = m_positionConstraints + i;
+ b2ContactConstraint* c = m_constraints + i;
+ b2Body* bodyA = c->bodyA;
+ b2Body* bodyB = c->bodyB;
- 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)
+ qreal massA = 0.0f;
+ if (bodyA == toiBodyA || bodyA == toiBodyB)
{
- mA = pc->invMassA;
- iA = pc->invIA;
+ massA = bodyA->m_mass;
}
- float32 mB = 0.0f;
- float32 iB = 0.;
- if (indexB == toiIndexA || indexB == toiIndexB)
+ qreal massB = 0.0f;
+ if (bodyB == toiBodyA || bodyB == toiBodyB)
{
- mB = pc->invMassB;
- iB = pc->invIB;
+ massB = bodyB->m_mass;
}
- b2Vec2 cA = m_positions[indexA].c;
- float32 aA = m_positions[indexA].a;
-
- b2Vec2 cB = m_positions[indexB].c;
- float32 aB = m_positions[indexB].a;
+ qreal invMassA = bodyA->m_mass * bodyA->m_invMass;
+ qreal invIA = bodyA->m_mass * bodyA->m_invI;
+ qreal invMassB = bodyB->m_mass * bodyB->m_invMass;
+ qreal invIB = bodyB->m_mass * bodyB->m_invI;
// Solve normal constraints
- for (int32 j = 0; j < pointCount; ++j)
+ for (int32 j = 0; j < c->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);
+ psm.Initialize(c, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
- float32 separation = psm.separation;
+ qreal separation = psm.separation;
- b2Vec2 rA = point - cA;
- b2Vec2 rB = point - cB;
+ b2Vec2 rA = point - bodyA->m_sweep.c;
+ b2Vec2 rB = point - bodyB->m_sweep.c;
// 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);
+ qreal C = b2Clamp(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;
+ qreal rnA = b2Cross(rA, normal);
+ qreal rnB = b2Cross(rB, normal);
+ qreal K = invMassA + invMassB + invIA * rnA * rnA + invIB * rnB * rnB;
// Compute normal impulse
- float32 impulse = K > 0.0f ? - C / K : 0.0f;
+ qreal impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
- cA -= mA * P;
- aA -= iA * b2Cross(rA, P);
+ bodyA->m_sweep.c -= invMassA * P;
+ bodyA->m_sweep.a -= invIA * b2Cross(rA, P);
+ bodyA->SynchronizeTransform();
- cB += mB * P;
- aB += iB * b2Cross(rB, P);
+ bodyB->m_sweep.c += invMassB * P;
+ bodyB->m_sweep.a += invIB * b2Cross(rB, P);
+ bodyB->SynchronizeTransform();
}
-
- 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
diff --git a/external/Box2D/Dynamics/Contacts/b2ContactSolver.h b/external/Box2D/Dynamics/Contacts/b2ContactSolver.h
index a622e30..64c440f 100644
--- a/external/Box2D/Dynamics/Contacts/b2ContactSolver.h
+++ b/external/Box2D/Dynamics/Contacts/b2ContactSolver.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,49 +21,49 @@
#include <Box2D/Common/b2Math.h>
#include <Box2D/Collision/b2Collision.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
+#include <Box2D/Dynamics/b2Island.h>
class b2Contact;
class b2Body;
class b2StackAllocator;
-struct b2ContactPositionConstraint;
-struct b2VelocityConstraintPoint
+struct b2ContactConstraintPoint
{
+ b2Vec2 localPoint;
b2Vec2 rA;
b2Vec2 rB;
- float32 normalImpulse;
- float32 tangentImpulse;
- float32 normalMass;
- float32 tangentMass;
- float32 velocityBias;
+ qreal normalImpulse;
+ qreal tangentImpulse;
+ qreal normalMass;
+ qreal tangentMass;
+ qreal velocityBias;
};
-struct b2ContactVelocityConstraint
+struct b2ContactConstraint
{
- b2VelocityConstraintPoint points[b2_maxManifoldPoints];
+ b2ContactConstraintPoint points[b2_maxManifoldPoints];
+ b2Vec2 localNormal;
+ b2Vec2 localPoint;
b2Vec2 normal;
b2Mat22 normalMass;
b2Mat22 K;
- int32 indexA;
- int32 indexB;
- float32 invMassA, invMassB;
- float32 invIA, invIB;
- float32 friction;
- float32 restitution;
- float32 tangentSpeed;
+ b2Body* bodyA;
+ b2Body* bodyB;
+ b2Manifold::Type type;
+ qreal radiusA, radiusB;
+ qreal friction;
+ qreal restitution;
int32 pointCount;
- int32 contactIndex;
+ b2Manifold* manifold;
};
struct b2ContactSolverDef
{
- b2TimeStep step;
b2Contact** contacts;
int32 count;
- b2Position* positions;
- b2Velocity* velocities;
b2StackAllocator* allocator;
+ qreal impulseRatio;
+ bool warmStarting;
};
class b2ContactSolver
@@ -78,16 +78,11 @@ public:
void SolveVelocityConstraints();
void StoreImpulses();
- bool SolvePositionConstraints();
- bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
+ bool SolvePositionConstraints(qreal baumgarte);
+ bool SolveTOIPositionConstraints(qreal baumgarte, const b2Body* toiBodyA, const b2Body* toiBodyB);
- b2TimeStep m_step;
- b2Position* m_positions;
- b2Velocity* m_velocities;
b2StackAllocator* m_allocator;
- b2ContactPositionConstraint* m_positionConstraints;
- b2ContactVelocityConstraint* m_velocityConstraints;
- b2Contact** m_contacts;
+ b2ContactConstraint* m_constraints;
int m_count;
};
diff --git a/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp b/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
index d1fd329..3206ec4 100644
--- a/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,6 +21,7 @@
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
+using namespace std;
b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
diff --git a/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h b/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h
index e7a3455..12e94f1 100644
--- a/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp b/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
index ba536bc..b293f70 100644
--- a/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,6 +21,7 @@
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
+using namespace std;
b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
diff --git a/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h b/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h
index 3b40f12..bd3b364 100644
--- a/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp b/external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp
new file mode 100644
index 0000000..ccf4782
--- /dev/null
+++ b/external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.cpp
@@ -0,0 +1,54 @@
+/*
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
+*
+* 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/b2LoopAndCircleContact.h>
+#include <Box2D/Common/b2BlockAllocator.h>
+#include <Box2D/Dynamics/b2Fixture.h>
+#include <Box2D/Collision/Shapes/b2LoopShape.h>
+#include <Box2D/Collision/Shapes/b2EdgeShape.h>
+
+#include <new>
+using namespace std;
+
+b2Contact* b2LoopAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+{
+ void* mem = allocator->Allocate(sizeof(b2LoopAndCircleContact));
+ return new (mem) b2LoopAndCircleContact(fixtureA, indexA, fixtureB, indexB);
+}
+
+void b2LoopAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+{
+ ((b2LoopAndCircleContact*)contact)->~b2LoopAndCircleContact();
+ allocator->Free(contact, sizeof(b2LoopAndCircleContact));
+}
+
+b2LoopAndCircleContact::b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+: b2Contact(fixtureA, indexA, fixtureB, indexB)
+{
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_loop);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
+}
+
+void b2LoopAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+{
+ b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape();
+ b2EdgeShape edge;
+ loop->GetChildEdge(&edge, m_indexA);
+ b2CollideEdgeAndCircle( manifold, &edge, xfA,
+ (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+}
diff --git a/external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h b/external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h
index 0c09af6..55e915a 100644
--- a/external/Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2LoopAndCircleContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -16,22 +16,22 @@
* 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
+#ifndef B2_LOOP_AND_CIRCLE_CONTACT_H
+#define B2_LOOP_AND_CIRCLE_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
-class b2ChainAndPolygonContact : public b2Contact
+class b2LoopAndCircleContact : 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() {}
+ b2LoopAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+ ~b2LoopAndCircleContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
diff --git a/external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp b/external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp
index 01c921f..04bccb5 100644
--- a/external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2010 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -16,38 +16,39 @@
* 3. This notice may not be removed or altered from any source distribution.
*/
-#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
+#include <Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
+#include <Box2D/Collision/Shapes/b2LoopShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
+using namespace std;
-b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
+b2Contact* b2LoopAndPolygonContact::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* mem = allocator->Allocate(sizeof(b2LoopAndPolygonContact));
+ return new (mem) b2LoopAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
}
-void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
+void b2LoopAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
- ((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
- allocator->Free(contact, sizeof(b2ChainAndCircleContact));
+ ((b2LoopAndPolygonContact*)contact)->~b2LoopAndPolygonContact();
+ allocator->Free(contact, sizeof(b2LoopAndPolygonContact));
}
-b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
+b2LoopAndPolygonContact::b2LoopAndPolygonContact(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);
+ b2Assert(m_fixtureA->GetType() == b2Shape::e_loop);
+ b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
}
-void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
+void b2LoopAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
- b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
+ b2LoopShape* loop = (b2LoopShape*)m_fixtureA->GetShape();
b2EdgeShape edge;
- chain->GetChildEdge(&edge, m_indexA);
- b2CollideEdgeAndCircle( manifold, &edge, xfA,
- (b2CircleShape*)m_fixtureB->GetShape(), xfB);
+ loop->GetChildEdge(&edge, m_indexA);
+ b2CollideEdgeAndPolygon( manifold, &edge, xfA,
+ (b2PolygonShape*)m_fixtureB->GetShape(), xfB);
}
diff --git a/external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h b/external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h
index b93c30e..080bb1c 100644
--- a/external/Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2LoopAndPolygonContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -16,22 +16,22 @@
* 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
+#ifndef B2_LOOP_AND_POLYGON_CONTACT_H
+#define B2_LOOP_AND_POLYGON_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
-class b2ChainAndCircleContact : public b2Contact
+class b2LoopAndPolygonContact : 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() {}
+ b2LoopAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
+ ~b2LoopAndPolygonContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
diff --git a/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp b/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
index 7c9cbcc..97dd81b 100644
--- a/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,6 +21,7 @@
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
+using namespace std;
b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
diff --git a/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h b/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h
index fe0645e..d8a638f 100644
--- a/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/Contacts/b2PolygonContact.cpp b/external/Box2D/Dynamics/Contacts/b2PolygonContact.cpp
index b5706ca..5bc53f1 100644
--- a/external/Box2D/Dynamics/Contacts/b2PolygonContact.cpp
+++ b/external/Box2D/Dynamics/Contacts/b2PolygonContact.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,6 +24,7 @@
#include <Box2D/Dynamics/b2WorldCallbacks.h>
#include <new>
+using namespace std;
b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
diff --git a/external/Box2D/Dynamics/Contacts/b2PolygonContact.h b/external/Box2D/Dynamics/Contacts/b2PolygonContact.h
index 434e6c9..99078aa 100644
--- a/external/Box2D/Dynamics/Contacts/b2PolygonContact.h
+++ b/external/Box2D/Dynamics/Contacts/b2PolygonContact.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/Joints/b2DistanceJoint.cpp b/external/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
index ba2b6c0..9b38a56 100644
--- a/external/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2DistanceJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -46,11 +46,12 @@ void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
length = d.Length();
}
+
b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
: b2Joint(def)
{
- m_localAnchorA = def->localAnchorA;
- m_localAnchorB = def->localAnchorB;
+ m_localAnchor1 = def->localAnchorA;
+ m_localAnchor2 = def->localAnchorB;
m_length = def->length;
m_frequencyHz = def->frequencyHz;
m_dampingRatio = def->dampingRatio;
@@ -59,35 +60,18 @@ b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
m_bias = 0.0f;
}
-void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2DistanceJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ // Compute the effective mass matrix.
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+ m_u = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
// Handle singularity.
- float32 length = m_u.Length();
+ qreal length = m_u.Length();
if (length > b2_linearSlop)
{
m_u *= 1.0f / length;
@@ -97,164 +81,131 @@ void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
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;
+ qreal cr1u = b2Cross(r1, m_u);
+ qreal cr2u = b2Cross(r2, m_u);
+ qreal invMass = b1->m_invMass + b1->m_invI * cr1u * cr1u + b2->m_invMass + b2->m_invI * cr2u * cr2u;
- // 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;
+ qreal C = length - m_length;
// Frequency
- float32 omega = 2.0f * b2_pi * m_frequencyHz;
+ qreal omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
- float32 d = 2.0f * m_mass * m_dampingRatio * omega;
+ qreal d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
- float32 k = m_mass * omega * omega;
+ qreal k = m_mass * omega * omega;
// magic formulas
- float32 h = data.step.dt;
- m_gamma = h * (d + h * k);
+ m_gamma = step.dt * (d + step.dt * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
- m_bias = C * h * k * m_gamma;
+ m_bias = C * step.dt * 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;
+ m_mass = invMass + m_gamma;
+ m_mass = m_mass != 0.0f ? 1.0f / m_mass : 0.0f;
}
- if (data.step.warmStarting)
+ if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
- m_impulse *= data.step.dtRatio;
+ m_impulse *= 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);
+ b1->m_linearVelocity -= b1->m_invMass * P;
+ b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+ b2->m_linearVelocity += b2->m_invMass * P;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, 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)
+void b2DistanceJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ B2_NOT_USED(step);
+
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// 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);
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+ qreal Cdot = b2Dot(m_u, v2 - v1);
- float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
+ qreal 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;
+ b1->m_linearVelocity -= b1->m_invMass * P;
+ b1->m_angularVelocity -= b1->m_invI * b2Cross(r1, P);
+ b2->m_linearVelocity += b2->m_invMass * P;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P);
}
-bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2DistanceJoint::SolvePositionConstraints(qreal baumgarte)
{
+ B2_NOT_USED(baumgarte);
+
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;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- b2Rot qA(aA), qB(aB);
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
- b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
- b2Vec2 u = cB + rB - cA - rA;
+ b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
- float32 length = u.Normalize();
- float32 C = length - m_length;
+ qreal length = d.Normalize();
+ qreal C = length - m_length;
C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
- float32 impulse = -m_mass * C;
- b2Vec2 P = impulse * u;
+ qreal impulse = -m_mass * C;
+ m_u = d;
+ b2Vec2 P = impulse * m_u;
- cA -= m_invMassA * P;
- aA -= m_invIA * b2Cross(rA, P);
- cB += m_invMassB * P;
- aB += m_invIB * b2Cross(rB, P);
+ b1->m_sweep.c -= b1->m_invMass * P;
+ b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
+ b2->m_sweep.c += b2->m_invMass * P;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, 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;
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
return b2Abs(C) < b2_linearSlop;
}
b2Vec2 b2DistanceJoint::GetAnchorA() const
{
- return m_bodyA->GetWorldPoint(m_localAnchorA);
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
}
b2Vec2 b2DistanceJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
}
-b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2DistanceJoint::GetReactionForce(qreal inv_dt) const
{
b2Vec2 F = (inv_dt * m_impulse) * m_u;
return F;
}
-float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2DistanceJoint::GetReactionTorque(qreal 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2DistanceJoint.h b/external/Box2D/Dynamics/Joints/b2DistanceJoint.h
index 2faba9d..d35795f 100644
--- a/external/Box2D/Dynamics/Joints/b2DistanceJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2DistanceJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -44,21 +44,20 @@ struct b2DistanceJointDef : public b2JointDef
void Initialize(b2Body* bodyA, b2Body* bodyB,
const b2Vec2& anchorA, const b2Vec2& anchorB);
- /// The local anchor point relative to bodyA's origin.
+ /// The local anchor point relative to body1's origin.
b2Vec2 localAnchorA;
- /// The local anchor point relative to bodyB's origin.
+ /// The local anchor point relative to body2's origin.
b2Vec2 localAnchorB;
/// The natural length between the anchor points.
- float32 length;
+ qreal length;
- /// The mass-spring-damper frequency in Hertz. A value of 0
- /// disables softness.
- float32 frequencyHz;
+ /// The mass-spring-damper frequency in Hertz.
+ qreal frequencyHz;
/// The damping ratio. 0 = no damping, 1 = critical damping.
- float32 dampingRatio;
+ qreal dampingRatio;
};
/// A distance joint constrains two points on two bodies
@@ -73,95 +72,72 @@ public:
/// Get the reaction force given the inverse time step.
/// Unit is N.
- b2Vec2 GetReactionForce(float32 inv_dt) const;
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
/// 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;
-
- /// 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; }
+ qreal GetReactionTorque(qreal inv_dt) const;
/// 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;
+ void SetLength(qreal length);
+ qreal GetLength() const;
- /// Set/get damping ratio.
- void SetDampingRatio(float32 ratio);
- float32 GetDampingRatio() const;
+ // Set/get frequency in Hz.
+ void SetFrequency(qreal hz);
+ qreal GetFrequency() const;
- /// Dump joint to dmLog
- void Dump();
+ // Set/get damping ratio.
+ void SetDampingRatio(qreal ratio);
+ qreal GetDampingRatio() const;
protected:
friend class b2Joint;
b2DistanceJoint(const b2DistanceJointDef* data);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
-
- 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;
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
- // Solver temp
- int32 m_indexA;
- int32 m_indexB;
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
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;
+ qreal m_frequencyHz;
+ qreal m_dampingRatio;
+ qreal m_gamma;
+ qreal m_bias;
+ qreal m_impulse;
+ qreal m_mass;
+ qreal m_length;
};
-inline void b2DistanceJoint::SetLength(float32 length)
+inline void b2DistanceJoint::SetLength(qreal length)
{
m_length = length;
}
-inline float32 b2DistanceJoint::GetLength() const
+inline qreal b2DistanceJoint::GetLength() const
{
return m_length;
}
-inline void b2DistanceJoint::SetFrequency(float32 hz)
+inline void b2DistanceJoint::SetFrequency(qreal hz)
{
m_frequencyHz = hz;
}
-inline float32 b2DistanceJoint::GetFrequency() const
+inline qreal b2DistanceJoint::GetFrequency() const
{
return m_frequencyHz;
}
-inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
+inline void b2DistanceJoint::SetDampingRatio(qreal ratio)
{
m_dampingRatio = ratio;
}
-inline float32 b2DistanceJoint::GetDampingRatio() const
+inline qreal b2DistanceJoint::GetDampingRatio() const
{
return m_dampingRatio;
}
diff --git a/external/Box2D/Dynamics/Joints/b2FrictionJoint.cpp b/external/Box2D/Dynamics/Joints/b2FrictionJoint.cpp
index 412d257..0cea612 100644
--- a/external/Box2D/Dynamics/Joints/b2FrictionJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2FrictionJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -53,30 +53,14 @@ b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
m_maxTorque = def->maxTorque;
}
-void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2FrictionJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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);
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
// Compute the effective mass matrix.
- m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -87,15 +71,22 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
// [ -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;
+ qreal mA = bA->m_invMass, mB = bB->m_invMass;
+ qreal iA = bA->m_invI, iB = bB->m_invI;
+
+ b2Mat22 K1;
+ K1.col1.x = mA + mB; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = mA + mB;
- 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;
+ b2Mat22 K2;
+ K2.col1.x = iA * rA.y * rA.y; K2.col2.x = -iA * rA.x * rA.y;
+ K2.col1.y = -iA * rA.x * rA.y; K2.col2.y = iA * rA.x * rA.x;
+ b2Mat22 K3;
+ K3.col1.x = iB * rB.y * rB.y; K3.col2.x = -iB * rB.x * rB.y;
+ K3.col1.y = -iB * rB.x * rB.y; K3.col2.y = iB * rB.x * rB.x;
+
+ b2Mat22 K = K1 + K2 + K3;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
@@ -104,49 +95,52 @@ void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
m_angularMass = 1.0f / m_angularMass;
}
- if (data.step.warmStarting)
+ if (step.warmStarting)
{
// Scale impulses to support a variable time step.
- m_linearImpulse *= data.step.dtRatio;
- m_angularImpulse *= data.step.dtRatio;
+ m_linearImpulse *= step.dtRatio;
+ m_angularImpulse *= 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);
+
+ bA->m_linearVelocity -= mA * P;
+ bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_angularImpulse);
+
+ bB->m_linearVelocity += mB * P;
+ bB->m_angularVelocity += iB * (b2Cross(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)
+void b2FrictionJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ B2_NOT_USED(step);
+
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
+
+ b2Vec2 vA = bA->m_linearVelocity;
+ qreal wA = bA->m_angularVelocity;
+ b2Vec2 vB = bB->m_linearVelocity;
+ qreal wB = bB->m_angularVelocity;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ qreal mA = bA->m_invMass, mB = bB->m_invMass;
+ qreal iA = bA->m_invI, iB = bB->m_invI;
- float32 h = data.step.dt;
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// Solve angular friction
{
- float32 Cdot = wB - wA;
- float32 impulse = -m_angularMass * Cdot;
+ qreal Cdot = wB - wA;
+ qreal impulse = -m_angularMass * Cdot;
- float32 oldImpulse = m_angularImpulse;
- float32 maxImpulse = h * m_maxTorque;
+ qreal oldImpulse = m_angularImpulse;
+ qreal maxImpulse = step.dt * m_maxTorque;
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
@@ -156,13 +150,13 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
// Solve linear friction
{
- b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+ b2Vec2 Cdot = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
- float32 maxImpulse = h * m_maxForce;
+ qreal maxImpulse = step.dt * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
@@ -173,21 +167,21 @@ void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
- wA -= iA * b2Cross(m_rA, impulse);
+ wA -= iA * b2Cross(rA, impulse);
vB += mB * impulse;
- wB += iB * b2Cross(m_rB, impulse);
+ wB += iB * b2Cross(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;
+ bA->m_linearVelocity = vA;
+ bA->m_angularVelocity = wA;
+ bB->m_linearVelocity = vB;
+ bB->m_angularVelocity = wB;
}
-bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2FrictionJoint::SolvePositionConstraints(qreal baumgarte)
{
- B2_NOT_USED(data);
+ B2_NOT_USED(baumgarte);
return true;
}
@@ -202,50 +196,34 @@ b2Vec2 b2FrictionJoint::GetAnchorB() const
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
-b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2FrictionJoint::GetReactionForce(qreal inv_dt) const
{
return inv_dt * m_linearImpulse;
}
-float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2FrictionJoint::GetReactionTorque(qreal inv_dt) const
{
return inv_dt * m_angularImpulse;
}
-void b2FrictionJoint::SetMaxForce(float32 force)
+void b2FrictionJoint::SetMaxForce(qreal force)
{
b2Assert(b2IsValid(force) && force >= 0.0f);
m_maxForce = force;
}
-float32 b2FrictionJoint::GetMaxForce() const
+qreal b2FrictionJoint::GetMaxForce() const
{
return m_maxForce;
}
-void b2FrictionJoint::SetMaxTorque(float32 torque)
+void b2FrictionJoint::SetMaxTorque(qreal torque)
{
b2Assert(b2IsValid(torque) && torque >= 0.0f);
m_maxTorque = torque;
}
-float32 b2FrictionJoint::GetMaxTorque() const
+qreal 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2FrictionJoint.h b/external/Box2D/Dynamics/Joints/b2FrictionJoint.h
index d57abe8..a789239 100644
--- a/external/Box2D/Dynamics/Joints/b2FrictionJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2FrictionJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -44,10 +44,10 @@ struct b2FrictionJointDef : public b2JointDef
b2Vec2 localAnchorB;
/// The maximum friction force in N.
- float32 maxForce;
+ qreal maxForce;
/// The maximum friction torque in N-m.
- float32 maxTorque;
+ qreal maxTorque;
};
/// Friction joint. This is used for top-down friction.
@@ -58,29 +58,20 @@ public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// 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; }
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// Set the maximum friction force in N.
- void SetMaxForce(float32 force);
+ void SetMaxForce(qreal force);
/// Get the maximum friction force in N.
- float32 GetMaxForce() const;
+ qreal GetMaxForce() const;
/// Set the maximum friction torque in N*m.
- void SetMaxTorque(float32 torque);
+ void SetMaxTorque(qreal torque);
/// Get the maximum friction torque in N*m.
- float32 GetMaxTorque() const;
-
- /// Dump joint to dmLog
- void Dump();
+ qreal GetMaxTorque() const;
protected:
@@ -88,32 +79,21 @@ protected:
b2FrictionJoint(const b2FrictionJointDef* def);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
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;
+ qreal m_angularMass;
+
+ b2Vec2 m_linearImpulse;
+ qreal m_angularImpulse;
+
+ qreal m_maxForce;
+ qreal m_maxTorque;
};
#endif
diff --git a/external/Box2D/Dynamics/Joints/b2GearJoint.cpp b/external/Box2D/Dynamics/Joints/b2GearJoint.cpp
index 155eb0c..bd4834f 100644
--- a/external/Box2D/Dynamics/Joints/b2GearJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2GearJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,8 +24,9 @@
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
-// C = (coordinate1 + ratio * coordinate2) - C0 = 0
-// J = [J1 ratio * J2]
+// C = C0 - (cordinate1 + ratio * coordinate2) = 0
+// Cdot = -(Cdot1 + ratio * Cdot2)
+// J = -[J1 ratio * J2]
// K = J * invM * JT
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
//
@@ -44,324 +45,177 @@
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
: b2Joint(def)
{
- m_joint1 = def->joint1;
- m_joint2 = def->joint2;
+ b2JointType type1 = def->joint1->GetType();
+ b2JointType type2 = def->joint2->GetType();
- m_typeA = m_joint1->GetType();
- m_typeB = m_joint2->GetType();
+ b2Assert(type1 == e_revoluteJoint || type1 == e_prismaticJoint);
+ b2Assert(type2 == e_revoluteJoint || type2 == e_prismaticJoint);
+ b2Assert(def->joint1->GetBodyA()->GetType() == b2_staticBody);
+ b2Assert(def->joint2->GetBodyA()->GetType() == b2_staticBody);
- b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
- b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
+ m_revolute1 = NULL;
+ m_prismatic1 = NULL;
+ m_revolute2 = NULL;
+ m_prismatic2 = NULL;
- float32 coordinateA, coordinateB;
+ qreal coordinate1, coordinate2;
- // 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)
+ m_ground1 = def->joint1->GetBodyA();
+ m_bodyA = def->joint1->GetBodyB();
+ if (type1 == 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;
+ m_revolute1 = (b2RevoluteJoint*)def->joint1;
+ m_groundAnchor1 = m_revolute1->m_localAnchor1;
+ m_localAnchor1 = m_revolute1->m_localAnchor2;
+ coordinate1 = m_revolute1->GetJointAngle();
}
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_prismatic1 = (b2PrismaticJoint*)def->joint1;
+ m_groundAnchor1 = m_prismatic1->m_localAnchor1;
+ m_localAnchor1 = m_prismatic1->m_localAnchor2;
+ coordinate1 = m_prismatic1->GetJointTranslation();
}
- 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)
+ m_ground2 = def->joint2->GetBodyA();
+ m_bodyB = def->joint2->GetBodyB();
+ if (type2 == 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;
+ m_revolute2 = (b2RevoluteJoint*)def->joint2;
+ m_groundAnchor2 = m_revolute2->m_localAnchor1;
+ m_localAnchor2 = m_revolute2->m_localAnchor2;
+ coordinate2 = m_revolute2->GetJointAngle();
}
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_prismatic2 = (b2PrismaticJoint*)def->joint2;
+ m_groundAnchor2 = m_prismatic2->m_localAnchor1;
+ m_localAnchor2 = m_prismatic2->m_localAnchor2;
+ coordinate2 = m_prismatic2->GetJointTranslation();
}
m_ratio = def->ratio;
- m_constant = coordinateA + m_ratio * coordinateB;
+ m_constant = coordinate1 + m_ratio * coordinate2;
m_impulse = 0.0f;
}
-void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2GearJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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)
+ b2Body* g1 = m_ground1;
+ b2Body* g2 = m_ground2;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ qreal K = 0.0f;
+ m_J.SetZero();
+
+ if (m_revolute1)
{
- m_JvAC.SetZero();
- m_JwA = 1.0f;
- m_JwC = 1.0f;
- m_mass += m_iA + m_iC;
+ m_J.angularA = -1.0f;
+ K += b1->m_invI;
}
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;
+ b2Vec2 ug = b2Mul(g1->GetTransform().R, m_prismatic1->m_localXAxis1);
+ b2Vec2 r = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ qreal crug = b2Cross(r, ug);
+ m_J.linearA = -ug;
+ m_J.angularA = -crug;
+ K += b1->m_invMass + b1->m_invI * crug * crug;
}
- if (m_typeB == e_revoluteJoint)
+ if (m_revolute2)
{
- m_JvBD.SetZero();
- m_JwB = m_ratio;
- m_JwD = m_ratio;
- m_mass += m_ratio * m_ratio * (m_iB + m_iD);
+ m_J.angularB = -m_ratio;
+ K += m_ratio * m_ratio * b2->m_invI;
}
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;
+ b2Vec2 ug = b2Mul(g2->GetTransform().R, m_prismatic2->m_localXAxis1);
+ b2Vec2 r = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+ qreal crug = b2Cross(r, ug);
+ m_J.linearB = -m_ratio * ug;
+ m_J.angularB = -m_ratio * crug;
+ K += m_ratio * m_ratio * (b2->m_invMass + b2->m_invI * crug * crug);
}
// Compute effective mass.
- m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
+ m_mass = K > 0.0f ? 1.0f / K : 0.0f;
- if (data.step.warmStarting)
+ if (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;
+ // Warm starting.
+ b1->m_linearVelocity += b1->m_invMass * m_impulse * m_J.linearA;
+ b1->m_angularVelocity += b1->m_invI * m_impulse * m_J.angularA;
+ b2->m_linearVelocity += b2->m_invMass * m_impulse * m_J.linearB;
+ b2->m_angularVelocity += b2->m_invI * m_impulse * m_J.angularB;
}
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)
+void b2GearJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ B2_NOT_USED(step);
- 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;
-}
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
-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;
+ qreal Cdot = m_J.Compute( b1->m_linearVelocity, b1->m_angularVelocity,
+ b2->m_linearVelocity, b2->m_angularVelocity);
- b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
+ qreal impulse = m_mass * (-Cdot);
+ m_impulse += impulse;
- float32 linearError = 0.0f;
+ b1->m_linearVelocity += b1->m_invMass * impulse * m_J.linearA;
+ b1->m_angularVelocity += b1->m_invI * impulse * m_J.angularA;
+ b2->m_linearVelocity += b2->m_invMass * impulse * m_J.linearB;
+ b2->m_angularVelocity += b2->m_invI * impulse * m_J.angularB;
+}
- float32 coordinateA, coordinateB;
+bool b2GearJoint::SolvePositionConstraints(qreal baumgarte)
+{
+ B2_NOT_USED(baumgarte);
+
+ qreal linearError = 0.0f;
- b2Vec2 JvAC, JvBD;
- float32 JwA, JwB, JwC, JwD;
- float32 mass = 0.0f;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- if (m_typeA == e_revoluteJoint)
+ qreal coordinate1, coordinate2;
+ if (m_revolute1)
{
- JvAC.SetZero();
- JwA = 1.0f;
- JwC = 1.0f;
- mass += m_iA + m_iC;
-
- coordinateA = aA - aC - m_referenceAngleA;
+ coordinate1 = m_revolute1->GetJointAngle();
}
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);
+ coordinate1 = m_prismatic1->GetJointTranslation();
}
- if (m_typeB == e_revoluteJoint)
+ if (m_revolute2)
{
- JvBD.SetZero();
- JwB = m_ratio;
- JwD = m_ratio;
- mass += m_ratio * m_ratio * (m_iB + m_iD);
-
- coordinateB = aB - aD - m_referenceAngleB;
+ coordinate2 = m_revolute2->GetJointAngle();
}
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);
+ coordinate2 = m_prismatic2->GetJointTranslation();
}
- float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
+ qreal C = m_constant - (coordinate1 + m_ratio * coordinate2);
- float32 impulse = 0.0f;
- if (mass > 0.0f)
- {
- impulse = -C / mass;
- }
+ qreal impulse = m_mass * (-C);
+
+ b1->m_sweep.c += b1->m_invMass * impulse * m_J.linearA;
+ b1->m_sweep.a += b1->m_invI * impulse * m_J.angularA;
+ b2->m_sweep.c += b2->m_invMass * impulse * m_J.linearB;
+ b2->m_sweep.a += b2->m_invI * impulse * m_J.angularB;
- 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;
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
// TODO_ERIN not implemented
return linearError < b2_linearSlop;
@@ -369,51 +223,37 @@ bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
b2Vec2 b2GearJoint::GetAnchorA() const
{
- return m_bodyA->GetWorldPoint(m_localAnchorA);
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
}
b2Vec2 b2GearJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
}
-b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2GearJoint::GetReactionForce(qreal inv_dt) const
{
- b2Vec2 P = m_impulse * m_JvAC;
+ // TODO_ERIN not tested
+ b2Vec2 P = m_impulse * m_J.linearB;
return inv_dt * P;
}
-float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2GearJoint::GetReactionTorque(qreal inv_dt) const
{
- float32 L = m_impulse * m_JwA;
+ // TODO_ERIN not tested
+ b2Vec2 r = b2Mul(m_bodyB->GetTransform().R, m_localAnchor2 - m_bodyB->GetLocalCenter());
+ b2Vec2 P = m_impulse * m_J.linearB;
+ qreal L = m_impulse * m_J.angularB - b2Cross(r, P);
return inv_dt * L;
}
-void b2GearJoint::SetRatio(float32 ratio)
+void b2GearJoint::SetRatio(qreal ratio)
{
b2Assert(b2IsValid(ratio));
m_ratio = ratio;
}
-float32 b2GearJoint::GetRatio() const
+qreal 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2GearJoint.h b/external/Box2D/Dynamics/Joints/b2GearJoint.h
index fd3d7b2..0c9d5e4 100644
--- a/external/Box2D/Dynamics/Joints/b2GearJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2GearJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,8 +21,12 @@
#include <Box2D/Dynamics/Joints/b2Joint.h>
+class b2RevoluteJoint;
+class b2PrismaticJoint;
+
/// Gear joint definition. This definition requires two existing
/// revolute or prismatic joints (any combination will work).
+/// The provided joints must attach a dynamic body to a static body.
struct b2GearJointDef : public b2JointDef
{
b2GearJointDef()
@@ -41,7 +45,7 @@ struct b2GearJointDef : public b2JointDef
/// The gear ratio.
/// @see b2GearJoint for explanation.
- float32 ratio;
+ qreal ratio;
};
/// A gear joint is used to connect two joints together. Either joint
@@ -51,75 +55,57 @@ struct b2GearJointDef : public b2JointDef
/// 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.
+/// @warning The revolute and prismatic joints must be attached to
+/// fixed bodies (which must be body1 on those joints).
class b2GearJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// Get the first joint.
- b2Joint* GetJoint1() { return m_joint1; }
-
- /// Get the second joint.
- b2Joint* GetJoint2() { return m_joint2; }
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// Set/Get the gear ratio.
- void SetRatio(float32 ratio);
- float32 GetRatio() const;
-
- /// Dump joint to dmLog
- void Dump();
+ void SetRatio(qreal ratio);
+ qreal GetRatio() const;
protected:
friend class b2Joint;
b2GearJoint(const b2GearJointDef* data);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
- b2Joint* m_joint1;
- b2Joint* m_joint2;
+ b2Body* m_ground1;
+ b2Body* m_ground2;
- b2JointType m_typeA;
- b2JointType m_typeB;
+ // One of these is NULL.
+ b2RevoluteJoint* m_revolute1;
+ b2PrismaticJoint* m_prismatic1;
- // Body A is connected to body C
- // Body B is connected to body D
- b2Body* m_bodyC;
- b2Body* m_bodyD;
+ // One of these is NULL.
+ b2RevoluteJoint* m_revolute2;
+ b2PrismaticJoint* m_prismatic2;
- // Solver shared
- b2Vec2 m_localAnchorA;
- b2Vec2 m_localAnchorB;
- b2Vec2 m_localAnchorC;
- b2Vec2 m_localAnchorD;
+ b2Vec2 m_groundAnchor1;
+ b2Vec2 m_groundAnchor2;
- b2Vec2 m_localAxisC;
- b2Vec2 m_localAxisD;
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
- float32 m_referenceAngleA;
- float32 m_referenceAngleB;
+ b2Jacobian m_J;
- float32 m_constant;
- float32 m_ratio;
+ qreal m_constant;
+ qreal m_ratio;
- float32 m_impulse;
+ // Effective mass
+ qreal m_mass;
- // 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;
+ // Impulse for accumulation/warm starting.
+ qreal m_impulse;
};
#endif
diff --git a/external/Box2D/Dynamics/Joints/b2Joint.cpp b/external/Box2D/Dynamics/Joints/b2Joint.cpp
index dd55be9..49a1763 100644
--- a/external/Box2D/Dynamics/Joints/b2Joint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2Joint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -18,7 +18,7 @@
#include <Box2D/Dynamics/Joints/b2Joint.h>
#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
-#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
+#include <Box2D/Dynamics/Joints/b2LineJoint.h>
#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
@@ -27,7 +27,6 @@
#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
-#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Common/b2BlockAllocator.h>
@@ -43,77 +42,70 @@ b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
case e_distanceJoint:
{
void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
- joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
+ joint = new (mem) b2DistanceJoint((b2DistanceJointDef*)def);
}
break;
case e_mouseJoint:
{
void* mem = allocator->Allocate(sizeof(b2MouseJoint));
- joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
+ joint = new (mem) b2MouseJoint((b2MouseJointDef*)def);
}
break;
case e_prismaticJoint:
{
void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
- joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
+ joint = new (mem) b2PrismaticJoint((b2PrismaticJointDef*)def);
}
break;
case e_revoluteJoint:
{
void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
- joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
+ joint = new (mem) b2RevoluteJoint((b2RevoluteJointDef*)def);
}
break;
case e_pulleyJoint:
{
void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
- joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
+ joint = new (mem) b2PulleyJoint((b2PulleyJointDef*)def);
}
break;
case e_gearJoint:
{
void* mem = allocator->Allocate(sizeof(b2GearJoint));
- joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
+ joint = new (mem) b2GearJoint((b2GearJointDef*)def);
}
break;
- case e_wheelJoint:
+ case e_lineJoint:
{
- void* mem = allocator->Allocate(sizeof(b2WheelJoint));
- joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
+ void* mem = allocator->Allocate(sizeof(b2LineJoint));
+ joint = new (mem) b2LineJoint((b2LineJointDef*)def);
}
break;
case e_weldJoint:
{
void* mem = allocator->Allocate(sizeof(b2WeldJoint));
- joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
+ joint = new (mem) b2WeldJoint((b2WeldJointDef*)def);
}
break;
case e_frictionJoint:
{
void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
- joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
+ joint = new (mem) b2FrictionJoint((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));
+ joint = new (mem) b2RopeJoint((b2RopeJointDef*)def);
}
break;
@@ -154,8 +146,8 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
allocator->Free(joint, sizeof(b2GearJoint));
break;
- case e_wheelJoint:
- allocator->Free(joint, sizeof(b2WheelJoint));
+ case e_lineJoint:
+ allocator->Free(joint, sizeof(b2LineJoint));
break;
case e_weldJoint:
@@ -170,10 +162,6 @@ void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
allocator->Free(joint, sizeof(b2RopeJoint));
break;
- case e_motorJoint:
- allocator->Free(joint, sizeof(b2MotorJoint));
- break;
-
default:
b2Assert(false);
break;
@@ -189,7 +177,6 @@ b2Joint::b2Joint(const b2JointDef* def)
m_next = NULL;
m_bodyA = def->bodyA;
m_bodyB = def->bodyB;
- m_index = 0;
m_collideConnected = def->collideConnected;
m_islandFlag = false;
m_userData = def->userData;
diff --git a/external/Box2D/Dynamics/Joints/b2Joint.h b/external/Box2D/Dynamics/Joints/b2Joint.h
index dd076b6..22c3bf2 100644
--- a/external/Box2D/Dynamics/Joints/b2Joint.h
+++ b/external/Box2D/Dynamics/Joints/b2Joint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -23,7 +23,7 @@
class b2Body;
class b2Joint;
-struct b2SolverData;
+struct b2TimeStep;
class b2BlockAllocator;
enum b2JointType
@@ -35,11 +35,10 @@ enum b2JointType
e_pulleyJoint,
e_mouseJoint,
e_gearJoint,
- e_wheelJoint,
+ e_lineJoint,
e_weldJoint,
e_frictionJoint,
- e_ropeJoint,
- e_motorJoint
+ e_ropeJoint
};
enum b2LimitState
@@ -52,9 +51,14 @@ enum b2LimitState
struct b2Jacobian
{
- b2Vec2 linear;
- float32 angularA;
- float32 angularB;
+ b2Vec2 linearA;
+ qreal angularA;
+ b2Vec2 linearB;
+ qreal angularB;
+
+ void SetZero();
+ void Set(const b2Vec2& x1, qreal a1, const b2Vec2& x2, qreal a2);
+ qreal Compute(const b2Vec2& x1, qreal a1, const b2Vec2& x2, qreal a2);
};
/// A joint edge is used to connect bodies and joints together
@@ -119,11 +123,11 @@ public:
/// 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 force on body2 at the joint anchor in Newtons.
+ virtual b2Vec2 GetReactionForce(qreal inv_dt) const = 0;
- /// Get the reaction torque on bodyB in N*m.
- virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
+ /// Get the reaction torque on body2 in N*m.
+ virtual qreal GetReactionTorque(qreal inv_dt) const = 0;
/// Get the next joint the world joint list.
b2Joint* GetNext();
@@ -138,22 +142,10 @@ public:
/// 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);
@@ -161,11 +153,11 @@ protected:
b2Joint(const b2JointDef* def);
virtual ~b2Joint() {}
- virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
- virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
+ virtual void InitVelocityConstraints(const b2TimeStep& step) = 0;
+ virtual void SolveVelocityConstraints(const b2TimeStep& step) = 0;
// This returns true if the position errors are within tolerance.
- virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
+ virtual bool SolvePositionConstraints(qreal baumgarte) = 0;
b2JointType m_type;
b2Joint* m_prev;
@@ -175,14 +167,35 @@ protected:
b2Body* m_bodyA;
b2Body* m_bodyB;
- int32 m_index;
-
bool m_islandFlag;
bool m_collideConnected;
void* m_userData;
+
+ // Cache here per time step to reduce cache misses.
+ // TODO_ERIN will be wrong if the mass changes.
+ b2Vec2 m_localCenterA, m_localCenterB;
+ qreal m_invMassA, m_invIA;
+ qreal m_invMassB, m_invIB;
};
+inline void b2Jacobian::SetZero()
+{
+ linearA.SetZero(); angularA = 0.0f;
+ linearB.SetZero(); angularB = 0.0f;
+}
+
+inline void b2Jacobian::Set(const b2Vec2& x1, qreal a1, const b2Vec2& x2, qreal a2)
+{
+ linearA = x1; angularA = a1;
+ linearB = x2; angularB = a2;
+}
+
+inline qreal b2Jacobian::Compute(const b2Vec2& x1, qreal a1, const b2Vec2& x2, qreal a2)
+{
+ return b2Dot(linearA, x1) + angularA * a1 + b2Dot(linearB, x2) + angularB * a2;
+}
+
inline b2JointType b2Joint::GetType() const
{
return m_type;
@@ -218,9 +231,4 @@ inline void b2Joint::SetUserData(void* data)
m_userData = data;
}
-inline bool b2Joint::GetCollideConnected() const
-{
- return m_collideConnected;
-}
-
#endif
diff --git a/external/Box2D/Dynamics/Joints/b2LineJoint.cpp b/external/Box2D/Dynamics/Joints/b2LineJoint.cpp
new file mode 100644
index 0000000..b63d5e0
--- /dev/null
+++ b/external/Box2D/Dynamics/Joints/b2LineJoint.cpp
@@ -0,0 +1,591 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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/Joints/b2LineJoint.h>
+#include <Box2D/Dynamics/b2Body.h>
+#include <Box2D/Dynamics/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)]
+//
+// K = J * invM * JT
+//
+// J = [-a -s1 a s2]
+// 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
+// [-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(2) = max(f2(2), 0)
+// upper: f2(2) = min(f2(2), 0)
+//
+// Solve for correct f2(1)
+// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * f2(2) + K(1,1:2) * f1
+// = -Cdot(1) - K(1,2) * f2(2) + K(1,1) * f1(1) + K(1,2) * f1(2)
+// K(1,1) * f2(1) = -Cdot(1) - K(1,2) * (f2(2) - f1(2)) + K(1,1) * f1(1)
+// f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
+//
+// Now compute impulse to be applied:
+// df = f2 - f1
+
+void b2LineJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
+{
+ bodyA = b1;
+ bodyB = b2;
+ localAnchorA = bodyA->GetLocalPoint(anchor);
+ localAnchorB = bodyB->GetLocalPoint(anchor);
+ localAxisA = bodyA->GetLocalVector(axis);
+}
+
+b2LineJoint::b2LineJoint(const b2LineJointDef* def)
+: b2Joint(def)
+{
+ m_localAnchor1 = def->localAnchorA;
+ m_localAnchor2 = def->localAnchorB;
+ m_localXAxis1 = def->localAxisA;
+ m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+
+ m_impulse.SetZero();
+ m_motorMass = 0.0;
+ 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 b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ m_localCenterA = b1->GetLocalCenter();
+ m_localCenterB = b2->GetLocalCenter();
+
+ b2Transform xf1 = b1->GetTransform();
+ b2Transform xf2 = b2->GetTransform();
+
+ // Compute the effective masses.
+ b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
+ b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
+ b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+
+ m_invMassA = b1->m_invMass;
+ m_invIA = b1->m_invI;
+ m_invMassB = b2->m_invMass;
+ m_invIB = b2->m_invI;
+
+ // Compute motor Jacobian and effective mass.
+ {
+ m_axis = b2Mul(xf1.R, m_localXAxis1);
+ m_a1 = b2Cross(d + r1, m_axis);
+ m_a2 = b2Cross(r2, m_axis);
+
+ m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
+ if (m_motorMass > b2_epsilon)
+ {
+ m_motorMass = 1.0f / m_motorMass;
+ }
+ else
+ {
+ m_motorMass = 0.0f;
+ }
+ }
+
+ // Prismatic constraint.
+ {
+ m_perp = b2Mul(xf1.R, m_localYAxis1);
+
+ m_s1 = b2Cross(d + r1, m_perp);
+ m_s2 = b2Cross(r2, m_perp);
+
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
+
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+ qreal k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+ qreal k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+ m_K.col1.Set(k11, k12);
+ m_K.col2.Set(k12, k22);
+ }
+
+ // Compute motor and limit terms.
+ if (m_enableLimit)
+ {
+ qreal 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.y = 0.0f;
+ }
+ }
+ else if (jointTranslation >= m_upperTranslation)
+ {
+ if (m_limitState != e_atUpperLimit)
+ {
+ m_limitState = e_atUpperLimit;
+ m_impulse.y = 0.0f;
+ }
+ }
+ else
+ {
+ m_limitState = e_inactiveLimit;
+ m_impulse.y = 0.0f;
+ }
+ }
+ else
+ {
+ m_limitState = e_inactiveLimit;
+ }
+
+ if (m_enableMotor == false)
+ {
+ m_motorImpulse = 0.0f;
+ }
+
+ if (step.warmStarting)
+ {
+ // Account for variable time step.
+ m_impulse *= step.dtRatio;
+ m_motorImpulse *= step.dtRatio;
+
+ b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;
+ qreal L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
+ qreal L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;
+
+ b1->m_linearVelocity -= m_invMassA * P;
+ b1->m_angularVelocity -= m_invIA * L1;
+
+ b2->m_linearVelocity += m_invMassB * P;
+ b2->m_angularVelocity += m_invIB * L2;
+ }
+ else
+ {
+ m_impulse.SetZero();
+ m_motorImpulse = 0.0f;
+ }
+}
+
+void b2LineJoint::SolveVelocityConstraints(const b2TimeStep& step)
+{
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 v1 = b1->m_linearVelocity;
+ qreal w1 = b1->m_angularVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ qreal w2 = b2->m_angularVelocity;
+
+ // Solve linear motor constraint.
+ if (m_enableMotor && m_limitState != e_equalLimits)
+ {
+ qreal Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+ qreal impulse = m_motorMass * (m_motorSpeed - Cdot);
+ qreal oldImpulse = m_motorImpulse;
+ qreal maxImpulse = step.dt * m_maxMotorForce;
+ m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
+ impulse = m_motorImpulse - oldImpulse;
+
+ b2Vec2 P = impulse * m_axis;
+ qreal L1 = impulse * m_a1;
+ qreal L2 = impulse * m_a2;
+
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
+
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
+ }
+
+ qreal Cdot1 = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
+
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
+ {
+ // Solve prismatic and limit constraint in block form.
+ qreal Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+ b2Vec2 Cdot(Cdot1, Cdot2);
+
+ b2Vec2 f1 = m_impulse;
+ b2Vec2 df = m_K.Solve(-Cdot);
+ m_impulse += df;
+
+ if (m_limitState == e_atLowerLimit)
+ {
+ m_impulse.y = b2Max(m_impulse.y, 0.0f);
+ }
+ else if (m_limitState == e_atUpperLimit)
+ {
+ m_impulse.y = b2Min(m_impulse.y, 0.0f);
+ }
+
+ // f2(1) = invK(1,1) * (-Cdot(1) - K(1,2) * (f2(2) - f1(2))) + f1(1)
+ qreal b = -Cdot1 - (m_impulse.y - f1.y) * m_K.col2.x;
+ qreal f2r;
+ if (m_K.col1.x != 0.0f)
+ {
+ f2r = b / m_K.col1.x + f1.x;
+ }
+ else
+ {
+ f2r = f1.x;
+ }
+
+ m_impulse.x = f2r;
+
+ df = m_impulse - f1;
+
+ b2Vec2 P = df.x * m_perp + df.y * m_axis;
+ qreal L1 = df.x * m_s1 + df.y * m_a1;
+ qreal L2 = df.x * m_s2 + df.y * m_a2;
+
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
+
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
+ }
+ else
+ {
+ // Limit is inactive, just solve the prismatic constraint in block form.
+ qreal df;
+ if (m_K.col1.x != 0.0f)
+ {
+ df = - Cdot1 / m_K.col1.x;
+ }
+ else
+ {
+ df = 0.0f;
+ }
+ m_impulse.x += df;
+
+ b2Vec2 P = df * m_perp;
+ qreal L1 = df * m_s1;
+ qreal L2 = df * m_s2;
+
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
+
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
+ }
+
+ b1->m_linearVelocity = v1;
+ b1->m_angularVelocity = w1;
+ b2->m_linearVelocity = v2;
+ b2->m_angularVelocity = w2;
+}
+
+bool b2LineJoint::SolvePositionConstraints(qreal baumgarte)
+{
+ B2_NOT_USED(baumgarte);
+
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 c1 = b1->m_sweep.c;
+ qreal a1 = b1->m_sweep.a;
+
+ b2Vec2 c2 = b2->m_sweep.c;
+ qreal a2 = b2->m_sweep.a;
+
+ // Solve linear limit constraint.
+ qreal linearError = 0.0f, angularError = 0.0f;
+ bool active = false;
+ qreal C2 = 0.0f;
+
+ b2Mat22 R1(a1), R2(a2);
+
+ b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
+ b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
+ b2Vec2 d = c2 + r2 - c1 - r1;
+
+ if (m_enableLimit)
+ {
+ m_axis = b2Mul(R1, m_localXAxis1);
+
+ m_a1 = b2Cross(d + r1, m_axis);
+ m_a2 = b2Cross(r2, m_axis);
+
+ qreal translation = b2Dot(m_axis, d);
+ if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
+ {
+ // Prevent large angular corrections
+ C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
+ 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 = 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 = translation - m_upperTranslation;
+ active = true;
+ }
+ }
+
+ m_perp = b2Mul(R1, m_localYAxis1);
+
+ m_s1 = b2Cross(d + r1, m_perp);
+ m_s2 = b2Cross(r2, m_perp);
+
+ b2Vec2 impulse;
+ qreal C1;
+ C1 = b2Dot(m_perp, d);
+
+ linearError = b2Max(linearError, b2Abs(C1));
+ angularError = 0.0f;
+
+ if (active)
+ {
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
+
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+ qreal k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+ qreal k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
+
+ m_K.col1.Set(k11, k12);
+ m_K.col2.Set(k12, k22);
+
+ b2Vec2 C;
+ C.x = C1;
+ C.y = C2;
+
+ impulse = m_K.Solve(-C);
+ }
+ else
+ {
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
+
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+
+ qreal impulse1;
+ if (k11 != 0.0f)
+ {
+ impulse1 = - C1 / k11;
+ }
+ else
+ {
+ impulse1 = 0.0f;
+ }
+
+ impulse.x = impulse1;
+ impulse.y = 0.0f;
+ }
+
+ b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
+ qreal L1 = impulse.x * m_s1 + impulse.y * m_a1;
+ qreal L2 = impulse.x * m_s2 + impulse.y * m_a2;
+
+ c1 -= m_invMassA * P;
+ a1 -= m_invIA * L1;
+ c2 += m_invMassB * P;
+ a2 += m_invIB * L2;
+
+ // TODO_ERIN remove need for this.
+ b1->m_sweep.c = c1;
+ b1->m_sweep.a = a1;
+ b2->m_sweep.c = c2;
+ b2->m_sweep.a = a2;
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
+ return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
+}
+
+b2Vec2 b2LineJoint::GetAnchorA() const
+{
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
+}
+
+b2Vec2 b2LineJoint::GetAnchorB() const
+{
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
+}
+
+b2Vec2 b2LineJoint::GetReactionForce(qreal inv_dt) const
+{
+ return inv_dt * (m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis);
+}
+
+qreal b2LineJoint::GetReactionTorque(qreal inv_dt) const
+{
+ B2_NOT_USED(inv_dt);
+ return 0.0f;
+}
+
+qreal b2LineJoint::GetJointTranslation() const
+{
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+ b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ qreal translation = b2Dot(d, axis);
+ return translation;
+}
+
+qreal b2LineJoint::GetJointSpeed() const
+{
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
+
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ qreal w1 = b1->m_angularVelocity;
+ qreal w2 = b2->m_angularVelocity;
+
+ qreal speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
+ return speed;
+}
+
+bool b2LineJoint::IsLimitEnabled() const
+{
+ return m_enableLimit;
+}
+
+void b2LineJoint::EnableLimit(bool flag)
+{
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_enableLimit = flag;
+}
+
+qreal b2LineJoint::GetLowerLimit() const
+{
+ return m_lowerTranslation;
+}
+
+qreal b2LineJoint::GetUpperLimit() const
+{
+ return m_upperTranslation;
+}
+
+void b2LineJoint::SetLimits(qreal lower, qreal upper)
+{
+ b2Assert(lower <= upper);
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_lowerTranslation = lower;
+ m_upperTranslation = upper;
+}
+
+bool b2LineJoint::IsMotorEnabled() const
+{
+ return m_enableMotor;
+}
+
+void b2LineJoint::EnableMotor(bool flag)
+{
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_enableMotor = flag;
+}
+
+void b2LineJoint::SetMotorSpeed(qreal speed)
+{
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_motorSpeed = speed;
+}
+
+void b2LineJoint::SetMaxMotorForce(qreal force)
+{
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_maxMotorForce = force;
+}
+
+qreal b2LineJoint::GetMotorForce(qreal inv_dt) const
+{
+ return inv_dt * m_motorImpulse;
+}
+
+
+
+
+
diff --git a/external/Box2D/Dynamics/Joints/b2LineJoint.h b/external/Box2D/Dynamics/Joints/b2LineJoint.h
new file mode 100644
index 0000000..3425b81
--- /dev/null
+++ b/external/Box2D/Dynamics/Joints/b2LineJoint.h
@@ -0,0 +1,175 @@
+/*
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
+*
+* 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_LINE_JOINT_H
+#define B2_LINE_JOINT_H
+
+#include <Box2D/Dynamics/Joints/b2Joint.h>
+
+/// Line 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 b2LineJointDef : public b2JointDef
+{
+ b2LineJointDef()
+ {
+ type = e_lineJoint;
+ localAnchorA.SetZero();
+ localAnchorB.SetZero();
+ localAxisA.Set(1.0f, 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 world axis.
+ void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor, const b2Vec2& axis);
+
+ /// The local anchor point relative to body1's origin.
+ b2Vec2 localAnchorA;
+
+ /// The local anchor point relative to body2's origin.
+ b2Vec2 localAnchorB;
+
+ /// The local translation axis in body1.
+ b2Vec2 localAxisA;
+
+ /// Enable/disable the joint limit.
+ bool enableLimit;
+
+ /// The lower translation limit, usually in meters.
+ qreal lowerTranslation;
+
+ /// The upper translation limit, usually in meters.
+ qreal upperTranslation;
+
+ /// Enable/disable the joint motor.
+ bool enableMotor;
+
+ /// The maximum motor torque, usually in N-m.
+ qreal maxMotorForce;
+
+ /// The desired motor speed in radians per second.
+ qreal motorSpeed;
+};
+
+/// A line joint. This joint provides two degrees of freedom: translation
+/// along an axis fixed in body1 and rotation in the plane. 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 b2LineJoint : public b2Joint
+{
+public:
+ b2Vec2 GetAnchorA() const;
+ b2Vec2 GetAnchorB() const;
+
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
+
+ /// Get the current joint translation, usually in meters.
+ qreal GetJointTranslation() const;
+
+ /// Get the current joint translation speed, usually in meters per second.
+ qreal 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.
+ qreal GetLowerLimit() const;
+
+ /// Get the upper joint limit, usually in meters.
+ qreal GetUpperLimit() const;
+
+ /// Set the joint limits, usually in meters.
+ void SetLimits(qreal lower, qreal 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(qreal speed);
+
+ /// Get the motor speed, usually in meters per second.
+ qreal GetMotorSpeed() const;
+
+ /// Set/Get the maximum motor force, usually in N.
+ void SetMaxMotorForce(qreal force);
+ qreal GetMaxMotorForce() const;
+
+ /// Get the current motor force given the inverse time step, usually in N.
+ qreal GetMotorForce(qreal inv_dt) const;
+
+protected:
+
+ friend class b2Joint;
+ b2LineJoint(const b2LineJointDef* def);
+
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
+
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+ b2Vec2 m_localXAxis1;
+ b2Vec2 m_localYAxis1;
+
+ b2Vec2 m_axis, m_perp;
+ qreal m_s1, m_s2;
+ qreal m_a1, m_a2;
+
+ b2Mat22 m_K;
+ b2Vec2 m_impulse;
+
+ qreal m_motorMass; // effective mass for motor/limit translational constraint.
+ qreal m_motorImpulse;
+
+ qreal m_lowerTranslation;
+ qreal m_upperTranslation;
+ qreal m_maxMotorForce;
+ qreal m_motorSpeed;
+
+ bool m_enableLimit;
+ bool m_enableMotor;
+ b2LimitState m_limitState;
+};
+
+inline qreal b2LineJoint::GetMotorSpeed() const
+{
+ return m_motorSpeed;
+}
+
+inline qreal b2LineJoint::GetMaxMotorForce() const
+{
+ return m_maxMotorForce;
+}
+
+#endif
diff --git a/external/Box2D/Dynamics/Joints/b2MotorJoint.cpp b/external/Box2D/Dynamics/Joints/b2MotorJoint.cpp
deleted file mode 100644
index e5bbcb1..0000000
--- a/external/Box2D/Dynamics/Joints/b2MotorJoint.cpp
+++ /dev/null
@@ -1,304 +0,0 @@
-/*
-* 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 <Box2D/Dynamics/Joints/b2MotorJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/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 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_localCenterA);
- m_rB = b2Mul(qB, -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;
- }
-
- m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
- 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2MotorJoint.h b/external/Box2D/Dynamics/Joints/b2MotorJoint.h
deleted file mode 100644
index 0b76d6f..0000000
--- a/external/Box2D/Dynamics/Joints/b2MotorJoint.h
+++ /dev/null
@@ -1,133 +0,0 @@
-/*
-* 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 <Box2D/Dynamics/Joints/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;
- b2Vec2 GetAnchorB() const;
-
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// 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();
-
-protected:
-
- friend class b2Joint;
-
- b2MotorJoint(const b2MotorJointDef* def);
-
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
-
- // 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
diff --git a/external/Box2D/Dynamics/Joints/b2MouseJoint.cpp b/external/Box2D/Dynamics/Joints/b2MouseJoint.cpp
index ea96edd..52ddebd 100644
--- a/external/Box2D/Dynamics/Joints/b2MouseJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2MouseJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -36,8 +36,8 @@ b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
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_target = def->target;
+ m_localAnchor = b2MulT(m_bodyB->GetTransform(), m_target);
m_maxForce = def->maxForce;
m_impulse.SetZero();
@@ -55,168 +55,143 @@ void b2MouseJoint::SetTarget(const b2Vec2& target)
{
m_bodyB->SetAwake(true);
}
- m_targetA = target;
+ m_target = target;
}
const b2Vec2& b2MouseJoint::GetTarget() const
{
- return m_targetA;
+ return m_target;
}
-void b2MouseJoint::SetMaxForce(float32 force)
+void b2MouseJoint::SetMaxForce(qreal force)
{
m_maxForce = force;
}
-float32 b2MouseJoint::GetMaxForce() const
+qreal b2MouseJoint::GetMaxForce() const
{
return m_maxForce;
}
-void b2MouseJoint::SetFrequency(float32 hz)
+void b2MouseJoint::SetFrequency(qreal hz)
{
m_frequencyHz = hz;
}
-float32 b2MouseJoint::GetFrequency() const
+qreal b2MouseJoint::GetFrequency() const
{
return m_frequencyHz;
}
-void b2MouseJoint::SetDampingRatio(float32 ratio)
+void b2MouseJoint::SetDampingRatio(qreal ratio)
{
m_dampingRatio = ratio;
}
-float32 b2MouseJoint::GetDampingRatio() const
+qreal b2MouseJoint::GetDampingRatio() const
{
return m_dampingRatio;
}
-void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2MouseJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* b = m_bodyB;
- 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();
+ qreal mass = b->GetMass();
// Frequency
- float32 omega = 2.0f * b2_pi * m_frequencyHz;
+ qreal omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
- float32 d = 2.0f * mass * m_dampingRatio * omega;
+ qreal d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
- float32 k = mass * (omega * omega);
+ qreal 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);
+ b2Assert(d + step.dt * k > b2_epsilon);
+ m_gamma = step.dt * (d + step.dt * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
- m_beta = h * k * m_gamma;
+ m_beta = step.dt * k * m_gamma;
// Compute the effective mass matrix.
- m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
// 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;
+ qreal invMass = b->m_invMass;
+ qreal invI = b->m_invI;
+
+ b2Mat22 K1;
+ K1.col1.x = invMass; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = invMass;
+
+ b2Mat22 K2;
+ K2.col1.x = invI * r.y * r.y; K2.col2.x = -invI * r.x * r.y;
+ K2.col1.y = -invI * r.x * r.y; K2.col2.y = invI * r.x * r.x;
+
+ b2Mat22 K = K1 + K2;
+ K.col1.x += m_gamma;
+ K.col2.y += m_gamma;
m_mass = K.GetInverse();
- m_C = cB + m_rB - m_targetA;
- m_C *= m_beta;
+ m_C = b->m_sweep.c + r - m_target;
// 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();
- }
+ b->m_angularVelocity *= 0.98f;
- data.velocities[m_indexB].v = vB;
- data.velocities[m_indexB].w = wB;
+ // Warm starting.
+ m_impulse *= step.dtRatio;
+ b->m_linearVelocity += invMass * m_impulse;
+ b->m_angularVelocity += invI * b2Cross(r, m_impulse);
}
-void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
+void b2MouseJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- b2Vec2 vB = data.velocities[m_indexB].v;
- float32 wB = data.velocities[m_indexB].w;
+ b2Body* b = m_bodyB;
+
+ b2Vec2 r = b2Mul(b->GetTransform().R, m_localAnchor - b->GetLocalCenter());
// 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 Cdot = b->m_linearVelocity + b2Cross(b->m_angularVelocity, r);
+ b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_beta * m_C + m_gamma * m_impulse));
b2Vec2 oldImpulse = m_impulse;
m_impulse += impulse;
- float32 maxImpulse = data.step.dt * m_maxForce;
+ qreal maxImpulse = 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;
+ b->m_linearVelocity += b->m_invMass * impulse;
+ b->m_angularVelocity += b->m_invI * b2Cross(r, impulse);
}
b2Vec2 b2MouseJoint::GetAnchorA() const
{
- return m_targetA;
+ return m_target;
}
b2Vec2 b2MouseJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor);
}
-b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2MouseJoint::GetReactionForce(qreal inv_dt) const
{
return inv_dt * m_impulse;
}
-float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2MouseJoint::GetReactionTorque(qreal inv_dt) const
{
return inv_dt * 0.0f;
}
-
-void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
-{
- m_targetA -= newOrigin;
-}
diff --git a/external/Box2D/Dynamics/Joints/b2MouseJoint.h b/external/Box2D/Dynamics/Joints/b2MouseJoint.h
index 8b50a0e..2626b87 100644
--- a/external/Box2D/Dynamics/Joints/b2MouseJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2MouseJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -41,13 +41,13 @@ struct b2MouseJointDef : public b2JointDef
/// 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;
+ qreal maxForce;
/// The response speed.
- float32 frequencyHz;
+ qreal frequencyHz;
/// The damping ratio. 0 = no damping, 1 = critical damping.
- float32 dampingRatio;
+ qreal dampingRatio;
};
/// A mouse joint is used to make a point on a body track a
@@ -68,62 +68,47 @@ public:
b2Vec2 GetAnchorB() const;
/// Implements b2Joint.
- b2Vec2 GetReactionForce(float32 inv_dt) const;
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
/// Implements b2Joint.
- float32 GetReactionTorque(float32 inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// 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;
+ void SetMaxForce(qreal force);
+ qreal GetMaxForce() const;
/// Set/get the frequency in Hertz.
- void SetFrequency(float32 hz);
- float32 GetFrequency() const;
+ void SetFrequency(qreal hz);
+ qreal GetFrequency() const;
/// Set/get the damping ratio (dimensionless).
- void SetDampingRatio(float32 ratio);
- float32 GetDampingRatio() const;
-
- /// The mouse joint does not support dumping.
- void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }
-
- /// Implement b2Joint::ShiftOrigin
- void ShiftOrigin(const b2Vec2& newOrigin);
+ void SetDampingRatio(qreal ratio);
+ qreal GetDampingRatio() const;
protected:
friend class b2Joint;
b2MouseJoint(const b2MouseJointDef* def);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
-
- b2Vec2 m_localAnchorB;
- b2Vec2 m_targetA;
- float32 m_frequencyHz;
- float32 m_dampingRatio;
- float32 m_beta;
-
- // Solver shared
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte) { B2_NOT_USED(baumgarte); return true; }
+
+ b2Vec2 m_localAnchor;
+ b2Vec2 m_target;
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;
+
+ b2Mat22 m_mass; // effective mass for point-to-point constraint.
+ b2Vec2 m_C; // position error
+ qreal m_maxForce;
+ qreal m_frequencyHz;
+ qreal m_dampingRatio;
+ qreal m_beta;
+ qreal m_gamma;
};
#endif
diff --git a/external/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp b/external/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp
index 217e145..4eaffca 100644
--- a/external/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2PrismaticJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -87,28 +87,27 @@
// Now compute impulse to be applied:
// df = f2 - f1
-void b2PrismaticJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor, const b2Vec2& axis)
+void b2PrismaticJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor, const b2Vec2& axis)
{
- bodyA = bA;
- bodyB = bB;
+ bodyA = b1;
+ bodyB = b2;
localAnchorA = bodyA->GetLocalPoint(anchor);
localAnchorB = bodyB->GetLocalPoint(anchor);
- localAxisA = bodyA->GetLocalVector(axis);
+ localAxis1 = 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_localAnchor1 = def->localAnchorA;
+ m_localAnchor2 = def->localAnchorB;
+ m_localXAxis1 = def->localAxis1;
+ m_localYAxis1 = b2Cross(1.0f, m_localXAxis1);
+ m_refAngle = def->referenceAngle;
m_impulse.SetZero();
- m_motorMass = 0.0f;
+ m_motorMass = 0.0;
m_motorImpulse = 0.0f;
m_lowerTranslation = def->lowerTranslation;
@@ -123,45 +122,35 @@ b2PrismaticJoint::b2PrismaticJoint(const b2PrismaticJointDef* def)
m_perp.SetZero();
}
-void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2PrismaticJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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);
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ m_localCenterA = b1->GetLocalCenter();
+ m_localCenterB = b2->GetLocalCenter();
+
+ b2Transform xf1 = b1->GetTransform();
+ b2Transform xf2 = b2->GetTransform();
// 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;
+ b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
+ b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
+ b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ m_invMassA = b1->m_invMass;
+ m_invIA = b1->m_invI;
+ m_invMassB = b2->m_invMass;
+ m_invIB = b2->m_invI;
// 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_axis = b2Mul(xf1.R, m_localXAxis1);
+ m_a1 = b2Cross(d + r1, m_axis);
+ m_a2 = b2Cross(r2, m_axis);
- m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
- if (m_motorMass > 0.0f)
+ m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
+ if (m_motorMass > b2_epsilon)
{
m_motorMass = 1.0f / m_motorMass;
}
@@ -169,35 +158,34 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
// Prismatic constraint.
{
- m_perp = b2Mul(qA, m_localYAxisA);
+ m_perp = b2Mul(xf1.R, m_localYAxis1);
- m_s1 = b2Cross(d + rA, m_perp);
- m_s2 = b2Cross(rB, m_perp);
+ m_s1 = b2Cross(d + r1, m_perp);
+ m_s2 = b2Cross(r2, m_perp);
- float32 s1test;
- s1test = b2Cross(rA, m_perp);
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
- 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;
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+ qreal k12 = i1 * m_s1 + i2 * m_s2;
+ qreal k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+ qreal k22 = i1 + i2;
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;
+ qreal k23 = i1 * m_a1 + i2 * m_a2;
+ qreal k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * 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);
+ m_K.col1.Set(k11, k12, k13);
+ m_K.col2.Set(k12, k22, k23);
+ m_K.col3.Set(k13, k23, k33);
}
// Compute motor and limit terms.
if (m_enableLimit)
{
- float32 jointTranslation = b2Dot(m_axis, d);
+ qreal jointTranslation = b2Dot(m_axis, d);
if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
{
m_limitState = e_equalLimits;
@@ -235,74 +223,69 @@ void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
m_motorImpulse = 0.0f;
}
- if (data.step.warmStarting)
+ if (step.warmStarting)
{
// Account for variable time step.
- m_impulse *= data.step.dtRatio;
- m_motorImpulse *= data.step.dtRatio;
+ m_impulse *= step.dtRatio;
+ m_motorImpulse *= 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;
+ qreal L1 = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
+ qreal L2 = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;
- vA -= mA * P;
- wA -= iA * LA;
+ b1->m_linearVelocity -= m_invMassA * P;
+ b1->m_angularVelocity -= m_invIA * L1;
- vB += mB * P;
- wB += iB * LB;
+ b2->m_linearVelocity += m_invMassB * P;
+ b2->m_angularVelocity += m_invIB * L2;
}
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)
+void b2PrismaticJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ b2Vec2 v1 = b1->m_linearVelocity;
+ qreal w1 = b1->m_angularVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ qreal w2 = b2->m_angularVelocity;
// 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;
+ qreal Cdot = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
+ qreal impulse = m_motorMass * (m_motorSpeed - Cdot);
+ qreal oldImpulse = m_motorImpulse;
+ qreal maxImpulse = 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;
+ qreal L1 = impulse * m_a1;
+ qreal L2 = impulse * m_a2;
- vA -= mA * P;
- wA -= iA * LA;
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
- vB += mB * P;
- wB += iB * LB;
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
}
b2Vec2 Cdot1;
- Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
- Cdot1.y = wB - wA;
+ Cdot1.x = b2Dot(m_perp, v2 - v1) + m_s2 * w2 - m_s1 * w1;
+ Cdot1.y = w2 - w1;
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;
+ qreal Cdot2;
+ Cdot2 = b2Dot(m_axis, v2 - v1) + m_a2 * w2 - m_a1 * w1;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
b2Vec3 f1 = m_impulse;
@@ -319,7 +302,7 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
}
// 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 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.col3.x, m_K.col3.y);
b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
m_impulse.x = f2r.x;
m_impulse.y = f2r.y;
@@ -327,14 +310,14 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
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;
+ qreal L1 = df.x * m_s1 + df.y + df.z * m_a1;
+ qreal L2 = df.x * m_s2 + df.y + df.z * m_a2;
- vA -= mA * P;
- wA -= iA * LA;
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
- vB += mB * P;
- wB += iB * LB;
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
}
else
{
@@ -344,195 +327,211 @@ void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
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;
+ qreal L1 = df.x * m_s1 + df.y;
+ qreal L2 = df.x * m_s2 + df.y;
- vA -= mA * P;
- wA -= iA * LA;
+ v1 -= m_invMassA * P;
+ w1 -= m_invIA * L1;
- vB += mB * P;
- wB += iB * LB;
+ v2 += m_invMassB * P;
+ w2 += m_invIB * L2;
}
- data.velocities[m_indexA].v = vA;
- data.velocities[m_indexA].w = wA;
- data.velocities[m_indexB].v = vB;
- data.velocities[m_indexB].w = wB;
+ b1->m_linearVelocity = v1;
+ b1->m_angularVelocity = w1;
+ b2->m_linearVelocity = v2;
+ b2->m_angularVelocity = w2;
}
-bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2PrismaticJoint::SolvePositionConstraints(qreal baumgarte)
{
- 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;
+ B2_NOT_USED(baumgarte);
- b2Rot qA(aA), qB(aB);
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ b2Vec2 c1 = b1->m_sweep.c;
+ qreal a1 = b1->m_sweep.a;
- // 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 c2 = b2->m_sweep.c;
+ qreal a2 = b2->m_sweep.a;
- b2Vec2 axis = b2Mul(qA, m_localXAxisA);
- float32 a1 = b2Cross(d + rA, axis);
- float32 a2 = b2Cross(rB, axis);
- b2Vec2 perp = b2Mul(qA, m_localYAxisA);
+ // Solve linear limit constraint.
+ qreal linearError = 0.0f, angularError = 0.0f;
+ bool active = false;
+ qreal C2 = 0.0f;
- float32 s1 = b2Cross(d + rA, perp);
- float32 s2 = b2Cross(rB, perp);
+ b2Mat22 R1(a1), R2(a2);
- 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);
+ b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
+ b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
+ b2Vec2 d = c2 + r2 - c1 - r1;
- bool active = false;
- float32 C2 = 0.0f;
if (m_enableLimit)
{
- float32 translation = b2Dot(axis, d);
+ m_axis = b2Mul(R1, m_localXAxis1);
+
+ m_a1 = b2Cross(d + r1, m_axis);
+ m_a2 = b2Cross(r2, m_axis);
+
+ qreal translation = b2Dot(m_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));
+ 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);
+ 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);
+ linearError = translation - m_upperTranslation;
active = true;
}
}
+ m_perp = b2Mul(R1, m_localYAxis1);
+
+ m_s1 = b2Cross(d + r1, m_perp);
+ m_s2 = b2Cross(r2, m_perp);
+
+ b2Vec3 impulse;
+ b2Vec2 C1;
+ C1.x = b2Dot(m_perp, d);
+ C1.y = a2 - a1 - m_refAngle;
+
+ linearError = b2Max(linearError, b2Abs(C1.x));
+ angularError = b2Abs(C1.y);
+
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;
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
+
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+ qreal k12 = i1 * m_s1 + i2 * m_s2;
+ qreal k13 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
+ qreal k22 = i1 + i2;
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;
+ qreal k23 = i1 * m_a1 + i2 * m_a2;
+ qreal k33 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;
- b2Mat33 K;
- K.ex.Set(k11, k12, k13);
- K.ey.Set(k12, k22, k23);
- K.ez.Set(k13, k23, k33);
+ m_K.col1.Set(k11, k12, k13);
+ m_K.col2.Set(k12, k22, k23);
+ m_K.col3.Set(k13, k23, k33);
b2Vec3 C;
C.x = C1.x;
C.y = C1.y;
C.z = C2;
- impulse = K.Solve33(-C);
+ impulse = m_K.Solve33(-C);
}
else
{
- float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
- float32 k12 = iA * s1 + iB * s2;
- float32 k22 = iA + iB;
+ qreal m1 = m_invMassA, m2 = m_invMassB;
+ qreal i1 = m_invIA, i2 = m_invIB;
+
+ qreal k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
+ qreal k12 = i1 * m_s1 + i2 * m_s2;
+ qreal k22 = i1 + i2;
if (k22 == 0.0f)
{
k22 = 1.0f;
}
- b2Mat22 K;
- K.ex.Set(k11, k12);
- K.ey.Set(k12, k22);
+ m_K.col1.Set(k11, k12, 0.0f);
+ m_K.col2.Set(k12, k22, 0.0f);
- b2Vec2 impulse1 = K.Solve(-C1);
+ b2Vec2 impulse1 = m_K.Solve22(-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;
-
+ b2Vec2 P = impulse.x * m_perp + impulse.z * m_axis;
+ qreal L1 = impulse.x * m_s1 + impulse.y + impulse.z * m_a1;
+ qreal L2 = impulse.x * m_s2 + impulse.y + impulse.z * m_a2;
+
+ c1 -= m_invMassA * P;
+ a1 -= m_invIA * L1;
+ c2 += m_invMassB * P;
+ a2 += m_invIB * L2;
+
+ // TODO_ERIN remove need for this.
+ b1->m_sweep.c = c1;
+ b1->m_sweep.a = a1;
+ b2->m_sweep.c = c2;
+ b2->m_sweep.a = a2;
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+
return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
b2Vec2 b2PrismaticJoint::GetAnchorA() const
{
- return m_bodyA->GetWorldPoint(m_localAnchorA);
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
}
b2Vec2 b2PrismaticJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
}
-b2Vec2 b2PrismaticJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2PrismaticJoint::GetReactionForce(qreal 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
+qreal b2PrismaticJoint::GetReactionTorque(qreal inv_dt) const
{
return inv_dt * m_impulse.y;
}
-float32 b2PrismaticJoint::GetJointTranslation() const
+qreal 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);
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 p1 = b1->GetWorldPoint(m_localAnchor1);
+ b2Vec2 p2 = b2->GetWorldPoint(m_localAnchor2);
+ b2Vec2 d = p2 - p1;
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
- float32 translation = b2Dot(d, axis);
+ qreal translation = b2Dot(d, axis);
return translation;
}
-float32 b2PrismaticJoint::GetJointSpeed() const
+qreal b2PrismaticJoint::GetJointSpeed() const
{
- b2Body* bA = m_bodyA;
- b2Body* bB = m_bodyB;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = 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 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
b2Vec2 d = p2 - p1;
- b2Vec2 axis = b2Mul(bA->m_xf.q, m_localXAxisA);
+ b2Vec2 axis = b1->GetWorldVector(m_localXAxis1);
- b2Vec2 vA = bA->m_linearVelocity;
- b2Vec2 vB = bB->m_linearVelocity;
- float32 wA = bA->m_angularVelocity;
- float32 wB = bB->m_angularVelocity;
+ b2Vec2 v1 = b1->m_linearVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ qreal w1 = b1->m_angularVelocity;
+ qreal w2 = b2->m_angularVelocity;
- float32 speed = b2Dot(d, b2Cross(wA, axis)) + b2Dot(axis, vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA));
+ qreal speed = b2Dot(d, b2Cross(w1, axis)) + b2Dot(axis, v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1));
return speed;
}
@@ -543,36 +542,28 @@ bool b2PrismaticJoint::IsLimitEnabled() const
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;
- }
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_enableLimit = flag;
}
-float32 b2PrismaticJoint::GetLowerLimit() const
+qreal b2PrismaticJoint::GetLowerLimit() const
{
return m_lowerTranslation;
}
-float32 b2PrismaticJoint::GetUpperLimit() const
+qreal b2PrismaticJoint::GetUpperLimit() const
{
return m_upperTranslation;
}
-void b2PrismaticJoint::SetLimits(float32 lower, float32 upper)
+void b2PrismaticJoint::SetLimits(qreal lower, qreal 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;
- }
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_lowerTranslation = lower;
+ m_upperTranslation = upper;
}
bool b2PrismaticJoint::IsMotorEnabled() const
@@ -587,43 +578,21 @@ void b2PrismaticJoint::EnableMotor(bool flag)
m_enableMotor = flag;
}
-void b2PrismaticJoint::SetMotorSpeed(float32 speed)
+void b2PrismaticJoint::SetMotorSpeed(qreal speed)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
m_motorSpeed = speed;
}
-void b2PrismaticJoint::SetMaxMotorForce(float32 force)
+void b2PrismaticJoint::SetMaxMotorForce(qreal force)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
m_maxMotorForce = force;
}
-float32 b2PrismaticJoint::GetMotorForce(float32 inv_dt) const
+qreal b2PrismaticJoint::GetMotorForce(qreal 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2PrismaticJoint.h b/external/Box2D/Dynamics/Joints/b2PrismaticJoint.h
index 6ef5d3e..20d5d0b 100644
--- a/external/Box2D/Dynamics/Joints/b2PrismaticJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2PrismaticJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -27,6 +27,7 @@
/// 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.
+/// @warning at least one body should by dynamic with a non-fixed rotation.
struct b2PrismaticJointDef : public b2JointDef
{
b2PrismaticJointDef()
@@ -34,7 +35,7 @@ struct b2PrismaticJointDef : public b2JointDef
type = e_prismaticJoint;
localAnchorA.SetZero();
localAnchorB.SetZero();
- localAxisA.Set(1.0f, 0.0f);
+ localAxis1.Set(1.0f, 0.0f);
referenceAngle = 0.0f;
enableLimit = false;
lowerTranslation = 0.0f;
@@ -45,42 +46,42 @@ struct b2PrismaticJointDef : public b2JointDef
}
/// Initialize the bodies, anchors, axis, and reference angle using the world
- /// anchor and unit world axis.
+ /// 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.
+ /// The local anchor point relative to body1's origin.
b2Vec2 localAnchorA;
- /// The local anchor point relative to bodyB's origin.
+ /// The local anchor point relative to body2's origin.
b2Vec2 localAnchorB;
- /// The local translation unit axis in bodyA.
- b2Vec2 localAxisA;
+ /// The local translation axis in body1.
+ b2Vec2 localAxis1;
- /// The constrained angle between the bodies: bodyB_angle - bodyA_angle.
- float32 referenceAngle;
+ /// The constrained angle between the bodies: body2_angle - body1_angle.
+ qreal referenceAngle;
/// Enable/disable the joint limit.
bool enableLimit;
/// The lower translation limit, usually in meters.
- float32 lowerTranslation;
+ qreal lowerTranslation;
/// The upper translation limit, usually in meters.
- float32 upperTranslation;
+ qreal upperTranslation;
/// Enable/disable the joint motor.
bool enableMotor;
/// The maximum motor torque, usually in N-m.
- float32 maxMotorForce;
+ qreal maxMotorForce;
/// The desired motor speed in radians per second.
- float32 motorSpeed;
+ qreal motorSpeed;
};
/// A prismatic joint. This joint provides one degree of freedom: translation
-/// along an axis fixed in bodyA. Relative rotation is prevented. You can
+/// along an axis fixed in body1. 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
@@ -89,26 +90,14 @@ public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// 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; }
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// Get the current joint translation, usually in meters.
- float32 GetJointTranslation() const;
+ qreal GetJointTranslation() const;
/// Get the current joint translation speed, usually in meters per second.
- float32 GetJointSpeed() const;
+ qreal GetJointSpeed() const;
/// Is the joint limit enabled?
bool IsLimitEnabled() const;
@@ -117,13 +106,13 @@ public:
void EnableLimit(bool flag);
/// Get the lower joint limit, usually in meters.
- float32 GetLowerLimit() const;
+ qreal GetLowerLimit() const;
/// Get the upper joint limit, usually in meters.
- float32 GetUpperLimit() const;
+ qreal GetUpperLimit() const;
/// Set the joint limits, usually in meters.
- void SetLimits(float32 lower, float32 upper);
+ void SetLimits(qreal lower, qreal upper);
/// Is the joint motor enabled?
bool IsMotorEnabled() const;
@@ -132,63 +121,53 @@ public:
void EnableMotor(bool flag);
/// Set the motor speed, usually in meters per second.
- void SetMotorSpeed(float32 speed);
+ void SetMotorSpeed(qreal speed);
/// Get the motor speed, usually in meters per second.
- float32 GetMotorSpeed() const;
+ qreal GetMotorSpeed() const;
/// Set the maximum motor force, usually in N.
- void SetMaxMotorForce(float32 force);
- float32 GetMaxMotorForce() const { return m_maxMotorForce; }
+ void SetMaxMotorForce(qreal force);
/// Get the current motor force given the inverse time step, usually in N.
- float32 GetMotorForce(float32 inv_dt) const;
-
- /// Dump to b2Log
- void Dump();
+ qreal GetMotorForce(qreal inv_dt) const;
protected:
friend class b2Joint;
friend class b2GearJoint;
b2PrismaticJoint(const b2PrismaticJointDef* def);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
+
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+ b2Vec2 m_localXAxis1;
+ b2Vec2 m_localYAxis1;
+ qreal m_refAngle;
+
+ b2Vec2 m_axis, m_perp;
+ qreal m_s1, m_s2;
+ qreal m_a1, m_a2;
- // Solver shared
- b2Vec2 m_localAnchorA;
- b2Vec2 m_localAnchorB;
- b2Vec2 m_localXAxisA;
- b2Vec2 m_localYAxisA;
- float32 m_referenceAngle;
+ b2Mat33 m_K;
b2Vec3 m_impulse;
- float32 m_motorImpulse;
- float32 m_lowerTranslation;
- float32 m_upperTranslation;
- float32 m_maxMotorForce;
- float32 m_motorSpeed;
+
+ qreal m_motorMass; // effective mass for motor/limit translational constraint.
+ qreal m_motorImpulse;
+
+ qreal m_lowerTranslation;
+ qreal m_upperTranslation;
+ qreal m_maxMotorForce;
+ qreal 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
+inline qreal b2PrismaticJoint::GetMotorSpeed() const
{
return m_motorSpeed;
}
diff --git a/external/Box2D/Dynamics/Joints/b2PulleyJoint.cpp b/external/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
index 9cd340f..4e2386b 100644
--- a/external/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2PulleyJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,258 +24,372 @@
// length1 = norm(p1 - s1)
// length2 = norm(p2 - s2)
// C0 = (length1 + ratio * length2)_initial
-// C = C0 - (length1 + ratio * length2)
+// C = C0 - (length1 + ratio * length2) >= 0
// 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)
+//
+// Limit:
+// C = maxLength - length
+// u = (p - s) / norm(p - s)
+// Cdot = -dot(u, v + cross(w, r))
+// K = invMass + invI * cross(r, u)^2
+// 0 <= impulse
+
+void b2PulleyJointDef::Initialize(b2Body* b1, b2Body* b2,
+ const b2Vec2& ga1, const b2Vec2& ga2,
+ const b2Vec2& anchor1, const b2Vec2& anchor2,
+ qreal 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();
+ bodyA = b1;
+ bodyB = b2;
+ groundAnchorA = ga1;
+ groundAnchorB = ga2;
+ localAnchorA = bodyA->GetLocalPoint(anchor1);
+ localAnchorB = bodyB->GetLocalPoint(anchor2);
+ b2Vec2 d1 = anchor1 - ga1;
+ lengthA = d1.Length();
+ b2Vec2 d2 = anchor2 - ga2;
+ lengthB = d2.Length();
ratio = r;
b2Assert(ratio > b2_epsilon);
+ qreal C = lengthA + ratio * lengthB;
+ maxLengthA = C - ratio * b2_minPulleyLength;
+ maxLengthB = (C - b2_minPulleyLength) / ratio;
}
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;
+ m_groundAnchor1 = def->groundAnchorA;
+ m_groundAnchor2 = def->groundAnchorB;
+ m_localAnchor1 = def->localAnchorA;
+ m_localAnchor2 = def->localAnchorB;
b2Assert(def->ratio != 0.0f);
m_ratio = def->ratio;
m_constant = def->lengthA + m_ratio * def->lengthB;
+ m_maxLength1 = b2Min(def->maxLengthA, m_constant - m_ratio * b2_minPulleyLength);
+ m_maxLength2 = b2Min(def->maxLengthB, (m_constant - b2_minPulleyLength) / m_ratio);
+
m_impulse = 0.0f;
+ m_limitImpulse1 = 0.0f;
+ m_limitImpulse2 = 0.0f;
}
-void b2PulleyJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2PulleyJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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);
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ b2Vec2 s1 = m_groundAnchor1;
+ b2Vec2 s2 = m_groundAnchor2;
// Get the pulley axes.
- m_uA = cA + m_rA - m_groundAnchorA;
- m_uB = cB + m_rB - m_groundAnchorB;
+ m_u1 = p1 - s1;
+ m_u2 = p2 - s2;
- float32 lengthA = m_uA.Length();
- float32 lengthB = m_uB.Length();
+ qreal length1 = m_u1.Length();
+ qreal length2 = m_u2.Length();
- if (lengthA > 10.0f * b2_linearSlop)
+ if (length1 > b2_linearSlop)
{
- m_uA *= 1.0f / lengthA;
+ m_u1 *= 1.0f / length1;
}
else
{
- m_uA.SetZero();
+ m_u1.SetZero();
}
- if (lengthB > 10.0f * b2_linearSlop)
+ if (length2 > b2_linearSlop)
{
- m_uB *= 1.0f / lengthB;
+ m_u2 *= 1.0f / length2;
}
else
{
- m_uB.SetZero();
+ m_u2.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;
+ qreal C = m_constant - length1 - m_ratio * length2;
+ if (C > 0.0f)
+ {
+ m_state = e_inactiveLimit;
+ m_impulse = 0.0f;
+ }
+ else
+ {
+ m_state = e_atUpperLimit;
+ }
- m_mass = mA + m_ratio * m_ratio * mB;
+ if (length1 < m_maxLength1)
+ {
+ m_limitState1 = e_inactiveLimit;
+ m_limitImpulse1 = 0.0f;
+ }
+ else
+ {
+ m_limitState1 = e_atUpperLimit;
+ }
- if (m_mass > 0.0f)
+ if (length2 < m_maxLength2)
+ {
+ m_limitState2 = e_inactiveLimit;
+ m_limitImpulse2 = 0.0f;
+ }
+ else
{
- m_mass = 1.0f / m_mass;
+ m_limitState2 = e_atUpperLimit;
}
- if (data.step.warmStarting)
+ // Compute effective mass.
+ qreal cr1u1 = b2Cross(r1, m_u1);
+ qreal cr2u2 = b2Cross(r2, m_u2);
+
+ m_limitMass1 = b1->m_invMass + b1->m_invI * cr1u1 * cr1u1;
+ m_limitMass2 = b2->m_invMass + b2->m_invI * cr2u2 * cr2u2;
+ m_pulleyMass = m_limitMass1 + m_ratio * m_ratio * m_limitMass2;
+ b2Assert(m_limitMass1 > b2_epsilon);
+ b2Assert(m_limitMass2 > b2_epsilon);
+ b2Assert(m_pulleyMass > b2_epsilon);
+ m_limitMass1 = 1.0f / m_limitMass1;
+ m_limitMass2 = 1.0f / m_limitMass2;
+ m_pulleyMass = 1.0f / m_pulleyMass;
+
+ if (step.warmStarting)
{
// Scale impulses to support variable time steps.
- m_impulse *= data.step.dtRatio;
+ m_impulse *= step.dtRatio;
+ m_limitImpulse1 *= step.dtRatio;
+ m_limitImpulse2 *= 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);
+ b2Vec2 P1 = -(m_impulse + m_limitImpulse1) * m_u1;
+ b2Vec2 P2 = (-m_ratio * m_impulse - m_limitImpulse2) * m_u2;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
else
{
m_impulse = 0.0f;
+ m_limitImpulse1 = 0.0f;
+ m_limitImpulse2 = 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)
+void b2PulleyJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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);
+ B2_NOT_USED(step);
- b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- // Get the pulley axes.
- b2Vec2 uA = cA + rA - m_groundAnchorA;
- b2Vec2 uB = cB + rB - m_groundAnchorB;
-
- float32 lengthA = uA.Length();
- float32 lengthB = uB.Length();
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
- if (lengthA > 10.0f * b2_linearSlop)
+ if (m_state == e_atUpperLimit)
{
- uA *= 1.0f / lengthA;
- }
- else
- {
- uA.SetZero();
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
+
+ qreal Cdot = -b2Dot(m_u1, v1) - m_ratio * b2Dot(m_u2, v2);
+ qreal impulse = m_pulleyMass * (-Cdot);
+ qreal oldImpulse = m_impulse;
+ m_impulse = b2Max(0.0f, m_impulse + impulse);
+ impulse = m_impulse - oldImpulse;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b2Vec2 P2 = -m_ratio * impulse * m_u2;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
}
- if (lengthB > 10.0f * b2_linearSlop)
+ if (m_limitState1 == e_atUpperLimit)
{
- uB *= 1.0f / lengthB;
+ b2Vec2 v1 = b1->m_linearVelocity + b2Cross(b1->m_angularVelocity, r1);
+
+ qreal Cdot = -b2Dot(m_u1, v1);
+ qreal impulse = -m_limitMass1 * Cdot;
+ qreal oldImpulse = m_limitImpulse1;
+ m_limitImpulse1 = b2Max(0.0f, m_limitImpulse1 + impulse);
+ impulse = m_limitImpulse1 - oldImpulse;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b1->m_linearVelocity += b1->m_invMass * P1;
+ b1->m_angularVelocity += b1->m_invI * b2Cross(r1, P1);
}
- else
+
+ if (m_limitState2 == e_atUpperLimit)
{
- uB.SetZero();
- }
+ b2Vec2 v2 = b2->m_linearVelocity + b2Cross(b2->m_angularVelocity, r2);
- // Compute effective mass.
- float32 ruA = b2Cross(rA, uA);
- float32 ruB = b2Cross(rB, uB);
+ qreal Cdot = -b2Dot(m_u2, v2);
+ qreal impulse = -m_limitMass2 * Cdot;
+ qreal oldImpulse = m_limitImpulse2;
+ m_limitImpulse2 = b2Max(0.0f, m_limitImpulse2 + impulse);
+ impulse = m_limitImpulse2 - oldImpulse;
- float32 mA = m_invMassA + m_invIA * ruA * ruA;
- float32 mB = m_invMassB + m_invIB * ruB * ruB;
+ b2Vec2 P2 = -impulse * m_u2;
+ b2->m_linearVelocity += b2->m_invMass * P2;
+ b2->m_angularVelocity += b2->m_invI * b2Cross(r2, P2);
+ }
+}
- float32 mass = mA + m_ratio * m_ratio * mB;
+bool b2PulleyJoint::SolvePositionConstraints(qreal baumgarte)
+{
+ B2_NOT_USED(baumgarte);
- if (mass > 0.0f)
- {
- mass = 1.0f / mass;
- }
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- float32 C = m_constant - lengthA - m_ratio * lengthB;
- float32 linearError = b2Abs(C);
+ b2Vec2 s1 = m_groundAnchor1;
+ b2Vec2 s2 = m_groundAnchor2;
- float32 impulse = -mass * C;
+ qreal linearError = 0.0f;
- b2Vec2 PA = -impulse * uA;
- b2Vec2 PB = -m_ratio * impulse * uB;
+ if (m_state == e_atUpperLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ // Get the pulley axes.
+ m_u1 = p1 - s1;
+ m_u2 = p2 - s2;
+
+ qreal length1 = m_u1.Length();
+ qreal length2 = m_u2.Length();
+
+ if (length1 > b2_linearSlop)
+ {
+ m_u1 *= 1.0f / length1;
+ }
+ else
+ {
+ m_u1.SetZero();
+ }
+
+ if (length2 > b2_linearSlop)
+ {
+ m_u2 *= 1.0f / length2;
+ }
+ else
+ {
+ m_u2.SetZero();
+ }
+
+ qreal C = m_constant - length1 - m_ratio * length2;
+ linearError = b2Max(linearError, -C);
+
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ qreal impulse = -m_pulleyMass * C;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b2Vec2 P2 = -m_ratio * impulse * m_u2;
+
+ b1->m_sweep.c += b1->m_invMass * P1;
+ b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+ b2->m_sweep.c += b2->m_invMass * P2;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
- cA += m_invMassA * PA;
- aA += m_invIA * b2Cross(rA, PA);
- cB += m_invMassB * PB;
- aB += m_invIB * b2Cross(rB, PB);
+ if (m_limitState1 == e_atUpperLimit)
+ {
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 p1 = b1->m_sweep.c + r1;
+
+ m_u1 = p1 - s1;
+ qreal length1 = m_u1.Length();
+
+ if (length1 > b2_linearSlop)
+ {
+ m_u1 *= 1.0f / length1;
+ }
+ else
+ {
+ m_u1.SetZero();
+ }
+
+ qreal C = m_maxLength1 - length1;
+ linearError = b2Max(linearError, -C);
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ qreal impulse = -m_limitMass1 * C;
+
+ b2Vec2 P1 = -impulse * m_u1;
+ b1->m_sweep.c += b1->m_invMass * P1;
+ b1->m_sweep.a += b1->m_invI * b2Cross(r1, P1);
+
+ b1->SynchronizeTransform();
+ }
- data.positions[m_indexA].c = cA;
- data.positions[m_indexA].a = aA;
- data.positions[m_indexB].c = cB;
- data.positions[m_indexB].a = aB;
+ if (m_limitState2 == e_atUpperLimit)
+ {
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+ b2Vec2 p2 = b2->m_sweep.c + r2;
+
+ m_u2 = p2 - s2;
+ qreal length2 = m_u2.Length();
+
+ if (length2 > b2_linearSlop)
+ {
+ m_u2 *= 1.0f / length2;
+ }
+ else
+ {
+ m_u2.SetZero();
+ }
+
+ qreal C = m_maxLength2 - length2;
+ linearError = b2Max(linearError, -C);
+ C = b2Clamp(C + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
+ qreal impulse = -m_limitMass2 * C;
+
+ b2Vec2 P2 = -impulse * m_u2;
+ b2->m_sweep.c += b2->m_invMass * P2;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, P2);
+
+ b2->SynchronizeTransform();
+ }
return linearError < b2_linearSlop;
}
b2Vec2 b2PulleyJoint::GetAnchorA() const
{
- return m_bodyA->GetWorldPoint(m_localAnchorA);
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
}
b2Vec2 b2PulleyJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
}
-b2Vec2 b2PulleyJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2PulleyJoint::GetReactionForce(qreal inv_dt) const
{
- b2Vec2 P = m_impulse * m_uB;
+ b2Vec2 P = m_impulse * m_u2;
return inv_dt * P;
}
-float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2PulleyJoint::GetReactionTorque(qreal inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
@@ -283,66 +397,31 @@ float32 b2PulleyJoint::GetReactionTorque(float32 inv_dt) const
b2Vec2 b2PulleyJoint::GetGroundAnchorA() const
{
- return m_groundAnchorA;
+ return m_groundAnchor1;
}
b2Vec2 b2PulleyJoint::GetGroundAnchorB() const
{
- return m_groundAnchorB;
-}
-
-float32 b2PulleyJoint::GetLengthA() const
-{
- return m_lengthA;
-}
-
-float32 b2PulleyJoint::GetLengthB() const
-{
- return m_lengthB;
+ return m_groundAnchor2;
}
-float32 b2PulleyJoint::GetRatio() const
+qreal b2PulleyJoint::GetLength1() const
{
- return m_ratio;
-}
-
-float32 b2PulleyJoint::GetCurrentLengthA() const
-{
- b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchorA);
- b2Vec2 s = m_groundAnchorA;
+ b2Vec2 p = m_bodyA->GetWorldPoint(m_localAnchor1);
+ b2Vec2 s = m_groundAnchor1;
b2Vec2 d = p - s;
return d.Length();
}
-float32 b2PulleyJoint::GetCurrentLengthB() const
+qreal b2PulleyJoint::GetLength2() const
{
- b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchorB);
- b2Vec2 s = m_groundAnchorB;
+ b2Vec2 p = m_bodyB->GetWorldPoint(m_localAnchor2);
+ b2Vec2 s = m_groundAnchor2;
b2Vec2 d = p - s;
return d.Length();
}
-void b2PulleyJoint::Dump()
+qreal b2PulleyJoint::GetRatio() const
{
- 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;
+ return m_ratio;
}
diff --git a/external/Box2D/Dynamics/Joints/b2PulleyJoint.h b/external/Box2D/Dynamics/Joints/b2PulleyJoint.h
index 44843e7..c826e91 100644
--- a/external/Box2D/Dynamics/Joints/b2PulleyJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2PulleyJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -21,10 +21,11 @@
#include <Box2D/Dynamics/Joints/b2Joint.h>
-const float32 b2_minPulleyLength = 2.0f;
+const qreal b2_minPulleyLength = 2.0f;
/// Pulley joint definition. This requires two ground anchors,
-/// two dynamic body anchor points, and a pulley ratio.
+/// two dynamic body anchor points, max lengths for each side,
+/// and a pulley ratio.
struct b2PulleyJointDef : public b2JointDef
{
b2PulleyJointDef()
@@ -35,7 +36,9 @@ struct b2PulleyJointDef : public b2JointDef
localAnchorA.Set(-1.0f, 0.0f);
localAnchorB.Set(1.0f, 0.0f);
lengthA = 0.0f;
+ maxLengthA = 0.0f;
lengthB = 0.0f;
+ maxLengthB = 0.0f;
ratio = 1.0f;
collideConnected = true;
}
@@ -44,7 +47,7 @@ struct b2PulleyJointDef : public b2JointDef
void Initialize(b2Body* bodyA, b2Body* bodyB,
const b2Vec2& groundAnchorA, const b2Vec2& groundAnchorB,
const b2Vec2& anchorA, const b2Vec2& anchorB,
- float32 ratio);
+ qreal ratio);
/// The first ground anchor in world coordinates. This point never moves.
b2Vec2 groundAnchorA;
@@ -59,31 +62,35 @@ struct b2PulleyJointDef : public b2JointDef
b2Vec2 localAnchorB;
/// The a reference length for the segment attached to bodyA.
- float32 lengthA;
+ qreal lengthA;
+
+ /// The maximum length of the segment attached to bodyA.
+ qreal maxLengthA;
/// The a reference length for the segment attached to bodyB.
- float32 lengthB;
+ qreal lengthB;
+
+ /// The maximum length of the segment attached to bodyB.
+ qreal maxLengthB;
/// The pulley ratio, used to simulate a block-and-tackle.
- float32 ratio;
+ qreal 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.
+/// The pulley also enforces a maximum length limit on both sides. This is
+/// useful to prevent one side of the pulley hitting the top.
class b2PulleyJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// Get the first ground anchor.
b2Vec2 GetGroundAnchorA() const;
@@ -91,62 +98,51 @@ public:
/// 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 body1.
+ qreal GetLength1() const;
- /// Get the current length of the segment attached to bodyB.
- float32 GetLengthB() const;
+ /// Get the current length of the segment attached to body2.
+ qreal GetLength2() 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();
-
- /// Implement b2Joint::ShiftOrigin
- void ShiftOrigin(const b2Vec2& newOrigin);
+ qreal GetRatio() const;
protected:
friend class b2Joint;
b2PulleyJoint(const b2PulleyJointDef* data);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
- b2Vec2 m_groundAnchorA;
- b2Vec2 m_groundAnchorB;
- float32 m_lengthA;
- float32 m_lengthB;
+ b2Vec2 m_groundAnchor1;
+ b2Vec2 m_groundAnchor2;
+ b2Vec2 m_localAnchor1;
+ b2Vec2 m_localAnchor2;
+
+ b2Vec2 m_u1;
+ b2Vec2 m_u2;
+
+ qreal m_constant;
+ qreal m_ratio;
- // 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;
+ qreal m_maxLength1;
+ qreal m_maxLength2;
+
+ // Effective masses
+ qreal m_pulleyMass;
+ qreal m_limitMass1;
+ qreal m_limitMass2;
+
+ // Impulses for accumulation/warm starting.
+ qreal m_impulse;
+ qreal m_limitImpulse1;
+ qreal m_limitImpulse2;
+
+ b2LimitState m_state;
+ b2LimitState m_limitState1;
+ b2LimitState m_limitState2;
};
#endif
diff --git a/external/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp b/external/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp
index 64bd190..f285733 100644
--- a/external/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2RevoluteJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -33,10 +33,10 @@
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
-void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
+void b2RevoluteJointDef::Initialize(b2Body* b1, b2Body* b2, const b2Vec2& anchor)
{
- bodyA = bA;
- bodyB = bB;
+ bodyA = b1;
+ bodyB = b2;
localAnchorA = bodyA->GetLocalPoint(anchor);
localAnchorB = bodyB->GetLocalPoint(anchor);
referenceAngle = bodyB->GetAngle() - bodyA->GetAngle();
@@ -45,8 +45,8 @@ void b2RevoluteJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor
b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
: b2Joint(def)
{
- m_localAnchorA = def->localAnchorA;
- m_localAnchorB = def->localAnchorB;
+ m_localAnchor1 = def->localAnchorA;
+ m_localAnchor2 = def->localAnchorB;
m_referenceAngle = def->referenceAngle;
m_impulse.SetZero();
@@ -61,68 +61,58 @@ b2RevoluteJoint::b2RevoluteJoint(const b2RevoluteJointDef* def)
m_limitState = e_inactiveLimit;
}
-void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2RevoluteJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- 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);
+ if (m_enableMotor || m_enableLimit)
+ {
+ // You cannot create a rotation limit between bodies that
+ // both have fixed rotation.
+ b2Assert(b1->m_invI > 0.0f || b2->m_invI > 0.0f);
+ }
- m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ // Compute the effective mass matrix.
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
// 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;
+ // K = [ m1+r1y^2*i1+m2+r2y^2*i2, -r1y*i1*r1x-r2y*i2*r2x, -r1y*i1-r2y*i2]
+ // [ -r1y*i1*r1x-r2y*i2*r2x, m1+r1x^2*i1+m2+r2x^2*i2, r1x*i1+r2x*i2]
+ // [ -r1y*i1-r2y*i2, r1x*i1+r2x*i2, i1+i2]
+
+ qreal m1 = b1->m_invMass, m2 = b2->m_invMass;
+ qreal i1 = b1->m_invI, i2 = b2->m_invI;
+
+ m_mass.col1.x = m1 + m2 + r1.y * r1.y * i1 + r2.y * r2.y * i2;
+ m_mass.col2.x = -r1.y * r1.x * i1 - r2.y * r2.x * i2;
+ m_mass.col3.x = -r1.y * i1 - r2.y * i2;
+ m_mass.col1.y = m_mass.col2.x;
+ m_mass.col2.y = m1 + m2 + r1.x * r1.x * i1 + r2.x * r2.x * i2;
+ m_mass.col3.y = r1.x * i1 + r2.x * i2;
+ m_mass.col1.z = m_mass.col3.x;
+ m_mass.col2.z = m_mass.col3.y;
+ m_mass.col3.z = i1 + i2;
+
+ m_motorMass = i1 + i2;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
- if (m_enableMotor == false || fixedRotation)
+ if (m_enableMotor == false)
{
m_motorImpulse = 0.0f;
}
- if (m_enableLimit && fixedRotation == false)
+ if (m_enableLimit)
{
- float32 jointAngle = aB - aA - m_referenceAngle;
+ qreal jointAngle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
{
m_limitState = e_equalLimits;
@@ -154,66 +144,66 @@ void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
m_limitState = e_inactiveLimit;
}
- if (data.step.warmStarting)
+ if (step.warmStarting)
{
// Scale impulses to support a variable time step.
- m_impulse *= data.step.dtRatio;
- m_motorImpulse *= data.step.dtRatio;
+ m_impulse *= step.dtRatio;
+ m_motorImpulse *= step.dtRatio;
b2Vec2 P(m_impulse.x, m_impulse.y);
- vA -= mA * P;
- wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);
+ b1->m_linearVelocity -= m1 * P;
+ b1->m_angularVelocity -= i1 * (b2Cross(r1, P) + m_motorImpulse + m_impulse.z);
- vB += mB * P;
- wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
+ b2->m_linearVelocity += m2 * P;
+ b2->m_angularVelocity += i2 * (b2Cross(r2, 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)
+void b2RevoluteJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ b2Vec2 v1 = b1->m_linearVelocity;
+ qreal w1 = b1->m_angularVelocity;
+ b2Vec2 v2 = b2->m_linearVelocity;
+ qreal w2 = b2->m_angularVelocity;
- bool fixedRotation = (iA + iB == 0.0f);
+ qreal m1 = b1->m_invMass, m2 = b2->m_invMass;
+ qreal i1 = b1->m_invI, i2 = b2->m_invI;
// Solve motor constraint.
- if (m_enableMotor && m_limitState != e_equalLimits && fixedRotation == false)
+ if (m_enableMotor && m_limitState != e_equalLimits)
{
- float32 Cdot = wB - wA - m_motorSpeed;
- float32 impulse = -m_motorMass * Cdot;
- float32 oldImpulse = m_motorImpulse;
- float32 maxImpulse = data.step.dt * m_maxMotorTorque;
+ qreal Cdot = w2 - w1 - m_motorSpeed;
+ qreal impulse = m_motorMass * (-Cdot);
+ qreal oldImpulse = m_motorImpulse;
+ qreal maxImpulse = step.dt * m_maxMotorTorque;
m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
- wA -= iA * impulse;
- wB += iB * impulse;
+ w1 -= i1 * impulse;
+ w2 += i2 * impulse;
}
// Solve limit constraint.
- if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
{
- b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
- float32 Cdot2 = wB - wA;
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
+ // Solve point-to-point constraint
+ b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
+ qreal Cdot2 = w2 - w1;
b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
- b2Vec3 impulse = -m_mass.Solve33(Cdot);
+ b2Vec3 impulse = m_mass.Solve33(-Cdot);
if (m_limitState == e_equalLimits)
{
@@ -221,11 +211,10 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
}
else if (m_limitState == e_atLowerLimit)
{
- float32 newImpulse = m_impulse.z + impulse.z;
+ qreal 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);
+ b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
@@ -233,18 +222,13 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
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;
+ qreal 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);
+ b2Vec2 reduced = m_mass.Solve22(-Cdot1);
impulse.x = reduced.x;
impulse.y = reduced.y;
impulse.z = -m_impulse.z;
@@ -252,72 +236,69 @@ void b2RevoluteJoint::SolveVelocityConstraints(const b2SolverData& data)
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);
+ v1 -= m1 * P;
+ w1 -= i1 * (b2Cross(r1, P) + impulse.z);
- vB += mB * P;
- wB += iB * (b2Cross(m_rB, P) + impulse.z);
+ v2 += m2 * P;
+ w2 += i2 * (b2Cross(r2, P) + impulse.z);
}
else
{
+ b2Vec2 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
+
// Solve point-to-point constraint
- b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+ b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
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);
+ v1 -= m1 * impulse;
+ w1 -= i1 * b2Cross(r1, impulse);
- vB += mB * impulse;
- wB += iB * b2Cross(m_rB, impulse);
+ v2 += m2 * impulse;
+ w2 += i2 * b2Cross(r2, 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;
+ b1->m_linearVelocity = v1;
+ b1->m_angularVelocity = w1;
+ b2->m_linearVelocity = v2;
+ b2->m_angularVelocity = w2;
}
-bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2RevoluteJoint::SolvePositionConstraints(qreal baumgarte)
{
- 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;
+ // TODO_ERIN block solve with limit.
- b2Rot qA(aA), qB(aB);
+ B2_NOT_USED(baumgarte);
- float32 angularError = 0.0f;
- float32 positionError = 0.0f;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
- bool fixedRotation = (m_invIA + m_invIB == 0.0f);
+ qreal angularError = 0.0f;
+ qreal positionError = 0.0f;
// Solve angular limit constraint.
- if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
+ if (m_enableLimit && m_limitState != e_inactiveLimit)
{
- float32 angle = aB - aA - m_referenceAngle;
- float32 limitImpulse = 0.0f;
+ qreal angle = b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
+ qreal limitImpulse = 0.0f;
if (m_limitState == e_equalLimits)
{
// Prevent large angular corrections
- float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
+ qreal 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;
+ qreal C = angle - m_lowerAngle;
angularError = -C;
// Prevent large angular corrections and allow some slop.
@@ -326,7 +307,7 @@ bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
}
else if (m_limitState == e_atUpperLimit)
{
- float32 C = angle - m_upperAngle;
+ qreal C = angle - m_upperAngle;
angularError = C;
// Prevent large angular corrections and allow some slop.
@@ -334,79 +315,104 @@ bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
limitImpulse = -m_motorMass * C;
}
- aA -= m_invIA * limitImpulse;
- aB += m_invIB * limitImpulse;
+ b1->m_sweep.a -= b1->m_invI * limitImpulse;
+ b2->m_sweep.a += b2->m_invI * limitImpulse;
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
}
// 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 r1 = b2Mul(b1->GetTransform().R, m_localAnchor1 - b1->GetLocalCenter());
+ b2Vec2 r2 = b2Mul(b2->GetTransform().R, m_localAnchor2 - b2->GetLocalCenter());
- b2Vec2 C = cB + rB - cA - rA;
+ b2Vec2 C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
positionError = C.Length();
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ qreal invMass1 = b1->m_invMass, invMass2 = b2->m_invMass;
+ qreal invI1 = b1->m_invI, invI2 = b2->m_invI;
- 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;
+ // Handle large detachment.
+ const qreal k_allowedStretch = 10.0f * b2_linearSlop;
+ if (C.LengthSquared() > k_allowedStretch * k_allowedStretch)
+ {
+ // Use a particle solution (no rotation).
+ b2Vec2 u = C; u.Normalize();
+ qreal m = invMass1 + invMass2;
+ if (m > 0.0f)
+ {
+ m = 1.0f / m;
+ }
+ b2Vec2 impulse = m * (-C);
+ const qreal k_beta = 0.5f;
+ b1->m_sweep.c -= k_beta * invMass1 * impulse;
+ b2->m_sweep.c += k_beta * invMass2 * impulse;
- b2Vec2 impulse = -K.Solve(C);
+ C = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;
+ }
- cA -= mA * impulse;
- aA -= iA * b2Cross(rA, impulse);
+ b2Mat22 K1;
+ K1.col1.x = invMass1 + invMass2; K1.col2.x = 0.0f;
+ K1.col1.y = 0.0f; K1.col2.y = invMass1 + invMass2;
- cB += mB * impulse;
- aB += iB * b2Cross(rB, impulse);
- }
+ b2Mat22 K2;
+ K2.col1.x = invI1 * r1.y * r1.y; K2.col2.x = -invI1 * r1.x * r1.y;
+ K2.col1.y = -invI1 * r1.x * r1.y; K2.col2.y = invI1 * r1.x * r1.x;
- data.positions[m_indexA].c = cA;
- data.positions[m_indexA].a = aA;
- data.positions[m_indexB].c = cB;
- data.positions[m_indexB].a = aB;
+ b2Mat22 K3;
+ K3.col1.x = invI2 * r2.y * r2.y; K3.col2.x = -invI2 * r2.x * r2.y;
+ K3.col1.y = -invI2 * r2.x * r2.y; K3.col2.y = invI2 * r2.x * r2.x;
+
+ b2Mat22 K = K1 + K2 + K3;
+ b2Vec2 impulse = K.Solve(-C);
+
+ b1->m_sweep.c -= b1->m_invMass * impulse;
+ b1->m_sweep.a -= b1->m_invI * b2Cross(r1, impulse);
+
+ b2->m_sweep.c += b2->m_invMass * impulse;
+ b2->m_sweep.a += b2->m_invI * b2Cross(r2, impulse);
+
+ b1->SynchronizeTransform();
+ b2->SynchronizeTransform();
+ }
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
b2Vec2 b2RevoluteJoint::GetAnchorA() const
{
- return m_bodyA->GetWorldPoint(m_localAnchorA);
+ return m_bodyA->GetWorldPoint(m_localAnchor1);
}
b2Vec2 b2RevoluteJoint::GetAnchorB() const
{
- return m_bodyB->GetWorldPoint(m_localAnchorB);
+ return m_bodyB->GetWorldPoint(m_localAnchor2);
}
-b2Vec2 b2RevoluteJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2RevoluteJoint::GetReactionForce(qreal inv_dt) const
{
b2Vec2 P(m_impulse.x, m_impulse.y);
return inv_dt * P;
}
-float32 b2RevoluteJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2RevoluteJoint::GetReactionTorque(qreal inv_dt) const
{
return inv_dt * m_impulse.z;
}
-float32 b2RevoluteJoint::GetJointAngle() const
+qreal b2RevoluteJoint::GetJointAngle() const
{
- b2Body* bA = m_bodyA;
- b2Body* bB = m_bodyB;
- return bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+ return b2->m_sweep.a - b1->m_sweep.a - m_referenceAngle;
}
-float32 b2RevoluteJoint::GetJointSpeed() const
+qreal b2RevoluteJoint::GetJointSpeed() const
{
- b2Body* bA = m_bodyA;
- b2Body* bB = m_bodyB;
- return bB->m_angularVelocity - bA->m_angularVelocity;
+ b2Body* b1 = m_bodyA;
+ b2Body* b2 = m_bodyB;
+ return b2->m_angularVelocity - b1->m_angularVelocity;
}
bool b2RevoluteJoint::IsMotorEnabled() const
@@ -421,19 +427,19 @@ void b2RevoluteJoint::EnableMotor(bool flag)
m_enableMotor = flag;
}
-float32 b2RevoluteJoint::GetMotorTorque(float32 inv_dt) const
+qreal b2RevoluteJoint::GetMotorTorque(qreal inv_dt) const
{
return inv_dt * m_motorImpulse;
}
-void b2RevoluteJoint::SetMotorSpeed(float32 speed)
+void b2RevoluteJoint::SetMotorSpeed(qreal speed)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
m_motorSpeed = speed;
}
-void b2RevoluteJoint::SetMaxMotorTorque(float32 torque)
+void b2RevoluteJoint::SetMaxMotorTorque(qreal torque)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
@@ -447,56 +453,26 @@ bool b2RevoluteJoint::IsLimitEnabled() const
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;
- }
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_enableLimit = flag;
}
-float32 b2RevoluteJoint::GetLowerLimit() const
+qreal b2RevoluteJoint::GetLowerLimit() const
{
return m_lowerAngle;
}
-float32 b2RevoluteJoint::GetUpperLimit() const
+qreal b2RevoluteJoint::GetUpperLimit() const
{
return m_upperAngle;
}
-void b2RevoluteJoint::SetLimits(float32 lower, float32 upper)
+void b2RevoluteJoint::SetLimits(qreal lower, qreal 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);
+ m_bodyA->SetAwake(true);
+ m_bodyB->SetAwake(true);
+ m_lowerAngle = lower;
+ m_upperAngle = upper;
}
diff --git a/external/Box2D/Dynamics/Joints/b2RevoluteJoint.h b/external/Box2D/Dynamics/Joints/b2RevoluteJoint.h
index bbed4a6..d77a34f 100644
--- a/external/Box2D/Dynamics/Joints/b2RevoluteJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2RevoluteJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -52,33 +52,33 @@ struct b2RevoluteJointDef : public b2JointDef
/// anchor point.
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
- /// The local anchor point relative to bodyA's origin.
+ /// The local anchor point relative to body1's origin.
b2Vec2 localAnchorA;
- /// The local anchor point relative to bodyB's origin.
+ /// The local anchor point relative to body2's origin.
b2Vec2 localAnchorB;
- /// The bodyB angle minus bodyA angle in the reference state (radians).
- float32 referenceAngle;
+ /// The body2 angle minus body1 angle in the reference state (radians).
+ qreal referenceAngle;
/// A flag to enable joint limits.
bool enableLimit;
/// The lower angle for the joint limit (radians).
- float32 lowerAngle;
+ qreal lowerAngle;
/// The upper angle for the joint limit (radians).
- float32 upperAngle;
+ qreal upperAngle;
/// A flag to enable the joint motor.
bool enableMotor;
/// The desired motor speed. Usually in radians per second.
- float32 motorSpeed;
+ qreal motorSpeed;
/// The maximum motor torque used to achieve the desired motor speed.
/// Usually in N-m.
- float32 maxMotorTorque;
+ qreal maxMotorTorque;
};
/// A revolute joint constrains two bodies to share a common point while they
@@ -93,20 +93,11 @@ public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- /// 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;
+ qreal GetJointAngle() const;
/// Get the current joint angle speed in radians per second.
- float32 GetJointSpeed() const;
+ qreal GetJointSpeed() const;
/// Is the joint limit enabled?
bool IsLimitEnabled() const;
@@ -115,13 +106,13 @@ public:
void EnableLimit(bool flag);
/// Get the lower joint limit in radians.
- float32 GetLowerLimit() const;
+ qreal GetLowerLimit() const;
/// Get the upper joint limit in radians.
- float32 GetUpperLimit() const;
+ qreal GetUpperLimit() const;
/// Set the joint limits in radians.
- void SetLimits(float32 lower, float32 upper);
+ void SetLimits(qreal lower, qreal upper);
/// Is the joint motor enabled?
bool IsMotorEnabled() const;
@@ -130,29 +121,25 @@ public:
void EnableMotor(bool flag);
/// Set the motor speed in radians per second.
- void SetMotorSpeed(float32 speed);
+ void SetMotorSpeed(qreal speed);
/// Get the motor speed in radians per second.
- float32 GetMotorSpeed() const;
+ qreal GetMotorSpeed() const;
/// Set the maximum motor torque, usually in N-m.
- void SetMaxMotorTorque(float32 torque);
- float32 GetMaxMotorTorque() const { return m_maxMotorTorque; }
+ void SetMaxMotorTorque(qreal torque);
/// Get the reaction force given the inverse time step.
/// Unit is N.
- b2Vec2 GetReactionForce(float32 inv_dt) const;
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
/// Get the reaction torque due to the joint limit given the inverse time step.
/// Unit is N*m.
- float32 GetReactionTorque(float32 inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
/// 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();
+ qreal GetMotorTorque(qreal inv_dt) const;
protected:
@@ -161,42 +148,31 @@ protected:
b2RevoluteJoint(const b2RevoluteJointDef* def);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
- // Solver shared
- b2Vec2 m_localAnchorA;
- b2Vec2 m_localAnchorB;
+ bool SolvePositionConstraints(qreal baumgarte);
+
+ b2Vec2 m_localAnchor1; // relative
+ b2Vec2 m_localAnchor2;
b2Vec3 m_impulse;
- float32 m_motorImpulse;
+ qreal m_motorImpulse;
+ b2Mat33 m_mass; // effective mass for point-to-point constraint.
+ qreal m_motorMass; // effective mass for motor/limit angular constraint.
+
bool m_enableMotor;
- float32 m_maxMotorTorque;
- float32 m_motorSpeed;
+ qreal m_maxMotorTorque;
+ qreal 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.
+ qreal m_referenceAngle;
+ qreal m_lowerAngle;
+ qreal m_upperAngle;
b2LimitState m_limitState;
};
-inline float32 b2RevoluteJoint::GetMotorSpeed() const
+inline qreal b2RevoluteJoint::GetMotorSpeed() const
{
return m_motorSpeed;
}
diff --git a/external/Box2D/Dynamics/Joints/b2RopeJoint.cpp b/external/Box2D/Dynamics/Joints/b2RopeJoint.cpp
index 952ee7a..70fe91a 100644
--- a/external/Box2D/Dynamics/Joints/b2RopeJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2RopeJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2007-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2007-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -43,36 +43,20 @@ b2RopeJoint::b2RopeJoint(const b2RopeJointDef* def)
m_length = 0.0f;
}
-void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
+void b2RopeJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
+
+ m_rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ m_rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
+
+ // Rope axis
+ m_u = bB->m_sweep.c + m_rB - bA->m_sweep.c - m_rA;
m_length = m_u.Length();
- float32 C = m_length - m_maxLength;
+ qreal C = m_length - m_maxLength;
if (C > 0.0f)
{
m_state = e_atUpperLimit;
@@ -95,100 +79,87 @@ void b2RopeJoint::InitVelocityConstraints(const b2SolverData& data)
}
// 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;
+ qreal crA = b2Cross(m_rA, m_u);
+ qreal crB = b2Cross(m_rB, m_u);
+ qreal invMass = bA->m_invMass + bA->m_invI * crA * crA + bB->m_invMass + bB->m_invI * crB * crB;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
- if (data.step.warmStarting)
+ if (step.warmStarting)
{
// Scale the impulse to support a variable time step.
- m_impulse *= data.step.dtRatio;
+ m_impulse *= 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);
+ bA->m_linearVelocity -= bA->m_invMass * P;
+ bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P);
+ bB->m_linearVelocity += bB->m_invMass * P;
+ bB->m_angularVelocity += bB->m_invI * 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)
+void b2RopeJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ B2_NOT_USED(step);
+
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
// 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);
+ b2Vec2 vA = bA->m_linearVelocity + b2Cross(bA->m_angularVelocity, m_rA);
+ b2Vec2 vB = bB->m_linearVelocity + b2Cross(bB->m_angularVelocity, m_rB);
+ qreal C = m_length - m_maxLength;
+ qreal Cdot = b2Dot(m_u, vB - vA);
// Predictive constraint.
if (C < 0.0f)
{
- Cdot += data.step.inv_dt * C;
+ Cdot += step.inv_dt * C;
}
- float32 impulse = -m_mass * Cdot;
- float32 oldImpulse = m_impulse;
+ qreal impulse = -m_mass * Cdot;
+ qreal 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;
+ bA->m_linearVelocity -= bA->m_invMass * P;
+ bA->m_angularVelocity -= bA->m_invI * b2Cross(m_rA, P);
+ bB->m_linearVelocity += bB->m_invMass * P;
+ bB->m_angularVelocity += bB->m_invI * b2Cross(m_rB, P);
}
-bool b2RopeJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2RopeJoint::SolvePositionConstraints(qreal baumgarte)
{
- 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;
+ B2_NOT_USED(baumgarte);
- b2Rot qA(aA), qB(aB);
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
- b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
- b2Vec2 u = cB + rB - cA - rA;
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
- float32 length = u.Normalize();
- float32 C = length - m_maxLength;
+ b2Vec2 u = bB->m_sweep.c + rB - bA->m_sweep.c - rA;
+
+ qreal length = u.Normalize();
+ qreal C = length - m_maxLength;
C = b2Clamp(C, 0.0f, b2_maxLinearCorrection);
- float32 impulse = -m_mass * C;
+ qreal 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);
+ bA->m_sweep.c -= bA->m_invMass * P;
+ bA->m_sweep.a -= bA->m_invI * b2Cross(rA, P);
+ bB->m_sweep.c += bB->m_invMass * P;
+ bB->m_sweep.a += bB->m_invI * 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;
+ bA->SynchronizeTransform();
+ bB->SynchronizeTransform();
return length - m_maxLength < b2_linearSlop;
}
@@ -203,19 +174,19 @@ b2Vec2 b2RopeJoint::GetAnchorB() const
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
-b2Vec2 b2RopeJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2RopeJoint::GetReactionForce(qreal inv_dt) const
{
b2Vec2 F = (inv_dt * m_impulse) * m_u;
return F;
}
-float32 b2RopeJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2RopeJoint::GetReactionTorque(qreal inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
}
-float32 b2RopeJoint::GetMaxLength() const
+qreal b2RopeJoint::GetMaxLength() const
{
return m_maxLength;
}
@@ -224,18 +195,3 @@ 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2RopeJoint.h b/external/Box2D/Dynamics/Joints/b2RopeJoint.h
index eaca575..6e742dd 100644
--- a/external/Box2D/Dynamics/Joints/b2RopeJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2RopeJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2010 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -44,7 +44,7 @@ struct b2RopeJointDef : public b2JointDef
/// The maximum length of the rope.
/// Warning: this must be larger than b2_linearSlop or
/// the joint will have no effect.
- float32 maxLength;
+ qreal maxLength;
};
/// A rope joint enforces a maximum distance between two points
@@ -61,53 +61,38 @@ public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
- /// 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;
+ /// Get the maximum length of the rope.
+ qreal GetMaxLength() const;
b2LimitState GetLimitState() const;
- /// Dump joint to dmLog
- void Dump();
-
protected:
friend class b2Joint;
b2RopeJoint(const b2RopeJointDef* data);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
+ bool SolvePositionConstraints(qreal baumgarte);
- // 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;
+
+ qreal m_maxLength;
+ qreal m_length;
+
+ // Jacobian info
+ b2Vec2 m_u, m_rA, m_rB;
+
+ // Effective mass
+ qreal m_mass;
+
+ // Impulses for accumulation/warm starting.
+ qreal m_impulse;
+
b2LimitState m_state;
};
diff --git a/external/Box2D/Dynamics/Joints/b2WeldJoint.cpp b/external/Box2D/Dynamics/Joints/b2WeldJoint.cpp
index ffdd386..df4f4bc 100644
--- a/external/Box2D/Dynamics/Joints/b2WeldJoint.cpp
+++ b/external/Box2D/Dynamics/Joints/b2WeldJoint.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -49,35 +49,18 @@ b2WeldJoint::b2WeldJoint(const b2WeldJointDef* 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)
+void b2WeldJoint::InitVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
- 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);
+ // Compute the effective mass matrix.
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
@@ -88,219 +71,128 @@ void b2WeldJoint::InitVelocityConstraints(const b2SolverData& data)
// [ -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;
+ qreal mA = bA->m_invMass, mB = bB->m_invMass;
+ qreal iA = bA->m_invI, iB = bB->m_invI;
- float32 C = aB - aA - m_referenceAngle;
+ m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+ m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+ m_mass.col3.x = -rA.y * iA - rB.y * iB;
+ m_mass.col1.y = m_mass.col2.x;
+ m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+ m_mass.col3.y = rA.x * iA + rB.x * iB;
+ m_mass.col1.z = m_mass.col3.x;
+ m_mass.col2.z = m_mass.col3.y;
+ m_mass.col3.z = iA + iB;
- // 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)
+ if (step.warmStarting)
{
// Scale impulses to support a variable time step.
- m_impulse *= data.step.dtRatio;
+ m_impulse *= step.dtRatio;
b2Vec2 P(m_impulse.x, m_impulse.y);
- vA -= mA * P;
- wA -= iA * (b2Cross(m_rA, P) + m_impulse.z);
+ bA->m_linearVelocity -= mA * P;
+ bA->m_angularVelocity -= iA * (b2Cross(rA, P) + m_impulse.z);
- vB += mB * P;
- wB += iB * (b2Cross(m_rB, P) + m_impulse.z);
+ bB->m_linearVelocity += mB * P;
+ bB->m_angularVelocity += iB * (b2Cross(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)
+void b2WeldJoint::SolveVelocityConstraints(const b2TimeStep& step)
{
- 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;
+ B2_NOT_USED(step);
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
- if (m_frequencyHz > 0.0f)
- {
- float32 Cdot2 = wB - wA;
+ b2Vec2 vA = bA->m_linearVelocity;
+ qreal wA = bA->m_angularVelocity;
+ b2Vec2 vB = bB->m_linearVelocity;
+ qreal wB = bB->m_angularVelocity;
- float32 impulse2 = -m_mass.ez.z * (Cdot2 + m_bias + m_gamma * m_impulse.z);
- m_impulse.z += impulse2;
+ qreal mA = bA->m_invMass, mB = bB->m_invMass;
+ qreal iA = bA->m_invI, iB = bB->m_invI;
- wA -= iA * impulse2;
- wB += iB * impulse2;
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
- b2Vec2 Cdot1 = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
+ // Solve point-to-point constraint
+ b2Vec2 Cdot1 = vB + b2Cross(wB, rB) - vA - b2Cross(wA, rA);
+ qreal Cdot2 = wB - wA;
+ b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);
- b2Vec2 impulse1 = -b2Mul22(m_mass, Cdot1);
- m_impulse.x += impulse1.x;
- m_impulse.y += impulse1.y;
+ b2Vec3 impulse = m_mass.Solve33(-Cdot);
+ m_impulse += impulse;
- b2Vec2 P = impulse1;
+ b2Vec2 P(impulse.x, impulse.y);
- vA -= mA * P;
- wA -= iA * b2Cross(m_rA, P);
+ vA -= mA * P;
+ wA -= iA * (b2Cross(rA, P) + impulse.z);
- 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(rB, 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;
+ bA->m_linearVelocity = vA;
+ bA->m_angularVelocity = wA;
+ bB->m_linearVelocity = vB;
+ bB->m_angularVelocity = wB;
}
-bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
+bool b2WeldJoint::SolvePositionConstraints(qreal baumgarte)
{
- 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;
+ B2_NOT_USED(baumgarte);
- b2Rot qA(aA), qB(aB);
+ b2Body* bA = m_bodyA;
+ b2Body* bB = m_bodyB;
- float32 mA = m_invMassA, mB = m_invMassB;
- float32 iA = m_invIA, iB = m_invIB;
+ qreal mA = bA->m_invMass, mB = bB->m_invMass;
+ qreal iA = bA->m_invI, iB = bB->m_invI;
- b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
- b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
+ b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
+ b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());
- float32 positionError, angularError;
+ b2Vec2 C1 = bB->m_sweep.c + rB - bA->m_sweep.c - rA;
+ qreal C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;
- 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)
+ // Handle large detachment.
+ const qreal k_allowedStretch = 10.0f * b2_linearSlop;
+ qreal positionError = C1.Length();
+ qreal angularError = b2Abs(C2);
+ if (positionError > k_allowedStretch)
{
- b2Vec2 C1 = cB + rB - cA - rA;
+ iA *= 1.0f;
+ iB *= 1.0f;
+ }
- positionError = C1.Length();
- angularError = 0.0f;
+ m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
+ m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
+ m_mass.col3.x = -rA.y * iA - rB.y * iB;
+ m_mass.col1.y = m_mass.col2.x;
+ m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
+ m_mass.col3.y = rA.x * iA + rB.x * iB;
+ m_mass.col1.z = m_mass.col3.x;
+ m_mass.col2.z = m_mass.col3.y;
+ m_mass.col3.z = iA + iB;
- b2Vec2 P = -K.Solve22(C1);
+ b2Vec3 C(C1.x, C1.y, C2);
- cA -= mA * P;
- aA -= iA * b2Cross(rA, P);
+ b2Vec3 impulse = m_mass.Solve33(-C);
- 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);
- }
+ b2Vec2 P(impulse.x, impulse.y);
+
+ bA->m_sweep.c -= mA * P;
+ bA->m_sweep.a -= iA * (b2Cross(rA, 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;
+ bB->m_sweep.c += mB * P;
+ bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);
+
+ bA->SynchronizeTransform();
+ bB->SynchronizeTransform();
return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
@@ -315,30 +207,13 @@ b2Vec2 b2WeldJoint::GetAnchorB() const
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
-b2Vec2 b2WeldJoint::GetReactionForce(float32 inv_dt) const
+b2Vec2 b2WeldJoint::GetReactionForce(qreal inv_dt) const
{
b2Vec2 P(m_impulse.x, m_impulse.y);
return inv_dt * P;
}
-float32 b2WeldJoint::GetReactionTorque(float32 inv_dt) const
+qreal b2WeldJoint::GetReactionTorque(qreal 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2WeldJoint.h b/external/Box2D/Dynamics/Joints/b2WeldJoint.h
index eed9bc1..7cb282a 100644
--- a/external/Box2D/Dynamics/Joints/b2WeldJoint.h
+++ b/external/Box2D/Dynamics/Joints/b2WeldJoint.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -32,29 +32,20 @@ struct b2WeldJointDef : public b2JointDef
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);
+ void Initialize(b2Body* body1, b2Body* body2, const b2Vec2& anchor);
- /// The local anchor point relative to bodyA's origin.
+ /// The local anchor point relative to body1's origin.
b2Vec2 localAnchorA;
- /// The local anchor point relative to bodyB's origin.
+ /// The local anchor point relative to body2'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;
+ /// The body2 angle minus body1 angle in the reference state (radians).
+ qreal referenceAngle;
};
/// A weld joint essentially glues two bodies together. A weld joint may
@@ -65,28 +56,8 @@ public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// 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();
+ b2Vec2 GetReactionForce(qreal inv_dt) const;
+ qreal GetReactionTorque(qreal inv_dt) const;
protected:
@@ -94,32 +65,17 @@ protected:
b2WeldJoint(const b2WeldJointDef* def);
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
+ void InitVelocityConstraints(const b2TimeStep& step);
+ void SolveVelocityConstraints(const b2TimeStep& step);
- float32 m_frequencyHz;
- float32 m_dampingRatio;
- float32 m_bias;
+ bool SolvePositionConstraints(qreal baumgarte);
- // Solver shared
b2Vec2 m_localAnchorA;
b2Vec2 m_localAnchorB;
- float32 m_referenceAngle;
- float32 m_gamma;
+ qreal m_referenceAngle;
+
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;
};
diff --git a/external/Box2D/Dynamics/Joints/b2WheelJoint.cpp b/external/Box2D/Dynamics/Joints/b2WheelJoint.cpp
deleted file mode 100644
index c9f6de6..0000000
--- a/external/Box2D/Dynamics/Joints/b2WheelJoint.cpp
+++ /dev/null
@@ -1,419 +0,0 @@
-/*
-* 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/Joints/b2WheelJoint.h>
-#include <Box2D/Dynamics/b2Body.h>
-#include <Box2D/Dynamics/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 d = 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 * (d + 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::GetJointSpeed() 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)
-{
- m_bodyA->SetAwake(true);
- m_bodyB->SetAwake(true);
- m_enableMotor = flag;
-}
-
-void b2WheelJoint::SetMotorSpeed(float32 speed)
-{
- m_bodyA->SetAwake(true);
- m_bodyB->SetAwake(true);
- m_motorSpeed = speed;
-}
-
-void b2WheelJoint::SetMaxMotorTorque(float32 torque)
-{
- 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);
-}
diff --git a/external/Box2D/Dynamics/Joints/b2WheelJoint.h b/external/Box2D/Dynamics/Joints/b2WheelJoint.h
deleted file mode 100644
index a33c77f..0000000
--- a/external/Box2D/Dynamics/Joints/b2WheelJoint.h
+++ /dev/null
@@ -1,210 +0,0 @@
-/*
-* 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 <Box2D/Dynamics/Joints/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;
- b2Vec2 GetAnchorB() const;
-
- b2Vec2 GetReactionForce(float32 inv_dt) const;
- float32 GetReactionTorque(float32 inv_dt) const;
-
- /// 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 translation speed, usually in meters per second.
- float32 GetJointSpeed() 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();
-
-protected:
-
- friend class b2Joint;
- b2WheelJoint(const b2WheelJointDef* def);
-
- void InitVelocityConstraints(const b2SolverData& data);
- void SolveVelocityConstraints(const b2SolverData& data);
- bool SolvePositionConstraints(const b2SolverData& data);
-
- 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
diff --git a/external/Box2D/Dynamics/b2Body.cpp b/external/Box2D/Dynamics/b2Body.cpp
index b655053..1fae38f 100644
--- a/external/Box2D/Dynamics/b2Body.cpp
+++ b/external/Box2D/Dynamics/b2Body.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2007 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -28,6 +28,7 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
b2Assert(bd->linearVelocity.IsValid());
b2Assert(b2IsValid(bd->angle));
b2Assert(b2IsValid(bd->angularVelocity));
+ b2Assert(b2IsValid(bd->inertiaScale) && bd->inertiaScale >= 0.0f);
b2Assert(b2IsValid(bd->angularDamping) && bd->angularDamping >= 0.0f);
b2Assert(b2IsValid(bd->linearDamping) && bd->linearDamping >= 0.0f);
@@ -56,15 +57,12 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
m_world = world;
- m_xf.p = bd->position;
- m_xf.q.Set(bd->angle);
+ m_xf.position = bd->position;
+ m_xf.R.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_sweep.a0 = m_sweep.a = bd->angle;
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
m_jointList = NULL;
m_contactList = NULL;
@@ -76,7 +74,6 @@ b2Body::b2Body(const b2BodyDef* bd, b2World* world)
m_linearDamping = bd->linearDamping;
m_angularDamping = bd->angularDamping;
- m_gravityScale = bd->gravityScale;
m_force.SetZero();
m_torque = 0.0f;
@@ -112,12 +109,6 @@ b2Body::~b2Body()
void b2Body::SetType(b2BodyType type)
{
- b2Assert(m_world->IsLocked() == false);
- if (m_world->IsLocked() == true)
- {
- return;
- }
-
if (m_type == type)
{
return;
@@ -131,9 +122,6 @@ void b2Body::SetType(b2BodyType type)
{
m_linearVelocity.SetZero();
m_angularVelocity = 0.0f;
- m_sweep.a0 = m_sweep.a;
- m_sweep.c0 = m_sweep.c;
- SynchronizeFixtures();
}
SetAwake(true);
@@ -141,25 +129,10 @@ void b2Body::SetType(b2BodyType type)
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 = NULL;
-
- // 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)
+ // Since the body type changed, we need to flag contacts for filtering.
+ for (b2ContactEdge* ce = m_contactList; ce; ce = ce->next)
{
- int32 proxyCount = f->m_proxyCount;
- for (int32 i = 0; i < proxyCount; ++i)
- {
- broadPhase->TouchProxy(f->m_proxies[i].proxyId);
- }
+ ce->contact->FlagForFiltering();
}
}
@@ -202,7 +175,7 @@ b2Fixture* b2Body::CreateFixture(const b2FixtureDef* def)
return fixture;
}
-b2Fixture* b2Body::CreateFixture(const b2Shape* shape, float32 density)
+b2Fixture* b2Body::CreateFixture(const b2Shape* shape, qreal density)
{
b2FixtureDef def;
def.shape = shape;
@@ -290,16 +263,14 @@ void b2Body::ResetMassData()
// 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;
+ m_sweep.c0 = m_sweep.c = m_xf.position;
return;
}
b2Assert(m_type == b2_dynamicBody);
// Accumulate mass over all fixtures.
- b2Vec2 localCenter = b2Vec2_zero;
+ b2Vec2 center = b2Vec2_zero;
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
{
if (f->m_density == 0.0f)
@@ -310,7 +281,7 @@ void b2Body::ResetMassData()
b2MassData massData;
f->GetMassData(&massData);
m_mass += massData.mass;
- localCenter += massData.mass * massData.center;
+ center += massData.mass * massData.center;
m_I += massData.I;
}
@@ -318,7 +289,7 @@ void b2Body::ResetMassData()
if (m_mass > 0.0f)
{
m_invMass = 1.0f / m_mass;
- localCenter *= m_invMass;
+ center *= m_invMass;
}
else
{
@@ -330,7 +301,7 @@ void b2Body::ResetMassData()
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);
+ m_I -= m_mass * b2Dot(center, center);
b2Assert(m_I > 0.0f);
m_invI = 1.0f / m_I;
@@ -343,7 +314,7 @@ void b2Body::ResetMassData()
// Move center of mass.
b2Vec2 oldCenter = m_sweep.c;
- m_sweep.localCenter = localCenter;
+ m_sweep.localCenter = center;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
// Update center of mass velocity.
@@ -384,7 +355,7 @@ void b2Body::SetMassData(const b2MassData* massData)
// Move center of mass.
b2Vec2 oldCenter = m_sweep.c;
- m_sweep.localCenter = massData->center;
+ m_sweep.localCenter = massData->center;
m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
// Update center of mass velocity.
@@ -414,7 +385,7 @@ bool b2Body::ShouldCollide(const b2Body* other) const
return true;
}
-void b2Body::SetTransform(const b2Vec2& position, float32 angle)
+void b2Body::SetTransform(const b2Vec2& position, qreal angle)
{
b2Assert(m_world->IsLocked() == false);
if (m_world->IsLocked() == true)
@@ -422,27 +393,26 @@ void b2Body::SetTransform(const b2Vec2& position, float32 angle)
return;
}
- m_xf.q.Set(angle);
- m_xf.p = position;
+ m_xf.R.Set(angle);
+ m_xf.position = position;
- m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
- m_sweep.a = angle;
-
- m_sweep.c0 = m_sweep.c;
- m_sweep.a0 = angle;
+ m_sweep.c0 = m_sweep.c = b2Mul(m_xf, m_sweep.localCenter);
+ m_sweep.a0 = m_sweep.a = 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);
}
+
+ m_world->m_contactManager.FindNewContacts();
}
void b2Body::SynchronizeFixtures()
{
b2Transform xf1;
- xf1.q.Set(m_sweep.a0);
- xf1.p = m_sweep.c0 - b2Mul(xf1.q, m_sweep.localCenter);
+ xf1.R.Set(m_sweep.a0);
+ xf1.position = m_sweep.c0 - b2Mul(xf1.R, m_sweep.localCenter);
b2BroadPhase* broadPhase = &m_world->m_contactManager.m_broadPhase;
for (b2Fixture* f = m_fixtureList; f; f = f->m_next)
@@ -496,54 +466,3 @@ void b2Body::SetActive(bool flag)
}
}
-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");
-}
diff --git a/external/Box2D/Dynamics/b2Body.h b/external/Box2D/Dynamics/b2Body.h
index 24bae64..1bc53cd 100644
--- a/external/Box2D/Dynamics/b2Body.h
+++ b/external/Box2D/Dynamics/b2Body.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -26,7 +26,6 @@
class b2Fixture;
class b2Joint;
class b2Contact;
-class b2Controller;
class b2World;
struct b2FixtureDef;
struct b2JointEdge;
@@ -66,7 +65,7 @@ struct b2BodyDef
bullet = false;
type = b2_staticBody;
active = true;
- gravityScale = 1.0f;
+ inertiaScale = 1.0f;
}
/// The body type: static, kinematic, or dynamic.
@@ -78,23 +77,23 @@ struct b2BodyDef
b2Vec2 position;
/// The world angle of the body in radians.
- float32 angle;
+ qreal angle;
/// The linear velocity of the body's origin in world co-ordinates.
b2Vec2 linearVelocity;
/// The angular velocity of the body.
- float32 angularVelocity;
+ qreal 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.
- float32 linearDamping;
+ qreal 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.
- float32 angularDamping;
+ qreal angularDamping;
/// Set this flag to false if this body should never fall asleep. Note that
/// this increases CPU usage.
@@ -118,8 +117,8 @@ struct b2BodyDef
/// Use this to store application specific body data.
void* userData;
- /// Scale the gravity applied to this body.
- float32 gravityScale;
+ /// Experimental: scales the inertia tensor.
+ qreal inertiaScale;
};
/// A rigid body. These are created via b2World::CreateBody.
@@ -142,7 +141,7 @@ public:
/// @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);
+ b2Fixture* CreateFixture(const b2Shape* shape, qreal density);
/// Destroy a fixture. This removes the fixture from the broad-phase and
/// destroys all contacts associated with this fixture. This will
@@ -154,11 +153,11 @@ public:
void DestroyFixture(b2Fixture* fixture);
/// Set the position of the body's origin and rotation.
+ /// This breaks any contacts and wakes the other bodies.
/// 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);
+ void SetTransform(const b2Vec2& position, qreal angle);
/// Get the body transform for the body's origin.
/// @return the world transform of the body's origin.
@@ -170,7 +169,7 @@ public:
/// Get the angle in radians.
/// @return the current world rotation angle in radians.
- float32 GetAngle() const;
+ qreal GetAngle() const;
/// Get the world position of the center of mass.
const b2Vec2& GetWorldCenter() const;
@@ -184,56 +183,47 @@ public:
/// Get the linear velocity of the center of mass.
/// @return the linear velocity of the center of mass.
- const b2Vec2& GetLinearVelocity() const;
+ b2Vec2 GetLinearVelocity() const;
/// Set the angular velocity.
/// @param omega the new angular velocity in radians/second.
- void SetAngularVelocity(float32 omega);
+ void SetAngularVelocity(qreal omega);
/// Get the angular velocity.
/// @return the angular velocity in radians/second.
- float32 GetAngularVelocity() const;
+ qreal 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);
+ void ApplyForce(const b2Vec2& force, const b2Vec2& point);
/// Apply a torque. This affects the angular velocity
/// without affecting the linear velocity of the center of mass.
/// This wakes up the body.
/// @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);
+ void ApplyTorque(qreal torque);
/// 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);
+ void ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point);
/// 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);
+ void ApplyAngularImpulse(qreal impulse);
/// Get the total mass of the body.
/// @return the mass, usually in kilograms (kg).
- float32 GetMass() const;
+ qreal 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;
+ qreal GetInertia() const;
/// Get the mass data of the body.
/// @return a struct containing the mass, inertia and center of the body.
@@ -282,22 +272,16 @@ public:
b2Vec2 GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint) const;
/// Get the linear damping of the body.
- float32 GetLinearDamping() const;
+ qreal GetLinearDamping() const;
/// Set the linear damping of the body.
- void SetLinearDamping(float32 linearDamping);
+ void SetLinearDamping(qreal linearDamping);
/// Get the angular damping of the body.
- float32 GetAngularDamping() const;
+ qreal 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);
+ void SetAngularDamping(qreal angularDamping);
/// Set the type of this body. This may alter the mass and velocity.
void SetType(b2BodyType type);
@@ -320,11 +304,11 @@ public:
/// 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.
+ /// @param flag set to true to put body to sleep, false to wake it.
void SetAwake(bool flag);
/// Get the sleeping state of this body.
- /// @return true if the body is awake.
+ /// @return true if the body is sleeping.
bool IsAwake() const;
/// Set the active state of the body. An inactive body is not
@@ -380,28 +364,23 @@ public:
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 b2LineJoint;
friend class b2MouseJoint;
friend class b2PrismaticJoint;
friend class b2PulleyJoint;
friend class b2RevoluteJoint;
- friend class b2RopeJoint;
friend class b2WeldJoint;
- friend class b2WheelJoint;
+ friend class b2FrictionJoint;
+ friend class b2RopeJoint;
// m_flags
enum
@@ -425,7 +404,7 @@ private:
// It may lie, depending on the collideConnected flag.
bool ShouldCollide(const b2Body* other) const;
- void Advance(float32 t);
+ void Advance(qreal t);
b2BodyType m_type;
@@ -437,10 +416,10 @@ private:
b2Sweep m_sweep; // the swept motion for CCD
b2Vec2 m_linearVelocity;
- float32 m_angularVelocity;
+ qreal m_angularVelocity;
b2Vec2 m_force;
- float32 m_torque;
+ qreal m_torque;
b2World* m_world;
b2Body* m_prev;
@@ -452,16 +431,15 @@ private:
b2JointEdge* m_jointList;
b2ContactEdge* m_contactList;
- float32 m_mass, m_invMass;
+ qreal m_mass, m_invMass;
// Rotational inertia about the center of mass.
- float32 m_I, m_invI;
+ qreal m_I, m_invI;
- float32 m_linearDamping;
- float32 m_angularDamping;
- float32 m_gravityScale;
+ qreal m_linearDamping;
+ qreal m_angularDamping;
- float32 m_sleepTime;
+ qreal m_sleepTime;
void* m_userData;
};
@@ -478,10 +456,10 @@ inline const b2Transform& b2Body::GetTransform() const
inline const b2Vec2& b2Body::GetPosition() const
{
- return m_xf.p;
+ return m_xf.position;
}
-inline float32 b2Body::GetAngle() const
+inline qreal b2Body::GetAngle() const
{
return m_sweep.a;
}
@@ -511,12 +489,12 @@ inline void b2Body::SetLinearVelocity(const b2Vec2& v)
m_linearVelocity = v;
}
-inline const b2Vec2& b2Body::GetLinearVelocity() const
+inline b2Vec2 b2Body::GetLinearVelocity() const
{
return m_linearVelocity;
}
-inline void b2Body::SetAngularVelocity(float32 w)
+inline void b2Body::SetAngularVelocity(qreal w)
{
if (m_type == b2_staticBody)
{
@@ -531,17 +509,17 @@ inline void b2Body::SetAngularVelocity(float32 w)
m_angularVelocity = w;
}
-inline float32 b2Body::GetAngularVelocity() const
+inline qreal b2Body::GetAngularVelocity() const
{
return m_angularVelocity;
}
-inline float32 b2Body::GetMass() const
+inline qreal b2Body::GetMass() const
{
return m_mass;
}
-inline float32 b2Body::GetInertia() const
+inline qreal b2Body::GetInertia() const
{
return m_I + m_mass * b2Dot(m_sweep.localCenter, m_sweep.localCenter);
}
@@ -560,7 +538,7 @@ inline b2Vec2 b2Body::GetWorldPoint(const b2Vec2& localPoint) const
inline b2Vec2 b2Body::GetWorldVector(const b2Vec2& localVector) const
{
- return b2Mul(m_xf.q, localVector);
+ return b2Mul(m_xf.R, localVector);
}
inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
@@ -570,7 +548,7 @@ inline b2Vec2 b2Body::GetLocalPoint(const b2Vec2& worldPoint) const
inline b2Vec2 b2Body::GetLocalVector(const b2Vec2& worldVector) const
{
- return b2MulT(m_xf.q, worldVector);
+ return b2MulT(m_xf.R, worldVector);
}
inline b2Vec2 b2Body::GetLinearVelocityFromWorldPoint(const b2Vec2& worldPoint) const
@@ -583,36 +561,26 @@ inline b2Vec2 b2Body::GetLinearVelocityFromLocalPoint(const b2Vec2& localPoint)
return GetLinearVelocityFromWorldPoint(GetWorldPoint(localPoint));
}
-inline float32 b2Body::GetLinearDamping() const
+inline qreal b2Body::GetLinearDamping() const
{
return m_linearDamping;
}
-inline void b2Body::SetLinearDamping(float32 linearDamping)
+inline void b2Body::SetLinearDamping(qreal linearDamping)
{
m_linearDamping = linearDamping;
}
-inline float32 b2Body::GetAngularDamping() const
+inline qreal b2Body::GetAngularDamping() const
{
return m_angularDamping;
}
-inline void b2Body::SetAngularDamping(float32 angularDamping)
+inline void b2Body::SetAngularDamping(qreal 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)
@@ -661,6 +629,20 @@ inline bool b2Body::IsActive() const
return (m_flags & e_activeFlag) == e_activeFlag;
}
+inline void b2Body::SetFixedRotation(bool flag)
+{
+ if (flag)
+ {
+ m_flags |= e_fixedRotationFlag;
+ }
+ else
+ {
+ m_flags &= ~e_fixedRotationFlag;
+ }
+
+ ResetMassData();
+}
+
inline bool b2Body::IsFixedRotation() const
{
return (m_flags & e_fixedRotationFlag) == e_fixedRotationFlag;
@@ -734,117 +716,79 @@ inline void* b2Body::GetUserData() const
return m_userData;
}
-inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point, bool wake)
+inline void b2Body::ApplyForce(const b2Vec2& force, const b2Vec2& point)
{
if (m_type != b2_dynamicBody)
{
return;
}
- if (wake && (m_flags & e_awakeFlag) == 0)
+ if (IsAwake() == false)
{
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;
- }
+ m_force += force;
+ m_torque += b2Cross(point - m_sweep.c, force);
}
-inline void b2Body::ApplyTorque(float32 torque, bool wake)
+inline void b2Body::ApplyTorque(qreal torque)
{
if (m_type != b2_dynamicBody)
{
return;
}
- if (wake && (m_flags & e_awakeFlag) == 0)
+ if (IsAwake() == false)
{
SetAwake(true);
}
- // Don't accumulate a force if the body is sleeping
- if (m_flags & e_awakeFlag)
- {
- m_torque += torque;
- }
+ m_torque += torque;
}
-inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point, bool wake)
+inline void b2Body::ApplyLinearImpulse(const b2Vec2& impulse, const b2Vec2& point)
{
if (m_type != b2_dynamicBody)
{
return;
}
- if (wake && (m_flags & e_awakeFlag) == 0)
+ if (IsAwake() == false)
{
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);
- }
+ m_linearVelocity += m_invMass * impulse;
+ m_angularVelocity += m_invI * b2Cross(point - m_sweep.c, impulse);
}
-inline void b2Body::ApplyAngularImpulse(float32 impulse, bool wake)
+inline void b2Body::ApplyAngularImpulse(qreal impulse)
{
if (m_type != b2_dynamicBody)
{
return;
}
- if (wake && (m_flags & e_awakeFlag) == 0)
+ if (IsAwake() == false)
{
SetAwake(true);
}
-
- // Don't accumulate velocity if the body is sleeping
- if (m_flags & e_awakeFlag)
- {
- m_angularVelocity += m_invI * impulse;
- }
+ 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);
+ m_xf.R.Set(m_sweep.a);
+ m_xf.position = m_sweep.c - b2Mul(m_xf.R, m_sweep.localCenter);
}
-inline void b2Body::Advance(float32 alpha)
+inline void b2Body::Advance(qreal alpha)
{
- // Advance to the new safe time. This doesn't sync the broad-phase.
+ // Advance to the new safe time.
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);
+ SynchronizeTransform();
}
inline b2World* b2Body::GetWorld()
diff --git a/external/Box2D/Dynamics/b2ContactManager.cpp b/external/Box2D/Dynamics/b2ContactManager.cpp
index d8a189b..a9eb15b 100644
--- a/external/Box2D/Dynamics/b2ContactManager.cpp
+++ b/external/Box2D/Dynamics/b2ContactManager.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -114,7 +114,13 @@ void b2ContactManager::Collide()
int32 indexB = c->GetChildIndexB();
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
-
+
+ if (bodyA->IsAwake() == false && bodyB->IsAwake() == false)
+ {
+ c = c->GetNext();
+ continue;
+ }
+
// Is this contact flagged for filtering?
if (c->m_flags & b2Contact::e_filterFlag)
{
@@ -140,16 +146,6 @@ void b2ContactManager::Collide()
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);
@@ -194,8 +190,6 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
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)
@@ -237,10 +231,6 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
// Call the factory.
b2Contact* c = b2Contact::Create(fixtureA, indexA, fixtureB, indexB, m_allocator);
- if (c == NULL)
- {
- return;
- }
// Contact creation may swap fixtures.
fixtureA = c->GetFixtureA();
@@ -285,12 +275,5 @@ void b2ContactManager::AddPair(void* proxyUserDataA, void* proxyUserDataB)
}
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;
}
diff --git a/external/Box2D/Dynamics/b2ContactManager.h b/external/Box2D/Dynamics/b2ContactManager.h
index 6a1d072..0b3c74d 100644
--- a/external/Box2D/Dynamics/b2ContactManager.h
+++ b/external/Box2D/Dynamics/b2ContactManager.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
diff --git a/external/Box2D/Dynamics/b2Fixture.cpp b/external/Box2D/Dynamics/b2Fixture.cpp
index fb17f0a..64c50ac 100644
--- a/external/Box2D/Dynamics/b2Fixture.cpp
+++ b/external/Box2D/Dynamics/b2Fixture.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -18,11 +18,10 @@
#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/Shapes/b2LoopShape.h>
#include <Box2D/Collision/b2BroadPhase.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Common/b2BlockAllocator.h>
@@ -103,11 +102,11 @@ void b2Fixture::Destroy(b2BlockAllocator* allocator)
}
break;
- case b2Shape::e_chain:
+ case b2Shape::e_loop:
{
- b2ChainShape* s = (b2ChainShape*)m_shape;
- s->~b2ChainShape();
- allocator->Free(s, sizeof(b2ChainShape));
+ b2LoopShape* s = (b2LoopShape*)m_shape;
+ s->~b2LoopShape();
+ allocator->Free(s, sizeof(b2LoopShape));
}
break;
@@ -167,7 +166,7 @@ void b2Fixture::Synchronize(b2BroadPhase* broadPhase, const b2Transform& transfo
proxy->aabb.Combine(aabb1, aabb2);
- b2Vec2 displacement = transform2.p - transform1.p;
+ b2Vec2 displacement = transform2.position - transform1.position;
broadPhase->MoveProxy(proxy->proxyId, proxy->aabb, displacement);
}
@@ -177,11 +176,6 @@ void b2Fixture::SetFilterData(const b2Filter& filter)
{
m_filter = filter;
- Refilter();
-}
-
-void b2Fixture::Refilter()
-{
if (m_body == NULL)
{
return;
@@ -201,103 +195,10 @@ void b2Fixture::Refilter()
edge = edge->next;
}
-
- b2World* world = m_body->GetWorld();
-
- if (world == NULL)
- {
- 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;
- }
+ 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);
-}
diff --git a/external/Box2D/Dynamics/b2Fixture.h b/external/Box2D/Dynamics/b2Fixture.h
index 753127f..5c4128d 100644
--- a/external/Box2D/Dynamics/b2Fixture.h
+++ b/external/Box2D/Dynamics/b2Fixture.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -31,13 +31,6 @@ 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;
@@ -63,9 +56,14 @@ struct b2FixtureDef
friction = 0.2f;
restitution = 0.0f;
density = 0.0f;
+ filter.categoryBits = 0x0001;
+ filter.maskBits = 0xFFFF;
+ filter.groupIndex = 0;
isSensor = false;
}
+ virtual ~b2FixtureDef() {}
+
/// The shape, this must be set. The shape will be cloned, so you
/// can create the shape on the stack.
const b2Shape* shape;
@@ -74,13 +72,13 @@ struct b2FixtureDef
void* userData;
/// The friction coefficient, usually in the range [0,1].
- float32 friction;
+ qreal friction;
/// The restitution (elasticity) usually in the range [0,1].
- float32 restitution;
+ qreal restitution;
/// The density, usually in kg/m^2.
- float32 density;
+ qreal density;
/// A sensor shape collects contact information but never generates a collision
/// response.
@@ -126,15 +124,11 @@ public:
/// 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 NULL if the fixture is not attached.
/// @return the parent body.
b2Body* GetBody();
@@ -168,33 +162,30 @@ public:
/// 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);
+ void SetDensity(qreal density);
/// Get the density of this fixture.
- float32 GetDensity() const;
+ qreal GetDensity() const;
/// Get the coefficient of friction.
- float32 GetFriction() const;
+ qreal GetFriction() const;
- /// Set the coefficient of friction. This will _not_ change the friction of
- /// existing contacts.
- void SetFriction(float32 friction);
+ /// Set the coefficient of friction. This will immediately update the mixed friction
+ /// on all associated contacts.
+ void SetFriction(qreal friction);
/// Get the coefficient of restitution.
- float32 GetRestitution() const;
+ qreal GetRestitution() const;
- /// Set the coefficient of restitution. This will _not_ change the restitution of
- /// existing contacts.
- void SetRestitution(float32 restitution);
+ /// Set the coefficient of restitution. This will immediately update the mixed restitution
+ /// on all associated contacts.
+ void SetRestitution(qreal 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;
@@ -215,15 +206,15 @@ protected:
void Synchronize(b2BroadPhase* broadPhase, const b2Transform& xf1, const b2Transform& xf2);
- float32 m_density;
+ qreal m_density;
b2Fixture* m_next;
b2Body* m_body;
b2Shape* m_shape;
- float32 m_friction;
- float32 m_restitution;
+ qreal m_friction;
+ qreal m_restitution;
b2FixtureProxy* m_proxies;
int32 m_proxyCount;
@@ -290,33 +281,33 @@ inline const b2Fixture* b2Fixture::GetNext() const
return m_next;
}
-inline void b2Fixture::SetDensity(float32 density)
+inline void b2Fixture::SetDensity(qreal density)
{
b2Assert(b2IsValid(density) && density >= 0.0f);
m_density = density;
}
-inline float32 b2Fixture::GetDensity() const
+inline qreal b2Fixture::GetDensity() const
{
return m_density;
}
-inline float32 b2Fixture::GetFriction() const
+inline qreal b2Fixture::GetFriction() const
{
return m_friction;
}
-inline void b2Fixture::SetFriction(float32 friction)
+inline void b2Fixture::SetFriction(qreal friction)
{
m_friction = friction;
}
-inline float32 b2Fixture::GetRestitution() const
+inline qreal b2Fixture::GetRestitution() const
{
return m_restitution;
}
-inline void b2Fixture::SetRestitution(float32 restitution)
+inline void b2Fixture::SetRestitution(qreal restitution)
{
m_restitution = restitution;
}
diff --git a/external/Box2D/Dynamics/b2Island.cpp b/external/Box2D/Dynamics/b2Island.cpp
index d46589d..859e225 100644
--- a/external/Box2D/Dynamics/b2Island.cpp
+++ b/external/Box2D/Dynamics/b2Island.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -25,7 +25,6 @@
#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
@@ -180,67 +179,59 @@ b2Island::~b2Island()
m_allocator->Free(m_bodies);
}
-void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
+void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
- b2Timer timer;
-
- float32 h = step.dt;
-
- // Integrate velocities and apply damping. Initialize the body state.
+ // Integrate velocities and apply damping.
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;
+ if (b->GetType() != b2_dynamicBody)
+ {
+ continue;
+ }
- // Store positions for continuous collision.
- b->m_sweep.c0 = b->m_sweep.c;
- b->m_sweep.a0 = b->m_sweep.a;
+ // Integrate velocities.
+ b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
+ b->m_angularVelocity += step.dt * 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
+ // Taylor expansion:
+ // v2 = (1.0f - c * dt) * v1
+ b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
+ b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
+ }
- if (b->m_type == b2_dynamicBody)
+ // Partition contacts so that contacts with static bodies are solved last.
+ int32 i1 = -1;
+ for (int32 i2 = 0; i2 < m_contactCount; ++i2)
+ {
+ b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
+ b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
+ b2Body* bodyA = fixtureA->GetBody();
+ b2Body* bodyB = fixtureB->GetBody();
+ bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
+ if (nonStatic)
{
- // 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);
+ ++i1;
+ b2Swap(m_contacts[i1], m_contacts[i2]);
}
-
- m_positions[i].c = c;
- m_positions[i].a = a;
- m_velocities[i].v = v;
- m_velocities[i].w = w;
}
- timer.Reset();
+ // Initialize velocity constraints.
+ b2ContactSolverDef solverDef;
+ solverDef.contacts = m_contacts;
+ solverDef.count = m_contactCount;
+ solverDef.allocator = m_allocator;
+ solverDef.impulseRatio = step.dtRatio;
+ solverDef.warmStarting = step.warmStarting;
- // Solver data
- b2SolverData solverData;
- solverData.step = step;
- solverData.positions = m_positions;
- solverData.velocities = m_velocities;
+ b2ContactSolver contactSolver(&solverDef);
- // 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)
@@ -250,103 +241,89 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
for (int32 i = 0; i < m_jointCount; ++i)
{
- m_joints[i]->InitVelocityConstraints(solverData);
+ m_joints[i]->InitVelocityConstraints(step);
}
- profile->solveInit = timer.GetMilliseconds();
-
- // Solve velocity constraints
- timer.Reset();
+ // Solve velocity constraints.
for (int32 i = 0; i < step.velocityIterations; ++i)
{
for (int32 j = 0; j < m_jointCount; ++j)
{
- m_joints[j]->SolveVelocityConstraints(solverData);
+ m_joints[j]->SolveVelocityConstraints(step);
}
contactSolver.SolveVelocityConstraints();
}
- // Store impulses for warm starting
+ // Post-solve (store impulses for warm starting).
contactSolver.StoreImpulses();
- profile->solveVelocity = timer.GetMilliseconds();
- // Integrate positions
+ // 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;
+ b2Body* b = m_bodies[i];
- // Check for large velocities
- b2Vec2 translation = h * v;
+ if (b->GetType() == b2_staticBody)
+ {
+ continue;
+ }
+
+ // Check for large velocities.
+ b2Vec2 translation = step.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
- float32 ratio = b2_maxTranslation / translation.Length();
- v *= ratio;
+ qreal ratio = b2_maxTranslation / translation.Length();
+ b->m_linearVelocity *= ratio;
}
- float32 rotation = h * w;
+ qreal rotation = step.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
- float32 ratio = b2_maxRotation / b2Abs(rotation);
- w *= ratio;
+ qreal ratio = b2_maxRotation / b2Abs(rotation);
+ b->m_angularVelocity *= ratio;
}
+ // Store positions for continuous collision.
+ b->m_sweep.c0 = b->m_sweep.c;
+ b->m_sweep.a0 = b->m_sweep.a;
+
// Integrate
- c += h * v;
- a += h * w;
+ b->m_sweep.c += step.dt * b->m_linearVelocity;
+ b->m_sweep.a += step.dt * b->m_angularVelocity;
- m_positions[i].c = c;
- m_positions[i].a = a;
- m_velocities[i].v = v;
- m_velocities[i].w = w;
+ // Compute new transform
+ b->SynchronizeTransform();
+
+ // Note: shapes are synchronized later.
}
- // Solve position constraints
- timer.Reset();
- bool positionSolved = false;
+ // Iterate over constraints.
for (int32 i = 0; i < step.positionIterations; ++i)
{
- bool contactsOkay = contactSolver.SolvePositionConstraints();
+ bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);
bool jointsOkay = true;
for (int32 i = 0; i < m_jointCount; ++i)
{
- bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
+ bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
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);
+ Report(contactSolver.m_constraints);
if (allowSleep)
{
- float32 minSleepTime = b2_maxFloat;
+ qreal minSleepTime = b2_maxFloat;
- const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
- const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
+ const qreal linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
+ const qreal angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;
for (int32 i = 0; i < m_bodyCount; ++i)
{
@@ -356,6 +333,12 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
continue;
}
+ if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
+ {
+ b->m_sleepTime = 0.0f;
+ minSleepTime = 0.0f;
+ }
+
if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
@@ -365,12 +348,12 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
}
else
{
- b->m_sleepTime += h;
+ b->m_sleepTime += step.dt;
minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
}
}
- if (minSleepTime >= b2_timeToSleep && positionSolved)
+ if (minSleepTime >= b2_timeToSleep)
{
for (int32 i = 0; i < m_bodyCount; ++i)
{
@@ -381,38 +364,30 @@ void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& g
}
}
-void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
+void b2Island::SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB)
{
- 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);
+ b2ContactSolverDef solverDef;
+ solverDef.contacts = m_contacts;
+ solverDef.count = m_contactCount;
+ solverDef.allocator = m_allocator;
+ solverDef.impulseRatio = subStep.dtRatio;
+ solverDef.warmStarting = subStep.warmStarting;
+ b2ContactSolver contactSolver(&solverDef);
// Solve position constraints.
+ const qreal k_toiBaumgarte = 0.75f;
for (int32 i = 0; i < subStep.positionIterations; ++i)
{
- bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
+ bool contactsOkay = contactSolver.SolveTOIPositionConstraints(k_toiBaumgarte, bodyA, bodyB);
if (contactsOkay)
{
break;
}
+
+ if (i == subStep.positionIterations - 1)
+ {
+ i += 0;
+ }
}
#if 0
@@ -449,10 +424,11 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd
#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;
+ for (int32 i = 0; i < m_bodyCount; ++i)
+ {
+ m_bodies[i]->m_sweep.a0 = m_bodies[i]->m_sweep.a;
+ m_bodies[i]->m_sweep.c0 = m_bodies[i]->m_sweep.c;
+ }
// No warm starting is needed for TOI events because warm
// starting impulses were applied in the discrete solver.
@@ -467,53 +443,51 @@ void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiInd
// Don't store the TOI contact forces for warm starting
// because they can be quite large.
- float32 h = subStep.dt;
-
- // Integrate positions
+ // 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;
+ b2Body* b = m_bodies[i];
+
+ if (b->GetType() == b2_staticBody)
+ {
+ continue;
+ }
- // Check for large velocities
- b2Vec2 translation = h * v;
+ // Check for large velocities.
+ b2Vec2 translation = subStep.dt * b->m_linearVelocity;
if (b2Dot(translation, translation) > b2_maxTranslationSquared)
{
- float32 ratio = b2_maxTranslation / translation.Length();
- v *= ratio;
+ translation.Normalize();
+ b->m_linearVelocity = (b2_maxTranslation * subStep.inv_dt) * translation;
}
- float32 rotation = h * w;
+ qreal rotation = subStep.dt * b->m_angularVelocity;
if (rotation * rotation > b2_maxRotationSquared)
{
- float32 ratio = b2_maxRotation / b2Abs(rotation);
- w *= ratio;
+ if (rotation < 0.0)
+ {
+ b->m_angularVelocity = -subStep.inv_dt * b2_maxRotation;
+ }
+ else
+ {
+ b->m_angularVelocity = subStep.inv_dt * b2_maxRotation;
+ }
}
// 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();
+ b->m_sweep.c += subStep.dt * b->m_linearVelocity;
+ b->m_sweep.a += subStep.dt * b->m_angularVelocity;
+
+ // Compute new transform
+ b->SynchronizeTransform();
+
+ // Note: shapes are synchronized later.
}
- Report(contactSolver.m_velocityConstraints);
+ Report(contactSolver.m_constraints);
}
-void b2Island::Report(const b2ContactVelocityConstraint* constraints)
+void b2Island::Report(const b2ContactConstraint* constraints)
{
if (m_listener == NULL)
{
@@ -524,14 +498,13 @@ void b2Island::Report(const b2ContactVelocityConstraint* constraints)
{
b2Contact* c = m_contacts[i];
- const b2ContactVelocityConstraint* vc = constraints + i;
+ const b2ContactConstraint* cc = constraints + i;
b2ContactImpulse impulse;
- impulse.count = vc->pointCount;
- for (int32 j = 0; j < vc->pointCount; ++j)
+ for (int32 j = 0; j < cc->pointCount; ++j)
{
- impulse.normalImpulses[j] = vc->points[j].normalImpulse;
- impulse.tangentImpulses[j] = vc->points[j].tangentImpulse;
+ impulse.normalImpulses[j] = cc->points[j].normalImpulse;
+ impulse.tangentImpulses[j] = cc->points[j].tangentImpulse;
}
m_listener->PostSolve(c, &impulse);
diff --git a/external/Box2D/Dynamics/b2Island.h b/external/Box2D/Dynamics/b2Island.h
index 4a814c4..cdd0fd9 100644
--- a/external/Box2D/Dynamics/b2Island.h
+++ b/external/Box2D/Dynamics/b2Island.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -27,8 +27,21 @@ class b2Contact;
class b2Joint;
class b2StackAllocator;
class b2ContactListener;
-struct b2ContactVelocityConstraint;
-struct b2Profile;
+struct b2ContactConstraint;
+
+/// This is an internal structure.
+struct b2Position
+{
+ b2Vec2 x;
+ qreal a;
+};
+
+/// This is an internal structure.
+struct b2Velocity
+{
+ b2Vec2 v;
+ qreal w;
+};
/// This is an internal class.
class b2Island
@@ -45,16 +58,15 @@ public:
m_jointCount = 0;
}
- void Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
+ void Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep);
- void SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB);
+ void SolveTOI(const b2TimeStep& subStep, const b2Body* bodyA, const b2Body* bodyB);
void Add(b2Body* body)
{
b2Assert(m_bodyCount < m_bodyCapacity);
body->m_islandIndex = m_bodyCount;
- m_bodies[m_bodyCount] = body;
- ++m_bodyCount;
+ m_bodies[m_bodyCount++] = body;
}
void Add(b2Contact* contact)
@@ -69,7 +81,7 @@ public:
m_joints[m_jointCount++] = joint;
}
- void Report(const b2ContactVelocityConstraint* constraints);
+ void Report(const b2ContactConstraint* constraints);
b2StackAllocator* m_allocator;
b2ContactListener* m_listener;
diff --git a/external/Box2D/Dynamics/b2TimeStep.h b/external/Box2D/Dynamics/b2TimeStep.h
index a372762..fc76b2d 100644
--- a/external/Box2D/Dynamics/b2TimeStep.h
+++ b/external/Box2D/Dynamics/b2TimeStep.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -19,52 +19,17 @@
#ifndef B2_TIME_STEP_H
#define B2_TIME_STEP_H
-#include <Box2D/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;
-};
+#include <Box2D/Common/b2Settings.h>
/// 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
+ qreal dt; // time step
+ qreal inv_dt; // inverse time step (0 if dt == 0).
+ qreal 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
diff --git a/external/Box2D/Dynamics/b2World.cpp b/external/Box2D/Dynamics/b2World.cpp
index c6cf295..a60dd55 100644
--- a/external/Box2D/Dynamics/b2World.cpp
+++ b/external/Box2D/Dynamics/b2World.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -27,17 +27,15 @@
#include <Box2D/Collision/b2BroadPhase.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
-#include <Box2D/Collision/Shapes/b2ChainShape.h>
+#include <Box2D/Collision/Shapes/b2LoopShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
-#include <Box2D/Common/b2Draw.h>
-#include <Box2D/Common/b2Timer.h>
#include <new>
-b2World::b2World(const b2Vec2& gravity)
+b2World::b2World(const b2Vec2& gravity, bool doSleep)
{
m_destructionListener = NULL;
- g_debugDraw = NULL;
+ m_debugDraw = NULL;
m_bodyList = NULL;
m_jointList = NULL;
@@ -51,7 +49,7 @@ b2World::b2World(const b2Vec2& gravity)
m_stepComplete = true;
- m_allowSleep = true;
+ m_allowSleep = doSleep;
m_gravity = gravity;
m_flags = e_clearForces;
@@ -59,8 +57,6 @@ b2World::b2World(const b2Vec2& gravity)
m_inv_dt0 = 0.0f;
m_contactManager.m_allocator = &m_blockAllocator;
-
- memset(&m_profile, 0, sizeof(b2Profile));
}
b2World::~b2World()
@@ -99,9 +95,9 @@ void b2World::SetContactListener(b2ContactListener* listener)
m_contactManager.m_contactListener = listener;
}
-void b2World::SetDebugDraw(b2Draw* debugDraw)
+void b2World::SetDebugDraw(b2DebugDraw* debugDraw)
{
- g_debugDraw = debugDraw;
+ m_debugDraw = debugDraw;
}
b2Body* b2World::CreateBody(const b2BodyDef* def)
@@ -364,31 +360,9 @@ void b2World::DestroyJoint(b2Joint* j)
}
}
-//
-void b2World::SetAllowSleeping(bool flag)
-{
- if (flag == m_allowSleep)
- {
- return;
- }
-
- m_allowSleep = flag;
- if (m_allowSleep == false)
- {
- for (b2Body* b = m_bodyList; b; b = b->m_next)
- {
- b->SetAwake(true);
- }
- }
-}
-
// Find islands, integrate and solve constraints, solve position constraints
void b2World::Solve(const b2TimeStep& step)
{
- m_profile.solveInit = 0.0f;
- m_profile.solveVelocity = 0.0f;
- m_profile.solvePosition = 0.0f;
-
// Size the island for the worst case.
b2Island island(m_bodyCount,
m_contactManager.m_contactCount,
@@ -527,11 +501,7 @@ void b2World::Solve(const b2TimeStep& step)
}
}
- b2Profile profile;
- island.Solve(&profile, step, m_gravity, m_allowSleep);
- m_profile.solveInit += profile.solveInit;
- m_profile.solveVelocity += profile.solveVelocity;
- m_profile.solvePosition += profile.solvePosition;
+ island.Solve(step, m_gravity, m_allowSleep);
// Post solve cleanup.
for (int32 i = 0; i < island.m_bodyCount; ++i)
@@ -547,30 +517,26 @@ void b2World::Solve(const b2TimeStep& step)
m_stackAllocator.Free(stack);
+ // Synchronize fixtures, check for out of range bodies.
+ for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
- b2Timer timer;
- // Synchronize fixtures, check for out of range bodies.
- for (b2Body* b = m_bodyList; b; b = b->GetNext())
+ // If a body was not in an island then it did not move.
+ if ((b->m_flags & b2Body::e_islandFlag) == 0)
{
- // If a body was not in an island then it did not move.
- if ((b->m_flags & b2Body::e_islandFlag) == 0)
- {
- continue;
- }
-
- if (b->GetType() == b2_staticBody)
- {
- continue;
- }
+ continue;
+ }
- // Update fixtures (for broad-phase).
- b->SynchronizeFixtures();
+ if (b->GetType() == b2_staticBody)
+ {
+ continue;
}
- // Look for new contacts.
- m_contactManager.FindNewContacts();
- m_profile.broadphase = timer.GetMilliseconds();
+ // Update fixtures (for broad-phase).
+ b->SynchronizeFixtures();
}
+
+ // Look for new contacts.
+ m_contactManager.FindNewContacts();
}
// Find TOI contacts and solve them.
@@ -600,7 +566,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
{
// Find the first TOI.
b2Contact* minContact = NULL;
- float32 minAlpha = 1.0f;
+ qreal minAlpha = 1.0f;
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->m_next)
{
@@ -616,7 +582,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
continue;
}
- float32 alpha = 1.0f;
+ qreal alpha = 1.0f;
if (c->m_flags & b2Contact::e_toiFlag)
{
// This contact has a valid cached TOI.
@@ -636,15 +602,15 @@ void b2World::SolveTOI(const b2TimeStep& step)
b2Body* bA = fA->GetBody();
b2Body* bB = fB->GetBody();
- b2BodyType typeA = bA->m_type;
- b2BodyType typeB = bB->m_type;
+ b2BodyType typeA = bA->GetType();
+ b2BodyType typeB = bB->GetType();
b2Assert(typeA == b2_dynamicBody || typeB == b2_dynamicBody);
- bool activeA = bA->IsAwake() && typeA != b2_staticBody;
- bool activeB = bB->IsAwake() && typeB != b2_staticBody;
+ bool awakeA = bA->IsAwake() && typeA != b2_staticBody;
+ bool awakeB = bB->IsAwake() && typeB != b2_staticBody;
- // Is at least one body active (awake and dynamic or kinematic)?
- if (activeA == false && activeB == false)
+ // Is at least one body awake?
+ if (awakeA == false && awakeB == false)
{
continue;
}
@@ -660,7 +626,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
// Compute the TOI for this contact.
// Put the sweeps onto the same time interval.
- float32 alpha0 = bA->m_sweep.alpha0;
+ qreal alpha0 = bA->m_sweep.alpha0;
if (bA->m_sweep.alpha0 < bB->m_sweep.alpha0)
{
@@ -690,7 +656,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
b2TimeOfImpact(&output, &input);
// Beta is the fraction of the remaining portion of the .
- float32 beta = output.t;
+ qreal beta = output.t;
if (output.state == b2TOIOutput::e_touching)
{
alpha = b2Min(alpha0 + (1.0f - alpha0) * beta, 1.0f);
@@ -768,18 +734,8 @@ void b2World::SolveTOI(const b2TimeStep& step)
b2Body* body = bodies[i];
if (body->m_type == b2_dynamicBody)
{
- for (b2ContactEdge* ce = body->m_contactList; ce; ce = ce->next)
+ for (b2ContactEdge* ce = body->m_contactList; ce && island.m_bodyCount < b2_maxTOIContacts; ce = ce->next)
{
- if (island.m_bodyCount == island.m_bodyCapacity)
- {
- break;
- }
-
- if (island.m_contactCount == island.m_contactCapacity)
- {
- break;
- }
-
b2Contact* contact = ce->contact;
// Has this contact already been added to the island?
@@ -860,7 +816,7 @@ void b2World::SolveTOI(const b2TimeStep& step)
subStep.positionIterations = 20;
subStep.velocityIterations = step.velocityIterations;
subStep.warmStarting = false;
- island.SolveTOI(subStep, bA->m_islandIndex, bB->m_islandIndex);
+ island.SolveTOI(subStep, bA, bB);
// Reset island flags and synchronize broad-phase proxies.
for (int32 i = 0; i < island.m_bodyCount; ++i)
@@ -894,10 +850,8 @@ void b2World::SolveTOI(const b2TimeStep& step)
}
}
-void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIterations)
+void b2World::Step(qreal dt, int32 velocityIterations, int32 positionIterations)
{
- b2Timer stepTimer;
-
// If new fixtures were added, we need to find the new contacts.
if (m_flags & e_newFixture)
{
@@ -923,28 +877,20 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration
step.dtRatio = m_inv_dt0 * dt;
step.warmStarting = m_warmStarting;
-
+
// Update contacts. This is where some contacts are destroyed.
- {
- b2Timer timer;
- m_contactManager.Collide();
- m_profile.collide = timer.GetMilliseconds();
- }
+ m_contactManager.Collide();
// Integrate velocities, solve velocity constraints, and integrate positions.
if (m_stepComplete && step.dt > 0.0f)
{
- b2Timer timer;
Solve(step);
- m_profile.solve = timer.GetMilliseconds();
}
// Handle TOI events.
if (m_continuousPhysics && step.dt > 0.0f)
{
- b2Timer timer;
SolveTOI(step);
- m_profile.solveTOI = timer.GetMilliseconds();
}
if (step.dt > 0.0f)
@@ -958,8 +904,6 @@ void b2World::Step(float32 dt, int32 velocityIterations, int32 positionIteration
}
m_flags &= ~e_locked;
-
- m_profile.step = stepTimer.GetMilliseconds();
}
void b2World::ClearForces()
@@ -993,7 +937,7 @@ void b2World::QueryAABB(b2QueryCallback* callback, const b2AABB& aabb) const
struct b2WorldRayCastWrapper
{
- float32 RayCastCallback(const b2RayCastInput& input, int32 proxyId)
+ qreal RayCastCallback(const b2RayCastInput& input, int32 proxyId)
{
void* userData = broadPhase->GetUserData(proxyId);
b2FixtureProxy* proxy = (b2FixtureProxy*)userData;
@@ -1004,7 +948,7 @@ struct b2WorldRayCastWrapper
if (hit)
{
- float32 fraction = output.fraction;
+ qreal fraction = output.fraction;
b2Vec2 point = (1.0f - fraction) * input.p1 + fraction * input.p2;
return callback->ReportFixture(fixture, point, output.normal, fraction);
}
@@ -1037,10 +981,10 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
b2CircleShape* circle = (b2CircleShape*)fixture->GetShape();
b2Vec2 center = b2Mul(xf, circle->m_p);
- float32 radius = circle->m_radius;
- b2Vec2 axis = b2Mul(xf.q, b2Vec2(1.0f, 0.0f));
+ qreal radius = circle->m_radius;
+ b2Vec2 axis = xf.R.col1;
- g_debugDraw->DrawSolidCircle(center, radius, axis, color);
+ m_debugDraw->DrawSolidCircle(center, radius, axis, color);
}
break;
@@ -1049,22 +993,21 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
b2EdgeShape* edge = (b2EdgeShape*)fixture->GetShape();
b2Vec2 v1 = b2Mul(xf, edge->m_vertex1);
b2Vec2 v2 = b2Mul(xf, edge->m_vertex2);
- g_debugDraw->DrawSegment(v1, v2, color);
+ m_debugDraw->DrawSegment(v1, v2, color);
}
break;
- case b2Shape::e_chain:
+ case b2Shape::e_loop:
{
- b2ChainShape* chain = (b2ChainShape*)fixture->GetShape();
- int32 count = chain->m_count;
- const b2Vec2* vertices = chain->m_vertices;
+ b2LoopShape* loop = (b2LoopShape*)fixture->GetShape();
+ int32 count = loop->GetCount();
+ const b2Vec2* vertices = loop->GetVertices();
- b2Vec2 v1 = b2Mul(xf, vertices[0]);
- for (int32 i = 1; i < count; ++i)
+ b2Vec2 v1 = b2Mul(xf, vertices[count - 1]);
+ for (int32 i = 0; i < count; ++i)
{
b2Vec2 v2 = b2Mul(xf, vertices[i]);
- g_debugDraw->DrawSegment(v1, v2, color);
- g_debugDraw->DrawCircle(v1, 0.05f, color);
+ m_debugDraw->DrawSegment(v1, v2, color);
v1 = v2;
}
}
@@ -1073,7 +1016,7 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
case b2Shape::e_polygon:
{
b2PolygonShape* poly = (b2PolygonShape*)fixture->GetShape();
- int32 vertexCount = poly->m_count;
+ int32 vertexCount = poly->m_vertexCount;
b2Assert(vertexCount <= b2_maxPolygonVertices);
b2Vec2 vertices[b2_maxPolygonVertices];
@@ -1082,12 +1025,9 @@ void b2World::DrawShape(b2Fixture* fixture, const b2Transform& xf, const b2Color
vertices[i] = b2Mul(xf, poly->m_vertices[i]);
}
- g_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
+ m_debugDraw->DrawSolidPolygon(vertices, vertexCount, color);
}
break;
-
- default:
- break;
}
}
@@ -1097,8 +1037,8 @@ void b2World::DrawJoint(b2Joint* joint)
b2Body* bodyB = joint->GetBodyB();
const b2Transform& xf1 = bodyA->GetTransform();
const b2Transform& xf2 = bodyB->GetTransform();
- b2Vec2 x1 = xf1.p;
- b2Vec2 x2 = xf2.p;
+ b2Vec2 x1 = xf1.position;
+ b2Vec2 x2 = xf2.position;
b2Vec2 p1 = joint->GetAnchorA();
b2Vec2 p2 = joint->GetAnchorB();
@@ -1107,7 +1047,7 @@ void b2World::DrawJoint(b2Joint* joint)
switch (joint->GetType())
{
case e_distanceJoint:
- g_debugDraw->DrawSegment(p1, p2, color);
+ m_debugDraw->DrawSegment(p1, p2, color);
break;
case e_pulleyJoint:
@@ -1115,9 +1055,9 @@ void b2World::DrawJoint(b2Joint* joint)
b2PulleyJoint* pulley = (b2PulleyJoint*)joint;
b2Vec2 s1 = pulley->GetGroundAnchorA();
b2Vec2 s2 = pulley->GetGroundAnchorB();
- g_debugDraw->DrawSegment(s1, p1, color);
- g_debugDraw->DrawSegment(s2, p2, color);
- g_debugDraw->DrawSegment(s1, s2, color);
+ m_debugDraw->DrawSegment(s1, p1, color);
+ m_debugDraw->DrawSegment(s2, p2, color);
+ m_debugDraw->DrawSegment(s1, s2, color);
}
break;
@@ -1126,22 +1066,22 @@ void b2World::DrawJoint(b2Joint* joint)
break;
default:
- g_debugDraw->DrawSegment(x1, p1, color);
- g_debugDraw->DrawSegment(p1, p2, color);
- g_debugDraw->DrawSegment(x2, p2, color);
+ m_debugDraw->DrawSegment(x1, p1, color);
+ m_debugDraw->DrawSegment(p1, p2, color);
+ m_debugDraw->DrawSegment(x2, p2, color);
}
}
void b2World::DrawDebugData()
{
- if (g_debugDraw == NULL)
+ if (m_debugDraw == NULL)
{
return;
}
- uint32 flags = g_debugDraw->GetFlags();
+ uint32 flags = m_debugDraw->GetFlags();
- if (flags & b2Draw::e_shapeBit)
+ if (flags & b2DebugDraw::e_shapeBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
@@ -1172,7 +1112,7 @@ void b2World::DrawDebugData()
}
}
- if (flags & b2Draw::e_jointBit)
+ if (flags & b2DebugDraw::e_jointBit)
{
for (b2Joint* j = m_jointList; j; j = j->GetNext())
{
@@ -1180,7 +1120,7 @@ void b2World::DrawDebugData()
}
}
- if (flags & b2Draw::e_pairBit)
+ if (flags & b2DebugDraw::e_pairBit)
{
b2Color color(0.3f, 0.9f, 0.9f);
for (b2Contact* c = m_contactManager.m_contactList; c; c = c->GetNext())
@@ -1191,11 +1131,11 @@ void b2World::DrawDebugData()
//b2Vec2 cA = fixtureA->GetAABB().GetCenter();
//b2Vec2 cB = fixtureB->GetAABB().GetCenter();
- //g_debugDraw->DrawSegment(cA, cB, color);
+ //m_debugDraw->DrawSegment(cA, cB, color);
}
}
- if (flags & b2Draw::e_aabbBit)
+ if (flags & b2DebugDraw::e_aabbBit)
{
b2Color color(0.9f, 0.3f, 0.9f);
b2BroadPhase* bp = &m_contactManager.m_broadPhase;
@@ -1219,19 +1159,19 @@ void b2World::DrawDebugData()
vs[2].Set(aabb.upperBound.x, aabb.upperBound.y);
vs[3].Set(aabb.lowerBound.x, aabb.upperBound.y);
- g_debugDraw->DrawPolygon(vs, 4, color);
+ m_debugDraw->DrawPolygon(vs, 4, color);
}
}
}
}
- if (flags & b2Draw::e_centerOfMassBit)
+ if (flags & b2DebugDraw::e_centerOfMassBit)
{
for (b2Body* b = m_bodyList; b; b = b->GetNext())
{
b2Transform xf = b->GetTransform();
- xf.p = b->GetWorldCenter();
- g_debugDraw->DrawTransform(xf);
+ xf.position = b->GetWorldCenter();
+ m_debugDraw->DrawTransform(xf);
}
}
}
@@ -1240,100 +1180,3 @@ int32 b2World::GetProxyCount() const
{
return m_contactManager.m_broadPhase.GetProxyCount();
}
-
-int32 b2World::GetTreeHeight() const
-{
- return m_contactManager.m_broadPhase.GetTreeHeight();
-}
-
-int32 b2World::GetTreeBalance() const
-{
- return m_contactManager.m_broadPhase.GetTreeBalance();
-}
-
-float32 b2World::GetTreeQuality() const
-{
- return m_contactManager.m_broadPhase.GetTreeQuality();
-}
-
-void b2World::ShiftOrigin(const b2Vec2& newOrigin)
-{
- b2Assert((m_flags & e_locked) == 0);
- if ((m_flags & e_locked) == e_locked)
- {
- return;
- }
-
- for (b2Body* b = m_bodyList; b; b = b->m_next)
- {
- b->m_xf.p -= newOrigin;
- b->m_sweep.c0 -= newOrigin;
- b->m_sweep.c -= newOrigin;
- }
-
- for (b2Joint* j = m_jointList; j; j = j->m_next)
- {
- j->ShiftOrigin(newOrigin);
- }
-
- m_contactManager.m_broadPhase.ShiftOrigin(newOrigin);
-}
-
-void b2World::Dump()
-{
- if ((m_flags & e_locked) == e_locked)
- {
- return;
- }
-
- b2Log("b2Vec2 g(%.15lef, %.15lef);\n", m_gravity.x, m_gravity.y);
- b2Log("m_world->SetGravity(g);\n");
-
- b2Log("b2Body** bodies = (b2Body**)b2Alloc(%d * sizeof(b2Body*));\n", m_bodyCount);
- b2Log("b2Joint** joints = (b2Joint**)b2Alloc(%d * sizeof(b2Joint*));\n", m_jointCount);
- int32 i = 0;
- for (b2Body* b = m_bodyList; b; b = b->m_next)
- {
- b->m_islandIndex = i;
- b->Dump();
- ++i;
- }
-
- i = 0;
- for (b2Joint* j = m_jointList; j; j = j->m_next)
- {
- j->m_index = i;
- ++i;
- }
-
- // First pass on joints, skip gear joints.
- for (b2Joint* j = m_jointList; j; j = j->m_next)
- {
- if (j->m_type == e_gearJoint)
- {
- continue;
- }
-
- b2Log("{\n");
- j->Dump();
- b2Log("}\n");
- }
-
- // Second pass on joints, only gear joints.
- for (b2Joint* j = m_jointList; j; j = j->m_next)
- {
- if (j->m_type != e_gearJoint)
- {
- continue;
- }
-
- b2Log("{\n");
- j->Dump();
- b2Log("}\n");
- }
-
- b2Log("b2Free(joints);\n");
- b2Log("b2Free(bodies);\n");
- b2Log("joints = NULL;\n");
- b2Log("bodies = NULL;\n");
-}
diff --git a/external/Box2D/Dynamics/b2World.h b/external/Box2D/Dynamics/b2World.h
index 3968cc7..aedcc5e 100644
--- a/external/Box2D/Dynamics/b2World.h
+++ b/external/Box2D/Dynamics/b2World.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2011 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,14 +24,12 @@
#include <Box2D/Common/b2StackAllocator.h>
#include <Box2D/Dynamics/b2ContactManager.h>
#include <Box2D/Dynamics/b2WorldCallbacks.h>
-#include <Box2D/Dynamics/b2TimeStep.h>
struct b2AABB;
struct b2BodyDef;
-struct b2Color;
struct b2JointDef;
+struct b2TimeStep;
class b2Body;
-class b2Draw;
class b2Fixture;
class b2Joint;
@@ -43,7 +41,8 @@ class b2World
public:
/// Construct a world object.
/// @param gravity the world gravity vector.
- b2World(const b2Vec2& gravity);
+ /// @param doSleep improve performance by not simulating inactive bodies.
+ b2World(const b2Vec2& gravity, bool doSleep);
/// Destruct the world. All physics entities are destroyed and all heap memory is released.
~b2World();
@@ -64,7 +63,7 @@ public:
/// 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);
+ void SetDebugDraw(b2DebugDraw* debugDraw);
/// Create a rigid body given a definition. No reference to the definition
/// is retained.
@@ -91,7 +90,7 @@ public:
/// @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,
+ void Step( qreal timeStep,
int32 velocityIterations,
int32 positionIterations);
@@ -104,7 +103,7 @@ public:
/// @see SetAutoClearForces
void ClearForces();
- /// Call this to draw shapes and other debug draw data. This is intentionally non-const.
+ /// Call this to draw shapes and other debug draw data.
void DrawDebugData();
/// Query the world for all fixtures that potentially overlap the
@@ -136,26 +135,18 @@ public:
/// Get the world contact list. With the returned contact, use b2Contact::GetNext to get
/// the next contact in the world list. A NULL 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.
+ /// @warning contacts are
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;
@@ -169,16 +160,6 @@ public:
/// 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);
@@ -194,21 +175,9 @@ public:
/// 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
@@ -220,7 +189,6 @@ private:
};
friend class b2Body;
- friend class b2Fixture;
friend class b2ContactManager;
friend class b2Controller;
@@ -247,11 +215,11 @@ private:
bool m_allowSleep;
b2DestructionListener* m_destructionListener;
- b2Draw* g_debugDraw;
+ b2DebugDraw* m_debugDraw;
// This is used to compute the time step ratio to
// support a variable time step.
- float32 m_inv_dt0;
+ qreal m_inv_dt0;
// These are for debugging the solver.
bool m_warmStarting;
@@ -259,8 +227,6 @@ private:
bool m_subStepping;
bool m_stepComplete;
-
- b2Profile m_profile;
};
inline b2Body* b2World::GetBodyList()
@@ -346,9 +312,4 @@ inline const b2ContactManager& b2World::GetContactManager() const
return m_contactManager;
}
-inline const b2Profile& b2World::GetProfile() const
-{
- return m_profile;
-}
-
#endif
diff --git a/external/Box2D/Dynamics/b2WorldCallbacks.cpp b/external/Box2D/Dynamics/b2WorldCallbacks.cpp
index 0f4b9c5..2cb9710 100644
--- a/external/Box2D/Dynamics/b2WorldCallbacks.cpp
+++ b/external/Box2D/Dynamics/b2WorldCallbacks.cpp
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -34,3 +34,28 @@ bool b2ContactFilter::ShouldCollide(b2Fixture* fixtureA, b2Fixture* fixtureB)
bool collide = (filterA.maskBits & filterB.categoryBits) != 0 && (filterA.categoryBits & filterB.maskBits) != 0;
return collide;
}
+
+b2DebugDraw::b2DebugDraw()
+{
+ m_drawFlags = 0;
+}
+
+void b2DebugDraw::SetFlags(uint32 flags)
+{
+ m_drawFlags = flags;
+}
+
+uint32 b2DebugDraw::GetFlags() const
+{
+ return m_drawFlags;
+}
+
+void b2DebugDraw::AppendFlags(uint32 flags)
+{
+ m_drawFlags |= flags;
+}
+
+void b2DebugDraw::ClearFlags(uint32 flags)
+{
+ m_drawFlags &= ~flags;
+}
diff --git a/external/Box2D/Dynamics/b2WorldCallbacks.h b/external/Box2D/Dynamics/b2WorldCallbacks.h
index 82ffc02..709ff2e 100644
--- a/external/Box2D/Dynamics/b2WorldCallbacks.h
+++ b/external/Box2D/Dynamics/b2WorldCallbacks.h
@@ -1,5 +1,5 @@
/*
-* Copyright (c) 2006-2009 Erin Catto http://www.box2d.org
+* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
@@ -24,7 +24,6 @@
struct b2Vec2;
struct b2Transform;
class b2Fixture;
-class b2Body;
class b2Joint;
class b2Contact;
struct b2ContactResult;
@@ -64,9 +63,8 @@ public:
/// match up one-to-one with the contact points in b2Manifold.
struct b2ContactImpulse
{
- float32 normalImpulses[b2_maxManifoldPoints];
- float32 tangentImpulses[b2_maxManifoldPoints];
- int32 count;
+ qreal normalImpulses[b2_maxManifoldPoints];
+ qreal tangentImpulses[b2_maxManifoldPoints];
};
/// Implement this class to get contact information. You can use these results for
@@ -148,8 +146,70 @@ public:
/// @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;
+ virtual qreal ReportFixture( b2Fixture* fixture, const b2Vec2& point,
+ const b2Vec2& normal, qreal fraction) = 0;
+};
+
+/// Color for debug drawing. Each value has the range [0,1].
+struct b2Color
+{
+ b2Color() {}
+ b2Color(qreal r, qreal g, qreal b) : r(r), g(g), b(b) {}
+ void Set(qreal ri, qreal gi, qreal bi) { r = ri; g = gi; b = bi; }
+ qreal r, g, b;
+};
+
+/// Implement and register this class with a b2World to provide debug drawing of physics
+/// entities in your game.
+class b2DebugDraw
+{
+public:
+ b2DebugDraw();
+
+ virtual ~b2DebugDraw() {}
+
+ 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, qreal radius, const b2Color& color) = 0;
+
+ /// Draw a solid circle.
+ virtual void DrawSolidCircle(const b2Vec2& center, qreal 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;
+
+protected:
+ uint32 m_drawFlags;
};
#endif
diff --git a/external/Box2D/Rope/b2Rope.cpp b/external/Box2D/Rope/b2Rope.cpp
deleted file mode 100644
index 7af3a6c..0000000
--- a/external/Box2D/Rope/b2Rope.cpp
+++ /dev/null
@@ -1,259 +0,0 @@
-/*
-* 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/Rope/b2Rope.h>
-#include <Box2D/Common/b2Draw.h>
-
-b2Rope::b2Rope()
-{
- m_count = 0;
- m_ps = NULL;
- m_p0s = NULL;
- m_vs = NULL;
- m_ims = NULL;
- m_Ls = NULL;
- m_as = NULL;
- m_gravity.SetZero();
- m_k2 = 1.0f;
- m_k3 = 0.1f;
-}
-
-b2Rope::~b2Rope()
-{
- b2Free(m_ps);
- b2Free(m_p0s);
- b2Free(m_vs);
- b2Free(m_ims);
- b2Free(m_Ls);
- b2Free(m_as);
-}
-
-void b2Rope::Initialize(const b2RopeDef* def)
-{
- b2Assert(def->count >= 3);
- m_count = def->count;
- m_ps = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
- m_p0s = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
- m_vs = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
- m_ims = (float32*)b2Alloc(m_count * sizeof(float32));
-
- for (int32 i = 0; i < m_count; ++i)
- {
- m_ps[i] = def->vertices[i];
- m_p0s[i] = def->vertices[i];
- m_vs[i].SetZero();
-
- float32 m = def->masses[i];
- if (m > 0.0f)
- {
- m_ims[i] = 1.0f / m;
- }
- else
- {
- m_ims[i] = 0.0f;
- }
- }
-
- int32 count2 = m_count - 1;
- int32 count3 = m_count - 2;
- m_Ls = (float32*)b2Alloc(count2 * sizeof(float32));
- m_as = (float32*)b2Alloc(count3 * sizeof(float32));
-
- for (int32 i = 0; i < count2; ++i)
- {
- b2Vec2 p1 = m_ps[i];
- b2Vec2 p2 = m_ps[i+1];
- m_Ls[i] = b2Distance(p1, p2);
- }
-
- for (int32 i = 0; i < count3; ++i)
- {
- b2Vec2 p1 = m_ps[i];
- b2Vec2 p2 = m_ps[i + 1];
- b2Vec2 p3 = m_ps[i + 2];
-
- b2Vec2 d1 = p2 - p1;
- b2Vec2 d2 = p3 - p2;
-
- float32 a = b2Cross(d1, d2);
- float32 b = b2Dot(d1, d2);
-
- m_as[i] = b2Atan2(a, b);
- }
-
- m_gravity = def->gravity;
- m_damping = def->damping;
- m_k2 = def->k2;
- m_k3 = def->k3;
-}
-
-void b2Rope::Step(float32 h, int32 iterations)
-{
- if (h == 0.0)
- {
- return;
- }
-
- float32 d = expf(- h * m_damping);
-
- for (int32 i = 0; i < m_count; ++i)
- {
- m_p0s[i] = m_ps[i];
- if (m_ims[i] > 0.0f)
- {
- m_vs[i] += h * m_gravity;
- }
- m_vs[i] *= d;
- m_ps[i] += h * m_vs[i];
-
- }
-
- for (int32 i = 0; i < iterations; ++i)
- {
- SolveC2();
- SolveC3();
- SolveC2();
- }
-
- float32 inv_h = 1.0f / h;
- for (int32 i = 0; i < m_count; ++i)
- {
- m_vs[i] = inv_h * (m_ps[i] - m_p0s[i]);
- }
-}
-
-void b2Rope::SolveC2()
-{
- int32 count2 = m_count - 1;
-
- for (int32 i = 0; i < count2; ++i)
- {
- b2Vec2 p1 = m_ps[i];
- b2Vec2 p2 = m_ps[i + 1];
-
- b2Vec2 d = p2 - p1;
- float32 L = d.Normalize();
-
- float32 im1 = m_ims[i];
- float32 im2 = m_ims[i + 1];
-
- if (im1 + im2 == 0.0f)
- {
- continue;
- }
-
- float32 s1 = im1 / (im1 + im2);
- float32 s2 = im2 / (im1 + im2);
-
- p1 -= m_k2 * s1 * (m_Ls[i] - L) * d;
- p2 += m_k2 * s2 * (m_Ls[i] - L) * d;
-
- m_ps[i] = p1;
- m_ps[i + 1] = p2;
- }
-}
-
-void b2Rope::SetAngle(float32 angle)
-{
- int32 count3 = m_count - 2;
- for (int32 i = 0; i < count3; ++i)
- {
- m_as[i] = angle;
- }
-}
-
-void b2Rope::SolveC3()
-{
- int32 count3 = m_count - 2;
-
- for (int32 i = 0; i < count3; ++i)
- {
- b2Vec2 p1 = m_ps[i];
- b2Vec2 p2 = m_ps[i + 1];
- b2Vec2 p3 = m_ps[i + 2];
-
- float32 m1 = m_ims[i];
- float32 m2 = m_ims[i + 1];
- float32 m3 = m_ims[i + 2];
-
- b2Vec2 d1 = p2 - p1;
- b2Vec2 d2 = p3 - p2;
-
- float32 L1sqr = d1.LengthSquared();
- float32 L2sqr = d2.LengthSquared();
-
- if (L1sqr * L2sqr == 0.0f)
- {
- continue;
- }
-
- float32 a = b2Cross(d1, d2);
- float32 b = b2Dot(d1, d2);
-
- float32 angle = b2Atan2(a, b);
-
- b2Vec2 Jd1 = (-1.0f / L1sqr) * d1.Skew();
- b2Vec2 Jd2 = (1.0f / L2sqr) * d2.Skew();
-
- b2Vec2 J1 = -Jd1;
- b2Vec2 J2 = Jd1 - Jd2;
- b2Vec2 J3 = Jd2;
-
- float32 mass = m1 * b2Dot(J1, J1) + m2 * b2Dot(J2, J2) + m3 * b2Dot(J3, J3);
- if (mass == 0.0f)
- {
- continue;
- }
-
- mass = 1.0f / mass;
-
- float32 C = angle - m_as[i];
-
- while (C > b2_pi)
- {
- angle -= 2 * b2_pi;
- C = angle - m_as[i];
- }
-
- while (C < -b2_pi)
- {
- angle += 2.0f * b2_pi;
- C = angle - m_as[i];
- }
-
- float32 impulse = - m_k3 * mass * C;
-
- p1 += (m1 * impulse) * J1;
- p2 += (m2 * impulse) * J2;
- p3 += (m3 * impulse) * J3;
-
- m_ps[i] = p1;
- m_ps[i + 1] = p2;
- m_ps[i + 2] = p3;
- }
-}
-
-void b2Rope::Draw(b2Draw* draw) const
-{
- b2Color c(0.4f, 0.5f, 0.7f);
-
- for (int32 i = 0; i < m_count - 1; ++i)
- {
- draw->DrawSegment(m_ps[i], m_ps[i+1], c);
- }
-}
diff --git a/external/Box2D/Rope/b2Rope.h b/external/Box2D/Rope/b2Rope.h
deleted file mode 100644
index 246cf92..0000000
--- a/external/Box2D/Rope/b2Rope.h
+++ /dev/null
@@ -1,115 +0,0 @@
-/*
-* Copyright (c) 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_H
-#define B2_ROPE_H
-
-#include <Box2D/Common/b2Math.h>
-
-class b2Draw;
-
-///
-struct b2RopeDef
-{
- b2RopeDef()
- {
- vertices = NULL;
- count = 0;
- masses = NULL;
- gravity.SetZero();
- damping = 0.1f;
- k2 = 0.9f;
- k3 = 0.1f;
- }
-
- ///
- b2Vec2* vertices;
-
- ///
- int32 count;
-
- ///
- float32* masses;
-
- ///
- b2Vec2 gravity;
-
- ///
- float32 damping;
-
- /// Stretching stiffness
- float32 k2;
-
- /// Bending stiffness. Values above 0.5 can make the simulation blow up.
- float32 k3;
-};
-
-///
-class b2Rope
-{
-public:
- b2Rope();
- ~b2Rope();
-
- ///
- void Initialize(const b2RopeDef* def);
-
- ///
- void Step(float32 timeStep, int32 iterations);
-
- ///
- int32 GetVertexCount() const
- {
- return m_count;
- }
-
- ///
- const b2Vec2* GetVertices() const
- {
- return m_ps;
- }
-
- ///