diff --git a/Hipmunk.cabal b/Hipmunk.cabal --- a/Hipmunk.cabal +++ b/Hipmunk.cabal @@ -3,39 +3,61 @@ Tested-With: GHC Category: Physics, Game Name: Hipmunk -Version: 0.2.2 +Version: 5.0.0 Stability: provisional License: OtherLicense License-File: LICENSE -Copyright: (c) 2008 Felipe A. Lessa +Copyright: (c) 2008-2009 Felipe A. Lessa Author: Felipe A. Lessa Maintainer: Felipe A. Lessa Synopsis: A Haskell binding for Chipmunk. Description: Chipmunk is a fast, simple, portable, 2D physics engine (). This - package contains the Chipmunk 4.1.0 source (see - ) + package contains a prerelease Chipmunk 5.0 source and Haskell bindings to all of its functions. It is - completely self-contained. + completely self-contained. Please see + + for a demonstration of this library. . + New in version 5.0.0: + . + * Updated for a prerelease of Chipmunk 5.0 from subversion + revision 192. Besides bugfixes this new version brings + the long awaited new constraints type, doubling the + number of different joint from four to nine. + . + * Tighter dependencies following the versioning policies. + . Licensed under the MIT license (like Chipmunk itself). Extra-Source-Files: NEWS, - chipmunk/chipmunk.h, - chipmunk/cpArbiter.h, - chipmunk/cpArray.h, - chipmunk/cpBB.h, - chipmunk/cpBody.h, - chipmunk/cpCollision.h, - chipmunk/cpHashSet.h, - chipmunk/cpJoint.h, - chipmunk/cpPolyShape.h, - chipmunk/cpShape.h, - chipmunk/cpSpace.h, - chipmunk/cpSpaceHash.h, - chipmunk/cpVect.h, - chipmunk/prime.h, + chipmunk-5.0/chipmunk.h, + chipmunk-5.0/chipmunk_types.h, + chipmunk-5.0/chipmunk_unsafe.h, + chipmunk-5.0/cpArbiter.h, + chipmunk-5.0/cpArray.h, + chipmunk-5.0/cpBB.h, + chipmunk-5.0/cpBody.h, + chipmunk-5.0/cpCollision.h, + chipmunk-5.0/cpHashSet.h, + chipmunk-5.0/cpPolyShape.h, + chipmunk-5.0/cpShape.h, + chipmunk-5.0/cpSpace.h, + chipmunk-5.0/cpSpaceHash.h, + chipmunk-5.0/cpVect.h, + chipmunk-5.0/prime.h, + chipmunk-5.0/constraints/cpConstraint.h, + chipmunk-5.0/constraints/cpDampedRotarySpring.h, + chipmunk-5.0/constraints/cpDampedSpring.h, + chipmunk-5.0/constraints/cpGearJoint.h, + chipmunk-5.0/constraints/cpGrooveJoint.h, + chipmunk-5.0/constraints/cpPinJoint.h, + chipmunk-5.0/constraints/cpPivotJoint.h, + chipmunk-5.0/constraints/cpRotaryLimitJoint.h, + chipmunk-5.0/constraints/cpSimpleMotor.h, + chipmunk-5.0/constraints/cpSlideJoint.h, + chipmunk-5.0/constraints/util.h, Physics/Hipmunk/wrapper.h Flag small_base @@ -48,33 +70,46 @@ Physics.Hipmunk.Common, Physics.Hipmunk.Body, Physics.Hipmunk.Shape, - Physics.Hipmunk.Joint, - Physics.Hipmunk.Space + Physics.Hipmunk.Constraint, + Physics.Hipmunk.Space, + Physics.Hipmunk.Unsafe Other-Modules: Physics.Hipmunk.Internal Include-Dirs: Physics/Hipmunk, - chipmunk + chipmunk-5.0 Includes: wrapper.h C-Sources: - chipmunk/chipmunk.c, - chipmunk/cpArbiter.c, - chipmunk/cpArray.c, - chipmunk/cpBB.c, - chipmunk/cpBody.c, - chipmunk/cpCollision.c, - chipmunk/cpHashSet.c, - chipmunk/cpJoint.c, - chipmunk/cpPolyShape.c, - chipmunk/cpShape.c, - chipmunk/cpSpace.c, - chipmunk/cpSpaceHash.c, - chipmunk/cpVect.c, + chipmunk-5.0/chipmunk.c, + chipmunk-5.0/cpArbiter.c, + chipmunk-5.0/cpArray.c, + chipmunk-5.0/cpBB.c, + chipmunk-5.0/cpBody.c, + chipmunk-5.0/cpCollision.c, + chipmunk-5.0/cpHashSet.c, + chipmunk-5.0/cpPolyShape.c, + chipmunk-5.0/cpShape.c, + chipmunk-5.0/cpSpace.c, + chipmunk-5.0/cpSpaceHash.c, + chipmunk-5.0/cpVect.c, + chipmunk-5.0/constraints/cpConstraint.c, + chipmunk-5.0/constraints/cpDampedRotarySpring.c, + chipmunk-5.0/constraints/cpDampedSpring.c, + chipmunk-5.0/constraints/cpGearJoint.c, + chipmunk-5.0/constraints/cpGrooveJoint.c, + chipmunk-5.0/constraints/cpPinJoint.c, + chipmunk-5.0/constraints/cpPivotJoint.c, + chipmunk-5.0/constraints/cpRotaryLimitJoint.c, + chipmunk-5.0/constraints/cpSimpleMotor.c, + chipmunk-5.0/constraints/cpSlideJoint.c, Physics/Hipmunk/wrapper.c - Build-Depends: base if flag(small_base) - Build-Depends: array, containers + Build-Depends: base >= 3 && < 5, + array >= 0.1 && < 0.3, + containers >= 0.1 && < 0.3 + else + Build-Depends: base == 2 Extensions: CPP, ForeignFunctionInterface Build-Tools: hsc2hs GHC-Options: -Wall diff --git a/NEWS b/NEWS --- a/NEWS +++ b/NEWS @@ -1,3 +1,10 @@ +Version 5.0.0 +================ + - Updated for a prerelease of Chipmunk 5.0 from subversion + revision 192. Besides bugfixes this new version brings the + long awaited new constraints type, doubling the number of + different joint from four to nine. + Version 0.2.2 ============= - Update Chipmunk to version 4.1.0. diff --git a/Physics/Hipmunk.hs b/Physics/Hipmunk.hs --- a/Physics/Hipmunk.hs +++ b/Physics/Hipmunk.hs @@ -1,7 +1,7 @@ ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Hipmunk.hs --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) Felipe A. Lessa 2008-2009 -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -23,7 +23,7 @@ (module Physics.Hipmunk.Common, module Physics.Hipmunk.Body, module Physics.Hipmunk.Shape, - module Physics.Hipmunk.Joint, + module Physics.Hipmunk.Constraint, module Physics.Hipmunk.Space ) where @@ -31,5 +31,5 @@ import Physics.Hipmunk.Common import Physics.Hipmunk.Body import Physics.Hipmunk.Shape -import Physics.Hipmunk.Joint +import Physics.Hipmunk.Constraint import Physics.Hipmunk.Space diff --git a/Physics/Hipmunk/Body.hsc b/Physics/Hipmunk/Body.hsc --- a/Physics/Hipmunk/Body.hsc +++ b/Physics/Hipmunk/Body.hsc @@ -1,7 +1,7 @@ ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Body.hsc --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) 2008-2009 Felipe A. Lessa -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -62,7 +62,7 @@ applyForce, applyOnlyForce, applyImpulse, - dampedSpring, + applyDampedSpring, -- * Utilities localToWorld, @@ -80,7 +80,7 @@ -- the given mass and moment of inertia. -- -- It is recommended to call 'setPosition' afterwards. -newBody :: CpFloat -> CpFloat -> IO Body +newBody :: Mass -> Moment -> IO Body newBody mass inertia = do b <- mallocForeignPtrBytes #{size cpBody} withForeignPtr b $ \ptr -> do @@ -227,7 +227,7 @@ -- -- Note that this function only needs to be called if you -- are not adding the body to a space. -updateVelocity :: Body -> Vector -> CpFloat -> Time -> IO () +updateVelocity :: Body -> Vector -> Damping -> Time -> IO () updateVelocity (B b) g d dt = withForeignPtr b $ \b_ptr -> with g $ \g_ptr -> do @@ -306,21 +306,20 @@ -- is the time step to apply the force over. Both anchors are -- in body coordinates. -- --- Note: not solving the damping forces in the impulse solver --- causes problems with large damping values. This function --- will eventually be replaced by a new constraint (joint) type. -dampedSpring :: (Body,Position) -> (Body,Position) -> CpFloat - -> CpFloat -> CpFloat -> Time -> IO () -dampedSpring (B b1,a1) (B b2, a2) rlen k dmp dt = +-- Note: large damping values can be unstable, you should use +-- the damped spring constraint instead. +applyDampedSpring :: (Body,Position) -> (Body,Position) -> Distance + -> CpFloat -> Damping -> Time -> IO () +applyDampedSpring (B b1,a1) (B b2, a2) rlen k dmp dt = withForeignPtr b1 $ \b1_ptr -> withForeignPtr b2 $ \b2_ptr -> with a1 $ \a1_ptr -> with a2 $ \a2_ptr -> do - wrDampedSpring b1_ptr b2_ptr a1_ptr a2_ptr rlen k dmp dt + wrApplyDampedSpring b1_ptr b2_ptr a1_ptr a2_ptr rlen k dmp dt foreign import ccall unsafe "wrapper.h" - wrDampedSpring :: BodyPtr -> BodyPtr -> VectorPtr -> VectorPtr - -> CpFloat -> CpFloat -> CpFloat -> Time -> IO () + wrApplyDampedSpring :: BodyPtr -> BodyPtr -> VectorPtr -> VectorPtr + -> CpFloat -> CpFloat -> CpFloat -> Time -> IO () -- | For a vector @p@ in body @b@'s coordinates, diff --git a/Physics/Hipmunk/Common.hsc b/Physics/Hipmunk/Common.hsc --- a/Physics/Hipmunk/Common.hsc +++ b/Physics/Hipmunk/Common.hsc @@ -1,23 +1,32 @@ {-# CFILES - chipmunk/chipmunk.c - chipmunk/cpArbiter.c - chipmunk/cpArray.c - chipmunk/cpBB.c - chipmunk/cpBody.c - chipmunk/cpCollision.c - chipmunk/cpHashSet.c - chipmunk/cpJoint.c - chipmunk/cpPolyShape.c - chipmunk/cpShape.c - chipmunk/cpSpace.c - chipmunk/cpSpaceHash.c - chipmunk/cpVect.c + chipmunk-5.0/chipmunk.c, + chipmunk-5.0/cpArbiter.c, + chipmunk-5.0/cpArray.c, + chipmunk-5.0/cpBB.c, + chipmunk-5.0/cpBody.c, + chipmunk-5.0/cpCollision.c, + chipmunk-5.0/cpHashSet.c, + chipmunk-5.0/cpPolyShape.c, + chipmunk-5.0/cpShape.c, + chipmunk-5.0/cpSpace.c, + chipmunk-5.0/cpSpaceHash.c, + chipmunk-5.0/cpVect.c, + chipmunk-5.0/constraints/cpConstraint.c, + chipmunk-5.0/constraints/cpDampedRotaryString.c, + chipmunk-5.0/constraints/cpDampedString.c, + chipmunk-5.0/constraints/cpGearJoint.c, + chipmunk-5.0/constraints/cpGrooveJoint.c, + chipmunk-5.0/constraints/cpPinJoint.c, + chipmunk-5.0/constraints/cpPivotJoint.c, + chipmunk-5.0/constraints/cpRotaryLimitJoint.c, + chipmunk-5.0/constraints/cpSimpleMotor.c, + chipmunk-5.0/constraints/cpSlideJoint.c, Physics/Hipmunk/wrapper.c #-} ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Common.hsc --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) 2008-2009 Felipe A. Lessa -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -38,6 +47,8 @@ infinity, Time, Angle, + Distance, + Damping, -- * Global variables -- $global_vars @@ -58,13 +69,14 @@ -- ** Bias coefficient -- $bias_coef + BiasCoef, getBiasCoef, setBiasCoef, - -- ** Joint bias coefficient - -- $joint_bias_coef - getJointBiasCoef, - setJointBiasCoef, + -- ** Constraint bias coefficient + -- $constraint_bias_coef + getConstraintBiasCoef, + setConstraintBiasCoef, -- * Vectors Vector(..), @@ -84,6 +96,7 @@ where import Foreign hiding (rotate) +import Foreign.C.Types (CInt) #include "wrapper.h" error' :: String -> a @@ -114,25 +127,36 @@ -- represents an angle in radians. type Angle = CpFloat +-- | Type synonym used to hint that the argument or result +-- represents a distance. +type Distance = CpFloat +-- | Type synonym used to hint that the argument or result +-- represents a damping constant. +type Damping = CpFloat + + -- $global_vars --- Chipmunk tries to maintein a very few number of global --- variables to allow multiple 'Physics.Hipmunk.Space.Space's --- to be used simultaneously, however there are some. +-- Chipmunk tries to maintain a very few number of global +-- variables to allow multiple @Space@s to be used +-- simultaneously, however there are some. -- $shape_counter -- The shape counter is a global counter used for creating -- unique hash identifiers to the shapes. --- | @resetShapeCounter@ reset the shape counter to its default value. --- This is used to add determinism to a simulation. As the ids --- created with this counter may affect the order in which the --- collisions happen, there may be very slight differences in --- different simulations. +-- | @resetShapeCounter@ reset the shape counter to its default +-- value. This is used to add determinism to a simulation. As +-- the ids created with this counter may affect the order in +-- which the collisions happen, there may be very slight +-- differences in different simulations. It may be very useful +-- to call @resetShapeCounter@ everytime you start a new +-- simulation. -- -- However, be careful as you should not use shapes created --- before a call to @resetCounter@ with shapes created after --- it as they may have the same id. +-- before a call to @resetCounter@ with shapes created after it +-- as they may have the same id. This means that can't add +-- shapes created after the call to a space created before it. resetShapeCounter :: IO () resetShapeCounter = cpResetShapeIdCounter @@ -145,14 +169,14 @@ -- It should be small as the cached contacts will only be -- close for a short time. (default is 3) -getContactPersistence :: IO #{type int} +getContactPersistence :: IO CInt getContactPersistence = peek cp_contact_persistence -setContactPersistence :: #{type int} -> IO () +setContactPersistence :: CInt -> IO () setContactPersistence = poke cp_contact_persistence foreign import ccall unsafe "wrapper.h &cp_contact_persistence" - cp_contact_persistence :: Ptr #{type int} + cp_contact_persistence :: Ptr CInt -- $collision_slop @@ -175,35 +199,35 @@ -- The amount of penetration to reduce in each step. Values should -- range from 0 to 1. Using large values will eliminate penetration in -- fewer steps, but can cause vibration. (default is 0.1) +type BiasCoef = CpFloat -getBiasCoef :: IO CpFloat +getBiasCoef :: IO BiasCoef getBiasCoef = peek cp_bias_coef -setBiasCoef :: CpFloat -> IO () +setBiasCoef :: BiasCoef -> IO () setBiasCoef = poke cp_bias_coef foreign import ccall unsafe "wrapper.h &cp_bias_coef" cp_bias_coef :: Ptr CpFloat --- $joint_bias_coef --- Similar to the bias coefficient, but for all joints. In the --- future, joints might have their own bias coefficient --- instead. (default is 0.1) +-- $constraint_bias_coef +-- Similar to the bias coefficient, but sets the default bias +-- for all constraints. (default is 0.1) -getJointBiasCoef :: IO CpFloat -getJointBiasCoef = peek cp_joint_bias_coef +getConstraintBiasCoef :: IO BiasCoef +getConstraintBiasCoef = peek cp_constraint_bias_coef -setJointBiasCoef :: CpFloat -> IO () -setJointBiasCoef = poke cp_joint_bias_coef +setConstraintBiasCoef :: BiasCoef -> IO () +setConstraintBiasCoef = poke cp_constraint_bias_coef -foreign import ccall unsafe "wrapper.h &cp_joint_bias_coef" - cp_joint_bias_coef :: Ptr CpFloat +foreign import ccall unsafe "wrapper.h &cp_constraint_bias_coef" + cp_constraint_bias_coef :: Ptr CpFloat -- | A two-dimensional vector. It is an instance of 'Num' --- however the operations 'signum' and '(*)' are not +-- however the operations 'signum' and @(*)@ are not -- supported. data Vector = Vector !CpFloat !CpFloat deriving (Eq, Show, Ord) @@ -238,42 +262,51 @@ -- angle (in radians). fromAngle :: Angle -> Vector fromAngle theta = Vector (cos theta) (sin theta) +{-# INLINE fromAngle #-} -- | The length of a vector. len :: Vector -> CpFloat len (Vector x y) = sqrt $ x*x + y*y +{-# INLINE len #-} -- | Normalizes the vector (i.e. divides it by its length). normalize :: Vector -> Vector normalize v = v `scale` (recip $ len v) +{-# INLINE normalize #-} -- | Scales the components of a vector by the same amount. scale :: Vector -> CpFloat -> Vector scale (Vector x y) s = Vector (x*s) (y*s) +{-# INLINE scale #-} -- | @toAngle v@ is the angle that @v@ has -- with the vector @Vector 1 0@ (modulo @2*pi@). toAngle :: Vector -> Angle toAngle (Vector x y) = atan2 y x +{-# INLINE toAngle #-} -- | @v1 \`dot\` v2@ computes the familiar dot operation. dot :: Vector -> Vector -> CpFloat dot (Vector x1 y1) (Vector x2 y2) = x1*x2 + y1*y2 +{-# INLINE dot #-} -- | @v1 \`cross\` v2@ computes the familiar cross operation. cross :: Vector -> Vector -> CpFloat cross (Vector x1 y1) (Vector x2 y2) = x1*y2 - y1*x2 +{-# INLINE cross #-} -- | @perp v@ is a vector of same length as @v@ but perpendicular -- to @v@ (i.e. @toAngle (perp v) - toAngle v@ equals @pi\/2@ -- modulo @2*pi@). perp :: Vector -> Vector perp (Vector x y) = Vector (-y) x +{-# INLINE perp #-} -- | @v1 \`project\` v2@ is the vector projection of @v1@ onto @v2@. project :: Vector -> Vector -> Vector project v1 v2 = v2 `scale` s where s = (v1 `dot` v2) / (v2 `dot` v2) +{-# INLINE project #-} -- | @v1 \`rotate\` v2@ uses complex multiplication -- to rotate (and scale) @v1@ by @v2@. @@ -281,6 +314,7 @@ rotate (Vector x1 y1) (Vector x2 y2) = Vector x3 y3 where x3 = x1*x2 - y1*y2 y3 = x1*y2 + y1*x2 +{-# INLINE rotate #-} -- | The inverse operation of @rotate@, such that -- @unrotate (rotate v1 v2) v2@ equals @v1@. @@ -288,3 +322,4 @@ unrotate (Vector x1 y1) (Vector x2 y2) = Vector x3 y3 where x3 = x1*x2 + y1*y2 y3 = y1*x2 - x1*y2 +{-# INLINE unrotate #-} diff --git a/Physics/Hipmunk/Constraint.hsc b/Physics/Hipmunk/Constraint.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Constraint.hsc @@ -0,0 +1,315 @@ +----------------------------------------------------------------------------- +-- | +-- Module : Physics/Hipmunk/Constraint.hsc +-- Copyright : (c) 2008-2009 Felipe A. Lessa +-- License : MIT (see LICENSE) +-- +-- Maintainer : felipe.lessa@gmail.com +-- Stability : provisional +-- Portability : portable (needs FFI) +-- +-- Constraints that restrict the bodies' movement. +-- +----------------------------------------------------------------------------- + +module Physics.Hipmunk.Constraint + (-- * Common interface + newConstraint, + redefineC, + setBiasCoefC, + Constraint, + -- ** Forgetting the phantom type + -- $phantom + Unknown, + forgetC, + + -- * Constraint types + -- $constraintTypes + + -- ** Pin joint + Pin(..), + -- ** Slide joint + Slide(..), + -- ** Pivot joint + Pivot(..), + -- ** Groove joint + Groove(..), + -- ** Gear joint + Gear(..), + -- ** Damped spring + DampedSpring(..), + -- ** Damped rotary spring + DampedRotarySpring(..), + -- ** Rotary limit + RotaryLimit(..), + -- ** Simple motor + SimpleMotor(..), + ) + where + +import Foreign +#include "wrapper.h" + +import Physics.Hipmunk.Common +import Physics.Hipmunk.Internal +import Physics.Hipmunk.Body (worldToLocal) + + +-- | @newConstraint b1 b2 type_@ connects the two bodies @b1@ and @b2@ +-- with a constraint of the given type. Note that you should +-- add the 'Constraint' to a space. +-- +-- The 'ConstraintType' type class is implemented by all +-- constraint types to allow them to be manipulated by the same +-- framework while retaining type-safety, consequently it isn't +-- exported. +newConstraint :: ConstraintType a => Body -> Body -> a -> IO (Constraint a) +newConstraint body1@(B b1) body2@(B b2) type_ = + withForeignPtr b1 $ \b1_ptr -> + withForeignPtr b2 $ \b2_ptr -> + mallocForeignPtrBytes (size type_) >>= \constraint -> + withForeignPtr constraint $ \constraint_ptr -> do + init_ type_ constraint_ptr b1_ptr b2_ptr + return (C constraint body1 body2) +{-# SPECIALISE newConstraint :: Body -> Body -> Pin -> IO (Constraint Pin) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> Slide -> IO (Constraint Slide) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> Pivot -> IO (Constraint Pivot) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> Groove -> IO (Constraint Groove) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> Gear -> IO (Constraint Gear) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> DampedSpring -> IO (Constraint DampedSpring) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> DampedRotarySpring -> IO (Constraint DampedRotarySpring) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> RotaryLimit -> IO (Constraint RotaryLimit) #-} +{-# SPECIALISE newConstraint :: Body -> Body -> SimpleMotor -> IO (Constraint SimpleMotor) #-} + +-- | @redefine constr type_@ redefines @constr@'s parameters +-- on-the-fly, allowing you to dynamically change the +-- constraint's behaviour. +redefineC :: ConstraintType a => Constraint a -> a -> IO () +redefineC (C c b1 b2) t = withForeignPtr c $ \c_ptr -> redef c_ptr b1 b2 t +{-# SPECIALISE redefineC :: Constraint Pin -> Pin -> IO () #-} +{-# SPECIALISE redefineC :: Constraint Slide -> Slide -> IO () #-} +{-# SPECIALISE redefineC :: Constraint Pivot -> Pivot -> IO () #-} +{-# SPECIALISE redefineC :: Constraint Groove -> Groove -> IO () #-} +{-# SPECIALISE redefineC :: Constraint Gear -> Gear -> IO () #-} +{-# SPECIALISE redefineC :: Constraint DampedSpring -> DampedSpring -> IO () #-} +{-# SPECIALISE redefineC :: Constraint DampedRotarySpring -> DampedRotarySpring -> IO () #-} +{-# SPECIALISE redefineC :: Constraint RotaryLimit -> RotaryLimit -> IO () #-} +{-# SPECIALISE redefineC :: Constraint SimpleMotor -> SimpleMotor -> IO () #-} + +-- | Sets the constraint's bias coefficient. By default it is +-- equal to the last value set globally with +-- 'setConstraintBiasCoef', which initially is @0.1@ +setBiasCoefC :: BiasCoef -> Constraint a -> IO () +setBiasCoefC b (C c _ _) = withForeignPtr c $ flip #{poke cpConstraint, biasCoef} b + +-- $phantom +-- These functions discard the phantom type of the constraint. +-- They're useful, for example, if you want to put different +-- constraints in a homogeneous data structure (such as a +-- list). + +-- | Completely safe function that discards the constraint type +-- (which is a phantom type). You can \"remember\" it again by +-- using @unsafeRemember@ from the @Unsafe@ module. +forgetC :: Constraint a -> Constraint Unknown +forgetC (C c b1 b2) = C c b1 b2 +{-# INLINE forgetC #-} + + + +-- $constraintTypes +-- There are currently nine types of constraints. When +-- appending a number to a property, we hint that it refer to +-- one of the bodies that the constraint is intercting with +-- (e.g. \"Second anchor\" is the position of the anchor on the +-- second body in its coordinates). + +-- | A pin joint connects the bodies with a solid pin. +-- The anchor points are kept at a fixed distance. +data Pin = Pin {pinAnchor1 :: !Position {-^ First anchor. -} + ,pinAnchor2 :: !Position {-^ Second anchor. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType Pin where + size _ = #{size cpPinJoint} + init_ (Pin a1 a2) = with2 a1 a2 $ wrPinJointInit + redef ptr _ _ (Pin a1 a2) = do + #{poke cpPinJoint, anchr1} ptr a1 + #{poke cpPinJoint, anchr2} ptr a2 + +-- | A slide joint is similar to a pin joint, however +-- it has a minimum and a maximum distance. +data Slide = Slide {slideAnchor1 :: !Position {-^ First anchor. -} + ,slideAnchor2 :: !Position {-^ Second anchor. -} + ,slideMinDist :: !Distance {-^ Minimum distance. -} + ,slideMaxDist :: !Distance {-^ Maximum distance. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType Slide where + size _ = #{size cpSlideJoint} + init_ (Slide a1 a2 mn mx) = with2 a1 a2 $ wrSlideJointInit mn mx + redef ptr _ _ (Slide a1 a2 mn mx) = do + #{poke cpSlideJoint, anchr1} ptr a1 + #{poke cpSlideJoint, anchr2} ptr a2 + #{poke cpSlideJoint, min} ptr mn + #{poke cpSlideJoint, max} ptr mx + +-- | A pivot joint allows the bodies to pivot around +-- a single point. +data Pivot = + -- | You may specify the pivot point in world's coordinates + -- (so both bodies should be already in place). + Pivot1 {pivotPos :: !Position {-^ Pivot point in world's coordinates. -}} + -- | Or you may specify the joint as two anchors (on each + -- body's coordinates), removing the need having the bodies + -- already in place. + | Pivot2 {pivotAnchor1 :: !Position {-^ First anchor. -} + ,pivotAnchor2 :: !Position {-^ Second anchor. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType Pivot where + size _ = #{size cpPivotJoint} + init_ (Pivot1 pos) = with1 pos $ wrPivot1JointInit + init_ (Pivot2 a1 a2) = with2 a1 a2 $ wrPivot2JointInit + redef ptr b1 b2 (Pivot1 pos) = do + worldToLocal b1 pos >>= #{poke cpPivotJoint, anchr1} ptr + worldToLocal b2 pos >>= #{poke cpPivotJoint, anchr2} ptr + redef ptr _ _ (Pivot2 a1 a2) = do + #{poke cpPivotJoint, anchr1} ptr a1 + #{poke cpPivotJoint, anchr2} ptr a2 + +-- | A groove joint attaches a point on the second body +-- to a groove in the first one. +data Groove = Groove { + groovePoints :: !(Position,Position) {-^ Groove, in first body's coordinates. -} + ,groovePivot :: !Position {-^ Pivot, in second body's coordinates. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType Groove where + size _ = #{size cpGrooveJoint} + init_ (Groove (g1,g2) anchor) = with3 g1 g2 anchor $ wrGrooveJointInit + redef ptr _ _ (Groove (g1,g2) anchor) = do + #{poke cpGrooveJoint, grv_a} ptr g1 + #{poke cpGrooveJoint, grv_b} ptr g2 + #{poke cpGrooveJoint, grv_n} ptr $ perp $ normalize $ g1 - g2 + #{poke cpGrooveJoint, anchr2} ptr anchor + +-- | A gear joint restricts the bodies movement to be +-- coordinated as if they were attached like dented gears. +data Gear = Gear {gearPhase :: !Angle {-^ Phase of the movement. -} + ,gearRatio :: !CpFloat {-^ Ratio between the gears. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType Gear where + size _ = #{size cpGearJoint} + init_ (Gear p r) = wrGearJointInit p r + redef ptr _ _ (Gear p r) = do + #{poke cpGearJoint, phase} ptr p + #{poke cpGearJoint, ratio} ptr r + +-- | A simple damped spring. Generally this constraint +-- should be used instead of @applyDampedSpring@. +data DampedSpring = DampedSpring { + dampedAnchor1 :: !Position {-^ First anchor. -} + ,dampedAnchor2 :: !Position {-^ Second anchor. -} + ,dampedRestLength :: !Distance {-^ Rest length. -} + ,dampedStiffness :: !CpFloat {-^ Stiffness. -} + ,dampedDamping :: !Damping {-^ Damping. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType DampedSpring where + size _ = #{size cpDampedSpring} + init_ (DampedSpring a1 a2 r s d) = with2 a1 a2 $ wrDampedSpringInit r s d + redef ptr _ _ (DampedSpring a1 a2 r s d) = do + #{poke cpDampedSpring, anchr1} ptr a1 + #{poke cpDampedSpring, anchr2} ptr a2 + #{poke cpDampedSpring, restLength} ptr r + #{poke cpDampedSpring, stiffness} ptr s + #{poke cpDampedSpring, damping} ptr d + +-- | A damped rotary spring constraint. +data DampedRotarySpring = DampedRotarySpring { + dampedRotRestAngle :: !Angle {-^ Rest angle. -} + ,dampedRotStiffness :: !CpFloat {-^ Stiffness. -} + ,dampedRotDamping :: !Damping {-^ Damping. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType DampedRotarySpring where + size _ = #{size cpDampedRotarySpring} + init_ (DampedRotarySpring r s d) = wrDampedRotarySpringInit r s d + redef ptr _ _ (DampedRotarySpring r s d) = do + #{poke cpDampedRotarySpring, restAngle} ptr r + #{poke cpDampedRotarySpring, stiffness} ptr s + #{poke cpDampedRotarySpring, damping} ptr d + +-- | A rotary limit constraints the difference of angle +-- between two bodies. +data RotaryLimit = RotaryLimit { + rotaryMinDist :: Distance {-^ Minimum distance. -} + ,rotaryMaxDist :: Distance {-^ Maximum distance. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType RotaryLimit where + size _ = #{size cpRotaryLimitJoint} + init_ (RotaryLimit mn mx) = wrRotaryLimitJointInit mn mx + redef ptr _ _ (RotaryLimit mn mx) = do + #{poke cpRotaryLimitJoint, min} ptr mn + #{poke cpRotaryLimitJoint, max} ptr mx + +-- | A simple motor that applies opposite impulses to each +-- body. The rate is used to compute the torque. +data SimpleMotor = SimpleMotor { + simpleMotorRate :: CpFloat {-^ Rate. -}} + deriving (Eq, Ord, Show) + +instance ConstraintType SimpleMotor where + size _ = #{size cpSimpleMotor} + init_ (SimpleMotor r) = wrSimpleMotorInit r + redef ptr _ _ (SimpleMotor r) = do + #{poke cpSimpleMotor, rate} ptr r + + + + + + +-- | Helper functions similar to 'with'. +with1 :: (Storable a) => a -> (Ptr a -> ConstraintInit) -> ConstraintInit +with1 x f c b1 b2 = + with x $ \x_ptr -> + f x_ptr c b1 b2 +with2 :: (Storable a, Storable b) => a -> b + -> (Ptr a -> Ptr b -> ConstraintInit) -> ConstraintInit +with2 x y f c b1 b2 = + with x $ \x_ptr -> + with y $ \y_ptr -> + f x_ptr y_ptr c b1 b2 +with3 :: (Storable a, Storable b, Storable c) => a -> b -> c + -> (Ptr a -> Ptr b -> Ptr c -> ConstraintInit) -> ConstraintInit +with3 x y z f c b1 b2 = + with x $ \x_ptr -> + with y $ \y_ptr -> + with z $ \z_ptr -> + f x_ptr y_ptr z_ptr c b1 b2 + +-- Boring imports +foreign import ccall unsafe "wrapper.h" + wrPinJointInit :: VectorPtr -> VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrSlideJointInit :: CpFloat -> CpFloat -> VectorPtr -> VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrPivot1JointInit :: VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrPivot2JointInit :: VectorPtr -> VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrGearJointInit :: CpFloat -> CpFloat -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrGrooveJointInit :: VectorPtr -> VectorPtr -> VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrDampedSpringInit :: CpFloat -> CpFloat -> CpFloat -> VectorPtr -> VectorPtr -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrDampedRotarySpringInit :: CpFloat -> CpFloat -> CpFloat -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrRotaryLimitJointInit :: CpFloat -> CpFloat -> ConstraintInit +foreign import ccall unsafe "wrapper.h" + wrSimpleMotorInit :: CpFloat -> ConstraintInit diff --git a/Physics/Hipmunk/Internal.hsc b/Physics/Hipmunk/Internal.hsc --- a/Physics/Hipmunk/Internal.hsc +++ b/Physics/Hipmunk/Internal.hsc @@ -1,7 +1,7 @@ ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Internal.hsc --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) 2008-2009 Felipe A. Lessa -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -21,9 +21,12 @@ Shape(..), unS, - JointPtr, - Joint(..), - unJ, + ConstraintPtr, + Constraint(..), + unC, + Unknown(..), + ConstraintInit, + ConstraintType(..), SpacePtr, Space(..), @@ -46,9 +49,9 @@ --- | A rigid body representing the physical properties of an object, --- but without a shape. It may help to think as a particle that --- is able to rotate. +-- | A rigid body representing the physical properties of an +-- object, but without a shape. It may help to think of it as a +-- particle that is able to rotate. newtype Body = B (ForeignPtr Body) type BodyPtr = Ptr Body @@ -63,7 +66,7 @@ --- | A collision shape is attached to a @Body@ to define its +-- | A collision shape is attached to a 'Body' to define its -- shape. Multiple shapes may be attached, including -- overlapping ones (shapes of a body don't generate collisions -- with each other). @@ -95,24 +98,39 @@ --- | A joint represents a constrain between two bodies. Don't --- forget to add the bodies and the joint to the space. -data Joint = J !(ForeignPtr Joint) !Body !Body -type JointPtr = Ptr Joint +-- | Represents a constraint between two bodies. Don't forget to +-- add the bodies and the constraint itself to the space. +-- The phantom type indicates the type of the constraint. +data Constraint a = C !(ForeignPtr (Constraint ())) !Body !Body +type ConstraintPtr = Ptr (Constraint ()) -unJ :: Joint -> ForeignPtr Joint -unJ (J j _ _) = j +unC :: Constraint a -> ForeignPtr (Constraint ()) +unC (C j _ _) = j -instance Eq Joint where - J j1 _ _ == J j2 _ _ = j1 == j2 +instance Eq (Constraint a) where + C j1 _ _ == C j2 _ _ = j1 == j2 -instance Ord Joint where - J j1 _ _ `compare` J j2 _ _ = j1 `compare` j2 +instance Ord (Constraint a) where + C j1 _ _ `compare` C j2 _ _ = j1 `compare` j2 +-- | An unknown constraint \"type\". Note that this isn't a +-- 'ConstraintType' because you can't create a constraint of +-- @Unknown@ type. +data Unknown = Unknown +-- | Type of generic constraint initializar. +type ConstraintInit = ConstraintPtr -> BodyPtr -> BodyPtr -> IO () +-- | Class implemented by all constraint types. +class ConstraintType a where + size :: a -> Int + init_ :: a -> ConstraintInit + redef :: ConstraintPtr -> Body -> Body -> a -> IO () + + + -- | A space is where the simulation really occurs. You add --- bodies, shapes and joints to a space and then step it +-- bodies, shapes and constraints to a space and then step it -- to update it as whole. data Space = P !(ForeignPtr Space) !(IORef Entities) -- Active and static entities @@ -120,8 +138,9 @@ type SpacePtr = Ptr Space type Entities = Map (Ptr ()) (Either (ForeignPtr ()) Shape) type Callbacks = (Maybe (FunPtr ()), -- Default - Map (#{type unsigned int}, #{type unsigned int}) - (FunPtr ())) + Map (CollisionType_, CollisionType_) (FunPtr ())) +type CollisionType_ = #{type cpCollisionType} +-- Duplicated to avoid bringing the documentation from Shape module. unP :: Space -> ForeignPtr Space unP (P sp _ _) = sp @@ -147,10 +166,10 @@ -- contains garbage), and by extension you can only know -- the impulse sum after @step@ returns as well. -- --- /IMPORTANT:/ You may maintain a reference to an array --- of @Contact@s that was passed to a callback to do any other --- processing later. However, /a new call to /@step@/ will --- invalidate any of those arrays/! Be careful. +-- /IMPORTANT:/ You may maintain a reference to an array of +-- @Contact@s that was passed to a callback to do any other +-- processing later. However, /a new call to/ @step@ /will/ +-- /invalidate any of those arrays!/ Be careful. data Contact = Contact { ctPos :: Position, -- ^ Position of the collision in world's coordinates. diff --git a/Physics/Hipmunk/Joint.hsc b/Physics/Hipmunk/Joint.hsc deleted file mode 100644 --- a/Physics/Hipmunk/Joint.hsc +++ /dev/null @@ -1,113 +0,0 @@ ------------------------------------------------------------------------------ --- | --- Module : Physics/Hipmunk/Joint.hsc --- Copyright : (c) Felipe A. Lessa 2008 --- License : MIT (see LICENSE) --- --- Maintainer : felipe.lessa@gmail.com --- Stability : provisional --- Portability : portable (needs FFI) --- --- Joints that constrain bodies. --- ------------------------------------------------------------------------------ - -module Physics.Hipmunk.Joint - (-- * Joints - Joint, - JointType(..), - newJoint - ) - where - -import Foreign -#include "wrapper.h" - -import Physics.Hipmunk.Common -import Physics.Hipmunk.Internal - - --- | There are currently four types of joints. When appending --- a number to a property, we hint that it refer to one of --- the bodies that the joint is contraining (e.g. @anchor2@ --- is the position of the anchor on the second body in its --- coordinates). -data JointType = - -- | A pin joint connects the bodies with a solid pin. - -- The anchor points are kept at a fixed distance. - Pin {anchor1, anchor2 :: Position} - - -- | A slide joint is similar to a pin joint, however - -- it has a minimum and a maximum distance. - | Slide {anchor1, anchor2 :: Position, - minDist, maxDist :: CpFloat} - - -- | A pivot joint allows the bodies to pivot around - -- a single point in world's coordinates. Both should - -- be already in place. - | Pivot {pivot :: Position} - - -- | A groove joint attaches a point on the second body - -- to a groove in the first one. - | Groove {groove1 :: (Position, Position), - pivot2 :: Position} - deriving (Eq, Ord, Show) - - --- | @newJoint b1 b2 type@ connects the two bodies @b1@ and @b2@ --- with a joint of the given type. Note that you should --- add the 'Joint' to a space. -newJoint :: Body -> Body -> JointType -> IO Joint -newJoint body1@(B b1) body2@(B b2) (Pin a1 a2) = - withForeignPtr b1 $ \b1_ptr -> - withForeignPtr b2 $ \b2_ptr -> - with a1 $ \a1_ptr -> - with a2 $ \a2_ptr -> - mallocForeignPtrBytes #{size cpPinJoint} >>= \joint -> - withForeignPtr joint $ \joint_ptr -> do - wrPinJointInit joint_ptr b1_ptr b2_ptr a1_ptr a2_ptr - return (J joint body1 body2) - -newJoint body1@(B b1) body2@(B b2) (Slide a1 a2 mn mx) = - withForeignPtr b1 $ \b1_ptr -> - withForeignPtr b2 $ \b2_ptr -> - with a1 $ \a1_ptr -> - with a2 $ \a2_ptr -> - mallocForeignPtrBytes #{size cpSlideJoint} >>= \joint -> - withForeignPtr joint $ \joint_ptr -> do - wrSlideJointInit joint_ptr b1_ptr b2_ptr a1_ptr a2_ptr mn mx - return (J joint body1 body2) - -newJoint body1@(B b1) body2@(B b2) (Pivot pos) = - withForeignPtr b1 $ \b1_ptr -> - withForeignPtr b2 $ \b2_ptr -> - with pos $ \pos_ptr -> - mallocForeignPtrBytes #{size cpPivotJoint} >>= \joint -> - withForeignPtr joint $ \joint_ptr -> do - wrPivotJointInit joint_ptr b1_ptr b2_ptr pos_ptr - return (J joint body1 body2) - -newJoint body1@(B b1) body2@(B b2) (Groove (g1,g2) anchor) = - withForeignPtr b1 $ \b1_ptr -> - withForeignPtr b2 $ \b2_ptr -> - with g1 $ \g1_ptr -> - with g2 $ \g2_ptr -> - with anchor $ \anchor_ptr -> - mallocForeignPtrBytes #{size cpGrooveJoint} >>= \joint -> - withForeignPtr joint $ \joint_ptr -> do - wrGrooveJointInit joint_ptr b1_ptr b2_ptr g1_ptr g2_ptr anchor_ptr - return (J joint body1 body2) - -foreign import ccall unsafe "wrapper.h" - wrPinJointInit :: JointPtr -> BodyPtr -> BodyPtr - -> VectorPtr -> VectorPtr -> IO () -foreign import ccall unsafe "wrapper.h" - wrSlideJointInit :: JointPtr -> BodyPtr -> BodyPtr -> VectorPtr - -> VectorPtr -> CpFloat -> CpFloat -> IO () -foreign import ccall unsafe "wrapper.h" - wrPivotJointInit :: JointPtr -> BodyPtr -> BodyPtr - -> VectorPtr -> IO () -foreign import ccall unsafe "wrapper.h" - wrGrooveJointInit :: JointPtr -> BodyPtr -> BodyPtr - -> VectorPtr -> VectorPtr -> VectorPtr -> IO () - diff --git a/Physics/Hipmunk/Shape.hsc b/Physics/Hipmunk/Shape.hsc --- a/Physics/Hipmunk/Shape.hsc +++ b/Physics/Hipmunk/Shape.hsc @@ -1,7 +1,7 @@ ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Shape.hsc --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) 2008-2009 Felipe A. Lessa -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -47,9 +47,12 @@ -- * Utilities getBody, + moment, momentForCircle, + momentForSegment, momentForPoly, - shapeQuery, + shapePointQuery, + shapeSegmentQuery, -- ** For polygons -- $polygon_util @@ -74,13 +77,14 @@ import Physics.Hipmunk.Common import Physics.Hipmunk.Internal +import Physics.Hipmunk.Body (Mass, Moment) -- | There are three types of shapes that can be attached -- to bodies: data ShapeType = -- | A circle is the fastest collision type. It also -- rolls smoothly. - Circle {radius :: !CpFloat} + Circle {radius :: !Distance} -- | A line segment is meant to be used as a static -- shape. (It can be used with moving bodies, however @@ -88,7 +92,7 @@ -- each other.) | LineSegment {start :: !Position, end :: !Position, - thickness :: !CpFloat} + thickness :: !Distance} -- | Polygons are the slowest of all shapes but -- the most flexible. The list of vertices must form @@ -146,17 +150,17 @@ -- | @getBody s@ is the body that this shape is associated --- to. Useful especially in 'Physics.Hipmunk.Space.Callback'. +-- to. Useful especially in a space callback. getBody :: Shape -> Body getBody (S _ b) = b -- | The collision type is used to determine which collision --- 'Physics.Hipmunk.Space.Callback' will be called. Its --- actual value doesn't have a meaning for Chipmunk other --- than the correspondence between shapes and the collision --- pair functions you add. (default is zero) -type CollisionType = #{type unsigned int} +-- callback will be called. Its actual value doesn't have a +-- meaning for Chipmunk other than the correspondence between +-- shapes and the collision pair functions you add. (default is +-- zero) +type CollisionType = #{type cpCollisionType} getCollisionType :: Shape -> IO CollisionType getCollisionType (S shape _) = withForeignPtr shape #{peek cpShape, collision_type} @@ -175,7 +179,7 @@ -- objects such as ragdolls. It may be thought as a lightweight -- alternative to creating a callback that filters the -- collisions. -type Group = #{type unsigned int} +type Group = #{type cpGroup} getGroup :: Shape -> IO Group getGroup (S shape _) = withForeignPtr shape #{peek cpShape, group} @@ -187,11 +191,11 @@ -- | Layers are similar to groups, but use a bitmask. For a collision -- to occur, two shapes must have at least one layer in common. -- In other words, @layer1 .&. layer2@ should be non-zero. --- (default is @0xFFFF@) +-- (default is @-1@, meaning all bits set) -- -- Note that although this type may have more than 32 bits, -- for portability you should only rely on the lower 32 bits. -type Layers = #{type unsigned int} +type Layers = #{type cpLayers} getLayers :: Shape -> IO Layers getLayers (S shape _) = withForeignPtr shape #{peek cpShape, layers} @@ -208,12 +212,11 @@ -- calculated by multiplying the elasticity of both shapes. -- (default is zero) -- --- /IMPORTANT:/ by default no elastic iterations are done --- when the space 'Physics.Hipmunk.Space.step's. This means --- that all shapes react as they had zero elasticity. --- So, if you want some elasticity, remember to call --- 'Physics.Hipmunk.Space.setElasticIterations' to something --- greater than zero, maybe @10@. +-- /IMPORTANT:/ by default old-style elastic iterations are +-- done when the space @step@s, which may result in a +-- not-so-good simulation. It may be a good idea to use +-- @setElasticIterations@ with something greater than zero, +-- maybe @10@. type Elasticity = CpFloat getElasticity :: Shape -> IO Elasticity getElasticity (S shape _) = @@ -256,21 +259,42 @@ + +-- | @moment m s off@ is a convenience function that calculates +-- the moment of inertia for shape @s@ with mass @m@ and at a +-- offset @off@ of the body's center. Uses 'momentForCircle', +-- 'momentForSegment' and 'momentForPoly' internally. +moment :: Mass -> ShapeType -> Position -> Moment +moment m (Circle r) off = m*(r*r + (off `dot` off)) +moment m (LineSegment p1 p2 _) off = momentForSegment m (p1+off) (p2+off) +moment m (Polygon verts) off = momentForPoly m verts off + -- | @momentForCircle m (ri,ro) off@ is the moment of inertia -- of a circle of @m@ mass, inner radius of @ri@, outer radius -- of @ro@ and at an offset @off@ from the center of the body. -momentForCircle :: CpFloat -> (CpFloat, CpFloat) -> Position -> CpFloat +momentForCircle :: Mass -> (Distance, Distance) -> Position -> Moment momentForCircle m (ri,ro) off = (m/2)*(ri*ri + ro*ro) + m*(off `dot` off) -- We recoded the C function to avoid FFI and unsafePerformIO -- on this simple function. +-- | @momentForSegment m p1 p2@ is the moment of inertia of a +-- segment of mass @m@ going from point @p1@ to point @p2@. +momentForSegment :: Mass -> Position -> Position -> Moment +momentForSegment m p1 p2 = + let len' = len (p2 - p1) + offset = scale (p1 + p2) (recip 2) + in m * len' * len' / 12 + m * offset `dot` offset +-- We recoded the C function to avoid FFI and unsafePerformIO +-- on this simple function. + + -- | @momentForPoly m verts off@ is the moment of inertia of a -- polygon of @m@ mass, at offset @off@ from the center of -- the body and comprised of @verts@ vertices. This is similar --- to 'shapePoly' (and the same restrictions for the vertices +-- to 'Polygon' (and the same restrictions for the vertices -- apply as well). -momentForPoly :: CpFloat -> [Position] -> Position -> CpFloat +momentForPoly :: Mass -> [Position] -> Position -> Moment momentForPoly m verts off = (m*sum1)/(6*sum2) where verts' = if off /= 0 then map (+off) verts else verts @@ -290,21 +314,45 @@ pairs :: (a -> a -> b) -> [a] -> [b] pairs f l = zipWith f l (tail $ cycle l) --- | @shapeQuery shape p@ returns @True@ iff the point in --- position @p@ (in world's coordinates) lies within --- the shape @shape@. -shapeQuery :: Shape -> Position -> IO Bool -shapeQuery (S shape _) p = +-- | @shapePointQuery shape p l g@ returns @True@ iff the point +-- in position @p@ (in world's coordinates) lies within the +-- shape @shape@, is not of the same group and share at least +-- one layer. +shapePointQuery :: Shape -> Position -> Layers -> Group -> IO Bool +shapePointQuery (S shape _) p layers group = withForeignPtr shape $ \shape_ptr -> with p $ \p_ptr -> do - i <- wrShapePointQuery shape_ptr p_ptr + i <- wrShapePointQuery shape_ptr p_ptr layers group return (i /= 0) foreign import ccall unsafe "wrapper.h" - wrShapePointQuery :: ShapePtr -> VectorPtr -> IO CInt + wrShapePointQuery :: ShapePtr -> VectorPtr -> Layers -> Group -> IO CInt +-- | @shapeSegmentQuery shape p1 p2 l g@ returns @Just (t,n)@ iff +-- the segment from @p1@ to @p2@ (in world's coordinates) +-- intersects with the shape @shape@, is not of the same group +-- and share at least one layer. In that case, @0 <= t <= 1@ +-- indicates that one of the intersections is at point @p1 + +-- (p2 - p1) \`scale\` t@ with normal @n@. +shapeSegmentQuery :: Shape -> Position -> Position -> Layers -> Group + -> IO (Maybe (CpFloat, Vector)) +shapeSegmentQuery (S shape _) p1 p2 layers group = + withForeignPtr shape $ \shape_ptr -> + with p1 $ \p1_ptr -> + with p2 $ \p2_ptr -> + allocaBytes #{size cpSegmentQueryInfo} $ \info_ptr -> do + i <- wrShapeSegmentQuery shape_ptr p1_ptr p2_ptr layers group info_ptr + if (i == 0) then return Nothing else do + t <- #{peek cpSegmentQueryInfo, t} info_ptr + n <- #{peek cpSegmentQueryInfo, n} info_ptr + return $ Just (t, n) +foreign import ccall unsafe "wrapper.h" + wrShapeSegmentQuery :: ShapePtr -> VectorPtr -> VectorPtr + -> Layers -> Group -> Ptr () -> IO CInt + + -- $polygon_util -- This section is inspired by @pymunk.util@, -- a Python module made from , @@ -429,7 +477,7 @@ -- Note that a very small polygon may be completely \"eaten\" -- if all its vertices are within a @delta@ radius from the -- first. -polyReduce :: CpFloat -> [Position] -> [Position] +polyReduce :: Distance -> [Position] -> [Position] polyReduce delta = go where go (p1:p2:ps) | len (p2-p1) < delta = go (p1:ps) @@ -483,190 +531,3 @@ | otherwise = go min_ (y:acc) ys go min_ acc [] = (min_, acc) - - - -{- - --- | /O((n+k)log n)/ [where /k/ is the number of intersections]. --- @intersections segs@ is the list of all intersections --- found between the list @segs@ of line segments. Each --- intersection is represented as two integers meant --- to be interpreted as two indexes of @segs@ (zero being --- the first line segment). --- --- It is a implementation of the Bentley-Ottmann algorithm (see --- --- for example), however intersection points are \"returned\" --- as soon as they are found. That is, the WHNF of @intersections segs@ --- only needs to calculate the necessary to find the first --- intersection, so if you want to know only if there is a --- an intersection or not then you only need /O(n log n)/ time. --- --- (Note that the @segs@ does not need to be a polygon at all.) -intersections :: [Segment] -> [InterIndexes] -intersections = bentleyOttmann . zip [0..] - -type InterIndexes = (Intersection, SegmentIndex, SegmentIndex) - - ---- Basic data types -type SegmentIndex = Int -type SegmentArray = Array SegmentIndex Segment -type IndSeg = (SegmentIndex, Segment) -type Neighbors = (SegmentIndex, SegmentIndex) -data Event = EvStart !Position !SegmentIndex - | EvEnd !Position !SegmentIndex - | EvInter !Position !SegmentIndex !SegmentIndex - deriving (Eq) - -instance Ord Event where - e1 `compare` e2 = case evPos e1 `compare` evPos e2 of - EQ -> evIdent e1 `compare` evIdent e2 - ot -> ot - where - evIdent (EvStart _ i) = (-2, i) - evIdent (EvEnd _ i) = (-1, i) - evIdent (EvInter _ i j) = (i, j) - -evPos :: Event -> Position -evPos (EvStart p _) = p -evPos (EvEnd p _) = p -evPos (EvInter p _ _) = p - -interErr :: a -interErr = error . ("Physics.Hipmunk.Shape.intersections: " ++) - -interDebug :: Bool -> Bool ---interDebug = const False -interDebug = id - ---- Event queue (a priority queue) -data EventQueue = EQNil - | EQBranch !Event EventQueue EventQueue - -eqSingle :: Event -> EventQueue -eqSingle ev = EQBranch ev EQNil EQNil - -eqGet :: EventQueue -> (Event, EventQueue) -eqGet EQNil = interErr "[eqGet] never get here" -eqGet (EQBranch ev q1 q2) = (ev, eqMerge q1 q2) - -eqMerge :: EventQueue -> EventQueue -> EventQueue -eqMerge EQNil other = other -eqMerge other EQNil = other -eqMerge left@(EQBranch evL _ _) right@(EQBranch evR r1 r2) - = case ev `compare` ev' of - LT -> helper left right - GT -> helper right left - EQ -> eqMerge left (eqMerge r1 r2) -- Discard ev'! - | ev <= ev' = helper left right - | otherwise = helper right left - where - helper (EQBranch ev EQNil q2) r = EQBranch ev r q2 - helper (EQBranch ev q1 q2) r = EQBranch ev q2 (eqMerge q1 r) - -eqInsert :: Event -> EventQueue -> EventQueue -eqInsert ev q = eqMerge q (eqSingle ev) - -eqFromList :: [Event] -> EventQueue -eqFromList evs = foldr eqMerge EQNil . map eqSingle - -eqRun :: (Event -> EventQueue -> ([a], EventQueue)) -> EventQueue -> [a] -eqRun _ EQNil = [] -eqRun f q = case uncurry f $ eqGet q of - (xs, q') -> xs ++ eqRun f q' - ---- Sweep line -type SweepElemRef = IORef (Maybe SweepElem) -data SweepElem = SE !SegmentIndex !SweepElemRef !SweepElemRef -type SweepLine = (IM.IntMap Position, SweepElem) - -slEmpty :: SweepLine -slEmpty = IM.empty - -slInsert :: SegmentIndex -> SweepLine -> IO (SweepLine, [Neighbors]) -slInsert s sl = do - newLeft <- newIORef Nothing - newRight <- newIORef Nothing - let newElem = SE s newLeft newRight - - let update _ set | IM.null set = return [] - update left set = do - let (extrm, oldElem@(SE _ l r)) = f set - where f = if left then IM.findMax else IM.findMin - let newRef = (if left then newLeft else newRight) - oldRef = (if left then r else l) - old <- readIORef oldRef - writeIORef oldRef newElem - writeIORef newRef oldElem - return [extrm] - - let (ltSet, gtSet) = IM.split s sl - l <- update True ltSet - r <- update False gtSet - return (IM.insert s newElem sl, l ++ r) - -slSwap :: Neighbors -> SweepLine -> (SweepLine, [Neighbors]) -slSwap (s1,s2) sl = - let (s1L, s1R) = sl IM.! s1 - (s2L, s2R) = sl IM.! s2 - - newSl = IM.insert s1 (s2L, s2R) $ - IM.insert s2 (s1L, s1R) $ sl - changes = (if s1L == siNone then id else ((s1L,s2):)) - (if s2R == siNone then [] else [(s1,s2R)]) - in if interDebug (s1R /= s2 || s2L /= s1) - then interErr "[slSwap] they're not neighbors!" - else (newSl, changes) - - - - --- -- This looks like Data.Set, however with some peculiarities. --- type SLSize = Int --- data SweepLine = SLNil --- | SLBranch !SLSize !SegmentIndex SweepLine SweepLine - --- slSize :: SweepLine -> SLSize --- slSize SLNil = 0 --- slSize (SLBranch size _ _ _) = size - --- slSingle :: SegmentIndex -> SweepLine --- slSingle s = SLBranch 1 s SLNil SLNil - --- slInsert :: SegmentIndex -> SweepLine -> SweepLine --- slInsert s SLNil = slSingle s --- slInsert s (SLBranch size t l r) = --- case s `compare` t of --- LT -> slBalance t (slInsert s l) r --- GT -> slBalance t l (slInsert s r) --- EQ -> interErr "[slInsert] segment already on sweepline" - --- slRemove :: SegmentIndex -> SweepLine -> SweepLine --- slRemove s SLNil = interErr "[slRemove] segment not on sweepline" --- slRemove s (SLBranch size t l r) = --- case s `compare` t of --- LT -> slBalance t (slRemove s l) r --- GT -> slBalance t l (slRemove s r) - --- slBalance - ---- Bentley-Ottmann functions -boEvents :: IndSeg -> [Event] -boEvents (index, a0,a1) = [EvStart start index, EvEnd end index] - where (start,end) | a0 < a1 = (a0,a1) - | otherwise = (a1,a0) - -bentleyOttmann :: [IndSeg] -> [InterIndexes] -bentleyOttmann isegs = eqRun go . eqFromList . concatMap boEvents - where - segArray = array (0, length isegs - 1) isegs - - go EQNil = [] - go - - ---------- END OF BENTLEY-OTTMANN --------- - --} diff --git a/Physics/Hipmunk/Space.hsc b/Physics/Hipmunk/Space.hsc --- a/Physics/Hipmunk/Space.hsc +++ b/Physics/Hipmunk/Space.hsc @@ -1,7 +1,7 @@ ----------------------------------------------------------------------------- -- | -- Module : Physics/Hipmunk/Space.hsc --- Copyright : (c) Felipe A. Lessa 2008 +-- Copyright : (c) 2008-2009 Felipe A. Lessa -- License : MIT (see LICENSE) -- -- Maintainer : felipe.lessa@gmail.com @@ -38,7 +38,6 @@ getGravity, setGravity, -- ** Damping - Damping, getDamping, setDamping, -- ** Time stamp @@ -52,7 +51,6 @@ rehashStatic, -- ** Point query -- $point_query - QueryType(..), spaceQuery, spaceQueryList, @@ -78,6 +76,7 @@ import Data.IORef import qualified Data.Map as M import Foreign hiding (new) +import Foreign.C.Types (CInt) #include "wrapper.h" import Physics.Hipmunk.Common @@ -216,13 +215,13 @@ foreign import ccall unsafe "wrapper.h" cpSpaceRemoveShape :: SpacePtr -> ShapePtr -> IO () -instance Entity Joint where - spaceAdd = spaceAddHelper unJ cpSpaceAddJoint (const Nothing) - spaceRemove = spaceRemoveHelper unJ cpSpaceRemoveJoint +instance Entity (Constraint a) where + spaceAdd = spaceAddHelper unC cpSpaceAddConstraint (const Nothing) + spaceRemove = spaceRemoveHelper unC cpSpaceRemoveConstraint foreign import ccall unsafe "wrapper.h" - cpSpaceAddJoint :: SpacePtr -> JointPtr -> IO () + cpSpaceAddConstraint :: SpacePtr -> ConstraintPtr -> IO () foreign import ccall unsafe "wrapper.h" - cpSpaceRemoveJoint :: SpacePtr -> JointPtr -> IO () + cpSpaceRemoveConstraint :: SpacePtr -> ConstraintPtr -> IO () -- | A 'StaticShape' is a 'Shape' container that, when added @@ -251,7 +250,7 @@ -- | The number of iterations to use when solving constraints. -- (default is 10). -type Iterations = #{type int} +type Iterations = CInt getIterations :: Space -> IO Iterations getIterations (P sp _ _) = withForeignPtr sp #{peek cpSpace, iterations} @@ -260,9 +259,10 @@ withForeignPtr sp $ \sp_ptr -> do #{poke cpSpace, iterations} sp_ptr it --- | The number of elastic iterations to use when solving constraints. --- (default is 0). -type ElasticIterations = #{type int} +-- | The number of elastic iterations to use when solving +-- constraints. (default is 0, meaning old-style elastic code +-- will be used, which probably isn't what you want). +type ElasticIterations = CInt getElasticIterations :: Space -> IO ElasticIterations getElasticIterations (P sp _ _) = withForeignPtr sp #{peek cpSpace, elasticIterations} @@ -283,7 +283,6 @@ -- | The amount of viscous damping applied to the system. -- (default is 1) -type Damping = CpFloat getDamping :: Space -> IO Damping getDamping (P sp _ _) = withForeignPtr sp #{peek cpSpace, damping} @@ -294,7 +293,7 @@ -- | The time stamp of the simulation, increased in 1 -- every time 'step' is called. -type TimeStamp = #{type int} +type TimeStamp = CInt getTimeStamp :: Space -> IO TimeStamp getTimeStamp (P sp _ _) = withForeignPtr sp #{peek cpSpace, stamp} @@ -312,34 +311,34 @@ -- -- Chipmunk's performance is highly sensitive to both -- parameters, which should be hand-tuned to maximize --- performance. It is in general recommended to set --- @dim@ as the average object size and @count@ around --- 10 times the number of objects in the hash. Usually --- bigger numbers are better to @count@, but only to --- point. By default dim is @100.0@ and count is @1000@. +-- performance. It is in general recommended to set @dim@ as +-- the average object size and @count@ around 10 times the +-- number of objects in the hash. Usually bigger numbers are +-- better to @count@, but only to a certain point. By default +-- dim is @100.0@ and count is @1000@. -- -- Note that in the case of the static hash you may try -- larger numbers as the static hash is only rehashed -- when requested by 'rehashStatic', however that will -- use more memory. -resizeStaticHash :: Space -> CpFloat -> #{type int} -> IO () +resizeStaticHash :: Space -> Distance -> CInt -> IO () resizeStaticHash (P sp _ _) dim count = withForeignPtr sp $ \sp_ptr -> do cpSpaceResizeStaticHash sp_ptr dim count foreign import ccall unsafe "wrapper.h" cpSpaceResizeStaticHash :: SpacePtr -> CpFloat - -> #{type int} -> IO () + -> CInt -> IO () -resizeActiveHash :: Space -> CpFloat -> #{type int} -> IO () +resizeActiveHash :: Space -> Distance -> CInt -> IO () resizeActiveHash (P sp _ _) dim count = withForeignPtr sp $ \sp_ptr -> do cpSpaceResizeActiveHash sp_ptr dim count foreign import ccall unsafe "wrapper.h" cpSpaceResizeActiveHash :: SpacePtr -> CpFloat - -> #{type int} -> IO () + -> CInt -> IO () -- | Rehashes the shapes in the static spatial hash. -- You only need to call this if you move one of the @@ -360,32 +359,26 @@ -- for example, to know if a shape was clicked by -- the user. --- | You may query the static hash, the active hash --- or both. -data QueryType = ActiveHash | StaticHash | Both - --- | @spaceQuery sp query pos cb@ will call @cb@ for every +-- | @spaceQuery sp pos l g cb@ will call @cb@ for every -- shape that -- -- * Contains point @pos@ (in world's coordinates). -- --- * Is in the hash selected by @query@ (see 'QueryType'). +-- * Isn't of the same group as @g@. -- +-- * Shares at least on layer with @l@. +-- -- The order in which the callback is called is unspecified. -- However it is guaranteed that it will be called once, -- and only once, for each of the shapes described above -- (and never for those who aren't). -spaceQuery :: Space -> QueryType -> Position -> (Shape -> IO ()) -> IO () -spaceQuery spce@(P sp _ _) query pos callback = +spaceQuery :: Space -> Position -> Layers -> Group -> (Shape -> IO ()) -> IO () +spaceQuery spce@(P sp _ _) pos layers group callback = withForeignPtr sp $ \sp_ptr -> bracket (makePointQueryFunc cb) freeHaskellFunPtr $ \cb_ptr -> with pos $ \pos_ptr -> - func sp_ptr pos_ptr cb_ptr + wrSpacePointQuery sp_ptr pos_ptr layers group cb_ptr where - func = case query of - ActiveHash -> wrSpaceActiveShapePointQuery - StaticHash -> wrSpaceStaticShapePointQuery - Both -> wrSpaceBothShapePointQuery cb shape_ptr _ = retriveShape spce shape_ptr >>= callback type PointQueryFunc = ShapePtr -> Ptr () -> IO () @@ -393,23 +386,17 @@ foreign import ccall "wrapper" makePointQueryFunc :: PointQueryFunc -> IO PointQueryFuncPtr foreign import ccall safe "wrapper.h" - wrSpaceActiveShapePointQuery - :: SpacePtr -> VectorPtr -> PointQueryFuncPtr -> IO () -foreign import ccall safe "wrapper.h" - wrSpaceStaticShapePointQuery - :: SpacePtr -> VectorPtr -> PointQueryFuncPtr -> IO () -foreign import ccall safe "wrapper.h" - wrSpaceBothShapePointQuery - :: SpacePtr -> VectorPtr -> PointQueryFuncPtr -> IO () + wrSpacePointQuery :: SpacePtr -> VectorPtr -> Layers -> Group + -> PointQueryFuncPtr -> IO () --- | @spaceQueryList sp query pos@ acts like 'spaceQuery' but +-- | @spaceQueryList sp pos l g@ acts like 'spaceQuery' but -- returns a list of 'Shape's instead of calling a callback. -- This is just a convenience function. -spaceQueryList :: Space -> QueryType -> Position -> IO [Shape] -spaceQueryList spce query pos = do +spaceQueryList :: Space -> Position -> Layers -> Group -> IO [Shape] +spaceQueryList spce pos layers group = do var <- newIORef [] - spaceQuery spce query pos $ modifyIORef var . (:) + spaceQuery spce pos layers group $ modifyIORef var . (:) readIORef var @@ -438,18 +425,18 @@ -- If @False@, then the collision will be ignored. -- -- The callbacks themselves may execute arbitrary operations --- with a simple exception: /callbacks cannot add or remove --- entities from the space/. You can of course create a queue +-- with a simple exception: /callbacks cannot add or remove/ +-- /entities from the space/. You can of course create a queue -- of add\/remove actions and then process it after 'step' -- returns. -- --- As for the events that trigger collision pair functions, --- the rule is simple. All shapes have a 'CollisionType'. --- When shapes @a@ and @b@ collide, if there was a callback --- associated with @a@'s and @b@'s collision types, then --- it is called. Otherwise the default callback is called. --- By default, the default callback always returns @True@ --- (i.e. all collisions are treated). +-- As for the events that trigger collision pair functions, the +-- rule is simple. All shapes have a 'CollisionType'. When +-- shapes @a@ and @b@ collide, if there was a callback +-- associated with @a@'s and @b@'s collision types, then it is +-- called. Otherwise the default callback is called. The +-- default callback always returns @True@ (i.e. all collisions +-- are treated). -- | A 'Callback' function can be of three types: @@ -480,7 +467,7 @@ -- | Internal. Type of callback used by Chipmunk. -type ChipmunkCB = ShapePtr -> ShapePtr -> ContactPtr -> #{type int} +type ChipmunkCB = ShapePtr -> ShapePtr -> ContactPtr -> CInt -> CpFloat -> Ptr () -> IO Int type ChipmunkCBPtr = FunPtr ChipmunkCB @@ -644,7 +631,7 @@ sumImpulses = sumImpulsesInternal wrContactsSumImpulses foreign import ccall unsafe "wrapper.h" - wrContactsSumImpulses :: ContactPtr -> #{type int} + wrContactsSumImpulses :: ContactPtr -> CInt -> VectorPtr -> IO () -- | Sums the impulses applied to the given contact points. @@ -655,10 +642,10 @@ sumImpulsesInternal wrContactsSumImpulsesWithFriction foreign import ccall unsafe "wrapper.h" - wrContactsSumImpulsesWithFriction :: ContactPtr -> #{type int} + wrContactsSumImpulsesWithFriction :: ContactPtr -> CInt -> VectorPtr -> IO () -sumImpulsesInternal :: (ContactPtr -> #{type int} -> VectorPtr -> IO ()) +sumImpulsesInternal :: (ContactPtr -> CInt -> VectorPtr -> IO ()) -> StorableArray Int Contact -> IO Vector sumImpulsesInternal func sa = do (i1,i2) <- getBounds sa diff --git a/Physics/Hipmunk/Unsafe.hsc b/Physics/Hipmunk/Unsafe.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Unsafe.hsc @@ -0,0 +1,87 @@ +----------------------------------------------------------------------------- +-- | +-- Module : Physics/Hipmunk/Unsafe.hsc +-- Copyright : (c) 2008-2009 Felipe A. Lessa +-- License : MIT (see LICENSE) +-- +-- Maintainer : felipe.lessa@gmail.com +-- Stability : provisional +-- Portability : portable (needs FFI) +-- +-- All functions on this module are /UNSAFE/ in the sense that +-- they may reduce the physical accuracy or numerical stability +-- of the simulation if you use them correctly, or may crash your +-- system if you are not careful enough. Read their +-- documentation carefully and use them only if you really need +-- and know what you are doing. +-- +----------------------------------------------------------------------------- + +module Physics.Hipmunk.Unsafe + (-- * Shapes + unsafeShapeRedefine, + -- * Constraints + unsafeRememberC + ) + where + +import Foreign hiding (rotate, new) +import Foreign.C +#include "wrapper.h" + +import Physics.Hipmunk.Common +import Physics.Hipmunk.Internal +import Physics.Hipmunk.Shape +import Physics.Hipmunk.Constraint (Unknown) + +-- | @unsafeShapeRedefine shape type off@ redefines @shape@ to +-- have new parameters described on @type@ and to be at offset +-- @off@. Be careful, /you should not change the shape type/. +-- For example, it is unsafe to change a circle shape's radius, +-- but it is an error to try to change a circle into a segment +-- or a polygon. Note also that these errors /are not +-- checked/, meaning /they will probably crash Chipmunk/. +unsafeShapeRedefine :: Shape -> ShapeType -> Position -> IO () +unsafeShapeRedefine (S shape _) (Circle r) off = + withForeignPtr shape $ \shape_ptr -> + with off $ \off_ptr -> do + cpCircleShapeSetRadius shape_ptr r + wrCircleShapeSetOffset shape_ptr off_ptr + +unsafeShapeRedefine (S shape _) (LineSegment p1 p2 r) off = + withForeignPtr shape $ \shape_ptr -> + with (p1+off) $ \p1off_ptr -> + with (p2+off) $ \p2off_ptr -> do + wrSegmentShapeSetEndpoints shape_ptr p1off_ptr p2off_ptr + cpSegmentShapeSetRadius shape_ptr r + +unsafeShapeRedefine (S shape _) (Polygon verts) off = + withForeignPtr shape $ \shape_ptr -> + with off $ \off_ptr -> + withArrayLen verts $ \verts_len verts_ptr -> do + let verts_len' = fromIntegral verts_len + wrPolyShapeSetVerts shape_ptr verts_len' verts_ptr off_ptr + +foreign import ccall unsafe "wrapper.h" + cpCircleShapeSetRadius :: ShapePtr -> CpFloat -> IO () +foreign import ccall unsafe "wrapper.h" + wrCircleShapeSetOffset :: ShapePtr -> VectorPtr -> IO () +foreign import ccall unsafe "wrapper.h" + wrSegmentShapeSetEndpoints :: ShapePtr -> VectorPtr -> VectorPtr -> IO () +foreign import ccall unsafe "wrapper.h" + cpSegmentShapeSetRadius :: ShapePtr -> CpFloat -> IO () +foreign import ccall unsafe "wrapper.h" + wrPolyShapeSetVerts :: ShapePtr -> CInt -> VectorPtr -> VectorPtr -> IO () + + + + +-- | Unsafe function that changes the constraint type to +-- anything. It is unsafe because you should call 'redefine' +-- only on the same kind of constraint you created, and this +-- function allows you to bypass the type system checks. Note +-- Also that, unlike Chipmunk, we don't check at run-time that +-- 'redefine' is being called on the right type! +unsafeRememberC :: ConstraintType a => Constraint Unknown -> Constraint a +unsafeRememberC (C c b1 b2) = C c b1 b2 +{-# INLINE unsafeRememberC #-} diff --git a/Physics/Hipmunk/wrapper.c b/Physics/Hipmunk/wrapper.c --- a/Physics/Hipmunk/wrapper.c +++ b/Physics/Hipmunk/wrapper.c @@ -14,9 +14,9 @@ void wrBodyApplyImpulse(cpBody *b, cpVect *j, cpVect *r) { cpBodyApplyImpulse(b, *j, *r); } -void wrDampedSpring(cpBody *b1, cpBody *b2, cpVect *a1, cpVect *a2, - cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) { - cpDampedSpring(b1, b2, *a1, *a2, rlen, k, dmp, dt); +void wrApplyDampedSpring(cpBody *b1, cpBody *b2, cpVect *a1, cpVect *a2, + cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) { + cpApplyDampedSpring(b1, b2, *a1, *a2, rlen, k, dmp, dt); } void wrBodyLocal2World(cpBody *b, cpVect *v) { cpVect ret = cpBodyLocal2World(b, *v); @@ -42,36 +42,67 @@ cpVect *a, cpVect *b, cpFloat r) { cpSegmentShapeInit(seg, body, *a, *b, r); } -int wrShapePointQuery(cpShape *shape, cpVect *p) { - return cpShapePointQuery(shape, *p); +int wrShapePointQuery(cpShape *shape, cpVect *p, + cpLayers layers, cpGroup group) { + return cpShapePointQuery(shape, *p, layers, group); } +int cpShapeSegmentQuery(cpShape*, cpVect, cpVect, cpLayers, cpGroup, cpSegmentQueryInfo*); +int wrShapeSegmentQuery(cpShape *shape, cpVect *a, cpVect *b, + cpLayers layers, cpGroup group, + cpSegmentQueryInfo *info) { + return cpShapeSegmentQuery(shape, *a, *b, layers, group, info); +} + // From cpPolyShape.h void wrPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect *offset) { cpPolyShapeInit(poly, body, numVerts, verts, *offset); } -// From cpJoint.h -void wrPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, - cpVect *anchr1, cpVect *anchr2) { +// From various constraints +void wrPinJointInit(cpVect *anchr1, cpVect *anchr2, + cpPinJoint *joint, cpBody *a, cpBody *b) { cpPinJointInit(joint, a, b, *anchr1, *anchr2); } -void wrSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, - cpVect *anchr1, cpVect *anchr2, - cpFloat min, cpFloat max) { +void wrSlideJointInit(cpFloat min, cpFloat max, cpVect *anchr1, cpVect *anchr2, + cpSlideJoint *joint, cpBody *a, cpBody *b) { cpSlideJointInit(joint, a, b, *anchr1, *anchr2, min, max); } -void wrPivotJointInit(cpPivotJoint *joint, cpBody *a, - cpBody *b, cpVect *pivot) { - cpPivotJointInit(joint, a, b, *pivot); +void wrPivot1JointInit(cpVect *pivot, cpPivotJoint *joint, cpBody *a, cpBody *b) { + cpPivotJointInit(joint, a, b, cpBodyWorld2Local(a, *pivot), + cpBodyWorld2Local(b, *pivot)); } -void wrGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, - cpVect *groove_a, cpVect *groove_b, cpVect *anchr2) { +void wrPivot2JointInit(cpVect *anchr1, cpVect *anchr2, + cpPivotJoint *joint, cpBody *a, cpBody *b) { + cpPivotJointInit(joint, a, b, *anchr1, *anchr2); +} +void wrGrooveJointInit(cpVect *groove_a, cpVect *groove_b, cpVect *anchr2, + cpGrooveJoint *joint, cpBody *a, cpBody *b) { cpGrooveJointInit(joint, a, b, *groove_a, *groove_b, *anchr2); } - +void wrGearJointInit(cpFloat phase, cpFloat ratio, + cpGearJoint *joint, cpBody *a, cpBody *b) { + cpGearJointInit(joint, a, b, phase, ratio); +} +void wrDampedSpringInit(cpFloat restLength, cpFloat stiffness, cpFloat damping, + cpVect *anchr1, cpVect *anchr2, + cpDampedSpring *joint, cpBody *a, cpBody *b) { + cpDampedSpringInit(joint, a, b, *anchr1, *anchr2, + restLength, stiffness, damping); +} +void wrDampedRotarySpringInit(cpFloat restAngle, cpFloat stiffness, cpFloat damping, + cpDampedRotarySpring *joint, cpBody *a, cpBody *b) { + cpDampedRotarySpringInit(joint, a, b, restAngle, stiffness, damping); +} +void wrRotaryLimitJointInit(cpFloat min, cpFloat max, + cpRotaryLimitJoint *joint, cpBody *a, cpBody *b) { + cpRotaryLimitJointInit(joint, a, b, min, max); +} +void wrSimpleMotorInit(cpFloat rate, cpSimpleMotor *joint, cpBody *a, cpBody *b) { + cpSimpleMotorInit(joint, a, b, rate); +} // From cpArbiter.h void wrContactsSumImpulses(cpContact *contacts, int numContacts, cpVect *ret) { @@ -83,13 +114,18 @@ } // From cpSpace.h -void wrSpaceActiveShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func) { - cpSpaceShapePointQuery(space, *point, func, NULL); +void wrSpacePointQuery(cpSpace *space, cpVect *point, cpLayers layers, + cpGroup group, cpSpacePointQueryFunc func) { + cpSpacePointQuery(space, *point, layers, group, func, NULL); } -void wrSpaceStaticShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func) { - cpSpaceStaticShapePointQuery(space, *point, func, NULL); + +// From chipmunk_unsafe.h +void wrCircleShapeSetOffset(cpShape *shape, cpVect *offset) { + cpCircleShapeSetOffset(shape, *offset); } -void wrSpaceBothShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func) { - cpSpaceShapePointQuery(space, *point, func, NULL); - cpSpaceStaticShapePointQuery(space, *point, func, NULL); +void wrSegmentShapeSetEndpoints(cpShape *shape, cpVect *a, cpVect *b) { + cpSegmentShapeSetEndpoints(shape, *a, *b); +} +void wrPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect *offset) { + cpPolyShapeSetVerts(shape, numVerts, verts, *offset); } diff --git a/Physics/Hipmunk/wrapper.h b/Physics/Hipmunk/wrapper.h --- a/Physics/Hipmunk/wrapper.h +++ b/Physics/Hipmunk/wrapper.h @@ -1,51 +1,52 @@ #ifndef WRAPPER_H #define WRAPPER_H +#include #include "chipmunk.h" +#include "chipmunk_unsafe.h" // New functions -int wrConstantCallback(cpShape *a, cpShape *b, cpContact *contacts, - int numContacts, cpFloat normal_coef, void *data); - +int wrConstantCallback(cpShape*, cpShape*, cpContact*, int, cpFloat, void*); // From cpBody.h -void wrBodyUpdateVelocity(cpBody *b, cpVect *g, cpFloat d, cpFloat dt); -void wrBodyApplyImpulse(cpBody *b, cpVect *j, cpVect *r); -void wrDampedSpring(cpBody *b1, cpBody *b2, cpVect *a1, cpVect *a2, - cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); -void wrBodyLocal2World(cpBody *b, cpVect *v); -void wrBodyWorld2Local(cpBody *b, cpVect *v); -void wrBodyApplyForce(cpBody *b, cpVect *f, cpVect *p); +void wrBodyUpdateVelocity(cpBody*, cpVect*, cpFloat, cpFloat); +void wrBodyApplyImpulse(cpBody*, cpVect*, cpVect*); +void wrDampedSpring(cpBody*, cpBody*, cpVect*, cpVect*, cpFloat, cpFloat, cpFloat, cpFloat); +void wrBodyLocal2World(cpBody*, cpVect*); +void wrBodyWorld2Local(cpBody*, cpVect*); +void wrBodyApplyForce(cpBody*, cpVect*, cpVect*); // From cpShape.h -void wrCircleShapeInit(cpCircleShape *circle, cpBody *body, - cpVect *offset, cpFloat radius); -void wrSegmentShapeInit(cpSegmentShape *seg, cpBody *body, - cpVect *a, cpVect *b, cpFloat r); -int wrShapePointQuery(cpShape *shape, cpVect *p); +void wrCircleShapeInit(cpCircleShape*, cpBody*, cpVect*, cpFloat); +void wrSegmentShapeInit(cpSegmentShape*, cpBody*, cpVect*, cpVect*, cpFloat); +int wrShapePointQuery(cpShape*, cpVect*, cpLayers, cpGroup); +int wrShapeSegmentQuery(cpShape*, cpVect*, cpVect*, cpLayers, cpGroup, cpSegmentQueryInfo*); // From cpPolyShape.h -void wrPolyShapeInit(cpPolyShape *poly, cpBody *body, - int numVerts, cpVect *verts, cpVect *offset); +void wrPolyShapeInit(cpPolyShape*, cpBody*, int, cpVect*, cpVect*); -// From cpJoint.h -void wrPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, - cpVect *anchr1, cpVect *anchr2); -void wrSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, - cpVect *anchr1, cpVect *anchr2, - cpFloat min, cpFloat max); -void wrPivotJointInit(cpPivotJoint *joint, cpBody *a, - cpBody *b, cpVect *pivot); -void wrGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, - cpVect *groove_a, cpVect *groove_b, cpVect *anchr2); +// From various constraints +// Note that we change the argument order to allow easy currying on Haskell side. +void wrPinJointInit(cpVect*, cpVect*, cpPinJoint*, cpBody*, cpBody*); +void wrSlideJointInit(cpFloat, cpFloat, cpVect*, cpVect*, cpSlideJoint*, cpBody*, cpBody*); +void wrPivot1JointInit(cpVect*, cpPivotJoint*, cpBody*, cpBody*); +void wrPivot2JointInit(cpVect*, cpVect*, cpPivotJoint*, cpBody*, cpBody*); +void wrGrooveJointInit(cpVect*, cpVect*, cpVect*, cpGrooveJoint*, cpBody*, cpBody*); +void wrGearJointInit(cpFloat, cpFloat, cpGearJoint*, cpBody*, cpBody*); +void wrDampedSpringInit(cpFloat, cpFloat, cpFloat, cpVect*, cpVect*, cpDampedSpring*, cpBody*, cpBody*); +void wrDampedRotarySpringInit(cpFloat, cpFloat, cpFloat, cpDampedRotarySpring*, cpBody*, cpBody*); +void wrRotaryLimitJointInit(cpFloat, cpFloat, cpRotaryLimitJoint*, cpBody*, cpBody*); +void wrSimpleMotorInit(cpFloat, cpSimpleMotor*, cpBody*, cpBody*); // From cpArbiter.h -void wrContactsSumImpulses(cpContact *contacts, int numContacts, cpVect *ret); -void wrContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts, - cpVect *ret); +void wrContactsSumImpulses(cpContact*, int, cpVect*); +void wrContactsSumImpulsesWithFriction(cpContact*, int, cpVect*); // From cpSpace.h -void wrSpaceActiveShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func); -void wrSpaceStaticShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func); -void wrSpaceBothShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func); +void wrSpacePointQuery(cpSpace*, cpVect*, cpLayers, cpGroup, cpSpacePointQueryFunc); + +// From chipmunk_unsafe.h +void wrCircleShapeSetOffset(cpShape*, cpVect*); +void wrSegmentShapeSetEndpoints(cpShape*, cpVect*, cpVect*); +void wrPolyShapeSetVerts(cpShape*, int, cpVect*, cpVect*); #endif diff --git a/chipmunk-5.0/chipmunk.c b/chipmunk-5.0/chipmunk.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/chipmunk.c @@ -0,0 +1,78 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk.h" + +#ifdef __cplusplus +extern "C" { +#endif + void cpInitCollisionFuncs(void); +#ifdef __cplusplus +} +#endif + + +void +cpInitChipmunk(void) +{ + cpInitCollisionFuncs(); +} + +cpFloat +cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) +{ + return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset); +} + +cpFloat +cpMomentForSegment(cpFloat m, cpVect a, cpVect b) +{ + cpFloat length = cpvlength(cpvsub(b, a)); + cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f); + + return m*length*length/12.0f + m*cpvdot(offset, offset); +} + +cpFloat +cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset) +{ + cpVect *tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect)); + for(int i=0; i b) ? a : b; +} + +static inline cpFloat +cpfmin(cpFloat a, cpFloat b) +{ + return (a < b) ? a : b; +} + +static inline cpFloat +cpfabs(cpFloat n) +{ + return (n < 0) ? -n : n; +} + +static inline cpFloat +cpfclamp(cpFloat f, cpFloat min, cpFloat max){ + return cpfmin(cpfmax(f, min), max); +} + +#ifndef INFINITY + #ifdef _MSC_VER + union MSVC_EVIL_FLOAT_HACK + { + unsigned __int8 Bytes[4]; + float Value; + }; + static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; + #define INFINITY (INFINITY_HACK.Value) + #else + #define INFINITY (1e1000) + #endif +#endif + +#include "cpVect.h" +#include "cpBB.h" +#include "cpBody.h" +#include "cpArray.h" +#include "cpHashSet.h" +#include "cpSpaceHash.h" + +#include "cpShape.h" +#include "cpPolyShape.h" + +#include "cpArbiter.h" +#include "cpCollision.h" + +#include "constraints/cpConstraint.h" + +#include "cpSpace.h" + +#define CP_HASH_COEF (3344921057ul) +#define CP_HASH_PAIR(A, B) ((cpHashValue)(A)*CP_HASH_COEF ^ (cpHashValue)(B)*CP_HASH_COEF) + +void cpInitChipmunk(void); + +// Calculate the moment of inertia for a circle, r1 and r2 are the inner and outer diameters. +// (A solid circle has an inner diameter of 0) +cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset); + +// Calculate the moment of inertia for a line segment. (beveling radius not supported) +cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b); + +// Calculate the moment of inertia for a solid polygon shape. +cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/chipmunk-5.0/chipmunk_types.h b/chipmunk-5.0/chipmunk_types.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/chipmunk_types.h @@ -0,0 +1,26 @@ +typedef double cpFloat; +#define cpfsqrt sqrt +#define cpfsin sin +#define cpfcos cos +#define cpfatan2 atan2 +#define cpfmod fmod +#define cpfexp exp +#define cpfpow pow +#define cpffloor floor +#define cpfceil ceil + +//typedef float cpFloat; +//#define cpfsqrt sqrtf +//#define cpfsin sinf +//#define cpfcos cosf +//#define cpfatan2 atan2f +//#define cpfmod fmodf +//#define cpfexp expf +//#define cpfpow powf +//#define cpffloor floorf +//#define cpfceil ceilf + +typedef size_t cpHashValue; +typedef unsigned int cpCollisionType; +typedef unsigned int cpLayers; +typedef unsigned int cpGroup; diff --git a/chipmunk-5.0/chipmunk_unsafe.h b/chipmunk-5.0/chipmunk_unsafe.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/chipmunk_unsafe.h @@ -0,0 +1,54 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + + + +/* This header defines a number of "unsafe" operations on Chipmunk objects. + * In this case "unsafe" is referring to operations which may reduce the + * physical accuracy or numerical stability of the simulation, but will not + * cause crashes. + * + * The prime example is mutating collision shapes. Chipmunk does not support + * this directly. Mutating shapes using this API will caused objects in contact + * to be pushed apart using Chipmunk's overlap solver, but not using real + * persistent velocities. Probably not what you meant, but perhaps close enough. + */ + +#ifndef CHIPMUNK_UNSAFE_HEADER +#define CHIPMUNK_UNSAFE_HEADER + +#ifdef __cplusplus +extern "C" { +#endif + +void cpCircleShapeSetRadius(cpShape *shape, cpFloat radius); +void cpCircleShapeSetOffset(cpShape *shape, cpVect offset); + +void cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b); +void cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius); + +void cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset); + +#ifdef __cplusplus +} +#endif + +#endif diff --git a/chipmunk-5.0/constraints/cpConstraint.c b/chipmunk-5.0/constraints/cpConstraint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpConstraint.c @@ -0,0 +1,60 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "../chipmunk.h" +#include "util.h" + +// TODO: Comment me! + +cpFloat cp_constraint_bias_coef = 0.1f; + +void cpConstraintDestroy(cpConstraint *constraint){} + +void +cpConstraintFree(cpConstraint *constraint) +{ + if(constraint) cpConstraintDestroy(constraint); + free(constraint); +} + +void +cpConstraintCheckCast(cpConstraint *constraint, const cpConstraintClass *klass) +{ + assert(constraint->klass == klass); // Bad cpConstraint type in cast +} + + +// *** defined in util.h + +void +cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b) +{ + constraint->klass = klass; + constraint->a = a; + constraint->b = b; + + constraint->maxForce = INFINITY; + constraint->biasCoef = cp_constraint_bias_coef; + constraint->maxBias = INFINITY; +} diff --git a/chipmunk-5.0/constraints/cpConstraint.h b/chipmunk-5.0/constraints/cpConstraint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpConstraint.h @@ -0,0 +1,90 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// TODO: Comment me! + +extern cpFloat cp_constraint_bias_coef; + +struct cpConstraintClass; +struct cpConstraint; + +typedef void (*cpConstraintPreStepFunction)(struct cpConstraint *constraint, cpFloat dt, cpFloat dt_inv); +typedef void (*cpConstraintApplyImpulseFunction)(struct cpConstraint *constraint); +typedef cpFloat (*cpConstraintGetImpulseFunction)(struct cpConstraint *constraint); + +typedef struct cpConstraintClass { + cpConstraintPreStepFunction preStep; + cpConstraintApplyImpulseFunction applyImpulse; + cpConstraintGetImpulseFunction getImpulse; +} cpConstraintClass; + + + +typedef struct cpConstraint { + const cpConstraintClass *klass; + + cpBody *a, *b; + cpFloat maxForce; + cpFloat biasCoef; + cpFloat maxBias; + + void *data; +} cpConstraint; + +void cpConstraintDestroy(cpConstraint *constraint); +void cpConstraintFree(cpConstraint *constraint); + + +void cpConstraintCheckCast(cpConstraint *constraint, const cpConstraintClass *klass); + +#define CP_DefineConstraintProperty(struct, type, member, name) \ +static inline type \ +struct##Get##name(cpConstraint *constraint){ \ + cpConstraintCheckCast(constraint, struct##GetClass()); \ + return ((struct *)constraint)->member; \ +} \ +static inline void \ +struct##Set##name(cpConstraint *constraint, type value){ \ + cpConstraintCheckCast(constraint, struct##GetClass()); \ + ((struct *)constraint)->member = value; \ +} \ +/* These are for compatibility with the interim trunk version, for some reason I thought I needed the underscores to make the macro work */ \ +static inline type \ +struct##_get_##member(cpConstraint *constraint){ \ + cpConstraintCheckCast(constraint, struct##GetClass()); \ + return ((struct *)constraint)->member; \ +} \ +static inline void \ +struct##_set_##member(cpConstraint *constraint, type value){ \ + cpConstraintCheckCast(constraint, struct##GetClass()); \ + ((struct *)constraint)->member = value; \ +} + +// Built in Joint types +#include "cpPinJoint.h" +#include "cpSlideJoint.h" +#include "cpPivotJoint.h" +#include "cpGrooveJoint.h" +#include "cpDampedSpring.h" +#include "cpDampedRotarySpring.h" +#include "cpRotaryLimitJoint.h" +#include "cpGearJoint.h" +#include "cpSimpleMotor.h" diff --git a/chipmunk-5.0/constraints/cpDampedRotarySpring.c b/chipmunk-5.0/constraints/cpDampedRotarySpring.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpDampedRotarySpring.c @@ -0,0 +1,106 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "../chipmunk.h" +#include "util.h" + +static cpFloat +defaultSpringTorque(cpDampedRotarySpring *spring, cpFloat relativeAngle){ + return (relativeAngle - spring->restAngle)*spring->stiffness; +} + +static void +preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + spring->iSum = 1.0f/(a->i_inv + b->i_inv); + + spring->dt = dt; + spring->target_wrn = 0.0f; + + // apply spring torque + cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt; + a->w -= j_spring*a->i_inv; + b->w += j_spring*b->i_inv; +} + +static void +applyImpulse(cpDampedRotarySpring *spring) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + // compute relative velocity + cpFloat wrn = a->w - b->w;//normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat w_damp = wrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->iSum)); + spring->target_wrn = wrn - w_damp; + + //apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); + cpFloat j_damp = w_damp*spring->iSum; + a->w -= j_damp*a->i_inv; + b->w += j_damp*b->i_inv; +} + +static cpFloat +getImpulse(cpConstraint *constraint) +{ + return 0.0f; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpDampedRotarySpring) + +cpDampedRotarySpring * +cpDampedRotarySpringAlloc(void) +{ + return (cpDampedRotarySpring *)malloc(sizeof(cpDampedRotarySpring)); +} + +cpDampedRotarySpring * +cpDampedRotarySpringInit(cpDampedRotarySpring *spring, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, &klass, a, b); + + spring->restAngle = restAngle; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springTorqueFunc = (cpDampedRotarySpringTorqueFunc)defaultSpringTorque; + + return spring; +} + +cpConstraint * +cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedRotarySpringInit(cpDampedRotarySpringAlloc(), a, b, restAngle, stiffness, damping); +} diff --git a/chipmunk-5.0/constraints/cpDampedRotarySpring.h b/chipmunk-5.0/constraints/cpDampedRotarySpring.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpDampedRotarySpring.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef cpFloat (*cpDampedRotarySpringTorqueFunc)(struct cpConstraint *spring, cpFloat relativeAngle); + +const cpConstraintClass *cpDampedRotarySpringGetClass(); + +typedef struct cpDampedRotarySpring { + cpConstraint constraint; + cpFloat restAngle; + cpFloat stiffness; + cpFloat damping; + cpDampedRotarySpringTorqueFunc springTorqueFunc; + + cpFloat dt; + cpFloat target_wrn; + + cpFloat iSum; +} cpDampedRotarySpring; + +cpDampedRotarySpring *cpDampedRotarySpringAlloc(void); +cpDampedRotarySpring *cpDampedRotarySpringInit(cpDampedRotarySpring *joint, cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); +cpConstraint *cpDampedRotarySpringNew(cpBody *a, cpBody *b, cpFloat restAngle, cpFloat stiffness, cpFloat damping); + +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, restAngle, RestAngle); +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, stiffness, Stiffness); +CP_DefineConstraintProperty(cpDampedRotarySpring, cpFloat, damping, Damping); diff --git a/chipmunk-5.0/constraints/cpDampedSpring.c b/chipmunk-5.0/constraints/cpDampedSpring.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpDampedSpring.c @@ -0,0 +1,117 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "../chipmunk.h" +#include "util.h" + +static cpFloat +defaultSpringForce(cpDampedSpring *spring, cpFloat dist){ + return (spring->restLength - dist)*spring->stiffness; +} + +static void +preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + spring->r1 = cpvrotate(spring->anchr1, a->rot); + spring->r2 = cpvrotate(spring->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1)); + cpFloat dist = cpvlength(delta); + spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + // calculate mass normal + spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n); + + spring->dt = dt; + spring->target_vrn = 0.0f; + + // apply spring force + cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist); + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt)); +} + +static void +applyImpulse(cpDampedSpring *spring) +{ + cpBody *a = spring->constraint.a; + cpBody *b = spring->constraint.b; + + cpVect n = spring->n; + cpVect r1 = spring->r1; + cpVect r2 = spring->r2; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn; + + // compute velocity loss from drag + // not 100% certain this is derived correctly, though it makes sense + cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass)); + spring->target_vrn = vrn + v_damp; + + apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass)); +} + +static cpFloat +getImpulse(cpConstraint *constraint) +{ + return 0.0f; +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpDampedSpring) + +cpDampedSpring * +cpDampedSpringAlloc(void) +{ + return (cpDampedSpring *)malloc(sizeof(cpDampedSpring)); +} + +cpDampedSpring * +cpDampedSpringInit(cpDampedSpring *spring, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + cpConstraintInit((cpConstraint *)spring, cpDampedSpringGetClass(), a, b); + + spring->anchr1 = anchr1; + spring->anchr2 = anchr2; + + spring->restLength = restLength; + spring->stiffness = stiffness; + spring->damping = damping; + spring->springForceFunc = (cpDampedSpringForceFunc)defaultSpringForce; + + return spring; +} + +cpConstraint * +cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping) +{ + return (cpConstraint *)cpDampedSpringInit(cpDampedSpringAlloc(), a, b, anchr1, anchr2, restLength, stiffness, damping); +} diff --git a/chipmunk-5.0/constraints/cpDampedSpring.h b/chipmunk-5.0/constraints/cpDampedSpring.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpDampedSpring.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +struct cpDampedSpring; + +typedef cpFloat (*cpDampedSpringForceFunc)(struct cpConstraint *spring, cpFloat dist); + +const cpConstraintClass *cpDampedSpringGetClass(); + +typedef struct cpDampedSpring { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat restLength; + cpFloat stiffness; + cpFloat damping; + cpDampedSpringForceFunc springForceFunc; + + cpFloat dt; + cpFloat target_vrn; + + cpVect r1, r2; + cpFloat nMass; + cpVect n; +} cpDampedSpring; + +cpDampedSpring *cpDampedSpringAlloc(void); +cpDampedSpring *cpDampedSpringInit(cpDampedSpring *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); +cpConstraint *cpDampedSpringNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat restLength, cpFloat stiffness, cpFloat damping); + +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpDampedSpring, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, restLength, RestLength); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, stiffness, Stiffness); +CP_DefineConstraintProperty(cpDampedSpring, cpFloat, damping, Damping); +CP_DefineConstraintProperty(cpDampedSpring, cpDampedSpringForceFunc, springForceFunc, SpringForceFunc); diff --git a/chipmunk-5.0/constraints/cpGearJoint.c b/chipmunk-5.0/constraints/cpGearJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpGearJoint.c @@ -0,0 +1,104 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv*cpfabs(joint->ratio) + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv*joint->ratio; +} + +static void +applyImpulse(cpGearJoint *joint) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // compute relative rotational velocity + cpFloat wr = b->w*joint->ratio - a->w; + + // compute normal impulse + cpFloat j = (joint->bias - wr)*joint->iSum; + cpFloat jOld = joint->jAcc; + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv*joint->ratio; +} + +static cpFloat +getImpulse(cpGearJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpGearJoint) + +cpGearJoint * +cpGearJointAlloc(void) +{ + return (cpGearJoint *)malloc(sizeof(cpGearJoint)); +} + +cpGearJoint * +cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->phase = phase; + joint->ratio = ratio; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio) +{ + return (cpConstraint *)cpGearJointInit(cpGearJointAlloc(), a, b, phase, ratio); +} diff --git a/chipmunk-5.0/constraints/cpGearJoint.h b/chipmunk-5.0/constraints/cpGearJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpGearJoint.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpGearJointGetClass(); + +typedef struct cpGearJoint { + cpConstraint constraint; + cpFloat phase, ratio; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc, jMax; +} cpGearJoint; + +cpGearJoint *cpGearJointAlloc(void); +cpGearJoint *cpGearJointInit(cpGearJoint *joint, cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); +cpConstraint *cpGearJointNew(cpBody *a, cpBody *b, cpFloat phase, cpFloat ratio); + +CP_DefineConstraintProperty(cpGearJoint, cpFloat, phase, Phase); +CP_DefineConstraintProperty(cpGearJoint, cpFloat, ratio, Ratio); diff --git a/chipmunk-5.0/constraints/cpGrooveJoint.c b/chipmunk-5.0/constraints/cpGrooveJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpGrooveJoint.c @@ -0,0 +1,138 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate endpoints in worldspace + cpVect ta = cpBodyLocal2World(a, joint->grv_a); + cpVect tb = cpBodyLocal2World(a, joint->grv_b); + + // calculate axis + cpVect n = cpvrotate(joint->grv_n, a->rot); + cpFloat d = cpvdot(ta, n); + + joint->grv_tn = n; + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + // calculate tangential distance along the axis of r2 + cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n); + // calculate clamping factor and r2 + if(td <= cpvcross(ta, n)){ + joint->clamp = 1.0f; + joint->r1 = cpvsub(ta, a->p); + } else if(td >= cpvcross(tb, n)){ + joint->clamp = -1.0f; + joint->r1 = cpvsub(tb, a->p); + } else { + joint->clamp = 0.0f; + joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + } + + // Calculate mass tensor + k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); + + // compute max impulse + joint->jMaxLen = J_MAX(joint, dt); + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + // apply accumulated impulse + apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); +} + +static inline cpVect +grooveConstrain(cpGrooveJoint *joint, cpVect j){ + cpVect n = joint->grv_tn; + cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n); + return cpvclamp(jClamp, joint->jMaxLen); +} + +static void +applyImpulse(cpGrooveJoint *joint) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute impulse + cpVect vr = relative_velocity(a, b, r1, r2); + + cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect jOld = joint->jAcc; + joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j)); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpGrooveJoint *joint) +{ + return cpvlength(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpGrooveJoint) + +cpGrooveJoint * +cpGrooveJointAlloc(void) +{ + return (cpGrooveJoint *)malloc(sizeof(cpGrooveJoint)); +} + +cpGrooveJoint * +cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->grv_a = groove_a; + joint->grv_b = groove_b; + joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); + joint->anchr2 = anchr2; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + return (cpConstraint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); +} diff --git a/chipmunk-5.0/constraints/cpGrooveJoint.h b/chipmunk-5.0/constraints/cpGrooveJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpGrooveJoint.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpGrooveJointGetClass(); + +typedef struct cpGrooveJoint { + cpConstraint constraint; + cpVect grv_n, grv_a, grv_b; + cpVect anchr2; + + cpVect grv_tn; + cpFloat clamp; + cpVect r1, r2; + cpVect k1, k2; + + cpVect jAcc; + cpFloat jMaxLen; + cpVect bias; +} cpGrooveJoint; + +cpGrooveJoint *cpGrooveJointAlloc(void); +cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); +cpConstraint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); + +// TODO setters for the groove. +CP_DefineConstraintProperty(cpGrooveJoint, cpVect, anchr2, Anchr2); diff --git a/chipmunk-5.0/constraints/cpPinJoint.c b/chipmunk-5.0/constraints/cpPinJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpPinJoint.c @@ -0,0 +1,117 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +//#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpPinJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + joint->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias); + + // compute max impulse + joint->jnMax = J_MAX(joint, dt); + + // apply accumulated impulse + cpVect j = cpvmult(joint->n, joint->jnAcc); + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static void +applyImpulse(cpPinJoint *joint) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + cpVect n = joint->n; + + // compute relative velocity + cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n); + + // compute normal impulse + cpFloat jn = (joint->bias - vrn)*joint->nMass; + cpFloat jnOld = joint->jnAcc; + joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpPinJoint *joint) +{ + return cpfabs(joint->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpPinJoint); + + +cpPinJoint * +cpPinJointAlloc(void) +{ + return (cpPinJoint *)malloc(sizeof(cpPinJoint)); +} + +cpPinJoint * +cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + + cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot)); + cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot)); + joint->dist = cpvlength(cpvsub(p2, p1)); + + joint->jnAcc = 0.0; + + return joint; +} + +cpConstraint * +cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return (cpConstraint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); +} diff --git a/chipmunk-5.0/constraints/cpPinJoint.h b/chipmunk-5.0/constraints/cpPinJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpPinJoint.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpPinJointGetClass(); + +typedef struct cpPinJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat dist; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc, jnMax; + cpFloat bias; +} cpPinJoint; + +cpPinJoint *cpPinJointAlloc(void); +cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +cpConstraint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); + +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpPinJoint, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpPinJoint, cpFloat, dist, Dist); diff --git a/chipmunk-5.0/constraints/cpPivotJoint.c b/chipmunk-5.0/constraints/cpPivotJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpPivotJoint.c @@ -0,0 +1,116 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + // Calculate mass tensor + k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2); + + // compute max impulse + joint->jMaxLen = J_MAX(joint, dt); + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias); + + // apply accumulated impulse + apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc); +} + +static void +applyImpulse(cpPivotJoint *joint) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + + // compute normal impulse + cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); + cpVect jOld = joint->jAcc; + joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen); + j = cpvsub(joint->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, j); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpvlength(((cpPivotJoint *)joint)->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpPivotJoint) + +cpPivotJoint * +cpPivotJointAlloc(void) +{ + return (cpPivotJoint *)malloc(sizeof(cpPivotJoint)); +} + +cpPivotJoint * +cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + +// joint->anchr1 = cpvunrotate(cpvsub(pivot, a->p), a->rot); +// joint->anchr2 = cpvunrotate(cpvsub(pivot, b->p), b->rot); + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + + joint->jAcc = cpvzero; + + return joint; +} + +cpConstraint * +cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return (cpConstraint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, anchr1, anchr2); +} + +cpConstraint * +cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) +{ + return cpPivotJointNew2(a, b, cpBodyWorld2Local(a, pivot), cpBodyWorld2Local(b, pivot)); +} diff --git a/chipmunk-5.0/constraints/cpPivotJoint.h b/chipmunk-5.0/constraints/cpPivotJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpPivotJoint.h @@ -0,0 +1,42 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpPivotJointGetClass(); + +typedef struct cpPivotJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + + cpVect r1, r2; + cpVect k1, k2; + + cpVect jAcc; + cpFloat jMaxLen; + cpVect bias; +} cpPivotJoint; + +cpPivotJoint *cpPivotJointAlloc(void); +cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); +cpConstraint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); +cpConstraint *cpPivotJointNew2(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); + +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpPivotJoint, cpVect, anchr2, Anchr2); diff --git a/chipmunk-5.0/constraints/cpRotaryLimitJoint.c b/chipmunk-5.0/constraints/cpRotaryLimitJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpRotaryLimitJoint.c @@ -0,0 +1,122 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpFloat dist = b->a - a->a; + cpFloat pdist = 0.0; + if(dist > joint->max) { + pdist = joint->max - dist; + } else if(dist < joint->min) { + pdist = joint->min - dist; + } + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // If the bias is 0, the joint is not at a limit. Reset the impulse. + if(!joint->bias) + joint->jAcc = 0.0f; + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv; +} + +static void +applyImpulse(cpRotaryLimitJoint *joint) +{ + if(!joint->bias) return; // early exit + + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // compute relative rotational velocity + cpFloat wr = b->w - a->w; + + // compute normal impulse + cpFloat j = -(joint->bias + wr)*joint->iSum; + cpFloat jOld = joint->jAcc; + if(joint->bias < 0.0f){ + joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax); + } else { + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f); + } + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpRotaryLimitJoint *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpRotaryLimitJoint) + +cpRotaryLimitJoint * +cpRotaryLimitJointAlloc(void) +{ + return (cpRotaryLimitJoint *)malloc(sizeof(cpRotaryLimitJoint)); +} + +cpRotaryLimitJoint * +cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->min = min; + joint->max = max; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpRotaryLimitJointInit(cpRotaryLimitJointAlloc(), a, b, min, max); +} diff --git a/chipmunk-5.0/constraints/cpRotaryLimitJoint.h b/chipmunk-5.0/constraints/cpRotaryLimitJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpRotaryLimitJoint.h @@ -0,0 +1,39 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpRotaryLimitJointGetClass(); + +typedef struct cpRotaryLimitJoint { + cpConstraint constraint; + cpFloat min, max; + + cpFloat iSum; + + cpFloat bias; + cpFloat jAcc, jMax; +} cpRotaryLimitJoint; + +cpRotaryLimitJoint *cpRotaryLimitJointAlloc(void); +cpRotaryLimitJoint *cpRotaryLimitJointInit(cpRotaryLimitJoint *joint, cpBody *a, cpBody *b, cpFloat min, cpFloat max); +cpConstraint *cpRotaryLimitJointNew(cpBody *a, cpBody *b, cpFloat min, cpFloat max); + +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, min, Min); +CP_DefineConstraintProperty(cpRotaryLimitJoint, cpFloat, max, Max); diff --git a/chipmunk-5.0/constraints/cpSimpleMotor.c b/chipmunk-5.0/constraints/cpSimpleMotor.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpSimpleMotor.c @@ -0,0 +1,99 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // calculate moment of inertia coefficient. + joint->iSum = 1.0f/(a->i_inv + b->i_inv); + + // compute max impulse + joint->jMax = J_MAX(joint, dt); + + // apply joint torque + a->w -= joint->jAcc*a->i_inv; + b->w += joint->jAcc*b->i_inv; +} + +static void +applyImpulse(cpSimpleMotor *joint) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + // compute relative rotational velocity + cpFloat wr = b->w - a->w + joint->rate; + + // compute normal impulse + cpFloat j = -wr*joint->iSum; + cpFloat jOld = joint->jAcc; + joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax); + j = joint->jAcc - jOld; + + // apply impulse + a->w -= j*a->i_inv; + b->w += j*b->i_inv; +} + +static cpFloat +getImpulse(cpSimpleMotor *joint) +{ + return cpfabs(joint->jAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpSimpleMotor) + +cpSimpleMotor * +cpSimpleMotorAlloc(void) +{ + return (cpSimpleMotor *)malloc(sizeof(cpSimpleMotor)); +} + +cpSimpleMotor * +cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->rate = rate; + + joint->jAcc = 0.0f; + + return joint; +} + +cpConstraint * +cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate) +{ + return (cpConstraint *)cpSimpleMotorInit(cpSimpleMotorAlloc(), a, b, rate); +} diff --git a/chipmunk-5.0/constraints/cpSimpleMotor.h b/chipmunk-5.0/constraints/cpSimpleMotor.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpSimpleMotor.h @@ -0,0 +1,37 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpSimpleMotorGetClass(); + +typedef struct cpSimpleMotor { + cpConstraint constraint; + cpFloat rate; + + cpFloat iSum; + + cpFloat jAcc, jMax; +} cpSimpleMotor; + +cpSimpleMotor *cpSimpleMotorAlloc(void); +cpSimpleMotor *cpSimpleMotorInit(cpSimpleMotor *joint, cpBody *a, cpBody *b, cpFloat rate); +cpConstraint *cpSimpleMotorNew(cpBody *a, cpBody *b, cpFloat rate); + +CP_DefineConstraintProperty(cpSimpleMotor, cpFloat, rate, Rate); diff --git a/chipmunk-5.0/constraints/cpSlideJoint.c b/chipmunk-5.0/constraints/cpSlideJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpSlideJoint.c @@ -0,0 +1,131 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "../chipmunk.h" +#include "util.h" + +static void +preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv) +{ + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + joint->r1 = cpvrotate(joint->anchr1, a->rot); + joint->r2 = cpvrotate(joint->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1)); + cpFloat dist = cpvlength(delta); + cpFloat pdist = 0.0; + if(dist > joint->max) { + pdist = dist - joint->max; + } else if(dist < joint->min) { + pdist = joint->min - dist; + dist = -dist; + } + joint->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + // calculate mass normal + joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n); + + // calculate bias velocity + cpFloat maxBias = joint->constraint.maxBias; + joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias); + + // compute max impulse + joint->jnMax = J_MAX(joint, dt); + + // apply accumulated impulse + if(!joint->bias) //{ + // if bias is 0, then the joint is not at a limit. + joint->jnAcc = 0.0f; +// } else { + cpVect j = cpvmult(joint->n, joint->jnAcc); + apply_impulses(a, b, joint->r1, joint->r2, j); +// } +} + +static void +applyImpulse(cpSlideJoint *joint) +{ + if(!joint->bias) return; // early exit + + cpBody *a = joint->constraint.a; + cpBody *b = joint->constraint.b; + + cpVect n = joint->n; + cpVect r1 = joint->r1; + cpVect r2 = joint->r2; + + // compute relative velocity + cpVect vr = relative_velocity(a, b, r1, r2); + cpFloat vrn = cpvdot(vr, n); + + // compute normal impulse + cpFloat jn = (joint->bias - vrn)*joint->nMass; + cpFloat jnOld = joint->jnAcc; + joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f); + jn = joint->jnAcc - jnOld; + + // apply impulse + apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn)); +} + +static cpFloat +getImpulse(cpConstraint *joint) +{ + return cpfabs(((cpSlideJoint *)joint)->jnAcc); +} + +static const cpConstraintClass klass = { + (cpConstraintPreStepFunction)preStep, + (cpConstraintApplyImpulseFunction)applyImpulse, + (cpConstraintGetImpulseFunction)getImpulse, +}; +CP_DefineClassGetter(cpSlideJoint) + +cpSlideJoint * +cpSlideJointAlloc(void) +{ + return (cpSlideJoint *)malloc(sizeof(cpSlideJoint)); +} + +cpSlideJoint * +cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + cpConstraintInit((cpConstraint *)joint, &klass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + joint->min = min; + joint->max = max; + + joint->jnAcc = 0.0; + + return joint; +} + +cpConstraint * +cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + return (cpConstraint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); +} diff --git a/chipmunk-5.0/constraints/cpSlideJoint.h b/chipmunk-5.0/constraints/cpSlideJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/cpSlideJoint.h @@ -0,0 +1,44 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +const cpConstraintClass *cpSlideJointGetClass(); + +typedef struct cpSlideJoint { + cpConstraint constraint; + cpVect anchr1, anchr2; + cpFloat min, max; + + cpVect r1, r2; + cpVect n; + cpFloat nMass; + + cpFloat jnAcc, jnMax; + cpFloat bias; +} cpSlideJoint; + +cpSlideJoint *cpSlideJointAlloc(void); +cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); +cpConstraint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); + +CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr1, Anchr1); +CP_DefineConstraintProperty(cpSlideJoint, cpVect, anchr2, Anchr2); +CP_DefineConstraintProperty(cpSlideJoint, cpFloat, min, Min); +CP_DefineConstraintProperty(cpSlideJoint, cpFloat, max, Max); diff --git a/chipmunk-5.0/constraints/util.h b/chipmunk-5.0/constraints/util.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/constraints/util.h @@ -0,0 +1,110 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#define CP_DefineClassGetter(t) const cpConstraintClass * t##GetClass(){return (cpConstraintClass *)&klass;} + +void cpConstraintInit(cpConstraint *constraint, const cpConstraintClass *klass, cpBody *a, cpBody *b); + +#define J_MAX(constraint, dt) (((cpConstraint *)constraint)->maxForce*(dt)) + +static inline cpVect +relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2){ + cpVect v1_sum = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2_sum = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + + return cpvsub(v2_sum, v1_sum); +} + +static inline cpFloat +normal_relative_velocity(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n){ + return cpvdot(relative_velocity(a, b, r1, r2), n); +} + +static inline void +apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyImpulse(a, cpvneg(j), r1); + cpBodyApplyImpulse(b, j, r2); +} + +static inline void +apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyBiasImpulse(a, cpvneg(j), r1); + cpBodyApplyBiasImpulse(b, j, r2); +} + +static inline cpVect +clamp_vect(cpVect v, cpFloat len) +{ + return cpvclamp(v, len); +// return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +static inline cpFloat +k_scalar(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) +{ + cpFloat mass_sum = a->m_inv + b->m_inv; + cpFloat r1cn = cpvcross(r1, n); + cpFloat r2cn = cpvcross(r2, n); + + return mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; +} + +static inline void +k_tensor(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect *k1, cpVect *k2) +{ + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + cpFloat k11, k12, k21, k22; + cpFloat m_sum = a->m_inv + b->m_inv; + + // start with I*m_sum + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + // add the influence from r1 + cpFloat a_i_inv = a->i_inv; + cpFloat r1xsq = r1.x * r1.x * a_i_inv; + cpFloat r1ysq = r1.y * r1.y * a_i_inv; + cpFloat r1nxy = -r1.x * r1.y * a_i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + cpFloat b_i_inv = b->i_inv; + cpFloat r2xsq = r2.x * r2.x * b_i_inv; + cpFloat r2ysq = r2.y * r2.y * b_i_inv; + cpFloat r2nxy = -r2.x * r2.y * b_i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + cpFloat det_inv = 1.0f/(k11*k22 - k12*k21); + *k1 = cpv( k22*det_inv, -k12*det_inv); + *k2 = cpv(-k21*det_inv, k11*det_inv); +} + +static inline cpVect +mult_k(cpVect vr, cpVect k1, cpVect k2) +{ + return cpv(cpvdot(vr, k1), cpvdot(vr, k2)); +} diff --git a/chipmunk-5.0/cpArbiter.c b/chipmunk-5.0/cpArbiter.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpArbiter.c @@ -0,0 +1,254 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include + +#include "chipmunk.h" +#include "constraints/util.h" + +cpFloat cp_bias_coef = 0.1f; +cpFloat cp_collision_slop = 0.1f; + +cpContact* +cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash) +{ + con->p = p; + con->n = n; + con->dist = dist; + + con->jnAcc = 0.0f; + con->jtAcc = 0.0f; + con->jBias = 0.0f; + + con->hash = hash; + + return con; +} + +cpVect +cpContactsSumImpulses(cpContact *contacts, int numContacts) +{ + cpVect sum = cpvzero; + + for(int i=0; in, con->jnAcc)); + } + + return sum; +} + +cpVect +cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts) +{ + cpVect sum = cpvzero; + + for(int i=0; in, cpv(con->jnAcc, con->jtAcc))); + } + + return sum; +} + +cpFloat +cpContactsEstimateCrushingImpulse(cpContact *contacts, int numContacts) +{ + cpFloat fsum = 0.0f; + cpVect vsum = cpvzero; + + for(int i=0; in, cpv(con->jnAcc, con->jtAcc)); + + fsum += cpvlength(j); + vsum = cpvadd(vsum, j); + } + + cpFloat vmag = cpvlength(vsum); + return (1.0f - vmag/fsum); +} + +cpArbiter* +cpArbiterAlloc(void) +{ + return (cpArbiter *)calloc(1, sizeof(cpArbiter)); +} + +cpArbiter* +cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp) +{ + arb->numContacts = 0; + arb->contacts = NULL; + + arb->a = a; + arb->b = b; + + arb->stamp = stamp; + + return arb; +} + +cpArbiter* +cpArbiterNew(cpShape *a, cpShape *b, int stamp) +{ + return cpArbiterInit(cpArbiterAlloc(), a, b, stamp); +} + +void +cpArbiterDestroy(cpArbiter *arb) +{ + free(arb->contacts); +} + +void +cpArbiterFree(cpArbiter *arb) +{ + if(arb) cpArbiterDestroy(arb); + free(arb); +} + +void +cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts) +{ + // Iterate over the possible pairs to look for hash value matches. + for(int i=0; inumContacts; i++){ + cpContact *old = &arb->contacts[i]; + + for(int j=0; jhash == old->hash){ + // Copy the persistant contact information. + new_contact->jnAcc = old->jnAcc; + new_contact->jtAcc = old->jtAcc; + } + } + } + + free(arb->contacts); + + arb->contacts = contacts; + arb->numContacts = numContacts; +} + +void +cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) +{ + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; + + cpFloat e = shapea->e * shapeb->e; + arb->u = shapea->u * shapeb->u; + arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); + + cpBody *a = shapea->body; + cpBody *b = shapeb->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + + // Calculate the offsets. + con->r1 = cpvsub(con->p, a->p); + con->r2 = cpvsub(con->p, b->p); + + // Calculate the mass normal and mass tangent. + con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, con->n); + con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(con->n)); + + // Calculate the target bias velocity. + con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop); + con->jBias = 0.0f; + + // Calculate the target bounce velocity. + con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, con->n)*e;//cpvdot(con->n, cpvsub(v2, v1))*e; + } +} + +void +cpArbiterApplyCachedImpulse(cpArbiter *arb) +{ + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; + + arb->u = shapea->u * shapeb->u; + arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); + + cpBody *a = shapea->body; + cpBody *b = shapeb->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc))); + } +} + +void +cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef) +{ + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + cpVect n = con->n; + cpVect r1 = con->r1; + cpVect r2 = con->r2; + + // Calculate the relative bias velocities. + cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); + cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); + cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + + // Calculate and clamp the bias impulse. + cpFloat jbn = (con->bias - vbn)*con->nMass; + cpFloat jbnOld = con->jBias; + con->jBias = cpfmax(jbnOld + jbn, 0.0f); + jbn = con->jBias - jbnOld; + + // Apply the bias impulse. + apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn)); + + // Calculate the relative velocity. + cpVect vr = relative_velocity(a, b, r1, r2); + cpFloat vrn = cpvdot(vr, n); + + // Calculate and clamp the normal impulse. + cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass; + cpFloat jnOld = con->jnAcc; + con->jnAcc = cpfmax(jnOld + jn, 0.0f); + jn = con->jnAcc - jnOld; + + // Calculate the relative tangent velocity. + cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), cpvperp(n)); + + // Calculate and clamp the friction impulse. + cpFloat jtMax = arb->u*con->jnAcc; + cpFloat jt = -vrt*con->tMass; + cpFloat jtOld = con->jtAcc; + con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); + jt = con->jtAcc - jtOld; + + // Apply the final impulse. + apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt))); + } +} diff --git a/chipmunk-5.0/cpArbiter.h b/chipmunk-5.0/cpArbiter.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpArbiter.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Determines how fast penetrations resolve themselves. +extern cpFloat cp_bias_coef; +// Amount of allowed penetration. Used to reduce vibrating contacts. +extern cpFloat cp_collision_slop; + +// Data structure for contact points. +typedef struct cpContact{ + // Contact point and normal. + cpVect p, n; + // Penetration distance. + cpFloat dist; + + // Calculated by cpArbiterPreStep(). + cpVect r1, r2; + cpFloat nMass, tMass, bounce; + + // Persistant contact information. + cpFloat jnAcc, jtAcc, jBias; + cpFloat bias; + + // Hash value used to (mostly) uniquely identify a contact. + cpHashValue hash; +} cpContact; + +// Contacts are always allocated in groups. +cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, cpHashValue hash); + +// Sum the contact impulses. (Can be used after cpSpaceStep() returns) +cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts); +cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts); + +// Data structure for tracking collisions between shapes. +typedef struct cpArbiter{ + // Information on the contact points between the objects. + int numContacts; + cpContact *contacts; + + // The two shapes involved in the collision. + cpShape *a, *b; + + // Calculated by cpArbiterPreStep(). + cpFloat u; + cpVect target_v; + + // Time stamp of the arbiter. (from cpSpace) + int stamp; +} cpArbiter; + +// Basic allocation/destruction functions. +cpArbiter* cpArbiterAlloc(void); +cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp); +cpArbiter* cpArbiterNew(cpShape *a, cpShape *b, int stamp); + +void cpArbiterDestroy(cpArbiter *arb); +void cpArbiterFree(cpArbiter *arb); + +// These functions are all intended to be used internally. +// Inject new contact points into the arbiter while preserving contact history. +void cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts); +// Precalculate values used by the solver. +void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv); +void cpArbiterApplyCachedImpulse(cpArbiter *arb); +// Run an iteration of the solver on the arbiter. +void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef); diff --git a/chipmunk-5.0/cpArray.c b/chipmunk-5.0/cpArray.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpArray.c @@ -0,0 +1,114 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + + +//#define CP_ARRAY_INCREMENT 10 + +// NOTE: cpArray is rarely used and will probably go away. + +cpArray* +cpArrayAlloc(void) +{ + return (cpArray *)calloc(1, sizeof(cpArray)); +} + +cpArray* +cpArrayInit(cpArray *arr, int size) +{ + arr->num = 0; + + size = (size ? size : 4); + arr->max = size; + arr->arr = (void **)malloc(size*sizeof(void**)); + + return arr; +} + +cpArray* +cpArrayNew(int size) +{ + return cpArrayInit(cpArrayAlloc(), size); +} + +void +cpArrayDestroy(cpArray *arr) +{ + free(arr->arr); +} + +void +cpArrayFree(cpArray *arr) +{ + if(!arr) return; + cpArrayDestroy(arr); + free(arr); +} + +void +cpArrayPush(cpArray *arr, void *object) +{ + if(arr->num == arr->max){ + arr->max *= 2; + arr->arr = (void **)realloc(arr->arr, arr->max*sizeof(void**)); + } + + arr->arr[arr->num] = object; + arr->num++; +} + +void +cpArrayDeleteIndex(cpArray *arr, int index) +{ + int last = --arr->num; + arr->arr[index] = arr->arr[last]; +} + +void +cpArrayDeleteObj(cpArray *arr, void *obj) +{ + for(int i=0; inum; i++){ + if(arr->arr[i] == obj){ + cpArrayDeleteIndex(arr, i); + return; + } + } +} + +void +cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) +{ + for(int i=0; inum; i++) + iterFunc(arr->arr[i], data); +} + +int +cpArrayContains(cpArray *arr, void *ptr) +{ + for(int i=0; inum; i++) + if(arr->arr[i] == ptr) return 1; + + return 0; +} diff --git a/chipmunk-5.0/cpArray.h b/chipmunk-5.0/cpArray.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpArray.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// NOTE: cpArray is rarely used and will probably go away. + +typedef struct cpArray{ + int num, max; + void **arr; +} cpArray; + +typedef void (*cpArrayIter)(void *ptr, void *data); + +cpArray *cpArrayAlloc(void); +cpArray *cpArrayInit(cpArray *arr, int size); +cpArray *cpArrayNew(int size); + +void cpArrayDestroy(cpArray *arr); +void cpArrayFree(cpArray *arr); + +void cpArrayClear(cpArray *arr); + +void cpArrayPush(cpArray *arr, void *object); +void cpArrayDeleteIndex(cpArray *arr, int index); +void cpArrayDeleteObj(cpArray *arr, void *obj); + +void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data); +int cpArrayContains(cpArray *arr, void *ptr); diff --git a/chipmunk-5.0/cpBB.c b/chipmunk-5.0/cpBB.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpBB.c @@ -0,0 +1,47 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + +cpVect +cpBBClampVect(const cpBB bb, const cpVect v) +{ + cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r); + cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t); + return cpv(x, y); +} + +cpVect +cpBBWrapVect(const cpBB bb, const cpVect v) +{ + cpFloat ix = fabsf(bb.r - bb.l); + cpFloat modx = cpfmod(v.x - bb.l, ix); + cpFloat x = (modx > 0.0f) ? modx : modx + ix; + + cpFloat iy = fabsf(bb.t - bb.b); + cpFloat mody = cpfmod(v.y - bb.b, iy); + cpFloat y = (mody > 0.0f) ? mody : mody + iy; + + return cpv(x + bb.l, y + bb.b); +} diff --git a/chipmunk-5.0/cpBB.h b/chipmunk-5.0/cpBB.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpBB.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef struct cpBB{ + cpFloat l, b, r ,t; +} cpBB; + +static inline cpBB +cpBBNew(const cpFloat l, const cpFloat b, + const cpFloat r, const cpFloat t) +{ + cpBB bb = {l, b, r, t}; + return bb; +} + +static inline int +cpBBintersects(const cpBB a, const cpBB b) +{ + return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t); +} + +static inline int +cpBBcontainsBB(const cpBB bb, const cpBB other) +{ + return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t); +} + +static inline int +cpBBcontainsVect(const cpBB bb, const cpVect v) +{ + return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y); +} + +cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox +cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox diff --git a/chipmunk-5.0/cpBody.c b/chipmunk-5.0/cpBody.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpBody.c @@ -0,0 +1,179 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + +cpBody* +cpBodyAlloc(void) +{ + return (cpBody *)malloc(sizeof(cpBody)); +} + +cpBody* +cpBodyInit(cpBody *body, cpFloat m, cpFloat i) +{ + body->velocity_func = cpBodyUpdateVelocity; + body->position_func = cpBodyUpdatePosition; + + cpBodySetMass(body, m); + cpBodySetMoment(body, i); + + body->p = cpvzero; + body->v = cpvzero; + body->f = cpvzero; + + cpBodySetAngle(body, 0.0f); + body->w = 0.0f; + body->t = 0.0f; + + body->v_bias = cpvzero; + body->w_bias = 0.0f; + + body->data = NULL; +// body->active = 1; + + return body; +} + +cpBody* +cpBodyNew(cpFloat m, cpFloat i) +{ + return cpBodyInit(cpBodyAlloc(), m, i); +} + +void cpBodyDestroy(cpBody *body){} + +void +cpBodyFree(cpBody *body) +{ + if(body) cpBodyDestroy(body); + free(body); +} + +void +cpBodySetMass(cpBody *body, cpFloat mass) +{ + body->m = mass; + body->m_inv = 1.0f/mass; +} + +void +cpBodySetMoment(cpBody *body, cpFloat moment) +{ + body->i = moment; + body->i_inv = 1.0f/moment; +} + +void +cpBodySetAngle(cpBody *body, cpFloat angle) +{ + body->a = angle;//fmod(a, (cpFloat)M_PI*2.0f); + body->rot = cpvforangle(angle); +} + +void +cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) +{ + cpVect delta = cpvsub(pos, body->p); + body->v = cpvmult(delta, 1.0/dt); +} + +void +cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) +{ + body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)); + body->w = body->w*damping + body->t*body->i_inv*dt; +} + +void +cpBodyUpdatePosition(cpBody *body, cpFloat dt) +{ + body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); + cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt); + + body->v_bias = cpvzero; + body->w_bias = 0.0f; +} + +void +cpBodyResetForces(cpBody *body) +{ + body->f = cpvzero; + body->t = 0.0f; +} + +void +cpBodyApplyForce(cpBody *body, cpVect force, cpVect r) +{ + body->f = cpvadd(body->f, force); + body->t += cpvcross(r, force); +} + +void +cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) +{ + // Calculate the world space anchor coordinates. + cpVect r1 = cpvrotate(anchr1, a->rot); + cpVect r2 = cpvrotate(anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1)); + cpFloat dist = cpvlength(delta); + cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero; + + cpFloat f_spring = (dist - rlen)*k; + + // Calculate the world relative velocities of the anchor points. + cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + + // Calculate the damping force. + // This really should be in the impulse solver and can produce problems when using large damping values. + cpFloat vrn = cpvdot(cpvsub(v2, v1), n); + cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv))); + + // Apply! + cpVect f = cpvmult(n, f_spring + f_damp); + cpBodyApplyForce(a, f, r1); + cpBodyApplyForce(b, cpvneg(f), r2); +} + +//int +//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max) +//{ +// cpFloat ke = body->m*cpvdot(body->v, body->v); +// cpFloat re = body->i*body->w*body->w; +// +// if(ke + re > body->m*dvsq) +// body->active = 1; +// else if(body->active) +// body->active = (body->active + 1)%(max + 1); +// else { +// body->v = cpvzero; +// body->v_bias = cpvzero; +// body->w = 0.0f; +// body->w_bias = 0.0f; +// } +// +// return body->active; +//} diff --git a/chipmunk-5.0/cpBody.h b/chipmunk-5.0/cpBody.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpBody.h @@ -0,0 +1,153 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +struct cpBody; +typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt); + + +typedef struct cpBody{ + // *** Integration Functions.ntoehu + + // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) + cpBodyVelocityFunc velocity_func; + + // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) + cpBodyPositionFunc position_func; + + // *** Mass Properties + + // Mass and it's inverse. + // Always use cpBodySetMass() whenever changing the mass as these values must agree. + cpFloat m, m_inv; + + // Moment of inertia and it's inverse. + // Always use cpBodySetMass() whenever changing the mass as these values must agree. + cpFloat i, i_inv; + + // *** Positional Properties + + // Linear components of motion (position, velocity, and force) + cpVect p, v, f; + + // Angular components of motion (angle, angular velocity, and torque) + // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. + cpFloat a, w, t; + + // Cached unit length vector representing the angle of the body. + // Used for fast vector rotation using cpvrotate(). + cpVect rot; + + // *** User Definable Fields + + // User defined data pointer. + void *data; + + // *** Internally Used Fields + + // Velocity bias values used when solving penetrations and correcting constraints. + cpVect v_bias; + cpFloat w_bias; + +// int active; +} cpBody; + +// Basic allocation/destruction functions +cpBody *cpBodyAlloc(void); +cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); +cpBody *cpBodyNew(cpFloat m, cpFloat i); + +void cpBodyDestroy(cpBody *body); +void cpBodyFree(cpBody *body); + +#define CP_DefineBodyGetter(type, member, name) static inline type cpBodyGet##name(cpBody *body){return body->member;} +#define CP_DefineBodySetter(type, member, name) static inline void cpBodySet##name(cpBody *body, type value){body->member = value;} + +#define CP_DefineBodyProperty(type, member, name) \ +CP_DefineBodyGetter(type, member, name) \ +CP_DefineBodySetter(type, member, name) + + +// Accessors for cpBody struct members +CP_DefineBodyGetter(cpFloat, m, Mass); +void cpBodySetMass(cpBody *body, cpFloat m); + +CP_DefineBodyGetter(cpFloat, i, Moment); +void cpBodySetMoment(cpBody *body, cpFloat i); + + +CP_DefineBodyProperty(cpVect, p, Pos); +CP_DefineBodyProperty(cpVect, v, Vel); +CP_DefineBodyProperty(cpVect, f, Force); +CP_DefineBodyGetter(cpFloat, a, Angle); +void cpBodySetAngle(cpBody *body, cpFloat a); +CP_DefineBodyProperty(cpFloat, w, AngVel); +CP_DefineBodyProperty(cpFloat, t, Torque); +CP_DefineBodyGetter(cpVect, rot, Rot); + +// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. +// Intended for objects that are moved manually with a custom velocity integration function. +void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); + +// Default Integration functions. +void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); +void cpBodyUpdatePosition(cpBody *body, cpFloat dt); + +// Convert body local to world coordinates +static inline cpVect +cpBodyLocal2World(cpBody *body, cpVect v) +{ + return cpvadd(body->p, cpvrotate(v, body->rot)); +} + +// Convert world to body local coordinates +static inline cpVect +cpBodyWorld2Local(cpBody *body, cpVect v) +{ + return cpvunrotate(cpvsub(v, body->p), body->rot); +} + +// Apply an impulse (in world coordinates) to the body at a point relative to the center of gravity (also in world coordinates). +static inline void +cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r) +{ + body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); + body->w += body->i_inv*cpvcross(r, j); +} + +// Not intended for external use. Used by cpArbiter.c and cpConstraint.c. +static inline void +cpBodyApplyBiasImpulse(cpBody *body, cpVect j, cpVect r) +{ + body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); + body->w_bias += body->i_inv*cpvcross(r, j); +} + +// Zero the forces on a body. +void cpBodyResetForces(cpBody *body); +// Apply a force (in world coordinates) to a body at a point relative to the center of gravity (also in world coordinates). +void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r); + +// Apply a damped spring force between two bodies. +// Warning: Large damping values can be unstable. Use a cpDampedSpring constraint for this instead. +void cpApplyDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); + +//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max); diff --git a/chipmunk-5.0/cpCollision.c b/chipmunk-5.0/cpCollision.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpCollision.c @@ -0,0 +1,403 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include "chipmunk.h" + +typedef int (*collisionFunc)(cpShape*, cpShape*, cpContact**); + +// Add contact points for circle to circle collisions. +// Used by several collision tests. +static int +circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact **con) +{ + cpFloat mindist = r1 + r2; + cpVect delta = cpvsub(p2, p1); + cpFloat distsq = cpvlengthsq(delta); + if(distsq >= mindist*mindist) return 0; + + cpFloat dist = cpfsqrt(distsq); + // To avoid singularities, do nothing in the case of dist = 0. + cpFloat non_zero_dist = (dist ? dist : INFINITY); + + // Allocate and initialize the contact. + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvadd(p1, cpvmult(delta, 0.5 + (r1 - 0.5*mindist)/non_zero_dist)), + cpvmult(delta, 1.0/non_zero_dist), + dist - mindist, + 0 + ); + + return 1; +} + +// Collide circle shapes. +static int +circle2circle(cpShape *shape1, cpShape *shape2, cpContact **arr) +{ + cpCircleShape *circ1 = (cpCircleShape *)shape1; + cpCircleShape *circ2 = (cpCircleShape *)shape2; + + return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); +} + +// Collide circles to segment shapes. +static int +circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con) +{ + cpCircleShape *circ = (cpCircleShape *)circleShape; + cpSegmentShape *seg = (cpSegmentShape *)segmentShape; + + // Radius sum + cpFloat rsum = circ->r + seg->r; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn); + cpFloat dist = cpfabs(dn) - rsum; + if(dist > 0.0f) return 0; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, circ->tc); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt < dtMin){ + if(dt < (dtMin - rsum)){ + return 0; + } else { + return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con); + } + } else { + if(dt < dtMax){ + cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn); + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)), + n, + dist, + 0 + ); + return 1; + } else { + if(dt < (dtMax + rsum)) { + return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con); + } else { + return 0; + } + } + } + + return 1; +} + +// Helper function for allocating contact point lists. +static cpContact * +addContactPoint(cpContact **arr, int *max, int *num) +{ + if(*arr == NULL){ + // Allocate the array if it hasn't been done. + (*max) = 2; + (*num) = 0; + (*arr) = (cpContact *)malloc((*max)*sizeof(cpContact)); + } else if(*num == *max){ + // Extend it if necessary. + (*max) *= 2; + (*arr) = (cpContact *)realloc(*arr, (*max)*sizeof(cpContact)); + } + + cpContact *con = &(*arr)[*num]; + (*num)++; + + return con; +} + +// Find the minimum separating axis for the give poly and axis list. +static inline int +findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out) +{ + int min_index = 0; + cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d); + if(min > 0.0) return -1; + + for(int i=1; i 0.0) { + return -1; + } else if(dist > min){ + min = dist; + min_index = i; + } + } + + (*min_out) = min; + return min_index; +} + +// Add contacts for penetrating vertexes. +static inline int +findVerts(cpContact **arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) +{ + int max = 0; + int num = 0; + + for(int i=0; inumVerts; i++){ + cpVect v = poly1->tVerts[i]; + if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) + cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i)); + } + + for(int i=0; inumVerts; i++){ + cpVect v = poly2->tVerts[i]; + if(cpPolyShapeContainsVertPartial(poly1, v, n)) + cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly2, i)); + } + + // if(!num) + // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0)); + + return num; +} + +// Collide poly shapes together. +static int +poly2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) +{ + cpPolyShape *poly1 = (cpPolyShape *)shape1; + cpPolyShape *poly2 = (cpPolyShape *)shape2; + + cpFloat min1; + int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1); + if(mini1 == -1) return 0; + + cpFloat min2; + int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2); + if(mini2 == -1) return 0; + + // There is overlap, find the penetrating verts + if(min1 > min2) + return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1); + else + return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2); +} + +// Like cpPolyValueOnAxis(), but for segments. +static inline float +segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d) +{ + cpFloat a = cpvdot(n, seg->ta) - seg->r; + cpFloat b = cpvdot(n, seg->tb) - seg->r; + return cpfmin(a, b) - d; +} + +// Identify vertexes that have penetrated the segment. +static inline void +findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) +{ + cpFloat dta = cpvcross(seg->tn, seg->ta); + cpFloat dtb = cpvcross(seg->tn, seg->tb); + cpVect n = cpvmult(seg->tn, coef); + + for(int i=0; inumVerts; i++){ + cpVect v = poly->tVerts[i]; + if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){ + cpFloat dt = cpvcross(seg->tn, v); + if(dta >= dt && dt >= dtb){ + cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly, i)); + } + } + } +} + +// This one is complicated and gross. Just don't go there... +// TODO: Comment me! +static int +seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + cpFloat segD = cpvdot(seg->tn, seg->ta); + cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; + cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; + if(minNeg > 0.0f || minNorm > 0.0f) return 0; + + int mini = 0; + cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); + if(poly_min > 0.0f) return 0; + for(int i=0; inumVerts; i++){ + cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); + if(dist > 0.0f){ + return 0; + } else if(dist > poly_min){ + poly_min = dist; + mini = i; + } + } + + int max = 0; + int num = 0; + + cpVect poly_n = cpvneg(axes[mini].n); + + cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); + cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); + if(cpPolyShapeContainsVert(poly, va)) + cpContactInit(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0)); + if(cpPolyShapeContainsVert(poly, vb)) + cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 1)); + + // Floating point precision problems here. + // This will have to do for now. + poly_min -= cp_collision_slop; + if(minNorm >= poly_min || minNeg >= poly_min) { + if(minNorm > minNeg) + findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f); + else + findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f); + } + + // If no other collision points are found, try colliding endpoints. + if(num == 0){ + cpVect poly_a = poly->tVerts[mini]; + cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts]; + + if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) + return 1; + + if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) + return 1; + } + + return num; +} + +// This one is less gross, but still gross. +// TODO: Comment me! +static int +circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con) +{ + cpCircleShape *circ = (cpCircleShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + int mini = 0; + cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r; + for(int i=0; inumVerts; i++){ + cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r; + if(dist > 0.0){ + return 0; + } else if(dist > min) { + min = dist; + mini = i; + } + } + + cpVect n = axes[mini].n; + cpVect a = poly->tVerts[mini]; + cpVect b = poly->tVerts[(mini + 1)%poly->numVerts]; + cpFloat dta = cpvcross(n, a); + cpFloat dtb = cpvcross(n, b); + cpFloat dt = cpvcross(n, circ->tc); + + if(dt < dtb){ + return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con); + } else if(dt < dta) { + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)), + cpvneg(n), + min, + 0 + ); + + return 1; + } else { + return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con); + } +} + +//static const collisionFunc builtinCollisionFuncs[9] = { +// circle2circle, +// NULL, +// NULL, +// circle2segment, +// NULL, +// NULL, +// circle2poly, +// seg2poly, +// poly2poly, +//}; +//static const collisionFunc *colfuncs = builtinCollisionFuncs; + +static collisionFunc *colfuncs = NULL; + +static void +addColFunc(cpShapeType a, cpShapeType b, collisionFunc func) +{ + colfuncs[a + b*CP_NUM_SHAPES] = func; +} + +#ifdef __cplusplus +extern "C" { +#endif + // Initializes the array of collision functions. + // Called by cpInitChipmunk(). + void + cpInitCollisionFuncs(void) + { + if(!colfuncs) + colfuncs = (collisionFunc *)calloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc)); + + addColFunc(CP_CIRCLE_SHAPE, CP_CIRCLE_SHAPE, circle2circle); + addColFunc(CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, circle2segment); + addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE, seg2poly); + addColFunc(CP_CIRCLE_SHAPE, CP_POLY_SHAPE, circle2poly); + addColFunc(CP_POLY_SHAPE, CP_POLY_SHAPE, poly2poly); + } +#ifdef __cplusplus +} +#endif + +int +cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr) +{ + // Their shape types must be in order. + assert(a->klass->type <= b->klass->type); + + collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES]; + return (cfunc) ? cfunc(a, b, arr) : 0; +} diff --git a/chipmunk-5.0/cpCollision.h b/chipmunk-5.0/cpCollision.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpCollision.h @@ -0,0 +1,23 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Collides two cpShape structures. (this function is lonely :( ) +int cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr); diff --git a/chipmunk-5.0/cpHashSet.c b/chipmunk-5.0/cpHashSet.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpHashSet.c @@ -0,0 +1,225 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" +#include "prime.h" + +void +cpHashSetDestroy(cpHashSet *set) +{ + // Free the chains. + for(int i=0; isize; i++){ + // Free the bins in the chain. + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + free(bin); + bin = next; + } + } + + // Free the table. + free(set->table); +} + +void +cpHashSetFree(cpHashSet *set) +{ + if(set) cpHashSetDestroy(set); + free(set); +} + +cpHashSet * +cpHashSetAlloc(void) +{ + return (cpHashSet *)calloc(1, sizeof(cpHashSet)); +} + +cpHashSet * +cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) +{ + set->size = next_prime(size); + set->entries = 0; + + set->eql = eqlFunc; + set->trans = trans; + + set->default_value = NULL; + + set->table = (cpHashSetBin **)calloc(set->size, sizeof(cpHashSetBin *)); + + return set; +} + +cpHashSet * +cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) +{ + return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, trans); +} + +static int +setIsFull(cpHashSet *set) +{ + return (set->entries >= set->size); +} + +static void +cpHashSetResize(cpHashSet *set) +{ + // Get the next approximate doubled prime. + int newSize = next_prime(set->size + 1); + // Allocate a new table. + cpHashSetBin **newTable = (cpHashSetBin **)calloc(newSize, sizeof(cpHashSetBin *)); + + // Iterate over the chains. + for(int i=0; isize; i++){ + // Rehash the bins into the new table. + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + int index = bin->hash%newSize; + bin->next = newTable[index]; + newTable[index] = bin; + + bin = next; + } + } + + free(set->table); + + set->table = newTable; + set->size = newSize; +} + +void * +cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data) +{ + int index = hash%set->size; + + // Find the bin with the matching element. + cpHashSetBin *bin = set->table[index]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + // Create it necessary. + if(!bin){ + bin = (cpHashSetBin *)malloc(sizeof(cpHashSetBin)); + bin->hash = hash; + bin->elt = set->trans(ptr, data); // Transform the pointer. + + bin->next = set->table[index]; + set->table[index] = bin; + + set->entries++; + + // Resize the set if it's full. + if(setIsFull(set)) + cpHashSetResize(set); + } + + return bin->elt; +} + +void * +cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr) +{ + int index = hash%set->size; + + // Pointer to the previous bin pointer. + cpHashSetBin **prev_ptr = &set->table[index]; + // Pointer the the current bin. + cpHashSetBin *bin = set->table[index]; + + // Find the bin + while(bin && !set->eql(ptr, bin->elt)){ + prev_ptr = &bin->next; + bin = bin->next; + } + + // Remove it if it exists. + if(bin){ + // Update the previous bin pointer to point to the next bin. + (*prev_ptr) = bin->next; + set->entries--; + + void *return_value = bin->elt; + +// *bin = (cpHashSetBin){}; + free(bin); + + return return_value; + } + + return NULL; +} + +void * +cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr) +{ + int index = hash%set->size; + cpHashSetBin *bin = set->table[index]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + return (bin ? bin->elt : set->default_value); +} + +void +cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data) +{ + for(int i=0; isize; i++){ + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + func(bin->elt, data); + bin = next; + } + } +} + +void +cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data) +{ + // Iterate over all the chains. + for(int i=0; isize; i++){ + // The rest works similarly to cpHashSetRemove() above. + cpHashSetBin **prev_ptr = &set->table[i]; + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + if(func(bin->elt, data)){ + prev_ptr = &bin->next; + } else { + (*prev_ptr) = next; + + set->entries--; + free(bin); + } + + bin = next; + } + } +} diff --git a/chipmunk-5.0/cpHashSet.h b/chipmunk-5.0/cpHashSet.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpHashSet.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// cpHashSet uses a chained hashtable implementation. +// Other than the transformation functions, there is nothing fancy going on. + +// cpHashSetBin's form the linked lists in the chained hash table. +typedef struct cpHashSetBin { + // Pointer to the element. + void *elt; + // Hash value of the element. + cpHashValue hash; + // Next element in the chain. + struct cpHashSetBin *next; +} cpHashSetBin; + +// Equality function. Returns true if ptr is equal to elt. +typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt); +// Used by cpHashSetInsert(). Called to transform the ptr into an element. +typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); +// Iterator function for a hashset. +typedef void (*cpHashSetIterFunc)(void *elt, void *data); +// Reject function. Returns true if elt should be dropped. +typedef int (*cpHashSetRejectFunc)(void *elt, void *data); + +typedef struct cpHashSet { + // Number of elements stored in the table. + int entries; + // Number of cells in the table. + int size; + + cpHashSetEqlFunc eql; + cpHashSetTransFunc trans; + + // Default value returned by cpHashSetFind() when no element is found. + // Defaults to NULL. + void *default_value; + + cpHashSetBin **table; +} cpHashSet; + +// Basic allocation/destruction functions. +void cpHashSetDestroy(cpHashSet *set); +void cpHashSetFree(cpHashSet *set); + +cpHashSet *cpHashSetAlloc(void); +cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); +cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); + +// Insert an element into the set, returns the element. +// If it doesn't already exist, the transformation function is applied. +void *cpHashSetInsert(cpHashSet *set, cpHashValue hash, void *ptr, void *data); +// Remove and return an element from the set. +void *cpHashSetRemove(cpHashSet *set, cpHashValue hash, void *ptr); +// Find an element in the set. Returns the default value if the element isn't found. +void *cpHashSetFind(cpHashSet *set, cpHashValue hash, void *ptr); + +// Iterate over a hashset. +void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); +// Iterate over a hashset while rejecting certain elements. +void cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data); diff --git a/chipmunk-5.0/cpPolyShape.c b/chipmunk-5.0/cpPolyShape.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpPolyShape.c @@ -0,0 +1,219 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include + +#include "chipmunk.h" + +cpPolyShape * +cpPolyShapeAlloc(void) +{ + return (cpPolyShape *)calloc(1, sizeof(cpPolyShape)); +} + +static void +cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpVect *src = poly->verts; + cpVect *dst = poly->tVerts; + + for(int i=0; inumVerts; i++) + dst[i] = cpvadd(p, cpvrotate(src[i], rot)); +} + +static void +cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpPolyShapeAxis *src = poly->axes; + cpPolyShapeAxis *dst = poly->tAxes; + + for(int i=0; inumVerts; i++){ + cpVect n = cpvrotate(src[i].n, rot); + dst[i].n = n; + dst[i].d = cpvdot(p, n) + src[i].d; + } +} + +static cpBB +cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + cpFloat l, b, r, t; + + cpPolyShapeTransformAxes(poly, p, rot); + cpPolyShapeTransformVerts(poly, p, rot); + + cpVect *verts = poly->tVerts; + l = r = verts[0].x; + b = t = verts[0].y; + + // TODO do as part of cpPolyShapeTransformVerts? + for(int i=1; inumVerts; i++){ + cpVect v = verts[i]; + + l = cpfmin(l, v.x); + r = cpfmax(r, v.x); + + b = cpfmin(b, v.y); + t = cpfmax(t, v.y); + } + + return cpBBNew(l, b, r, t); +} + +static void +cpPolyShapeDestroy(cpShape *shape) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + free(poly->verts); + free(poly->tVerts); + + free(poly->axes); + free(poly->tAxes); +} + +static int +cpPolyShapePointQuery(cpShape *shape, cpVect p){ + return cpBBcontainsVect(shape->bb, p) && cpPolyShapeContainsVert((cpPolyShape *)shape, p); +} + +static void +cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + cpPolyShapeAxis *axes = poly->tAxes; + cpVect *verts = poly->tVerts; + int numVerts = poly->numVerts; + + for(int i=0; i an) continue; + + cpFloat bn = cpvdot(b, n); + cpFloat t = (axes[i].d - an)/(bn - an); + if(t < 0.0f || 1.0f < t) continue; + + cpVect point = cpvlerp(a, b, t); + cpFloat dt = -cpvcross(n, point); + cpFloat dtMin = -cpvcross(n, verts[i]); + cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]); + + if(dtMin <= dt && dt <= dtMax){ + info->shape = shape; + info->t = t; + info->n = n; + } + } +} + +static const cpShapeClass polyClass = { + CP_POLY_SHAPE, + cpPolyShapeCacheData, + cpPolyShapeDestroy, + cpPolyShapePointQuery, + cpPolyShapeSegmentQuery, +}; + +int +cpPolyValidate(cpVect *verts, int numVerts) +{ + for(int i=0; i= 0.0f) + return 0; + } + + return 1; +} + +int +cpPolyShapeGetNumVerts(cpShape *shape) +{ + assert(shape->klass == &polyClass); + return ((cpPolyShape *)shape)->numVerts; +} + +cpVect +cpPolyShapeGetVert(cpShape *shape, int index) +{ + assert(shape->klass == &polyClass); + assert(index < cpPolyShapeGetNumVerts(shape)); + + return ((cpPolyShape *)shape)->verts[index]; +} + + +static void +setUpVerts(cpPolyShape *poly, int numVerts, cpVect *verts, cpVect offset) +{ + poly->numVerts = numVerts; + + poly->verts = (cpVect *)calloc(numVerts, sizeof(cpVect)); + poly->tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect)); + poly->axes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); + poly->tAxes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); + + for(int i=0; iverts[i] = a; + poly->axes[i].n = n; + poly->axes[i].d = cpvdot(n, a); + } +} + +cpPolyShape * +cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + // Fail if the user attempts to pass a concave poly, or a bad winding. + assert(cpPolyValidate(verts, numVerts)); + + setUpVerts(poly, numVerts, verts, offset); + cpShapeInit((cpShape *)poly, &polyClass, body); + + return poly; +} + +cpShape * +cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset); +} + +// Unsafe API (chipmunk_unsafe.h) + +void +cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset) +{ + assert(shape->klass == &polyClass); + cpPolyShapeDestroy(shape); + setUpVerts((cpPolyShape *)shape, numVerts, verts, offset); +} diff --git a/chipmunk-5.0/cpPolyShape.h b/chipmunk-5.0/cpPolyShape.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpPolyShape.h @@ -0,0 +1,100 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Axis structure used by cpPolyShape. +typedef struct cpPolyShapeAxis{ + // normal + cpVect n; + // distance from origin + cpFloat d; +} cpPolyShapeAxis; + +// Convex polygon shape structure. +typedef struct cpPolyShape{ + cpShape shape; + + // Vertex and axis lists. + int numVerts; + cpVect *verts; + cpPolyShapeAxis *axes; + + // Transformed vertex and axis lists. + cpVect *tVerts; + cpPolyShapeAxis *tAxes; +} cpPolyShape; + +// Basic allocation functions. +cpPolyShape *cpPolyShapeAlloc(void); +cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset); +cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset); + +// Check that a set of vertexes has a correct winding and that they are convex +int cpPolyValidate(cpVect *verts, int numVerts); + +int cpPolyShapeGetNumVerts(cpShape *shape); +cpVect cpPolyShapeGetVert(cpShape *shape, int index); + +// *** inlined utility functions + +// Returns the minimum distance of the polygon to the axis. +static inline cpFloat +cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) +{ + cpVect *verts = poly->tVerts; + cpFloat min = cpvdot(n, verts[0]); + + int i; + for(i=1; inumVerts; i++) + min = cpfmin(min, cpvdot(n, verts[i])); + + return min - d; +} + +// Returns true if the polygon contains the vertex. +static inline int +cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v) +{ + cpPolyShapeAxis *axes = poly->tAxes; + + int i; + for(i=0; inumVerts; i++){ + cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + if(dist > 0.0) return 0; + } + + return 1; +} + +// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal. +static inline int +cpPolyShapeContainsVertPartial(cpPolyShape *poly, cpVect v, cpVect n) +{ + cpPolyShapeAxis *axes = poly->tAxes; + + int i; + for(i=0; inumVerts; i++){ + if(cpvdot(axes[i].n, n) < 0.0f) continue; + cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + if(dist > 0.0) return 0; + } + + return 1; +} diff --git a/chipmunk-5.0/cpShape.c b/chipmunk-5.0/cpShape.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpShape.c @@ -0,0 +1,401 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include "chipmunk.h" + +#define CP_DefineShapeGetter(struct, type, member, name) \ +CP_DeclareShapeGetter(struct, type, name){ \ + assert(shape->klass == &struct##Class); \ + return ((struct *)shape)->member; \ +} +cpHashValue SHAPE_ID_COUNTER = 0; + +void +cpResetShapeIdCounter(void) +{ + SHAPE_ID_COUNTER = 0; +} + + +cpShape* +cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) +{ + shape->klass = klass; + + shape->id = SHAPE_ID_COUNTER; + SHAPE_ID_COUNTER++; + + shape->body = body; + + shape->e = 0.0f; + shape->u = 0.0f; + shape->surface_v = cpvzero; + + shape->collision_type = 0; + shape->group = 0; + shape->layers = -1; + + shape->data = NULL; + + cpShapeCacheBB(shape); + + return shape; +} + +void +cpShapeDestroy(cpShape *shape) +{ + if(shape->klass->destroy) shape->klass->destroy(shape); +} + +void +cpShapeFree(cpShape *shape) +{ + if(shape) cpShapeDestroy(shape); + free(shape); +} + +cpBB +cpShapeCacheBB(cpShape *shape) +{ + cpBody *body = shape->body; + + shape->bb = shape->klass->cacheData(shape, body->p, body->rot); + return shape->bb; +} + +int +cpShapePointQuery(cpShape *shape, cpVect p, cpLayers layers, cpGroup group){ + if(!(group && shape->group && group == shape->group) && (layers&shape->layers)){ + return shape->klass->pointQuery(shape, p); + } + + return 0; +} + +int +cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpLayers layers, cpGroup group, cpSegmentQueryInfo *info){ + if(!(group && shape->group && group == shape->group) && (layers&shape->layers)){ + shape->klass->segmentQuery(shape, a, b, info); + } + + return (info->shape != NULL); +} + +void +cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info) +{ + printf("Segment Query:\n"); + printf("\tt: %f\n", info->t); +// printf("\tdist: %f\n", info->dist); +// printf("\tpoint: %s\n", cpvstr(info->point)); + printf("\tn: %s\n", cpvstr(info->n)); +} + + + + +cpCircleShape * +cpCircleShapeAlloc(void) +{ + return (cpCircleShape *)calloc(1, sizeof(cpCircleShape)); +} + +static inline cpBB +bbFromCircle(const cpVect c, const cpFloat r) +{ + return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r); +} + +static cpBB +cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); + return bbFromCircle(circle->tc, circle->r); +} + +static int +cpCircleShapePointQuery(cpShape *shape, cpVect p){ + cpCircleShape *circle = (cpCircleShape *)shape; + return cpvnear(circle->tc, p, circle->r); +} + +static void +circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + // umm... gross I normally frown upon such things + a = cpvsub(a, center); + b = cpvsub(b, center); + + cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b); + cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b); + cpFloat qc = cpvdot(a, a) - r*r; + + cpFloat det = qb*qb - 4.0f*qa*qc; + + if(det >= 0.0f){ + cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa); + if(0.0 <= t && t <= 1.0f){ + info->shape = shape; + info->t = t; + info->n = cpvnormalize(cpvlerp(a, b, t)); + } + } +} + +static void +cpCircleShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + cpCircleShape *circle = (cpCircleShape *)shape; + circleSegmentQuery(shape, circle->tc, circle->r, a, b, info); +} + +static const cpShapeClass cpCircleShapeClass = { + CP_CIRCLE_SHAPE, + cpCircleShapeCacheData, + NULL, + cpCircleShapePointQuery, + cpCircleShapeSegmentQuery, +}; + +cpCircleShape * +cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) +{ + circle->c = offset; + circle->r = radius; + + cpShapeInit((cpShape *)circle, &cpCircleShapeClass, body); + + return circle; +} + +cpShape * +cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) +{ + return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); +} + +CP_DefineShapeGetter(cpCircleShape, cpVect, c, Center) +CP_DefineShapeGetter(cpCircleShape, cpFloat, r, Radius) + +cpSegmentShape * +cpSegmentShapeAlloc(void) +{ + return (cpSegmentShape *)calloc(1, sizeof(cpSegmentShape)); +} + +static cpBB +cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); + seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); + seg->tn = cpvrotate(seg->n, rot); + + cpFloat l,r,s,t; + + if(seg->ta.x < seg->tb.x){ + l = seg->ta.x; + r = seg->tb.x; + } else { + l = seg->tb.x; + r = seg->ta.x; + } + + if(seg->ta.y < seg->tb.y){ + s = seg->ta.y; + t = seg->tb.y; + } else { + s = seg->tb.y; + t = seg->ta.y; + } + + cpFloat rad = seg->r; + return cpBBNew(l - rad, s - rad, r + rad, t + rad); +} + +static int +cpSegmentShapePointQuery(cpShape *shape, cpVect p){ + if(!cpBBcontainsVect(shape->bb, p)) return 0; + + cpSegmentShape *seg = (cpSegmentShape *)shape; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); + cpFloat dist = cpfabs(dn) - seg->r; + if(dist > 0.0f) return 0; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, p); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt <= dtMin){ + if(dt < (dtMin - seg->r)){ + return 0; + } else { + return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); + } + } else { + if(dt < dtMax){ + return 1; + } else { + if(dt < (dtMax + seg->r)) { + return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); + } else { + return 0; + } + } + } + + return 1; +} + +static void +cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape; + cpVect n = seg->tn; + // flip n if a is behind the axis + if(cpvdot(a, n) < cpvdot(seg->ta, n)) + n = cpvneg(n); + + cpFloat an = cpvdot(a, n); + cpFloat bn = cpvdot(b, n); + cpFloat d = cpvdot(seg->ta, n) + seg->r; + + cpFloat t = (d - an)/(bn - an); + if(0.0f < t && t < 1.0f){ + cpVect point = cpvlerp(a, b, t); + cpFloat dt = -cpvcross(seg->tn, point); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + if(dtMin < dt && dt < dtMax){ + info->shape = shape; + info->t = t; + info->n = n; + + return; // don't continue on and check endcaps + } + } + + if(seg->r) { + cpSegmentQueryInfo info1 = {.shape = NULL}; + cpSegmentQueryInfo info2 = {.shape = NULL}; + circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1); + circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2); + + if(info1.shape && !info2.shape){ + (*info) = info1; + } else if(info2.shape && !info1.shape){ + (*info) = info2; + } else if(info1.shape && info2.shape){ + if(info1.t < info2.t){ + (*info) = info1; + } else { + (*info) = info2; + } + } + } +} + +static const cpShapeClass cpSegmentShapeClass = { + CP_SEGMENT_SHAPE, + cpSegmentShapeCacheData, + NULL, + cpSegmentShapePointQuery, + cpSegmentShapeSegmentQuery, +}; + +cpSegmentShape * +cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); + + seg->r = r; + + cpShapeInit((cpShape *)seg, &cpSegmentShapeClass, body); + + return seg; +} + +cpShape* +cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); +} + +CP_DefineShapeGetter(cpSegmentShape, cpVect, a, A) +CP_DefineShapeGetter(cpSegmentShape, cpVect, b, B) +CP_DefineShapeGetter(cpSegmentShape, cpVect, n, Normal) +CP_DefineShapeGetter(cpSegmentShape, cpFloat, r, Radius) + +// Unsafe API (chipmunk_unsafe.h) + +void +cpCircleShapeSetRadius(cpShape *shape, cpFloat radius) +{ + assert(shape->klass == &cpCircleShapeClass); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->r = radius; +} + +void +cpCircleShapeSetOffset(cpShape *shape, cpVect offset) +{ + assert(shape->klass == &cpCircleShapeClass); + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->c = offset; +} + +void +cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b) +{ + assert(shape->klass == &cpSegmentShapeClass); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); +} + +void +cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius) +{ + assert(shape->klass == &cpSegmentShapeClass); + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->r = radius; +} diff --git a/chipmunk-5.0/cpShape.h b/chipmunk-5.0/cpShape.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpShape.h @@ -0,0 +1,155 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Forward declarations required for defining other structs. +struct cpShape; +struct cpShapeClass; + +typedef struct cpSegmentQueryInfo{ + struct cpShape *shape; // shape that was hit, NULL if no collision + cpFloat t; // line(t) = a + (b - a)t, will always be in the range [0, 1] + cpVect n; // normal of hit surface +} cpSegmentQueryInfo; + +// For determinism, you can reset the shape id counter. +void cpResetShapeIdCounter(void); + +// Enumeration of shape types. +typedef enum cpShapeType{ + CP_CIRCLE_SHAPE, + CP_SEGMENT_SHAPE, + CP_POLY_SHAPE, + CP_NUM_SHAPES +} cpShapeType; + +// Shape class. Holds function pointers and type data. +typedef struct cpShapeClass { + cpShapeType type; + + // Called by cpShapeCacheBB(). + cpBB (*cacheData)(struct cpShape *shape, cpVect p, cpVect rot); + // Called to by cpShapeDestroy(). + void (*destroy)(struct cpShape *shape); + + // called by cpShapePointQuery(). + int (*pointQuery)(struct cpShape *shape, cpVect p); + + // called by cpShapeSegmentQuery() + void (*segmentQuery)(struct cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info); +} cpShapeClass; + +// Basic shape struct that the others inherit from. +typedef struct cpShape{ + // The "class" of a shape as defined above + const cpShapeClass *klass; + + // cpBody that the shape is attached to. + cpBody *body; + + // Cached BBox for the shape. + cpBB bb; + + // *** Surface properties. + + // Coefficient of restitution. (elasticity) + cpFloat e; + // Coefficient of friction. + cpFloat u; + // Surface velocity used when solving for friction. + cpVect surface_v; + + // *** User Definable Fields + + // User defined data pointer for the shape. + void *data; + + // User defined collision type for the shape. + cpCollisionType collision_type; + // User defined collision group for the shape. + cpGroup group; + // User defined layer bitmask for the shape. + cpLayers layers; + + // *** Internally Used Fields + + // Unique id used as the hash value. + cpHashValue id; +} cpShape; + +// Low level shape initialization func. +cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body); + +// Basic destructor functions. (allocation functions are not shared) +void cpShapeDestroy(cpShape *shape); +void cpShapeFree(cpShape *shape); + +// Cache the BBox of the shape. +cpBB cpShapeCacheBB(cpShape *shape); + +// Test if a point lies within a shape. +int cpShapePointQuery(cpShape *shape, cpVect p, cpLayers layers, cpGroup group); +void cpSegmentQueryInfoPrint(cpSegmentQueryInfo *info); + +#define CP_DeclareShapeGetter(struct, type, name) type struct##Get##name(cpShape *shape) + +// Circle shape structure. +typedef struct cpCircleShape{ + cpShape shape; + + // Center in body space coordinates + cpVect c; + // Radius. + cpFloat r; + + // Transformed center. (world space coordinates) + cpVect tc; +} cpCircleShape; + +// Basic allocation functions for cpCircleShape. +cpCircleShape *cpCircleShapeAlloc(void); +cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); +cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); + +CP_DeclareShapeGetter(cpCircleShape, cpVect, Offset); +CP_DeclareShapeGetter(cpCircleShape, cpFloat, Radius); + +// Segment shape structure. +typedef struct cpSegmentShape{ + cpShape shape; + + // Endpoints and normal of the segment. (body space coordinates) + cpVect a, b, n; + // Radius of the segment. (Thickness) + cpFloat r; + + // Transformed endpoints and normal. (world space coordinates) + cpVect ta, tb, tn; +} cpSegmentShape; + +// Basic allocation functions for cpSegmentShape. +cpSegmentShape* cpSegmentShapeAlloc(void); +cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); +cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); + +CP_DeclareShapeGetter(cpSegmentShape, cpVect, A); +CP_DeclareShapeGetter(cpSegmentShape, cpVect, B); +CP_DeclareShapeGetter(cpSegmentShape, cpVect, Normal); +CP_DeclareShapeGetter(cpSegmentShape, cpFloat, Radius); diff --git a/chipmunk-5.0/cpSpace.c b/chipmunk-5.0/cpSpace.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpSpace.c @@ -0,0 +1,571 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include +#include + +#include "chipmunk.h" + +int cp_contact_persistence = 3; + +#pragma mark Contact Set Helpers + +// Equal function for contactSet. +static int +contactSetEql(cpShape **shapes, cpArbiter *arb) +{ + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); +} + +// Transformation function for contactSet. +static void * +contactSetTrans(cpShape **shapes, cpSpace *space) +{ + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + return cpArbiterNew(a, b, space->stamp); +} + +#pragma mark Collision Pair Function Helpers + +// Collision pair function wrapper struct. +typedef struct collFuncData { + cpCollFunc func; + void *data; +} collFuncData; + +// Equals function for collFuncSet. +static int +collFuncSetEql(cpCollisionType *ids, cpCollPairFunc *pair) +{ + cpCollisionType a = ids[0]; + cpCollisionType b = ids[1]; + + return ((a == pair->a && b == pair->b) || (b == pair->a && a == pair->b)); +} + +// Transformation function for collFuncSet. +static void * +collFuncSetTrans(cpCollisionType *ids, collFuncData *funcData) +{ + cpCollPairFunc *pair = (cpCollPairFunc *)malloc(sizeof(cpCollPairFunc)); + pair->a = ids[0]; + pair->b = ids[1]; + pair->func = funcData->func; + pair->data = funcData->data; + + return pair; +} + +#pragma mark Misc Helper Funcs + +// Default collision pair function. +static int +alwaysCollide(cpShape *a, cpShape *b, cpContact *arr, int numCon, cpFloat normal_coef, void *data) +{ + return 1; +} + +// BBfunc callback for the spatial hash. +static cpBB shapeBBFunc(cpShape *shape){return shape->bb;} + +// Iterator functions for destructors. +static void freeWrap(void *ptr, void *unused){ free(ptr);} +static void shapeFreeWrap(cpShape *ptr, void *unused){ cpShapeFree(ptr);} +static void arbiterFreeWrap(cpArbiter *ptr, void *unused){ cpArbiterFree(ptr);} +static void bodyFreeWrap(cpBody *ptr, void *unused){ cpBodyFree(ptr);} +static void constraintFreeWrap(cpConstraint *ptr, void *unused){cpConstraintFree(ptr);} + +#pragma mark Memory Management Functions + +cpSpace* +cpSpaceAlloc(void) +{ + return (cpSpace *)calloc(1, sizeof(cpSpace)); +} + +#define DEFAULT_DIM_SIZE 100.0f +#define DEFAULT_COUNT 1000 +#define DEFAULT_ITERATIONS 10 +#define DEFAULT_ELASTIC_ITERATIONS 0 + +cpSpace* +cpSpaceInit(cpSpace *space) +{ + space->iterations = DEFAULT_ITERATIONS; + space->elasticIterations = DEFAULT_ELASTIC_ITERATIONS; +// space->sleepTicks = 300; + + space->gravity = cpvzero; + space->damping = 1.0f; + + space->stamp = 0; + + space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc); + space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, (cpSpaceHashBBFunc)shapeBBFunc); + + space->bodies = cpArrayNew(0); + space->arbiters = cpArrayNew(0); + space->contactSet = cpHashSetNew(0, (cpHashSetEqlFunc)contactSetEql, (cpHashSetTransFunc)contactSetTrans); + + space->constraints = cpArrayNew(0); + + cpCollPairFunc pairFunc = {0, 0, alwaysCollide, NULL}; + space->defaultPairFunc = pairFunc; + space->collFuncSet = cpHashSetNew(0, (cpHashSetEqlFunc)collFuncSetEql, (cpHashSetTransFunc)collFuncSetTrans); + space->collFuncSet->default_value = &space->defaultPairFunc; + + return space; +} + +cpSpace* +cpSpaceNew(void) +{ + return cpSpaceInit(cpSpaceAlloc()); +} + +void +cpSpaceDestroy(cpSpace *space) +{ + cpSpaceHashFree(space->staticShapes); + cpSpaceHashFree(space->activeShapes); + + cpArrayFree(space->bodies); + + cpArrayFree(space->constraints); + + if(space->contactSet) + cpHashSetEach(space->contactSet, (cpHashSetIterFunc)&arbiterFreeWrap, NULL); + + cpHashSetFree(space->contactSet); + cpArrayFree(space->arbiters); + + if(space->collFuncSet) + cpHashSetEach(space->collFuncSet, &freeWrap, NULL); + cpHashSetFree(space->collFuncSet); +} + +void +cpSpaceFree(cpSpace *space) +{ + if(space) cpSpaceDestroy(space); + free(space); +} + +void +cpSpaceFreeChildren(cpSpace *space) +{ + cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL); + cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&shapeFreeWrap, NULL); + cpArrayEach(space->bodies, (cpArrayIter)&bodyFreeWrap, NULL); + cpArrayEach(space->constraints, (cpArrayIter)&constraintFreeWrap, NULL); +} + +#pragma mark Collision Pair Function Management + +void +cpSpaceAddCollisionPairFunc(cpSpace *space, cpCollisionType a, cpCollisionType b, cpCollFunc func, void *data) +{ + cpCollisionType ids[] = {a, b}; + cpHashValue hash = CP_HASH_PAIR(a, b); + // Remove any old function so the new one will get added. + cpSpaceRemoveCollisionPairFunc(space, a, b); + + collFuncData funcData = {func, data}; + cpHashSetInsert(space->collFuncSet, hash, ids, &funcData); +} + +void +cpSpaceRemoveCollisionPairFunc(cpSpace *space, cpCollisionType a, cpCollisionType b) +{ + cpCollisionType ids[] = {a, b}; + cpHashValue hash = CP_HASH_PAIR(a, b); + cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids); + free(old_pair); +} + +void +cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data) +{ + cpCollPairFunc pairFunc = {0, 0, (func ? func : alwaysCollide), (func ? data : NULL)}; + space->defaultPairFunc = pairFunc; +} + +#pragma mark Body, Shape, and Joint Management + +cpShape * +cpSpaceAddShape(cpSpace *space, cpShape *shape) +{ + assert(shape->body); + cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb); + + return shape; +} + +cpShape * +cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) +{ + assert(shape->body); + + cpShapeCacheBB(shape); + cpSpaceHashInsert(space->staticShapes, shape, shape->id, shape->bb); + + return shape; +} + +cpBody * +cpSpaceAddBody(cpSpace *space, cpBody *body) +{ + cpArrayPush(space->bodies, body); + + return body; +} + +cpConstraint * +cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpArrayPush(space->constraints, constraint); + + return constraint; +} + +static void +shapeRemovalArbiterReject(cpSpace *space, cpShape *shape) +{ + cpArray *old_ary = space->arbiters; + int num = old_ary->num; + + if(num == 0) return; + + // make a new arbiters array and copy over all valid arbiters + cpArray *new_ary = cpArrayNew(num); + + for(int i=0; iarr[i]; + if(arb->a != shape && arb->b != shape){ + cpArrayPush(new_ary, arb); + } + } + + space->arbiters = new_ary; + cpArrayFree(old_ary); +} + +void +cpSpaceRemoveShape(cpSpace *space, cpShape *shape) +{ + cpSpaceHashRemove(space->activeShapes, shape, shape->id); + shapeRemovalArbiterReject(space, shape); +} + +void +cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) +{ + cpSpaceHashRemove(space->staticShapes, shape, shape->id); + shapeRemovalArbiterReject(space, shape); +} + +void +cpSpaceRemoveBody(cpSpace *space, cpBody *body) +{ + cpArrayDeleteObj(space->bodies, body); +} + +void +cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint) +{ + cpArrayDeleteObj(space->constraints, constraint); +} + +#pragma mark Point Query Functions + +typedef struct pointQueryContext { + cpLayers layers; + cpGroup group; + cpSpacePointQueryFunc func; + void *data; +} pointQueryContext; + +static void +pointQueryHelper(cpVect *point, cpShape *shape, pointQueryContext *context) +{ + if(cpShapePointQuery(shape, *point, context->layers, context->group)) + context->func(shape, context->data); +} + +void +cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data) +{ + pointQueryContext context = {layers, group, func, data}; + cpSpaceHashPointQuery(space->activeShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context); + cpSpaceHashPointQuery(space->staticShapes, point, (cpSpaceHashQueryFunc)pointQueryHelper, &context); +} + +static void +rememberLastPointQuery(cpShape *shape, cpShape **outShape) +{ + (*outShape) = shape; +} + +cpShape * +cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group) +{ + cpShape *shape = NULL; + cpSpacePointQuery(space, point, layers, group, (cpSpacePointQueryFunc)rememberLastPointQuery, &shape); + + return shape; +} + +void +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) +{ + cpArray *bodies = space->bodies; + + for(int i=0; inum; i++) + func((cpBody *)bodies->arr[i], data); +} + +#pragma mark Spatial Hash Management + +// Iterator function used for updating shape BBoxes. +static void +updateBBCache(cpShape *shape, void *unused) +{ + cpShapeCacheBB(shape); +} + +void +cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpaceHashResize(space->staticShapes, dim, count); + cpSpaceHashRehash(space->staticShapes); +} + +void +cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpaceHashResize(space->activeShapes, dim, count); +} + +void +cpSpaceRehashStatic(cpSpace *space) +{ + cpSpaceHashEach(space->staticShapes, (cpSpaceHashIterator)&updateBBCache, NULL); + cpSpaceHashRehash(space->staticShapes); +} + +#pragma mark Collision Detection Functions + +static inline int +queryReject(cpShape *a, cpShape *b) +{ + return + // BBoxes must overlap + !cpBBintersects(a->bb, b->bb) + // Don't collide shapes attached to the same body. + || a->body == b->body + // Don't collide objects in the same non-zero group + || (a->group && b->group && a->group == b->group) + // Don't collide objects that don't share at least on layer. + || !(a->layers & b->layers); +} + +// Callback from the spatial hash. +static void +queryFunc(cpShape *a, cpShape *b, cpSpace *space) +{ + // Reject any of the simple cases + if(queryReject(a,b)) return; + + // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) + if(a->klass->type > b->klass->type){ + cpShape *temp = a; + a = b; + b = temp; + } + + // Narrow-phase collision detection. + cpContact *contacts = NULL; + int numContacts = cpCollideShapes(a, b, &contacts); + if(!numContacts) return; // Shapes are not colliding. + + // Get an arbiter from space->contactSet for the two shapes. + // This is where the persistant contact magic comes from. + cpShape *shape_pair[] = {a, b}; + cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space); + + // Timestamp the arbiter. + arb->stamp = space->stamp; + // For collisions between two similar primitive types, the order could have been swapped. + arb->a = a; arb->b = b; + // Inject the new contact points into the arbiter. + cpArbiterInject(arb, contacts, numContacts); + + // Add the arbiter to the list of active arbiters. + cpArrayPush(space->arbiters, arb); +} + +// Iterator for active/static hash collisions. +static void +active2staticIter(cpShape *shape, cpSpace *space) +{ + cpSpaceHashQuery(space->staticShapes, shape, shape->bb, (cpSpaceHashQueryFunc)&queryFunc, space); +} + +// Hashset reject func to throw away old arbiters. +static int +contactSetReject(cpArbiter *arb, cpSpace *space) +{ + if((space->stamp - arb->stamp) > cp_contact_persistence){ + cpArbiterFree(arb); + return 0; + } + + return 1; +} + +static void +filterArbiterByCallback(cpSpace *space) +{ + int num = space->arbiters->num; + + // copy to the stack + cpArbiter *ary[num]; + memcpy(ary, space->arbiters->arr, num*sizeof(void *)); + + for(int i=0; ia; + cpShape *b = arb->b; + cpFloat normal_coef = 1.0f; + + // Find the collision pair function for the shapes. + cpCollisionType ids[] = {a->collision_type, b->collision_type}; + cpHashValue hash = CP_HASH_PAIR(a->collision_type, b->collision_type); + cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids); + if(!pairFunc->func) continue; // A NULL pair function means don't collide at all. + + // Swap them if necessary. + if(a->collision_type != pairFunc->a){ + cpShape *temp = a; + a = b; + b = temp; + normal_coef = -1.0f; + } + + if(!pairFunc->func(a, b, arb->contacts, arb->numContacts, normal_coef, pairFunc->data)){ + cpArrayDeleteObj(space->arbiters, arb); + } + } +} + +#pragma mark All Important cpSpaceStep() Function + +void +cpSpaceStep(cpSpace *space, cpFloat dt) +{ + if(!dt) return; // prevents div by zero. + cpFloat dt_inv = 1.0f/dt; + + cpArray *bodies = space->bodies; + cpArray *constraints = space->constraints; + + // Empty the arbiter list. + cpHashSetReject(space->contactSet, (cpHashSetRejectFunc)&contactSetReject, space); + space->arbiters->num = 0; + + // Integrate positions. + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->position_func(body, dt); + } + + // Pre-cache BBoxes and shape data. + cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&updateBBCache, NULL); + + // Collide! + cpSpaceHashEach(space->activeShapes, (cpSpaceHashIterator)&active2staticIter, space); + cpSpaceHashQueryRehash(space->activeShapes, (cpSpaceHashQueryFunc)&queryFunc, space); + + // Filter arbiter list based on collision callbacks + filterArbiterByCallback(space); + + // Prestep the arbiters. + cpArray *arbiters = space->arbiters; + for(int i=0; inum; i++) + cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); + + // Prestep the constraints. + for(int i=0; inum; i++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[i]; + constraint->klass->preStep(constraint, dt, dt_inv); + } + + for(int i=0; ielasticIterations; i++){ + for(int j=0; jnum; j++) + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f); + + for(int j=0; jnum; j++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; + constraint->klass->applyImpulse(constraint); + } + } + + // Integrate velocities. + cpFloat damping = cpfpow(1.0f/space->damping, -dt); + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->velocity_func(body, space->gravity, damping, dt); + } + + for(int i=0; inum; i++) + cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]); + + // Run the impulse solver. + // run the old-style elastic solver if elastic iterations are disabled + cpFloat elasticCoef = (space->elasticIterations ? 0.0f : 1.0f); + for(int i=0; iiterations; i++){ + for(int j=0; jnum; j++) + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], elasticCoef); + + for(int j=0; jnum; j++){ + cpConstraint *constraint = (cpConstraint *)constraints->arr[j]; + constraint->klass->applyImpulse(constraint); + } + } + +// cpFloat dvsq = cpvdot(space->gravity, space->gravity); +// dvsq *= dt*dt * space->damping*space->damping; +// for(int i=0; inum; i++) +// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks); + + // Increment the stamp. + space->stamp++; +} diff --git a/chipmunk-5.0/cpSpace.h b/chipmunk-5.0/cpSpace.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpSpace.h @@ -0,0 +1,120 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Number of frames that contact information should persist. +extern int cp_contact_persistence; + +// User collision pair function. +typedef int (*cpCollFunc)(cpShape *a, cpShape *b, cpContact *contacts, int numContacts, cpFloat normal_coef, void *data); + +// Structure for holding collision pair function information. +// Used internally. +typedef struct cpCollPairFunc { + cpCollisionType a; + cpCollisionType b; + cpCollFunc func; + void *data; +} cpCollPairFunc; + +typedef struct cpSpace{ + // *** User definable fields + + // Number of iterations to use in the impulse solver to solve contacts. + int iterations; + + // Number of iterations to use in the impulse solver to solve elastic collisions. + int elasticIterations; + + // Default gravity to supply when integrating rigid body motions. + cpVect gravity; + + // Default damping to supply when integrating rigid body motions. + cpFloat damping; + + // *** Internally Used Fields + + // Time stamp. Is incremented on every call to cpSpaceStep(). + int stamp; + + // The static and active shape spatial hashes. + cpSpaceHash *staticShapes; + cpSpaceHash *activeShapes; + + // List of bodies in the system. + cpArray *bodies; + // List of active arbiters for the impulse solver. + cpArray *arbiters; + // Persistant contact set. + cpHashSet *contactSet; + + // List of constraints in the system. + cpArray *constraints; + + // Set of collisionpair functions. + cpHashSet *collFuncSet; + // Default collision pair function. + cpCollPairFunc defaultPairFunc; +} cpSpace; + +// Basic allocation/destruction functions. +cpSpace* cpSpaceAlloc(void); +cpSpace* cpSpaceInit(cpSpace *space); +cpSpace* cpSpaceNew(void); + +void cpSpaceDestroy(cpSpace *space); +void cpSpaceFree(cpSpace *space); + +// Convenience function. Frees all referenced entities. (bodies, shapes and constraints) +void cpSpaceFreeChildren(cpSpace *space); + +// Collision pair function management functions. +void cpSpaceAddCollisionPairFunc(cpSpace *space, cpCollisionType a, cpCollisionType b, + cpCollFunc func, void *data); +void cpSpaceRemoveCollisionPairFunc(cpSpace *space, cpCollisionType a, cpCollisionType b); +void cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data); + +// Add and remove entities from the system. +cpShape *cpSpaceAddShape(cpSpace *space, cpShape *shape); +cpShape *cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); +cpBody *cpSpaceAddBody(cpSpace *space, cpBody *body); +cpConstraint *cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint); + +void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); +void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); +void cpSpaceRemoveBody(cpSpace *space, cpBody *body); +void cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint); + +// Point query callback function +typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); +void cpSpacePointQuery(cpSpace *space, cpVect point, cpLayers layers, cpGroup group, cpSpacePointQueryFunc func, void *data); +cpShape *cpSpacePointQueryFirst(cpSpace *space, cpVect point, cpLayers layers, cpGroup group); + +// Iterator function for iterating the bodies in a space. +typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data); +void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data); + +// Spatial hash management functions. +void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count); +void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count); +void cpSpaceRehashStatic(cpSpace *space); + +// Update the space. +void cpSpaceStep(cpSpace *space, cpFloat dt); diff --git a/chipmunk-5.0/cpSpaceHash.c b/chipmunk-5.0/cpSpaceHash.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpSpaceHash.c @@ -0,0 +1,460 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include +#include +#include + +#include "chipmunk.h" +#include "prime.h" + +static cpHandle* +cpHandleAlloc(void) +{ + return (cpHandle *)malloc(sizeof(cpHandle)); +} + +static cpHandle* +cpHandleInit(cpHandle *hand, void *obj) +{ + hand->obj = obj; + hand->retain = 0; + hand->stamp = 0; + + return hand; +} + +static cpHandle* +cpHandleNew(void *obj) +{ + return cpHandleInit(cpHandleAlloc(), obj); +} + +static inline void +cpHandleRetain(cpHandle *hand) +{ + hand->retain++; +} + +static inline void +cpHandleFree(cpHandle *hand) +{ + free(hand); +} + +static inline void +cpHandleRelease(cpHandle *hand) +{ + hand->retain--; + if(hand->retain == 0) + cpHandleFree(hand); +} + + +cpSpaceHash* +cpSpaceHashAlloc(void) +{ + return (cpSpaceHash *)calloc(1, sizeof(cpSpaceHash)); +} + +// Frees the old table, and allocates a new one. +static void +cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) +{ + free(hash->table); + + hash->numcells = numcells; + hash->table = (cpSpaceHashBin **)calloc(numcells, sizeof(cpSpaceHashBin *)); +} + +// Equality function for the handleset. +static int +handleSetEql(void *obj, void *elt) +{ + cpHandle *hand = (cpHandle *)elt; + return (obj == hand->obj); +} + +// Transformation function for the handleset. +static void * +handleSetTrans(void *obj, void *unused) +{ + cpHandle *hand = cpHandleNew(obj); + cpHandleRetain(hand); + + return hand; +} + +cpSpaceHash* +cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc) +{ + cpSpaceHashAllocTable(hash, next_prime(numcells)); + hash->celldim = celldim; + hash->bbfunc = bbfunc; + + hash->bins = NULL; + hash->handleSet = cpHashSetNew(0, &handleSetEql, &handleSetTrans); + + hash->stamp = 1; + + return hash; +} + +cpSpaceHash* +cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc) +{ + return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc); +} + +static inline void +clearHashCell(cpSpaceHash *hash, int index) +{ + cpSpaceHashBin *bin = hash->table[index]; + while(bin){ + cpSpaceHashBin *next = bin->next; + + // Release the lock on the handle. + cpHandleRelease(bin->handle); + // Recycle the bin. + bin->next = hash->bins; + hash->bins = bin; + + bin = next; + } + + hash->table[index] = NULL; +} + +// Clear all cells in the hashtable. +static void +clearHash(cpSpaceHash *hash) +{ + for(int i=0; inumcells; i++) + clearHashCell(hash, i); +} + +// Free the recycled hash bins. +static void +freeBins(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->bins; + while(bin){ + cpSpaceHashBin *next = bin->next; + free(bin); + bin = next; + } +} + +// Hashset iterator function to free the handles. +static void +handleFreeWrap(void *elt, void *unused) +{ + cpHandle *hand = (cpHandle *)elt; + cpHandleFree(hand); +} + +void +cpSpaceHashDestroy(cpSpaceHash *hash) +{ + clearHash(hash); + freeBins(hash); + + // Free the handles. + cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL); + cpHashSetFree(hash->handleSet); + + free(hash->table); +} + +void +cpSpaceHashFree(cpSpaceHash *hash) +{ + if(!hash) return; + cpSpaceHashDestroy(hash); + free(hash); +} + +void +cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) +{ + // Clear the hash to release the old handle locks. + clearHash(hash); + + hash->celldim = celldim; + cpSpaceHashAllocTable(hash, next_prime(numcells)); +} + +// Return true if the chain contains the handle. +static inline int +containsHandle(cpSpaceHashBin *bin, cpHandle *hand) +{ + while(bin){ + if(bin->handle == hand) return 1; + bin = bin->next; + } + + return 0; +} + +// Get a recycled or new bin. +static inline cpSpaceHashBin * +getEmptyBin(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->bins; + + // Make a new one if necessary. + if(bin == NULL) return (cpSpaceHashBin *)malloc(sizeof(cpSpaceHashBin)); + + hash->bins = bin->next; + return bin; +} + +// The hash function itself. +static inline cpHashValue +hash_func(cpHashValue x, cpHashValue y, cpHashValue n) +{ + return (x*2185031351ul ^ y*4232417593ul) % n; +} + +static inline void +hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) +{ + // Find the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + int n = hash->numcells; + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int index = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[index]; + + // Don't add an object twice to the same cell. + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); + // Insert a new bin for the handle in this cell. + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[index] = newBin; + } + } +} + +void +cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB bb) +{ + cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, id, obj, NULL); + hashHandle(hash, hand, bb); +} + +void +cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue id) +{ + cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, id, obj); + hashHandle(hash, hand, hash->bbfunc(obj)); +} + +// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?) +static void +handleRehashHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + cpSpaceHash *hash = (cpSpaceHash *)data; + + hashHandle(hash, hand, hash->bbfunc(hand->obj)); +} + +void +cpSpaceHashRehash(cpSpaceHash *hash) +{ + clearHash(hash); + + // Rehash all of the handles. + cpHashSetEach(hash->handleSet, &handleRehashHelper, hash); +} + +void +cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, id, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand); + } +} + +// Used by the cpSpaceHashEach() iterator. +typedef struct eachPair { + cpSpaceHashIterator func; + void *data; +} eachPair; + +// Calls the user iterator function. (Gross I know.) +static void +eachHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + eachPair *pair = (eachPair *)data; + + pair->func(hand->obj, pair->data); +} + +// Iterate over the objects in the spatial hash. +void +cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data) +{ + // Bundle the callback up to send to the hashset iterator. + eachPair pair = {func, data}; + + cpHashSetEach(hash->handleSet, &eachHelper, &pair); +} + +// Calls the callback function for the objects in a given chain. +static inline void +query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data) +{ + for(; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + // Skip over certain conditions + if( + // Have we already tried this pair in this query? + hand->stamp == hash->stamp + // Is obj the same as other? + || obj == other + // Has other been removed since the last rehash? + || !other + ) continue; + + func(obj, other, data); + + // Stamp that the handle was checked already against this object. + hand->stamp = hash->stamp; + } +} + +void +cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data) +{ + cpFloat dim = hash->celldim; + int index = hash_func((int)(point.x/dim), (int)(point.y/dim), hash->numcells); + + query(hash, hash->table[index], &point, func, data); + + // Increment the stamp. + // Only one cell is checked, but query() requires it anyway. + hash->stamp++; +} + +void +cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data) +{ + // Get the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + int n = hash->numcells; + + // Iterate over the cells and query them. + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int index = hash_func(i,j,n); + query(hash, hash->table[index], obj, func, data); + } + } + + // Increment the stamp. + hash->stamp++; +} + +// Similar to struct eachPair above. +typedef struct queryRehashPair { + cpSpaceHash *hash; + cpSpaceHashQueryFunc func; + void *data; +} queryRehashPair; + +// Hashset iterator func used with cpSpaceHashQueryRehash(). +static void +handleQueryRehashHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + + // Unpack the user callback data. + queryRehashPair *pair = (queryRehashPair *)data; + cpSpaceHash *hash = pair->hash; + cpSpaceHashQueryFunc func = pair->func; + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + void *obj = hand->obj; + cpBB bb = hash->bbfunc(obj); + + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ +// // exit the loops if the object has been deleted in func(). +// if(!hand->obj) goto break_out; + + int index = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[index]; + + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); // this MUST be done first in case the object is removed in func() + query(hash, bin, obj, func, pair->data); + + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[index] = newBin; + } + } + +// break_out: + // Increment the stamp for each object we hash. + hash->stamp++; +} + +void +cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data) +{ + clearHash(hash); + + queryRehashPair pair = {hash, func, data}; + cpHashSetEach(hash->handleSet, &handleQueryRehashHelper, &pair); +} diff --git a/chipmunk-5.0/cpSpaceHash.h b/chipmunk-5.0/cpSpaceHash.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpSpaceHash.h @@ -0,0 +1,100 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// The spatial hash is Chipmunk's default (and currently only) spatial index type. +// Based on a chained hash table. + +// Used internally to track objects added to the hash +typedef struct cpHandle{ + // Pointer to the object + void *obj; + // Retain count + int retain; + // Query stamp. Used to make sure two objects + // aren't identified twice in the same query. + int stamp; +} cpHandle; + +// Linked list element for in the chains. +typedef struct cpSpaceHashBin{ + cpHandle *handle; + struct cpSpaceHashBin *next; +} cpSpaceHashBin; + +// BBox callback. Called whenever the hash needs a bounding box from an object. +typedef cpBB (*cpSpaceHashBBFunc)(void *obj); + +typedef struct cpSpaceHash{ + // Number of cells in the table. + int numcells; + // Dimentions of the cells. + cpFloat celldim; + + // BBox callback. + cpSpaceHashBBFunc bbfunc; + + // Hashset of all the handles. + cpHashSet *handleSet; + + cpSpaceHashBin **table; + // List of recycled bins. + cpSpaceHashBin *bins; + + // Incremented on each query. See cpHandle.stamp. + int stamp; +} cpSpaceHash; + +//Basic allocation/destruction functions. +cpSpaceHash *cpSpaceHashAlloc(void); +cpSpaceHash *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); +cpSpaceHash *cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); + +void cpSpaceHashDestroy(cpSpaceHash *hash); +void cpSpaceHashFree(cpSpaceHash *hash); + +// Resize the hashtable. (Does not rehash! You must call cpSpaceHashRehash() if needed.) +void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); + +// Add an object to the hash. +void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, cpHashValue id, cpBB bb); +// Remove an object from the hash. +void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, cpHashValue id); + +// Iterator function +typedef void (*cpSpaceHashIterator)(void *obj, void *data); +// Iterate over the objects in the hash. +void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data); + +// Rehash the contents of the hash. +void cpSpaceHashRehash(cpSpaceHash *hash); +// Rehash only a specific object. +void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, cpHashValue id); + +// Query callback. +typedef void (*cpSpaceHashQueryFunc)(void *obj1, void *obj2, void *data); +// Point query the hash. A reference to the query point is passed as obj1 to the query callback. +void cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data); +// Query the hash for a given BBox. +void cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); +// Run a query for the object, then insert it. (Optimized case) +void cpSpaceHashQueryInsert(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); +// Rehashes while querying for each object. (Optimized case) +void cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data); diff --git a/chipmunk-5.0/cpVect.c b/chipmunk-5.0/cpVect.c new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpVect.c @@ -0,0 +1,51 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + +cpFloat +cpvlength(const cpVect v) +{ + return cpfsqrt( cpvdot(v, v) ); +} + +cpVect +cpvforangle(const cpFloat a) +{ + return cpv(cpfcos(a), cpfsin(a)); +} + +cpFloat +cpvtoangle(const cpVect v) +{ + return cpfatan2(v.y, v.x); +} + +char* +cpvstr(const cpVect v) +{ + static char str[256]; + sprintf(str, "(% .3f, % .3f)", v.x, v.y); + return str; +} diff --git a/chipmunk-5.0/cpVect.h b/chipmunk-5.0/cpVect.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/cpVect.h @@ -0,0 +1,153 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef struct cpVect{ + cpFloat x,y; +} cpVect; + +static const cpVect cpvzero={0.0f,0.0f}; + +static inline cpVect +cpv(const cpFloat x, const cpFloat y) +{ + cpVect v = {x, y}; + return v; +} + +// non-inlined functions +cpFloat cpvlength(const cpVect v); +cpVect cpvforangle(const cpFloat a); // convert radians to a normalized vector +cpFloat cpvtoangle(const cpVect v); // convert a vector to radians +char *cpvstr(const cpVect v); // get a string representation of a vector + +static inline cpVect +cpvadd(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x + v2.x, v1.y + v2.y); +} + +static inline cpVect +cpvneg(const cpVect v) +{ + return cpv(-v.x, -v.y); +} + +static inline cpVect +cpvsub(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x - v2.x, v1.y - v2.y); +} + +static inline cpVect +cpvmult(const cpVect v, const cpFloat s) +{ + return cpv(v.x*s, v.y*s); +} + +static inline cpFloat +cpvdot(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.x + v1.y*v2.y; +} + +static inline cpFloat +cpvcross(const cpVect v1, const cpVect v2) +{ + return v1.x*v2.y - v1.y*v2.x; +} + +static inline cpVect +cpvperp(const cpVect v) +{ + return cpv(-v.y, v.x); +} + +static inline cpVect +cpvrperp(const cpVect v) +{ + return cpv(v.y, -v.x); +} + +static inline cpVect +cpvproject(const cpVect v1, const cpVect v2) +{ + return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); +} + +static inline cpVect +cpvrotate(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); +} + +static inline cpVect +cpvunrotate(const cpVect v1, const cpVect v2) +{ + return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); +} + +static inline cpFloat +cpvlengthsq(const cpVect v) +{ + return cpvdot(v, v); +} + +static inline cpVect +cpvlerp(const cpVect v1, const cpVect v2, const cpFloat t) +{ + return cpvadd(cpvmult(v1, 1.0f - t), cpvmult(v2, t)); +} + +static inline cpVect +cpvnormalize(const cpVect v) +{ + return cpvmult(v, 1.0f/cpvlength(v)); +} + +static inline cpVect +cpvnormalize_safe(const cpVect v) +{ + return (v.x == 0.0f && v.y == 0.0f ? cpvzero : cpvnormalize(v)); +} + +static inline cpVect +cpvclamp(const cpVect v, const cpFloat len) +{ + return (cpvdot(v,v) > len*len) ? cpvmult(cpvnormalize(v), len) : v; +} + +static inline cpFloat +cpvdist(const cpVect v1, const cpVect v2) +{ + return cpvlength(cpvsub(v1, v2)); +} + +static inline cpFloat +cpvdistsq(const cpVect v1, const cpVect v2) +{ + return cpvlengthsq(cpvsub(v1, v2)); +} + +static inline int +cpvnear(const cpVect v1, const cpVect v2, const cpFloat dist) +{ + return cpvdistsq(v1, v2) < dist*dist; +} diff --git a/chipmunk-5.0/prime.h b/chipmunk-5.0/prime.h new file mode 100644 --- /dev/null +++ b/chipmunk-5.0/prime.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Used for resizing hash tables. +// Values approximately double. + +static int primes[] = { + 5, //2^2 + 1 + 11, //2^3 + 3 + 17, //2^4 + 1 + 37, //2^5 + 5 + 67, //2^6 + 3 + 131, //2^7 + 3 + 257, //2^8 + 1 + 521, //2^9 + 9 + 1031, //2^10 + 7 + 2053, //2^11 + 5 + 4099, //2^12 + 3 + 8209, //2^13 + 17 + 16411, //2^14 + 27 + 32771, //2^15 + 3 + 65537, //2^16 + 1 + 131101, //2^17 + 29 + 262147, //2^18 + 3 + 524309, //2^19 + 21 + 1048583, //2^20 + 7 + 2097169, //2^21 + 17 + 4194319, //2^22 + 15 + 8388617, //2^23 + 9 + 16777259, //2^24 + 43 + 33554467, //2^25 + 35 + 67108879, //2^26 + 15 + 134217757, //2^27 + 29 + 268435459, //2^28 + 3 + 536870923, //2^29 + 11 + 1073741827, //2^30 + 3 + 0, +}; + +static inline int +next_prime(int n) +{ + int i = 0; + while(n > primes[i]){ + i++; + assert(primes[i]); // realistically this should never happen + } + + return primes[i]; +} diff --git a/chipmunk/chipmunk.c b/chipmunk/chipmunk.c deleted file mode 100644 --- a/chipmunk/chipmunk.c +++ /dev/null @@ -1,69 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "stdlib.h" - -#include "chipmunk.h" - -#ifdef __cplusplus -extern "C" { -#endif - void cpInitCollisionFuncs(void); -#ifdef __cplusplus -} -#endif - - -void -cpInitChipmunk(void) -{ - cpInitCollisionFuncs(); -} - -cpFloat -cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset) -{ - return (1.0f/2.0f)*m*(r1*r1 + r2*r2) + m*cpvdot(offset, offset); -} - -cpFloat -cpMomentForPoly(cpFloat m, const int numVerts, cpVect *verts, cpVect offset) -{ - cpVect *tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect)); - for(int i=0; i b) ? a : b; -} - -static inline cpFloat -cpfmin(cpFloat a, cpFloat b) -{ - return (a < b) ? a : b; -} - -static inline cpFloat -cpfclamp(cpFloat f, cpFloat min, cpFloat max){ - return cpfmin(cpfmax(f, min), max); -} - -#ifndef INFINITY - #ifdef _MSC_VER - union MSVC_EVIL_FLOAT_HACK - { - unsigned __int8 Bytes[4]; - float Value; - }; - static union MSVC_EVIL_FLOAT_HACK INFINITY_HACK = {{0x00, 0x00, 0x80, 0x7F}}; - #define INFINITY (INFINITY_HACK.Value) - #else - #define INFINITY (1e1000) - #endif -#endif - -#include "cpVect.h" -#include "cpBB.h" -#include "cpBody.h" -#include "cpArray.h" -#include "cpHashSet.h" -#include "cpSpaceHash.h" - -#include "cpShape.h" -#include "cpPolyShape.h" - -#include "cpArbiter.h" -#include "cpCollision.h" - -#include "cpJoint.h" - -#include "cpSpace.h" - -#define CP_HASH_COEF (3344921057ul) -#define CP_HASH_PAIR(A, B) ((unsigned int)(A)*CP_HASH_COEF ^ (unsigned int)(B)*CP_HASH_COEF) - -void cpInitChipmunk(void); - -cpFloat cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset); -cpFloat cpMomentForPoly(cpFloat m, int numVerts, cpVect *verts, cpVect offset); - -#ifdef __cplusplus -} -#endif - -#endif diff --git a/chipmunk/cpArbiter.c b/chipmunk/cpArbiter.c deleted file mode 100644 --- a/chipmunk/cpArbiter.c +++ /dev/null @@ -1,263 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk.h" - -cpFloat cp_bias_coef = 0.1f; -cpFloat cp_collision_slop = 0.1f; - -cpContact* -cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int hash) -{ - con->p = p; - con->n = n; - con->dist = dist; - - con->jnAcc = 0.0f; - con->jtAcc = 0.0f; - con->jBias = 0.0f; - - con->hash = hash; - - return con; -} - -cpVect -cpContactsSumImpulses(cpContact *contacts, int numContacts) -{ - cpVect sum = cpvzero; - - for(int i=0; in, con->jnAcc); - sum = cpvadd(sum, j); - } - - return sum; -} - -cpVect -cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts) -{ - cpVect sum = cpvzero; - - for(int i=0; in); - cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc)); - sum = cpvadd(sum, j); - } - - return sum; -} - -cpArbiter* -cpArbiterAlloc(void) -{ - return (cpArbiter *)calloc(1, sizeof(cpArbiter)); -} - -cpArbiter* -cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp) -{ - arb->numContacts = 0; - arb->contacts = NULL; - - arb->a = a; - arb->b = b; - - arb->stamp = stamp; - - return arb; -} - -cpArbiter* -cpArbiterNew(cpShape *a, cpShape *b, int stamp) -{ - return cpArbiterInit(cpArbiterAlloc(), a, b, stamp); -} - -void -cpArbiterDestroy(cpArbiter *arb) -{ - free(arb->contacts); -} - -void -cpArbiterFree(cpArbiter *arb) -{ - if(arb) cpArbiterDestroy(arb); - free(arb); -} - -void -cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts) -{ - // Iterate over the possible pairs to look for hash value matches. - for(int i=0; inumContacts; i++){ - cpContact *old = &arb->contacts[i]; - - for(int j=0; jhash == old->hash){ - // Copy the persistant contact information. - new_contact->jnAcc = old->jnAcc; - new_contact->jtAcc = old->jtAcc; - } - } - } - - free(arb->contacts); - - arb->contacts = contacts; - arb->numContacts = numContacts; -} - -void -cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) -{ - cpShape *shapea = arb->a; - cpShape *shapeb = arb->b; - - cpFloat e = shapea->e * shapeb->e; - arb->u = shapea->u * shapeb->u; - arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); - - cpBody *a = shapea->body; - cpBody *b = shapeb->body; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - - // Calculate the offsets. - con->r1 = cpvsub(con->p, a->p); - con->r2 = cpvsub(con->p, b->p); - - // Calculate the mass normal. - cpFloat mass_sum = a->m_inv + b->m_inv; - - cpFloat r1cn = cpvcross(con->r1, con->n); - cpFloat r2cn = cpvcross(con->r2, con->n); - cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; - con->nMass = 1.0f/kn; - - // Calculate the mass tangent. - cpVect t = cpvperp(con->n); - cpFloat r1ct = cpvcross(con->r1, t); - cpFloat r2ct = cpvcross(con->r2, t); - cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct; - con->tMass = 1.0f/kt; - - // Calculate the target bias velocity. - con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop); - con->jBias = 0.0f; - - // Calculate the target bounce velocity. - cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w)); - cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w)); - con->bounce = cpvdot(con->n, cpvsub(v2, v1))*e; - } -} - -void -cpArbiterApplyCachedImpulse(cpArbiter *arb) -{ - cpShape *shapea = arb->a; - cpShape *shapeb = arb->b; - - arb->u = shapea->u * shapeb->u; - arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); - - cpBody *a = shapea->body; - cpBody *b = shapeb->body; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - - cpVect t = cpvperp(con->n); - cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc)); - cpBodyApplyImpulse(a, cpvneg(j), con->r1); - cpBodyApplyImpulse(b, j, con->r2); - } -} - -void -cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef) -{ - cpBody *a = arb->a->body; - cpBody *b = arb->b->body; - - for(int i=0; inumContacts; i++){ - cpContact *con = &arb->contacts[i]; - cpVect n = con->n; - cpVect r1 = con->r1; - cpVect r2 = con->r2; - - // Calculate the relative bias velocities. - cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); - cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); - cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); - - // Calculate and clamp the bias impulse. - cpFloat jbn = (con->bias - vbn)*con->nMass; - cpFloat jbnOld = con->jBias; - con->jBias = cpfmax(jbnOld + jbn, 0.0f); - jbn = con->jBias - jbnOld; - - // Apply the bias impulse. - cpVect jb = cpvmult(n, jbn); - cpBodyApplyBiasImpulse(a, cpvneg(jb), r1); - cpBodyApplyBiasImpulse(b, jb, r2); - - // Calculate the relative velocity. - cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); - cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); - cpVect vr = cpvsub(v2, v1); - cpFloat vrn = cpvdot(vr, n); - - // Calculate and clamp the normal impulse. - cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass; - cpFloat jnOld = con->jnAcc; - con->jnAcc = cpfmax(jnOld + jn, 0.0f); - jn = con->jnAcc - jnOld; - - // Calculate the relative tangent velocity. - cpVect t = cpvperp(n); - cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), t); - - // Calculate and clamp the friction impulse. - cpFloat jtMax = arb->u*con->jnAcc; - cpFloat jt = -vrt*con->tMass; - cpFloat jtOld = con->jtAcc; - con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); - jt = con->jtAcc - jtOld; - - // Apply the final impulse. - cpVect j = cpvadd(cpvmult(n, jn), cpvmult(t, jt)); - cpBodyApplyImpulse(a, cpvneg(j), r1); - cpBodyApplyImpulse(b, j, r2); - } -} diff --git a/chipmunk/cpArbiter.h b/chipmunk/cpArbiter.h deleted file mode 100644 --- a/chipmunk/cpArbiter.h +++ /dev/null @@ -1,85 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Determines how fast penetrations resolve themselves. -extern cpFloat cp_bias_coef; -// Amount of allowed penetration. Used to reduce vibrating contacts. -extern cpFloat cp_collision_slop; - -// Data structure for contact points. -typedef struct cpContact{ - // Contact point and normal. - cpVect p, n; - // Penetration distance. - cpFloat dist; - - // Calculated by cpArbiterPreStep(). - cpVect r1, r2; - cpFloat nMass, tMass, bounce; - - // Persistant contact information. - cpFloat jnAcc, jtAcc, jBias; - cpFloat bias; - - // Hash value used to (mostly) uniquely identify a contact. - unsigned int hash; -} cpContact; - -// Contacts are always allocated in groups. -cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int hash); - -// Sum the contact impulses. (Can be used after cpSpaceStep() returns) -cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts); -cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts); - -// Data structure for tracking collisions between shapes. -typedef struct cpArbiter{ - // Information on the contact points between the objects. - int numContacts; - cpContact *contacts; - - // The two shapes involved in the collision. - cpShape *a, *b; - - // Calculated by cpArbiterPreStep(). - cpFloat u; - cpVect target_v; - - // Time stamp of the arbiter. (from cpSpace) - int stamp; -} cpArbiter; - -// Basic allocation/destruction functions. -cpArbiter* cpArbiterAlloc(void); -cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp); -cpArbiter* cpArbiterNew(cpShape *a, cpShape *b, int stamp); - -void cpArbiterDestroy(cpArbiter *arb); -void cpArbiterFree(cpArbiter *arb); - -// These functions are all intended to be used internally. -// Inject new contact points into the arbiter while preserving contact history. -void cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts); -// Precalculate values used by the solver. -void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv); -void cpArbiterApplyCachedImpulse(cpArbiter *arb); -// Run an iteration of the solver on the arbiter. -void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef); diff --git a/chipmunk/cpArray.c b/chipmunk/cpArray.c deleted file mode 100644 --- a/chipmunk/cpArray.c +++ /dev/null @@ -1,114 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk.h" - - -//#define CP_ARRAY_INCREMENT 10 - -// NOTE: cpArray is rarely used and will probably go away. - -cpArray* -cpArrayAlloc(void) -{ - return (cpArray *)calloc(1, sizeof(cpArray)); -} - -cpArray* -cpArrayInit(cpArray *arr, int size) -{ - arr->num = 0; - - size = (size ? size : 4); - arr->max = size; - arr->arr = (void **)malloc(size*sizeof(void**)); - - return arr; -} - -cpArray* -cpArrayNew(int size) -{ - return cpArrayInit(cpArrayAlloc(), size); -} - -void -cpArrayDestroy(cpArray *arr) -{ - free(arr->arr); -} - -void -cpArrayFree(cpArray *arr) -{ - if(!arr) return; - cpArrayDestroy(arr); - free(arr); -} - -void -cpArrayPush(cpArray *arr, void *object) -{ - if(arr->num == arr->max){ - arr->max *= 2; - arr->arr = (void **)realloc(arr->arr, arr->max*sizeof(void**)); - } - - arr->arr[arr->num] = object; - arr->num++; -} - -void -cpArrayDeleteIndex(cpArray *arr, int index) -{ - int last = --arr->num; - arr->arr[index] = arr->arr[last]; -} - -void -cpArrayDeleteObj(cpArray *arr, void *obj) -{ - for(int i=0; inum; i++){ - if(arr->arr[i] == obj){ - cpArrayDeleteIndex(arr, i); - return; - } - } -} - -void -cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) -{ - for(int i=0; inum; i++) - iterFunc(arr->arr[i], data); -} - -int -cpArrayContains(cpArray *arr, void *ptr) -{ - for(int i=0; inum; i++) - if(arr->arr[i] == ptr) return 1; - - return 0; -} diff --git a/chipmunk/cpArray.h b/chipmunk/cpArray.h deleted file mode 100644 --- a/chipmunk/cpArray.h +++ /dev/null @@ -1,45 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// NOTE: cpArray is rarely used and will probably go away. - -typedef struct cpArray{ - int num, max; - void **arr; -} cpArray; - -typedef void (*cpArrayIter)(void *ptr, void *data); - -cpArray *cpArrayAlloc(void); -cpArray *cpArrayInit(cpArray *arr, int size); -cpArray *cpArrayNew(int size); - -void cpArrayDestroy(cpArray *arr); -void cpArrayFree(cpArray *arr); - -void cpArrayClear(cpArray *arr); - -void cpArrayPush(cpArray *arr, void *object); -void cpArrayDeleteIndex(cpArray *arr, int index); -void cpArrayDeleteObj(cpArray *arr, void *obj); - -void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data); -int cpArrayContains(cpArray *arr, void *ptr); diff --git a/chipmunk/cpBB.c b/chipmunk/cpBB.c deleted file mode 100644 --- a/chipmunk/cpBB.c +++ /dev/null @@ -1,46 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include - -#include "chipmunk.h" - -cpVect -cpBBClampVect(const cpBB bb, const cpVect v) -{ - cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r); - cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t); - return cpv(x, y); -} - -cpVect -cpBBWrapVect(const cpBB bb, const cpVect v) -{ - cpFloat ix = fabsf(bb.r - bb.l); - cpFloat modx = fmodf(v.x - bb.l, ix); - cpFloat x = (modx > 0.0f) ? modx : modx + ix; - - cpFloat iy = fabsf(bb.t - bb.b); - cpFloat mody = fmodf(v.y - bb.b, iy); - cpFloat y = (mody > 0.0f) ? mody : mody + iy; - - return cpv(x + bb.l, y + bb.b); -} diff --git a/chipmunk/cpBB.h b/chipmunk/cpBB.h deleted file mode 100644 --- a/chipmunk/cpBB.h +++ /dev/null @@ -1,53 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -typedef struct cpBB{ - cpFloat l, b, r ,t; -} cpBB; - -static inline cpBB -cpBBNew(const cpFloat l, const cpFloat b, - const cpFloat r, const cpFloat t) -{ - cpBB bb = {l, b, r, t}; - return bb; -} - -static inline int -cpBBintersects(const cpBB a, const cpBB b) -{ - return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t); -} - -static inline int -cpBBcontainsBB(const cpBB bb, const cpBB other) -{ - return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t); -} - -static inline int -cpBBcontainsVect(const cpBB bb, const cpVect v) -{ - return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y); -} - -cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox -cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox diff --git a/chipmunk/cpBody.c b/chipmunk/cpBody.c deleted file mode 100644 --- a/chipmunk/cpBody.c +++ /dev/null @@ -1,180 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include - -#include "chipmunk.h" - -cpBody* -cpBodyAlloc(void) -{ - return (cpBody *)malloc(sizeof(cpBody)); -} - -cpBody* -cpBodyInit(cpBody *body, cpFloat m, cpFloat i) -{ - body->velocity_func = cpBodyUpdateVelocity; - body->position_func = cpBodyUpdatePosition; - - cpBodySetMass(body, m); - cpBodySetMoment(body, i); - - body->p = cpvzero; - body->v = cpvzero; - body->f = cpvzero; - - cpBodySetAngle(body, 0.0f); - body->w = 0.0f; - body->t = 0.0f; - - body->v_bias = cpvzero; - body->w_bias = 0.0f; - - body->data = NULL; -// body->active = 1; - - return body; -} - -cpBody* -cpBodyNew(cpFloat m, cpFloat i) -{ - return cpBodyInit(cpBodyAlloc(), m, i); -} - -void cpBodyDestroy(cpBody *body){} - -void -cpBodyFree(cpBody *body) -{ - if(body) cpBodyDestroy(body); - free(body); -} - -void -cpBodySetMass(cpBody *body, cpFloat m) -{ - body->m = m; - body->m_inv = 1.0f/m; -} - -void -cpBodySetMoment(cpBody *body, cpFloat i) -{ - body->i = i; - body->i_inv = 1.0f/i; -} - -void -cpBodySetAngle(cpBody *body, cpFloat a) -{ - body->a = fmod(a, (cpFloat)M_PI*2.0f); - body->rot = cpvforangle(a); -} - -void -cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) -{ - cpVect delta = cpvsub(pos, body->p); - body->v = cpvmult(delta, 1.0/dt); -} - -void -cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) -{ - body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)); - body->w = body->w*damping + body->t*body->i_inv*dt; -} - -void -cpBodyUpdatePosition(cpBody *body, cpFloat dt) -{ - body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); - cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt); - - body->v_bias = cpvzero; - body->w_bias = 0.0f; -} - -void -cpBodyResetForces(cpBody *body) -{ - body->f = cpvzero; - body->t = 0.0f; -} - -void -cpBodyApplyForce(cpBody *body, cpVect f, cpVect r) -{ - body->f = cpvadd(body->f, f); - body->t += cpvcross(r, f); -} - -void -cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) -{ - // Calculate the world space anchor coordinates. - cpVect r1 = cpvrotate(anchr1, a->rot); - cpVect r2 = cpvrotate(anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1)); - cpFloat dist = cpvlength(delta); - cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero; - - cpFloat f_spring = (dist - rlen)*k; - - // Calculate the world relative velocities of the anchor points. - cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); - cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); - - // Calculate the damping force. - // This really should be in the impulse solver and can produce problems when using large damping values. - cpFloat vrn = cpvdot(cpvsub(v2, v1), n); - cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv))); - - // Apply! - cpVect f = cpvmult(n, f_spring + f_damp); - cpBodyApplyForce(a, f, r1); - cpBodyApplyForce(b, cpvneg(f), r2); -} - -//int -//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max) -//{ -// cpFloat ke = body->m*cpvdot(body->v, body->v); -// cpFloat re = body->i*body->w*body->w; -// -// if(ke + re > body->m*dvsq) -// body->active = 1; -// else if(body->active) -// body->active = (body->active + 1)%(max + 1); -// else { -// body->v = cpvzero; -// body->v_bias = cpvzero; -// body->w = 0.0f; -// body->w_bias = 0.0f; -// } -// -// return body->active; -//} diff --git a/chipmunk/cpBody.h b/chipmunk/cpBody.h deleted file mode 100644 --- a/chipmunk/cpBody.h +++ /dev/null @@ -1,132 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -struct cpBody; -typedef void (*cpBodyVelocityFunc)(struct cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -typedef void (*cpBodyPositionFunc)(struct cpBody *body, cpFloat dt); - - -typedef struct cpBody{ - // *** Integration Functions. - - // Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity) - cpBodyVelocityFunc velocity_func; - - // Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition) - cpBodyPositionFunc position_func; - - // *** Mass Properties - - // Mass and it's inverse. - // Always use cpBodySetMass() whenever changing the mass as these values must agree. - cpFloat m, m_inv; - - // Moment of inertia and it's inverse. - // Always use cpBodySetMass() whenever changing the mass as these values must agree. - cpFloat i, i_inv; - - // *** Positional Properties - - // Linear components of motion (position, velocity, and force) - cpVect p, v, f; - - // Angular components of motion (angle, angular velocity, and torque) - // Always use cpBodySetAngle() to set the angle of the body as a and rot must agree. - cpFloat a, w, t; - - // Cached unit length vector representing the angle of the body. - // Used for fast vector rotation using cpvrotate(). - cpVect rot; - - // *** User Definable Fields - - // User defined data pointer. - void *data; - - // *** Internally Used Fields - - // Velocity bias values used when solving penetrations and correcting joints. - cpVect v_bias; - cpFloat w_bias; - -// int active; -} cpBody; - -// Basic allocation/destruction functions -cpBody *cpBodyAlloc(void); -cpBody *cpBodyInit(cpBody *body, cpFloat m, cpFloat i); -cpBody *cpBodyNew(cpFloat m, cpFloat i); - -void cpBodyDestroy(cpBody *body); -void cpBodyFree(cpBody *body); - -// Setters for some of the special properties (mandatory!) -void cpBodySetMass(cpBody *body, cpFloat m); -void cpBodySetMoment(cpBody *body, cpFloat i); -void cpBodySetAngle(cpBody *body, cpFloat a); - -// Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep. -// Intended for objects that are moved manually with a custom velocity integration function. -void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt); - -// Default Integration functions. -void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt); -void cpBodyUpdatePosition(cpBody *body, cpFloat dt); - -// Convert body local to world coordinates -static inline cpVect -cpBodyLocal2World(cpBody *body, cpVect v) -{ - return cpvadd(body->p, cpvrotate(v, body->rot)); -} - -// Convert world to body local coordinates -static inline cpVect -cpBodyWorld2Local(cpBody *body, cpVect v) -{ - return cpvunrotate(cpvsub(v, body->p), body->rot); -} - -// Apply an impulse (in world coordinates) to the body. -static inline void -cpBodyApplyImpulse(cpBody *body, cpVect j, cpVect r) -{ - body->v = cpvadd(body->v, cpvmult(j, body->m_inv)); - body->w += body->i_inv*cpvcross(r, j); -} - -// Not intended for external use. Used by cpArbiter.c and cpJoint.c. -static inline void -cpBodyApplyBiasImpulse(cpBody *body, cpVect j, cpVect r) -{ - body->v_bias = cpvadd(body->v_bias, cpvmult(j, body->m_inv)); - body->w_bias += body->i_inv*cpvcross(r, j); -} - -// Zero the forces on a body. -void cpBodyResetForces(cpBody *body); -// Apply a force (in world coordinates) to a body. -void cpBodyApplyForce(cpBody *body, cpVect f, cpVect r); - -// Apply a damped spring force between two bodies. -void cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt); - -//int cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max); diff --git a/chipmunk/cpCollision.c b/chipmunk/cpCollision.c deleted file mode 100644 --- a/chipmunk/cpCollision.c +++ /dev/null @@ -1,390 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include -#include - -#include "chipmunk.h" - -typedef int (*collisionFunc)(cpShape*, cpShape*, cpContact**); - -static collisionFunc *colfuncs = NULL; - -// Add contact points for circle to circle collisions. -// Used by several collision tests. -static int -circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact **con) -{ - cpFloat mindist = r1 + r2; - cpVect delta = cpvsub(p2, p1); - cpFloat distsq = cpvlengthsq(delta); - if(distsq >= mindist*mindist) return 0; - - cpFloat dist = sqrtf(distsq); - // To avoid singularities, do nothing in the case of dist = 0. - cpFloat non_zero_dist = (dist ? dist : INFINITY); - - // Allocate and initialize the contact. - (*con) = (cpContact *)malloc(sizeof(cpContact)); - cpContactInit( - (*con), - cpvadd(p1, cpvmult(delta, 0.5 + (r1 - 0.5*mindist)/non_zero_dist)), - cpvmult(delta, 1.0/non_zero_dist), - dist - mindist, - 0 - ); - - return 1; -} - -// Collide circle shapes. -static int -circle2circle(cpShape *shape1, cpShape *shape2, cpContact **arr) -{ - cpCircleShape *circ1 = (cpCircleShape *)shape1; - cpCircleShape *circ2 = (cpCircleShape *)shape2; - - return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); -} - -// Collide circles to segment shapes. -static int -circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con) -{ - cpCircleShape *circ = (cpCircleShape *)circleShape; - cpSegmentShape *seg = (cpSegmentShape *)segmentShape; - - // Radius sum - cpFloat rsum = circ->r + seg->r; - - // Calculate normal distance from segment. - cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn); - cpFloat dist = fabs(dn) - rsum; - if(dist > 0.0f) return 0; - - // Calculate tangential distance along segment. - cpFloat dt = -cpvcross(seg->tn, circ->tc); - cpFloat dtMin = -cpvcross(seg->tn, seg->ta); - cpFloat dtMax = -cpvcross(seg->tn, seg->tb); - - // Decision tree to decide which feature of the segment to collide with. - if(dt < dtMin){ - if(dt < (dtMin - rsum)){ - return 0; - } else { - return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con); - } - } else { - if(dt < dtMax){ - cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn); - (*con) = (cpContact *)malloc(sizeof(cpContact)); - cpContactInit( - (*con), - cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)), - n, - dist, - 0 - ); - return 1; - } else { - if(dt < (dtMax + rsum)) { - return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con); - } else { - return 0; - } - } - } - - return 1; -} - -// Helper function for allocating contact point lists. -static cpContact * -addContactPoint(cpContact **arr, int *max, int *num) -{ - if(*arr == NULL){ - // Allocate the array if it hasn't been done. - (*max) = 2; - (*num) = 0; - (*arr) = (cpContact *)malloc((*max)*sizeof(cpContact)); - } else if(*num == *max){ - // Extend it if necessary. - (*max) *= 2; - (*arr) = (cpContact *)realloc(*arr, (*max)*sizeof(cpContact)); - } - - cpContact *con = &(*arr)[*num]; - (*num)++; - - return con; -} - -// Find the minimum separating axis for the give poly and axis list. -static inline int -findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out) -{ - int min_index = 0; - cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d); - if(min > 0.0) return -1; - - for(int i=1; i 0.0) { - return -1; - } else if(dist > min){ - min = dist; - min_index = i; - } - } - - (*min_out) = min; - return min_index; -} - -// Add contacts for penetrating vertexes. -static inline int -findVerts(cpContact **arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) -{ - int max = 0; - int num = 0; - - for(int i=0; inumVerts; i++){ - cpVect v = poly1->tVerts[i]; - if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n))) - cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i)); - } - - for(int i=0; inumVerts; i++){ - cpVect v = poly2->tVerts[i]; - if(cpPolyShapeContainsVertPartial(poly1, v, n)) - cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly2, i)); - } - - // if(!num) - // addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0)); - - return num; -} - -// Collide poly shapes together. -static int -poly2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) -{ - cpPolyShape *poly1 = (cpPolyShape *)shape1; - cpPolyShape *poly2 = (cpPolyShape *)shape2; - - cpFloat min1; - int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1); - if(mini1 == -1) return 0; - - cpFloat min2; - int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2); - if(mini2 == -1) return 0; - - // There is overlap, find the penetrating verts - if(min1 > min2) - return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1); - else - return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2); -} - -// Like cpPolyValueOnAxis(), but for segments. -static inline float -segValueOnAxis(cpSegmentShape *seg, cpVect n, cpFloat d) -{ - cpFloat a = cpvdot(n, seg->ta) - seg->r; - cpFloat b = cpvdot(n, seg->tb) - seg->r; - return cpfmin(a, b) - d; -} - -// Identify vertexes that have penetrated the segment. -static inline void -findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef) -{ - cpFloat dta = cpvcross(seg->tn, seg->ta); - cpFloat dtb = cpvcross(seg->tn, seg->tb); - cpVect n = cpvmult(seg->tn, coef); - - for(int i=0; inumVerts; i++){ - cpVect v = poly->tVerts[i]; - if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){ - cpFloat dt = cpvcross(seg->tn, v); - if(dta >= dt && dt >= dtb){ - cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly, i)); - } - } - } -} - -// This one is complicated and gross. Just don't go there... -// TODO: Comment me! -static int -seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) -{ - cpSegmentShape *seg = (cpSegmentShape *)shape1; - cpPolyShape *poly = (cpPolyShape *)shape2; - cpPolyShapeAxis *axes = poly->tAxes; - - cpFloat segD = cpvdot(seg->tn, seg->ta); - cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; - cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; - if(minNeg > 0.0f || minNorm > 0.0f) return 0; - - int mini = 0; - cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); - if(poly_min > 0.0f) return 0; - for(int i=0; inumVerts; i++){ - cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); - if(dist > 0.0f){ - return 0; - } else if(dist > poly_min){ - poly_min = dist; - mini = i; - } - } - - int max = 0; - int num = 0; - - cpVect poly_n = cpvneg(axes[mini].n); - - cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); - cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); - if(cpPolyShapeContainsVert(poly, va)) - cpContactInit(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0)); - if(cpPolyShapeContainsVert(poly, vb)) - cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 1)); - - // Floating point precision problems here. - // This will have to do for now. - poly_min -= cp_collision_slop; - if(minNorm >= poly_min || minNeg >= poly_min) { - if(minNorm > minNeg) - findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f); - else - findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f); - } - - // If no other collision points are found, try colliding endpoints. - if(num == 0){ - cpVect poly_a = poly->tVerts[mini]; - cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts]; - - if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) - return 1; - - if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) - return 1; - } - - return num; -} - -// This one is less gross, but still gross. -// TODO: Comment me! -static int -circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con) -{ - cpCircleShape *circ = (cpCircleShape *)shape1; - cpPolyShape *poly = (cpPolyShape *)shape2; - cpPolyShapeAxis *axes = poly->tAxes; - - int mini = 0; - cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r; - for(int i=0; inumVerts; i++){ - cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r; - if(dist > 0.0){ - return 0; - } else if(dist > min) { - min = dist; - mini = i; - } - } - - cpVect n = axes[mini].n; - cpVect a = poly->tVerts[mini]; - cpVect b = poly->tVerts[(mini + 1)%poly->numVerts]; - cpFloat dta = cpvcross(n, a); - cpFloat dtb = cpvcross(n, b); - cpFloat dt = cpvcross(n, circ->tc); - - if(dt < dtb){ - return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con); - } else if(dt < dta) { - (*con) = (cpContact *)malloc(sizeof(cpContact)); - cpContactInit( - (*con), - cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)), - cpvneg(n), - min, - 0 - ); - - return 1; - } else { - return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con); - } -} - -static void -addColFunc(cpShapeType a, cpShapeType b, collisionFunc func) -{ - colfuncs[a + b*CP_NUM_SHAPES] = func; -} - -#ifdef __cplusplus -extern "C" { -#endif - // Initializes the array of collision functions. - // Called by cpInitChipmunk(). - void - cpInitCollisionFuncs(void) - { - if(!colfuncs) - colfuncs = (collisionFunc *)calloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc)); - - addColFunc(CP_CIRCLE_SHAPE, CP_CIRCLE_SHAPE, circle2circle); - addColFunc(CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, circle2segment); - addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE, seg2poly); - addColFunc(CP_CIRCLE_SHAPE, CP_POLY_SHAPE, circle2poly); - addColFunc(CP_POLY_SHAPE, CP_POLY_SHAPE, poly2poly); - } -#ifdef __cplusplus -} -#endif - -int -cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr) -{ - // Their shape types must be in order. - assert(a->klass->type <= b->klass->type); - - collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES]; - return (cfunc) ? cfunc(a, b, arr) : 0; -} diff --git a/chipmunk/cpCollision.h b/chipmunk/cpCollision.h deleted file mode 100644 --- a/chipmunk/cpCollision.h +++ /dev/null @@ -1,23 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Collides two cpShape structures. (this function is lonely :( ) -int cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr); diff --git a/chipmunk/cpHashSet.c b/chipmunk/cpHashSet.c deleted file mode 100644 --- a/chipmunk/cpHashSet.c +++ /dev/null @@ -1,219 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include - -#include "chipmunk.h" -#include "prime.h" - -void -cpHashSetDestroy(cpHashSet *set) -{ - // Free the chains. - for(int i=0; isize; i++){ - // Free the bins in the chain. - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - free(bin); - bin = next; - } - } - - // Free the table. - free(set->table); -} - -void -cpHashSetFree(cpHashSet *set) -{ - if(set) cpHashSetDestroy(set); - free(set); -} - -cpHashSet * -cpHashSetAlloc(void) -{ - return (cpHashSet *)calloc(1, sizeof(cpHashSet)); -} - -cpHashSet * -cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) -{ - set->size = next_prime(size); - set->entries = 0; - - set->eql = eqlFunc; - set->trans = trans; - - set->default_value = NULL; - - set->table = (cpHashSetBin **)calloc(set->size, sizeof(cpHashSetBin *)); - - return set; -} - -cpHashSet * -cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) -{ - return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, trans); -} - -static int -setIsFull(cpHashSet *set) -{ - return (set->entries >= set->size); -} - -static void -cpHashSetResize(cpHashSet *set) -{ - // Get the next approximate doubled prime. - int newSize = next_prime(set->size + 1); - // Allocate a new table. - cpHashSetBin **newTable = (cpHashSetBin **)calloc(newSize, sizeof(cpHashSetBin *)); - - // Iterate over the chains. - for(int i=0; isize; i++){ - // Rehash the bins into the new table. - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - int index = bin->hash%newSize; - bin->next = newTable[index]; - newTable[index] = bin; - - bin = next; - } - } - - free(set->table); - - set->table = newTable; - set->size = newSize; -} - -void * -cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data) -{ - int index = hash%set->size; - - // Find the bin with the matching element. - cpHashSetBin *bin = set->table[index]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - // Create it necessary. - if(!bin){ - bin = (cpHashSetBin *)malloc(sizeof(cpHashSetBin)); - bin->hash = hash; - bin->elt = set->trans(ptr, data); // Transform the pointer. - - bin->next = set->table[index]; - set->table[index] = bin; - - set->entries++; - - // Resize the set if it's full. - if(setIsFull(set)) - cpHashSetResize(set); - } - - return bin->elt; -} - -void * -cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr) -{ - int index = hash%set->size; - - // Pointer to the previous bin pointer. - cpHashSetBin **prev_ptr = &set->table[index]; - // Pointer the the current bin. - cpHashSetBin *bin = set->table[index]; - - // Find the bin - while(bin && !set->eql(ptr, bin->elt)){ - prev_ptr = &bin->next; - bin = bin->next; - } - - // Remove it if it exists. - if(bin){ - // Update the previos bin pointer to point to the next bin. - (*prev_ptr) = bin->next; - set->entries--; - - void *return_value = bin->elt; - free(bin); - return return_value; - } - - return NULL; -} - -void * -cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr) -{ - int index = hash%set->size; - cpHashSetBin *bin = set->table[index]; - while(bin && !set->eql(ptr, bin->elt)) - bin = bin->next; - - return (bin ? bin->elt : set->default_value); -} - -void -cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data) -{ - for(int i=0; isize; i++){ - cpHashSetBin *bin; - for(bin = set->table[i]; bin; bin = bin->next) - func(bin->elt, data); - } -} - -void -cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data) -{ - // Iterate over all the chains. - for(int i=0; isize; i++){ - // The rest works similarly to cpHashSetRemove() above. - cpHashSetBin **prev_ptr = &set->table[i]; - cpHashSetBin *bin = set->table[i]; - while(bin){ - cpHashSetBin *next = bin->next; - - if(func(bin->elt, data)){ - prev_ptr = &bin->next; - } else { - (*prev_ptr) = next; - - set->entries--; - free(bin); - } - - bin = next; - } - } -} diff --git a/chipmunk/cpHashSet.h b/chipmunk/cpHashSet.h deleted file mode 100644 --- a/chipmunk/cpHashSet.h +++ /dev/null @@ -1,79 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// cpHashSet uses a chained hashtable implementation. -// Other than the transformation functions, there is nothing fancy going on. - -// cpHashSetBin's form the linked lists in the chained hash table. -typedef struct cpHashSetBin { - // Pointer to the element. - void *elt; - // Hash value of the element. - unsigned int hash; - // Next element in the chain. - struct cpHashSetBin *next; -} cpHashSetBin; - -// Equality function. Returns true if ptr is equal to elt. -typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt); -// Used by cpHashSetInsert(). Called to transform the ptr into an element. -typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); -// Iterator function for a hashset. -typedef void (*cpHashSetIterFunc)(void *elt, void *data); -// Reject function. Returns true if elt should be dropped. -typedef int (*cpHashSetRejectFunc)(void *elt, void *data); - -typedef struct cpHashSet { - // Number of elements stored in the table. - int entries; - // Number of cells in the table. - int size; - - cpHashSetEqlFunc eql; - cpHashSetTransFunc trans; - - // Default value returned by cpHashSetFind() when no element is found. - // Defaults to NULL. - void *default_value; - - cpHashSetBin **table; -} cpHashSet; - -// Basic allocation/destruction functions. -void cpHashSetDestroy(cpHashSet *set); -void cpHashSetFree(cpHashSet *set); - -cpHashSet *cpHashSetAlloc(void); -cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); -cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); - -// Insert an element into the set, returns the element. -// If it doesn't already exist, the transformation function is applied. -void *cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data); -// Remove and return an element from the set. -void *cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr); -// Find an element in the set. Returns the default value if the element isn't found. -void *cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr); - -// Iterate over a hashset. -void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); -// Iterate over a hashset while rejecting certain elements. -void cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data); diff --git a/chipmunk/cpJoint.c b/chipmunk/cpJoint.c deleted file mode 100644 --- a/chipmunk/cpJoint.c +++ /dev/null @@ -1,553 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in -* all copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. -*/ - -#include -#include - -#include "chipmunk.h" - -// TODO: Comment me! - -cpFloat cp_joint_bias_coef = 0.1f; - -void cpJointDestroy(cpJoint *joint){} - -void -cpJointFree(cpJoint *joint) -{ - if(joint) cpJointDestroy(joint); - free(joint); -} - -static void -cpJointInit(cpJoint *joint, const cpJointClass *klass, cpBody *a, cpBody *b) -{ - joint->klass = klass; - joint->a = a; - joint->b = b; -} - - -static inline cpVect -relative_velocity(cpVect r1, cpVect v1, cpFloat w1, cpVect r2, cpVect v2, cpFloat w2){ - cpVect v1_sum = cpvadd(v1, cpvmult(cpvperp(r1), w1)); - cpVect v2_sum = cpvadd(v2, cpvmult(cpvperp(r2), w2)); - - return cpvsub(v2_sum, v1_sum); -} - -static inline cpFloat -scalar_k(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) -{ - cpFloat mass_sum = a->m_inv + b->m_inv; - cpFloat r1cn = cpvcross(r1, n); - cpFloat r2cn = cpvcross(r2, n); - - return mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; -} - -static inline void -apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - cpBodyApplyImpulse(a, cpvneg(j), r1); - cpBodyApplyImpulse(b, j, r2); -} - -static inline void -apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) -{ - cpBodyApplyBiasImpulse(a, cpvneg(j), r1); - cpBodyApplyBiasImpulse(b, j, r2); -} - - -static void -pinJointPreStep(cpJoint *joint, cpFloat dt_inv) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - cpPinJoint *jnt = (cpPinJoint *)joint; - - jnt->r1 = cpvrotate(jnt->anchr1, a->rot); - jnt->r2 = cpvrotate(jnt->anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); - cpFloat dist = cpvlength(delta); - jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); - - // calculate mass normal - jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n); - - // calculate bias velocity - jnt->bias = -cp_joint_bias_coef*dt_inv*(dist - jnt->dist); - jnt->jBias = 0.0f; - - // apply accumulated impulse - cpVect j = cpvmult(jnt->n, jnt->jnAcc); - apply_impulses(a, b, jnt->r1, jnt->r2, j); -} - -static void -pinJointApplyImpulse(cpJoint *joint) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - - cpPinJoint *jnt = (cpPinJoint *)joint; - cpVect n = jnt->n; - cpVect r1 = jnt->r1; - cpVect r2 = jnt->r2; - - //calculate bias impulse - cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); - cpFloat vbn = cpvdot(vbr, n); - - cpFloat jbn = (jnt->bias - vbn)*jnt->nMass; - jnt->jBias += jbn; - - cpVect jb = cpvmult(n, jbn); - apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); - - // compute relative velocity - cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); - cpFloat vrn = cpvdot(vr, n); - - // compute normal impulse - cpFloat jn = -vrn*jnt->nMass; - jnt->jnAcc =+ jn; - - // apply impulse - cpVect j = cpvmult(n, jn); - apply_impulses(a, b, jnt->r1, jnt->r2, j); -} - -static const cpJointClass pinJointClass = { - CP_PIN_JOINT, - pinJointPreStep, - pinJointApplyImpulse, -}; - -cpPinJoint * -cpPinJointAlloc(void) -{ - return (cpPinJoint *)malloc(sizeof(cpPinJoint)); -} - -cpPinJoint * -cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - cpJointInit((cpJoint *)joint, &pinJointClass, a, b); - - joint->anchr1 = anchr1; - joint->anchr2 = anchr2; - - cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot)); - cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot)); - joint->dist = cpvlength(cpvsub(p2, p1)); - - joint->jnAcc = 0.0; - - return joint; -} - -cpJoint * -cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) -{ - return (cpJoint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); -} - - - - -static void -slideJointPreStep(cpJoint *joint, cpFloat dt_inv) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - cpSlideJoint *jnt = (cpSlideJoint *)joint; - - jnt->r1 = cpvrotate(jnt->anchr1, a->rot); - jnt->r2 = cpvrotate(jnt->anchr2, b->rot); - - cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); - cpFloat dist = cpvlength(delta); - cpFloat pdist = 0.0; - if(dist > jnt->max) { - pdist = dist - jnt->max; - } else if(dist < jnt->min) { - pdist = jnt->min - dist; - dist = -dist; - } - jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); - - // calculate mass normal - jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n); - - // calculate bias velocity - jnt->bias = -cp_joint_bias_coef*dt_inv*(pdist); - jnt->jBias = 0.0f; - - // apply accumulated impulse - if(!jnt->bias) //{ - // if bias is 0, then the joint is not at a limit. - jnt->jnAcc = 0.0f; -// } else { - cpVect j = cpvmult(jnt->n, jnt->jnAcc); - apply_impulses(a, b, jnt->r1, jnt->r2, j); -// } -} - -static void -slideJointApplyImpulse(cpJoint *joint) -{ - cpSlideJoint *jnt = (cpSlideJoint *)joint; - if(!jnt->bias) return; // early exit - - cpBody *a = joint->a; - cpBody *b = joint->b; - - cpVect n = jnt->n; - cpVect r1 = jnt->r1; - cpVect r2 = jnt->r2; - - //calculate bias impulse - cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); - cpFloat vbn = cpvdot(vbr, n); - - cpFloat jbn = (jnt->bias - vbn)*jnt->nMass; - cpFloat jbnOld = jnt->jBias; - jnt->jBias = cpfmin(jbnOld + jbn, 0.0f); - jbn = jnt->jBias - jbnOld; - - cpVect jb = cpvmult(n, jbn); - apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); - - // compute relative velocity - cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); - cpFloat vrn = cpvdot(vr, n); - - // compute normal impulse - cpFloat jn = -vrn*jnt->nMass; - cpFloat jnOld = jnt->jnAcc; - jnt->jnAcc = cpfmin(jnOld + jn, 0.0f); - jn = jnt->jnAcc - jnOld; - - // apply impulse - cpVect j = cpvmult(n, jn); - apply_impulses(a, b, jnt->r1, jnt->r2, j); -} - -static const cpJointClass slideJointClass = { - CP_SLIDE_JOINT, - slideJointPreStep, - slideJointApplyImpulse, -}; - -cpSlideJoint * -cpSlideJointAlloc(void) -{ - return (cpSlideJoint *)malloc(sizeof(cpSlideJoint)); -} - -cpSlideJoint * -cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) -{ - cpJointInit((cpJoint *)joint, &slideJointClass, a, b); - - joint->anchr1 = anchr1; - joint->anchr2 = anchr2; - joint->min = min; - joint->max = max; - - joint->jnAcc = 0.0; - - return joint; -} - -cpJoint * -cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) -{ - return (cpJoint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); -} - - - - -static void -pivotJointPreStep(cpJoint *joint, cpFloat dt_inv) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - cpPivotJoint *jnt = (cpPivotJoint *)joint; - - jnt->r1 = cpvrotate(jnt->anchr1, a->rot); - jnt->r2 = cpvrotate(jnt->anchr2, b->rot); - - // calculate mass matrix - // If I wasn't lazy, this wouldn't be so gross... - cpFloat k11, k12, k21, k22; - - cpFloat m_sum = a->m_inv + b->m_inv; - k11 = m_sum; k12 = 0.0f; - k21 = 0.0f; k22 = m_sum; - - cpFloat r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv; - cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv; - cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv; - k11 += r1ysq; k12 += r1nxy; - k21 += r1nxy; k22 += r1xsq; - - cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv; - cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv; - cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv; - k11 += r2ysq; k12 += r2nxy; - k21 += r2nxy; k22 += r2xsq; - - cpFloat det_inv = 1.0f/(k11*k22 - k12*k21); - jnt->k1 = cpv( k22*det_inv, -k12*det_inv); - jnt->k2 = cpv(-k21*det_inv, k11*det_inv); - - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); - jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv); - jnt->jBias = cpvzero; - - // apply accumulated impulse - apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc); -} - -static void -pivotJointApplyImpulse(cpJoint *joint) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - - cpPivotJoint *jnt = (cpPivotJoint *)joint; - cpVect r1 = jnt->r1; - cpVect r2 = jnt->r2; - cpVect k1 = jnt->k1; - cpVect k2 = jnt->k2; - - //calculate bias impulse - cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); - vbr = cpvsub(jnt->bias, vbr); - - cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2)); - jnt->jBias = cpvadd(jnt->jBias, jb); - - apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); - - // compute relative velocity - cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); - - // compute normal impulse - cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2)); - jnt->jAcc = cpvadd(jnt->jAcc, j); - - // apply impulse - apply_impulses(a, b, jnt->r1, jnt->r2, j); -} - -static const cpJointClass pivotJointClass = { - CP_PIVOT_JOINT, - pivotJointPreStep, - pivotJointApplyImpulse, -}; - -cpPivotJoint * -cpPivotJointAlloc(void) -{ - return (cpPivotJoint *)malloc(sizeof(cpPivotJoint)); -} - -cpPivotJoint * -cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot) -{ - cpJointInit((cpJoint *)joint, &pivotJointClass, a, b); - - joint->anchr1 = cpvunrotate(cpvsub(pivot, a->p), a->rot); - joint->anchr2 = cpvunrotate(cpvsub(pivot, b->p), b->rot); - - joint->jAcc = cpvzero; - - return joint; -} - -cpJoint * -cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) -{ - return (cpJoint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, pivot); -} - - - - -static void -grooveJointPreStep(cpJoint *joint, cpFloat dt_inv) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - cpGrooveJoint *jnt = (cpGrooveJoint *)joint; - - // calculate endpoints in worldspace - cpVect ta = cpBodyLocal2World(a, jnt->grv_a); - cpVect tb = cpBodyLocal2World(a, jnt->grv_b); - - // calculate axis - cpVect n = cpvrotate(jnt->grv_n, a->rot); - cpFloat d = cpvdot(ta, n); - - jnt->grv_tn = n; - jnt->r2 = cpvrotate(jnt->anchr2, b->rot); - - // calculate tangential distance along the axis of r2 - cpFloat td = cpvcross(cpvadd(b->p, jnt->r2), n); - // calculate clamping factor and r2 - if(td <= cpvcross(ta, n)){ - jnt->clamp = 1.0f; - jnt->r1 = cpvsub(ta, a->p); - } else if(td >= cpvcross(tb, n)){ - jnt->clamp = -1.0f; - jnt->r1 = cpvsub(tb, a->p); - } else { - jnt->clamp = 0.0f; - jnt->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); - } - - // calculate mass matrix - // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... - cpFloat k11, k12, k21, k22; - cpFloat m_sum = a->m_inv + b->m_inv; - - // start with I*m_sum - k11 = m_sum; k12 = 0.0f; - k21 = 0.0f; k22 = m_sum; - - // add the influence from r1 - cpFloat r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv; - cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv; - cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv; - k11 += r1ysq; k12 += r1nxy; - k21 += r1nxy; k22 += r1xsq; - - // add the influnce from r2 - cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv; - cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv; - cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv; - k11 += r2ysq; k12 += r2nxy; - k21 += r2nxy; k22 += r2xsq; - - // invert - cpFloat det_inv = 1.0f/(k11*k22 - k12*k21); - jnt->k1 = cpv( k22*det_inv, -k12*det_inv); - jnt->k2 = cpv(-k21*det_inv, k11*det_inv); - - - // calculate bias velocity - cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); - jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv); - jnt->jBias = cpvzero; - - // apply accumulated impulse - apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc); -} - -static inline cpVect -grooveConstrain(cpGrooveJoint *jnt, cpVect j){ - cpVect n = jnt->grv_tn; - cpVect jn = cpvmult(n, cpvdot(j, n)); - - cpVect t = cpvperp(n); - cpFloat coef = (jnt->clamp*cpvcross(j, n) > 0.0f) ? 1.0f : 0.0f; - cpVect jt = cpvmult(t, cpvdot(j, t)*coef); - - return cpvadd(jn, jt); -} - -static void -grooveJointApplyImpulse(cpJoint *joint) -{ - cpBody *a = joint->a; - cpBody *b = joint->b; - - cpGrooveJoint *jnt = (cpGrooveJoint *)joint; - cpVect r1 = jnt->r1; - cpVect r2 = jnt->r2; - cpVect k1 = jnt->k1; - cpVect k2 = jnt->k2; - - //calculate bias impulse - cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); - vbr = cpvsub(jnt->bias, vbr); - - cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2)); - cpVect jbOld = jnt->jBias; - jnt->jBias = grooveConstrain(jnt, cpvadd(jbOld, jb)); - jb = cpvsub(jnt->jBias, jbOld); - - apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); - - // compute impulse - cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); - - cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2)); - cpVect jOld = jnt->jAcc; - jnt->jAcc = grooveConstrain(jnt, cpvadd(jOld, j)); - j = cpvsub(jnt->jAcc, jOld); - - // apply impulse - apply_impulses(a, b, jnt->r1, jnt->r2, j); -} - -static const cpJointClass grooveJointClass = { - CP_GROOVE_JOINT, - grooveJointPreStep, - grooveJointApplyImpulse, -}; - -cpGrooveJoint * -cpGrooveJointAlloc(void) -{ - return (cpGrooveJoint *)malloc(sizeof(cpGrooveJoint)); -} - -cpGrooveJoint * -cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) -{ - cpJointInit((cpJoint *)joint, &grooveJointClass, a, b); - - joint->grv_a = groove_a; - joint->grv_b = groove_b; - joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); - joint->anchr2 = anchr2; - - joint->jAcc = cpvzero; - - return joint; -} - -cpJoint * -cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) -{ - return (cpJoint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); -} - diff --git a/chipmunk/cpJoint.h b/chipmunk/cpJoint.h deleted file mode 100644 --- a/chipmunk/cpJoint.h +++ /dev/null @@ -1,122 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke -* -* Permission is hereby granted, free of charge, to any person obtaining a copy -* of this software and associated documentation files (the "Software"), to deal -* in the Software without restriction, including without limitation the rights -* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -* copies of the Software, and to permit persons to whom the Software is -* furnished to do so, subject to the following conditions: -* -* The above copyright notice and this permission notice shall be included in -* all copies or substantial portions of the Software. -* -* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -* SOFTWARE. -*/ - -// TODO: Comment me! - -extern cpFloat cp_joint_bias_coef; - -typedef enum cpJointType { - CP_PIN_JOINT, - CP_PIVOT_JOINT, - CP_SLIDE_JOINT, - CP_GROOVE_JOINT, - CP_CUSTOM_JOINT, // For user definable joint types. -} cpJointType; - -struct cpJoint; -struct cpJointClass; - -typedef struct cpJointClass { - cpJointType type; - - void (*preStep)(struct cpJoint *joint, cpFloat dt_inv); - void (*applyImpulse)(struct cpJoint *joint); -} cpJointClass; - -typedef struct cpJoint { - const cpJointClass *klass; - - cpBody *a, *b; -} cpJoint; - -void cpJointDestroy(cpJoint *joint); -void cpJointFree(cpJoint *joint); - - -typedef struct cpPinJoint { - cpJoint joint; - cpVect anchr1, anchr2; - cpFloat dist; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc, jBias; - cpFloat bias; -} cpPinJoint; - -cpPinJoint *cpPinJointAlloc(void); -cpPinJoint *cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); -cpJoint *cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2); - - -typedef struct cpSlideJoint { - cpJoint joint; - cpVect anchr1, anchr2; - cpFloat min, max; - - cpVect r1, r2; - cpVect n; - cpFloat nMass; - - cpFloat jnAcc, jBias; - cpFloat bias; -} cpSlideJoint; - -cpSlideJoint *cpSlideJointAlloc(void); -cpSlideJoint *cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); -cpJoint *cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max); - - -typedef struct cpPivotJoint { - cpJoint joint; - cpVect anchr1, anchr2; - - cpVect r1, r2; - cpVect k1, k2; - - cpVect jAcc, jBias; - cpVect bias; -} cpPivotJoint; - -cpPivotJoint *cpPivotJointAlloc(void); -cpPivotJoint *cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot); -cpJoint *cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot); - - -typedef struct cpGrooveJoint { - cpJoint joint; - cpVect grv_n, grv_a, grv_b; - cpVect anchr2; - - cpVect grv_tn; - cpFloat clamp; - cpVect r1, r2; - cpVect k1, k2; - - cpVect jAcc, jBias; - cpVect bias; -} cpGrooveJoint; - -cpGrooveJoint *cpGrooveJointAlloc(void); -cpGrooveJoint *cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); -cpJoint *cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2); diff --git a/chipmunk/cpPolyShape.c b/chipmunk/cpPolyShape.c deleted file mode 100644 --- a/chipmunk/cpPolyShape.c +++ /dev/null @@ -1,139 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include - -#include "chipmunk.h" - -cpPolyShape * -cpPolyShapeAlloc(void) -{ - return (cpPolyShape *)calloc(1, sizeof(cpPolyShape)); -} - -static void -cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) -{ - cpVect *src = poly->verts; - cpVect *dst = poly->tVerts; - - for(int i=0; inumVerts; i++) - dst[i] = cpvadd(p, cpvrotate(src[i], rot)); -} - -static void -cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) -{ - cpPolyShapeAxis *src = poly->axes; - cpPolyShapeAxis *dst = poly->tAxes; - - for(int i=0; inumVerts; i++){ - cpVect n = cpvrotate(src[i].n, rot); - dst[i].n = n; - dst[i].d = cpvdot(p, n) + src[i].d; - } -} - -static cpBB -cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot) -{ - cpPolyShape *poly = (cpPolyShape *)shape; - - cpFloat l, b, r, t; - - cpPolyShapeTransformAxes(poly, p, rot); - cpPolyShapeTransformVerts(poly, p, rot); - - cpVect *verts = poly->tVerts; - l = r = verts[0].x; - b = t = verts[0].y; - - // TODO do as part of cpPolyShapeTransformVerts? - for(int i=1; inumVerts; i++){ - cpVect v = verts[i]; - - l = cpfmin(l, v.x); - r = cpfmax(r, v.x); - - b = cpfmin(b, v.y); - t = cpfmax(t, v.y); - } - - return cpBBNew(l, b, r, t); -} - -static void -cpPolyShapeDestroy(cpShape *shape) -{ - cpPolyShape *poly = (cpPolyShape *)shape; - - free(poly->verts); - free(poly->tVerts); - - free(poly->axes); - free(poly->tAxes); -} - -static int -cpPolyShapePointQuery(cpShape *shape, cpVect p){ - // TODO Check against BB first? - return cpPolyShapeContainsVert((cpPolyShape *)shape, p); -} - -static const cpShapeClass polyClass = { - CP_POLY_SHAPE, - cpPolyShapeCacheData, - cpPolyShapeDestroy, - cpPolyShapePointQuery, -}; - -cpPolyShape * -cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset) -{ - poly->numVerts = numVerts; - - poly->verts = (cpVect *)calloc(numVerts, sizeof(cpVect)); - poly->tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect)); - poly->axes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); - poly->tAxes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); - - for(int i=0; iverts[i] = a; - poly->axes[i].n = n; - poly->axes[i].d = cpvdot(n, a); - } - - cpShapeInit((cpShape *)poly, &polyClass, body); - - return poly; -} - -cpShape * -cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset) -{ - return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset); -} diff --git a/chipmunk/cpPolyShape.h b/chipmunk/cpPolyShape.h deleted file mode 100644 --- a/chipmunk/cpPolyShape.h +++ /dev/null @@ -1,92 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Axis structure used by cpPolyShape. -typedef struct cpPolyShapeAxis{ - // normal - cpVect n; - // distance from origin - cpFloat d; -} cpPolyShapeAxis; - -// Convex polygon shape structure. -typedef struct cpPolyShape{ - cpShape shape; - - // Vertex and axis lists. - int numVerts; - cpVect *verts; - cpPolyShapeAxis *axes; - - // Transformed vertex and axis lists. - cpVect *tVerts; - cpPolyShapeAxis *tAxes; -} cpPolyShape; - -// Basic allocation functions. -cpPolyShape *cpPolyShapeAlloc(void); -cpPolyShape *cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset); -cpShape *cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset); - -// Returns the minimum distance of the polygon to the axis. -static inline cpFloat -cpPolyShapeValueOnAxis(const cpPolyShape *poly, const cpVect n, const cpFloat d) -{ - cpVect *verts = poly->tVerts; - cpFloat min = cpvdot(n, verts[0]); - - int i; - for(i=1; inumVerts; i++) - min = cpfmin(min, cpvdot(n, verts[i])); - - return min - d; -} - -// Returns true if the polygon contains the vertex. -static inline int -cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v) -{ - cpPolyShapeAxis *axes = poly->tAxes; - - int i; - for(i=0; inumVerts; i++){ - cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; - if(dist > 0.0) return 0; - } - - return 1; -} - -// Same as cpPolyShapeContainsVert() but ignores faces pointing away from the normal. -static inline int -cpPolyShapeContainsVertPartial(cpPolyShape *poly, cpVect v, cpVect n) -{ - cpPolyShapeAxis *axes = poly->tAxes; - - int i; - for(i=0; inumVerts; i++){ - if(cpvdot(axes[i].n, n) < 0.0f) continue; - cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; - if(dist > 0.0) return 0; - } - - return 1; -} diff --git a/chipmunk/cpShape.c b/chipmunk/cpShape.c deleted file mode 100644 --- a/chipmunk/cpShape.c +++ /dev/null @@ -1,244 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include - -#include "chipmunk.h" -#include "math.h" - -unsigned int SHAPE_ID_COUNTER = 0; - -void -cpResetShapeIdCounter(void) -{ - SHAPE_ID_COUNTER = 0; -} - - -cpShape* -cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) -{ - shape->klass = klass; - - shape->id = SHAPE_ID_COUNTER; - SHAPE_ID_COUNTER++; - - assert(body != NULL); - shape->body = body; - - shape->e = 0.0f; - shape->u = 0.0f; - shape->surface_v = cpvzero; - - shape->collision_type = 0; - shape->group = 0; - shape->layers = 0xFFFF; - - shape->data = NULL; - - cpShapeCacheBB(shape); - - return shape; -} - -void -cpShapeDestroy(cpShape *shape) -{ - if(shape->klass->destroy) shape->klass->destroy(shape); -} - -void -cpShapeFree(cpShape *shape) -{ - if(shape) cpShapeDestroy(shape); - free(shape); -} - -cpBB -cpShapeCacheBB(cpShape *shape) -{ - cpBody *body = shape->body; - - shape->bb = shape->klass->cacheData(shape, body->p, body->rot); - return shape->bb; -} - -int -cpShapePointQuery(cpShape *shape, cpVect p){ - return shape->klass->pointQuery(shape, p); -} - - - -cpCircleShape * -cpCircleShapeAlloc(void) -{ - return (cpCircleShape *)calloc(1, sizeof(cpCircleShape)); -} - -static inline cpBB -bbFromCircle(const cpVect c, const cpFloat r) -{ - return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r); -} - -static cpBB -cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) -{ - cpCircleShape *circle = (cpCircleShape *)shape; - - circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); - return bbFromCircle(circle->tc, circle->r); -} - -static int -cpCircleShapePointQuery(cpShape *shape, cpVect p){ - cpCircleShape *circle = (cpCircleShape *)shape; - - cpFloat distSQ = cpvlengthsq(cpvsub(circle->tc, p)); - return distSQ <= (circle->r*circle->r); -} - -static const cpShapeClass circleClass = { - CP_CIRCLE_SHAPE, - cpCircleShapeCacheData, - NULL, - cpCircleShapePointQuery, -}; - -cpCircleShape * -cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) -{ - circle->c = offset; - circle->r = radius; - - cpShapeInit((cpShape *)circle, &circleClass, body); - - return circle; -} - -cpShape * -cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) -{ - return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); -} - -cpSegmentShape * -cpSegmentShapeAlloc(void) -{ - return (cpSegmentShape *)calloc(1, sizeof(cpSegmentShape)); -} - -static cpBB -cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) -{ - cpSegmentShape *seg = (cpSegmentShape *)shape; - - seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); - seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); - seg->tn = cpvrotate(seg->n, rot); - - cpFloat l,r,s,t; - - if(seg->ta.x < seg->tb.x){ - l = seg->ta.x; - r = seg->tb.x; - } else { - l = seg->tb.x; - r = seg->ta.x; - } - - if(seg->ta.y < seg->tb.y){ - s = seg->ta.y; - t = seg->tb.y; - } else { - s = seg->tb.y; - t = seg->ta.y; - } - - cpFloat rad = seg->r; - return cpBBNew(l - rad, s - rad, r + rad, t + rad); -} - -static int -cpSegmentShapePointQuery(cpShape *shape, cpVect p){ - cpSegmentShape *seg = (cpSegmentShape *)shape; - - // Calculate normal distance from segment. - cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); - cpFloat dist = fabs(dn) - seg->r; - if(dist > 0.0f) return 0; - - // Calculate tangential distance along segment. - cpFloat dt = -cpvcross(seg->tn, p); - cpFloat dtMin = -cpvcross(seg->tn, seg->ta); - cpFloat dtMax = -cpvcross(seg->tn, seg->tb); - - // Decision tree to decide which feature of the segment to collide with. - if(dt <= dtMin){ - if(dt < (dtMin - seg->r)){ - return 0; - } else { - return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); - } - } else { - if(dt < dtMax){ - return 1; - } else { - if(dt < (dtMax + seg->r)) { - return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); - } else { - return 0; - } - } - } - - return 1; -} - -static const cpShapeClass segmentClass = { - CP_SEGMENT_SHAPE, - cpSegmentShapeCacheData, - NULL, - cpSegmentShapePointQuery, -}; - -cpSegmentShape * -cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - seg->a = a; - seg->b = b; - seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); - - seg->r = r; - - cpShapeInit((cpShape *)seg, &segmentClass, body); - - return seg; -} - -cpShape* -cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) -{ - return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); -} diff --git a/chipmunk/cpShape.h b/chipmunk/cpShape.h deleted file mode 100644 --- a/chipmunk/cpShape.h +++ /dev/null @@ -1,141 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// For determinism, you can reset the shape id counter. -void cpResetShapeIdCounter(void); - -// Enumeration of shape types. -typedef enum cpShapeType{ - CP_CIRCLE_SHAPE, - CP_SEGMENT_SHAPE, - CP_POLY_SHAPE, - CP_NUM_SHAPES -} cpShapeType; - -// Forward declarations required for defining the cpShape and cpShapeClass structs. -struct cpShape; -struct cpShapeClass; - -// Shape class. Holds function pointers and type data. -typedef struct cpShapeClass { - cpShapeType type; - - // Called by cpShapeCacheBB(). - cpBB (*cacheData)(struct cpShape *shape, cpVect p, cpVect rot); - // Called to by cpShapeDestroy(). - void (*destroy)(struct cpShape *shape); - - // called by cpShapeQueryPointQuery(). - int (*pointQuery)(struct cpShape *shape, cpVect p); -} cpShapeClass; - -// Basic shape struct that the others inherit from. -typedef struct cpShape{ - // The "class" of a shape as defined above - const cpShapeClass *klass; - - // cpBody that the shape is attached to. - cpBody *body; - - // Cached BBox for the shape. - cpBB bb; - - // *** Surface properties. - - // Coefficient of restitution. (elasticity) - cpFloat e; - // Coefficient of friction. - cpFloat u; - // Surface velocity used when solving for friction. - cpVect surface_v; - - // *** User Definable Fields - - // User defined data pointer for the shape. - void *data; - - // User defined collision type for the shape. - unsigned int collision_type; - // User defined collision group for the shape. - unsigned int group; - // User defined layer bitmask for the shape. - unsigned int layers; - - // *** Internally Used Fields - - // Unique id used as the hash value. - unsigned int id; -} cpShape; - -// Low level shape initialization func. -cpShape* cpShapeInit(cpShape *shape, const struct cpShapeClass *klass, cpBody *body); - -// Basic destructor functions. (allocation functions are not shared) -void cpShapeDestroy(cpShape *shape); -void cpShapeFree(cpShape *shape); - -// Cache the BBox of the shape. -cpBB cpShapeCacheBB(cpShape *shape); - -// Test if a point lies within a shape. -int cpShapePointQuery(cpShape *shape, cpVect p); - -// Test if a segment collides with a shape. -// Returns [0-1] if the segment collides and -1 otherwise. -// 0 would be a collision at point a, 1 would be a collision at point b. -//cpFloat cpShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b); - - -// Circle shape structure. -typedef struct cpCircleShape{ - cpShape shape; - - // Center. (body space coordinates) - cpVect c; - // Radius. - cpFloat r; - - // Transformed center. (world space coordinates) - cpVect tc; -} cpCircleShape; - -// Basic allocation functions for cpCircleShape. -cpCircleShape *cpCircleShapeAlloc(void); -cpCircleShape *cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset); -cpShape *cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset); - -// Segment shape structure. -typedef struct cpSegmentShape{ - cpShape shape; - - // Endpoints and normal of the segment. (body space coordinates) - cpVect a, b, n; - // Radius of the segment. (Thickness) - cpFloat r; - - // Transformed endpoints and normal. (world space coordinates) - cpVect ta, tb, tn; -} cpSegmentShape; - -// Basic allocation functions for cpSegmentShape. -cpSegmentShape* cpSegmentShapeAlloc(void); -cpSegmentShape* cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat radius); -cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat radius); diff --git a/chipmunk/cpSpace.c b/chipmunk/cpSpace.c deleted file mode 100644 --- a/chipmunk/cpSpace.c +++ /dev/null @@ -1,530 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include -#include - -#include "chipmunk.h" - -int cp_contact_persistence = 3; - -// Equal function for contactSet. -static int -contactSetEql(void *ptr, void *elt) -{ - cpShape **shapes = (cpShape **)ptr; - cpShape *a = shapes[0]; - cpShape *b = shapes[1]; - - cpArbiter *arb = (cpArbiter *)elt; - - return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); -} - -// Transformation function for contactSet. -static void * -contactSetTrans(void *ptr, void *data) -{ - cpShape **shapes = (cpShape **)ptr; - cpShape *a = shapes[0]; - cpShape *b = shapes[1]; - - cpSpace *space = (cpSpace *)data; - - return cpArbiterNew(a, b, space->stamp); -} - -// Collision pair function wrapper struct. -typedef struct collFuncData { - cpCollFunc func; - void *data; -} collFuncData; - -// Equals function for collFuncSet. -static int -collFuncSetEql(void *ptr, void *elt) -{ - unsigned int *ids = (unsigned int *)ptr; - unsigned int a = ids[0]; - unsigned int b = ids[1]; - - cpCollPairFunc *pair = (cpCollPairFunc *)elt; - - return ((a == pair->a && b == pair->b) || (b == pair->a && a == pair->b)); -} - -// Transformation function for collFuncSet. -static void * -collFuncSetTrans(void *ptr, void *data) -{ - unsigned int *ids = (unsigned int *)ptr; - collFuncData *funcData = (collFuncData *)data; - - cpCollPairFunc *pair = (cpCollPairFunc *)malloc(sizeof(cpCollPairFunc)); - pair->a = ids[0]; - pair->b = ids[1]; - pair->func = funcData->func; - pair->data = funcData->data; - - return pair; -} - -// Default collision pair function. -static int -alwaysCollide(cpShape *a, cpShape *b, cpContact *arr, int numCon, cpFloat normal_coef, void *data) -{ - return 1; -} - -// BBfunc callback for the spatial hash. -static cpBB -bbfunc(void *ptr) -{ - cpShape *shape = (cpShape *)ptr; - return shape->bb; -} - -// Iterator functions for destructors. -static void freeWrap(void *ptr, void *unused){ free( ptr);} -static void shapeFreeWrap(void *ptr, void *unused){ cpShapeFree((cpShape *) ptr);} -static void arbiterFreeWrap(void *ptr, void *unused){ cpArbiterFree((cpArbiter *)ptr);} -static void bodyFreeWrap(void *ptr, void *unused){ cpBodyFree((cpBody *) ptr);} -static void jointFreeWrap(void *ptr, void *unused){ cpJointFree((cpJoint *) ptr);} - -cpSpace* -cpSpaceAlloc(void) -{ - return (cpSpace *)calloc(1, sizeof(cpSpace)); -} - -#define DEFAULT_DIM_SIZE 100.0f -#define DEFAULT_COUNT 1000 -#define DEFAULT_ITERATIONS 10 -#define DEFAULT_ELASTIC_ITERATIONS 0 - -cpSpace* -cpSpaceInit(cpSpace *space) -{ - space->iterations = DEFAULT_ITERATIONS; - space->elasticIterations = DEFAULT_ELASTIC_ITERATIONS; -// space->sleepTicks = 300; - - space->gravity = cpvzero; - space->damping = 1.0f; - - space->stamp = 0; - - space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc); - space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc); - - space->bodies = cpArrayNew(0); - space->arbiters = cpArrayNew(0); - space->contactSet = cpHashSetNew(0, contactSetEql, contactSetTrans); - - space->joints = cpArrayNew(0); - - cpCollPairFunc pairFunc = {0, 0, alwaysCollide, NULL}; - space->defaultPairFunc = pairFunc; - space->collFuncSet = cpHashSetNew(0, collFuncSetEql, collFuncSetTrans); - space->collFuncSet->default_value = &space->defaultPairFunc; - - return space; -} - -cpSpace* -cpSpaceNew(void) -{ - return cpSpaceInit(cpSpaceAlloc()); -} - -void -cpSpaceDestroy(cpSpace *space) -{ - cpSpaceHashFree(space->staticShapes); - cpSpaceHashFree(space->activeShapes); - - cpArrayFree(space->bodies); - - cpArrayFree(space->joints); - - if(space->contactSet) - cpHashSetEach(space->contactSet, &arbiterFreeWrap, NULL); - cpHashSetFree(space->contactSet); - cpArrayFree(space->arbiters); - - if(space->collFuncSet) - cpHashSetEach(space->collFuncSet, &freeWrap, NULL); - cpHashSetFree(space->collFuncSet); -} - -void -cpSpaceFree(cpSpace *space) -{ - if(space) cpSpaceDestroy(space); - free(space); -} - -void -cpSpaceFreeChildren(cpSpace *space) -{ - cpSpaceHashEach(space->staticShapes, &shapeFreeWrap, NULL); - cpSpaceHashEach(space->activeShapes, &shapeFreeWrap, NULL); - cpArrayEach(space->bodies, &bodyFreeWrap, NULL); - cpArrayEach(space->joints, &jointFreeWrap, NULL); -} - -void -cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b, - cpCollFunc func, void *data) -{ - unsigned int ids[] = {a, b}; - unsigned int hash = CP_HASH_PAIR(a, b); - // Remove any old function so the new one will get added. - cpSpaceRemoveCollisionPairFunc(space, a, b); - - collFuncData funcData = {func, data}; - cpHashSetInsert(space->collFuncSet, hash, ids, &funcData); -} - -void -cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b) -{ - unsigned int ids[] = {a, b}; - unsigned int hash = CP_HASH_PAIR(a, b); - cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids); - free(old_pair); -} - -void -cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data) -{ - cpCollPairFunc pairFunc = {0, 0, (func ? func : alwaysCollide), (func ? data : NULL)}; - space->defaultPairFunc = pairFunc; -} - -void -cpSpaceAddShape(cpSpace *space, cpShape *shape) -{ - cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb); -} - -void -cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) -{ - cpShapeCacheBB(shape); - cpSpaceHashInsert(space->staticShapes, shape, shape->id, shape->bb); -} - -void -cpSpaceAddBody(cpSpace *space, cpBody *body) -{ - cpArrayPush(space->bodies, body); -} - -void -cpSpaceAddJoint(cpSpace *space, cpJoint *joint) -{ - cpArrayPush(space->joints, joint); -} - -void -cpSpaceRemoveShape(cpSpace *space, cpShape *shape) -{ - cpSpaceHashRemove(space->activeShapes, shape, shape->id); -} - -void -cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) -{ - cpSpaceHashRemove(space->staticShapes, shape, shape->id); -} - -void -cpSpaceRemoveBody(cpSpace *space, cpBody *body) -{ - cpArrayDeleteObj(space->bodies, body); -} - -void -cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint) -{ - cpArrayDeleteObj(space->joints, joint); -} - -void -cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) -{ - cpArray *bodies = space->bodies; - - for(int i=0; inum; i++) - func((cpBody *)bodies->arr[i], data); -} - -// Iterator function used for updating shape BBoxes. -static void -updateBBCache(void *ptr, void *unused) -{ - cpShape *shape = (cpShape *)ptr; - cpShapeCacheBB(shape); -} - -void -cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) -{ - cpSpaceHashResize(space->staticShapes, dim, count); - cpSpaceHashRehash(space->staticShapes); -} - -void -cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) -{ - cpSpaceHashResize(space->activeShapes, dim, count); -} - -void -cpSpaceRehashStatic(cpSpace *space) -{ - cpSpaceHashEach(space->staticShapes, &updateBBCache, NULL); - cpSpaceHashRehash(space->staticShapes); -} - -typedef struct pointQueryFuncPair { - cpSpacePointQueryFunc func; - void *data; -} pointQueryFuncPair; - -static int -pointQueryHelper(void *point, void *obj, void *data) -{ - cpShape *shape = (cpShape *)obj; - pointQueryFuncPair *pair = (pointQueryFuncPair *)data; - - if(cpShapePointQuery(shape, *((cpVect *)point))) - pair->func(shape, pair->data); - - return 1; // return value needed for historical reasons (value is ignored) -} - -static void -pointQuery(cpSpaceHash *hash, cpVect point, cpSpacePointQueryFunc func, void *data) -{ - pointQueryFuncPair pair = {func, data}; - cpSpaceHashPointQuery(hash, point, pointQueryHelper, &pair); -} - -void -cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data) -{ - pointQuery(space->activeShapes, point, func, data); -} - -void -cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data) -{ - pointQuery(space->staticShapes, point, func, data); -} - -static inline int -queryReject(cpShape *a, cpShape *b) -{ - return - // BBoxes must overlap - !cpBBintersects(a->bb, b->bb) - // Don't collide shapes attached to the same body. - || a->body == b->body - // Don't collide objects in the same non-zero group - || (a->group && b->group && a->group == b->group) - // Don't collide objects that don't share at least on layer. - || !(a->layers & b->layers); -} - -// Callback from the spatial hash. -// TODO: Refactor this into separate functions? -static int -queryFunc(void *p1, void *p2, void *data) -{ - // Cast the generic pointers from the spatial hash back to usefull types - cpShape *a = (cpShape *)p1; - cpShape *b = (cpShape *)p2; - cpSpace *space = (cpSpace *)data; - - // Reject any of the simple cases - if(queryReject(a,b)) return 0; - - // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) - if(a->klass->type > b->klass->type){ - cpShape *temp = a; - a = b; - b = temp; - } - - // Find the collision pair function for the shapes. - unsigned int ids[] = {a->collision_type, b->collision_type}; - unsigned int hash = CP_HASH_PAIR(a->collision_type, b->collision_type); - cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids); - if(!pairFunc->func) return 0; // A NULL pair function means don't collide at all. - - // Narrow-phase collision detection. - cpContact *contacts = NULL; - int numContacts = cpCollideShapes(a, b, &contacts); - if(!numContacts) return 0; // Shapes are not colliding. - - // The collision pair function requires objects to be ordered by their collision types. - cpShape *pair_a = a; - cpShape *pair_b = b; - cpFloat normal_coef = 1.0f; - - // Swap them if necessary. - if(pair_a->collision_type != pairFunc->a){ - cpShape *temp = pair_a; - pair_a = pair_b; - pair_b = temp; - normal_coef = -1.0f; - } - - if(pairFunc->func(pair_a, pair_b, contacts, numContacts, normal_coef, pairFunc->data)){ - // The collision pair function OKed the collision. Record the contact information. - - // Get an arbiter from space->contactSet for the two shapes. - // This is where the persistant contact magic comes from. - cpShape *shape_pair[] = {a, b}; - cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space); - - // Timestamp the arbiter. - arb->stamp = space->stamp; - arb->a = a; arb->b = b; // TODO: Investigate why this is still necessary? - // Inject the new contact points into the arbiter. - cpArbiterInject(arb, contacts, numContacts); - - // Add the arbiter to the list of active arbiters. - cpArrayPush(space->arbiters, arb); - - return numContacts; - } else { - // The collision pair function rejected the collision. - - free(contacts); - return 0; - } -} - -// Iterator for active/static hash collisions. -static void -active2staticIter(void *ptr, void *data) -{ - cpShape *shape = (cpShape *)ptr; - cpSpace *space = (cpSpace *)data; - cpSpaceHashQuery(space->staticShapes, shape, shape->bb, &queryFunc, space); -} - -// Hashset reject func to throw away old arbiters. -static int -contactSetReject(void *ptr, void *data) -{ - cpArbiter *arb = (cpArbiter *)ptr; - cpSpace *space = (cpSpace *)data; - - if((space->stamp - arb->stamp) > cp_contact_persistence){ - cpArbiterFree(arb); - return 0; - } - - return 1; -} - -void -cpSpaceStep(cpSpace *space, cpFloat dt) -{ - if(!dt) return; // prevents div by zero. - cpFloat dt_inv = 1.0f/dt; - - cpArray *bodies = space->bodies; - cpArray *arbiters = space->arbiters; - cpArray *joints = space->joints; - - // Empty the arbiter list. - cpHashSetReject(space->contactSet, &contactSetReject, space); - space->arbiters->num = 0; - - // Integrate positions. - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->position_func(body, dt); - } - - // Pre-cache BBoxes and shape data. - cpSpaceHashEach(space->activeShapes, &updateBBCache, NULL); - - // Collide! - cpSpaceHashEach(space->activeShapes, &active2staticIter, space); - cpSpaceHashQueryRehash(space->activeShapes, &queryFunc, space); - - // Prestep the arbiters. - for(int i=0; inum; i++) - cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); - - // Prestep the joints. - for(int i=0; inum; i++){ - cpJoint *joint = (cpJoint *)joints->arr[i]; - joint->klass->preStep(joint, dt_inv); - } - - for(int i=0; ielasticIterations; i++){ - for(int j=0; jnum; j++) - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f); - - for(int j=0; jnum; j++){ - cpJoint *joint = (cpJoint *)joints->arr[j]; - joint->klass->applyImpulse(joint); - } - } - - // Integrate velocities. - cpFloat damping = pow(1.0f/space->damping, -dt); - for(int i=0; inum; i++){ - cpBody *body = (cpBody *)bodies->arr[i]; - body->velocity_func(body, space->gravity, damping, dt); - } - - for(int i=0; inum; i++) - cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]); - - // Run the impulse solver. - for(int i=0; iiterations; i++){ - for(int j=0; jnum; j++) - cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 0.0f); - - for(int j=0; jnum; j++){ - cpJoint *joint = (cpJoint *)joints->arr[j]; - joint->klass->applyImpulse(joint); - } - } - -// cpFloat dvsq = cpvdot(space->gravity, space->gravity); -// dvsq *= dt*dt * space->damping*space->damping; -// for(int i=0; inum; i++) -// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks); - - // Increment the stamp. - space->stamp++; -} diff --git a/chipmunk/cpSpace.h b/chipmunk/cpSpace.h deleted file mode 100644 --- a/chipmunk/cpSpace.h +++ /dev/null @@ -1,120 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Number of frames that contact information should persist. -extern int cp_contact_persistence; - -// User collision pair function. -typedef int (*cpCollFunc)(cpShape *a, cpShape *b, cpContact *contacts, int numContacts, cpFloat normal_coef, void *data); - -// Structure for holding collision pair function information. -// Used internally. -typedef struct cpCollPairFunc { - unsigned int a; - unsigned int b; - cpCollFunc func; - void *data; -} cpCollPairFunc; - -typedef struct cpSpace{ - // *** User definable fields - - // Number of iterations to use in the impulse solver to solve contacts. - int iterations; - - // Number of iterations to use in the impulse solver to solve elastic collisions. - int elasticIterations; - - // Default gravity to supply when integrating rigid body motions. - cpVect gravity; - - // Default damping to supply when integrating rigid body motions. - cpFloat damping; - - // *** Internally Used Fields - - // Time stamp. Is incremented on every call to cpSpaceStep(). - int stamp; - - // The static and active shape spatial hashes. - cpSpaceHash *staticShapes; - cpSpaceHash *activeShapes; - - // List of bodies in the system. - cpArray *bodies; - // List of active arbiters for the impulse solver. - cpArray *arbiters; - // Persistant contact set. - cpHashSet *contactSet; - - // List of joints in the system. - cpArray *joints; - - // Set of collisionpair functions. - cpHashSet *collFuncSet; - // Default collision pair function. - cpCollPairFunc defaultPairFunc; -} cpSpace; - -// Basic allocation/destruction functions. -cpSpace* cpSpaceAlloc(void); -cpSpace* cpSpaceInit(cpSpace *space); -cpSpace* cpSpaceNew(void); - -void cpSpaceDestroy(cpSpace *space); -void cpSpaceFree(cpSpace *space); - -// Convenience function. Frees all referenced entities. (bodies, shapes and joints) -void cpSpaceFreeChildren(cpSpace *space); - -// Collision pair function management functions. -void cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b, - cpCollFunc func, void *data); -void cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b); -void cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data); - -// Add and remove entities from the system. -void cpSpaceAddShape(cpSpace *space, cpShape *shape); -void cpSpaceAddStaticShape(cpSpace *space, cpShape *shape); -void cpSpaceAddBody(cpSpace *space, cpBody *body); -void cpSpaceAddJoint(cpSpace *space, cpJoint *joint); - -void cpSpaceRemoveShape(cpSpace *space, cpShape *shape); -void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape); -void cpSpaceRemoveBody(cpSpace *space, cpBody *body); -void cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint); - -// Point query callback function -typedef void (*cpSpacePointQueryFunc)(cpShape *shape, void *data); -void cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data); -void cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data); - -// Iterator function for iterating the bodies in a space. -typedef void (*cpSpaceBodyIterator)(cpBody *body, void *data); -void cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data); - -// Spatial hash management functions. -void cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count); -void cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count); -void cpSpaceRehashStatic(cpSpace *space); - -// Update the space. -void cpSpaceStep(cpSpace *space, cpFloat dt); diff --git a/chipmunk/cpSpaceHash.c b/chipmunk/cpSpaceHash.c deleted file mode 100644 --- a/chipmunk/cpSpaceHash.c +++ /dev/null @@ -1,455 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include -#include -#include -#include - -#include "chipmunk.h" -#include "prime.h" - -static cpHandle* -cpHandleAlloc(void) -{ - return (cpHandle *)malloc(sizeof(cpHandle)); -} - -static cpHandle* -cpHandleInit(cpHandle *hand, void *obj) -{ - hand->obj = obj; - hand->retain = 0; - hand->stamp = 0; - - return hand; -} - -static cpHandle* -cpHandleNew(void *obj) -{ - return cpHandleInit(cpHandleAlloc(), obj); -} - -static inline void -cpHandleRetain(cpHandle *hand) -{ - hand->retain++; -} - -static inline void -cpHandleFree(cpHandle *hand) -{ - free(hand); -} - -static inline void -cpHandleRelease(cpHandle *hand) -{ - hand->retain--; - if(hand->retain == 0) - cpHandleFree(hand); -} - - -cpSpaceHash* -cpSpaceHashAlloc(void) -{ - return (cpSpaceHash *)calloc(1, sizeof(cpSpaceHash)); -} - -// Frees the old table, and allocates a new one. -static void -cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) -{ - free(hash->table); - - hash->numcells = numcells; - hash->table = (cpSpaceHashBin **)calloc(numcells, sizeof(cpSpaceHashBin *)); -} - -// Equality function for the handleset. -static int -handleSetEql(void *obj, void *elt) -{ - cpHandle *hand = (cpHandle *)elt; - return (obj == hand->obj); -} - -// Transformation function for the handleset. -static void * -handleSetTrans(void *obj, void *unused) -{ - cpHandle *hand = cpHandleNew(obj); - cpHandleRetain(hand); - - return hand; -} - -cpSpaceHash* -cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc) -{ - cpSpaceHashAllocTable(hash, next_prime(numcells)); - hash->celldim = celldim; - hash->bbfunc = bbfunc; - - hash->bins = NULL; - hash->handleSet = cpHashSetNew(0, &handleSetEql, &handleSetTrans); - - hash->stamp = 1; - - return hash; -} - -cpSpaceHash* -cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc) -{ - return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc); -} - -static inline void -clearHashCell(cpSpaceHash *hash, int index) -{ - cpSpaceHashBin *bin = hash->table[index]; - while(bin){ - cpSpaceHashBin *next = bin->next; - - // Release the lock on the handle. - cpHandleRelease(bin->handle); - // Recycle the bin. - bin->next = hash->bins; - hash->bins = bin; - - bin = next; - } - - hash->table[index] = NULL; -} - -// Clear all cells in the hashtable. -static void -clearHash(cpSpaceHash *hash) -{ - for(int i=0; inumcells; i++) - clearHashCell(hash, i); -} - -// Free the recycled hash bins. -static void -freeBins(cpSpaceHash *hash) -{ - cpSpaceHashBin *bin = hash->bins; - while(bin){ - cpSpaceHashBin *next = bin->next; - free(bin); - bin = next; - } -} - -// Hashset iterator function to free the handles. -static void -handleFreeWrap(void *elt, void *unused) -{ - cpHandle *hand = (cpHandle *)elt; - cpHandleFree(hand); -} - -void -cpSpaceHashDestroy(cpSpaceHash *hash) -{ - clearHash(hash); - freeBins(hash); - - // Free the handles. - cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL); - cpHashSetFree(hash->handleSet); - - free(hash->table); -} - -void -cpSpaceHashFree(cpSpaceHash *hash) -{ - if(!hash) return; - cpSpaceHashDestroy(hash); - free(hash); -} - -void -cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) -{ - // Clear the hash to release the old handle locks. - clearHash(hash); - - hash->celldim = celldim; - cpSpaceHashAllocTable(hash, next_prime(numcells)); -} - -// Return true if the chain contains the handle. -static inline int -containsHandle(cpSpaceHashBin *bin, cpHandle *hand) -{ - while(bin){ - if(bin->handle == hand) return 1; - bin = bin->next; - } - - return 0; -} - -// Get a recycled or new bin. -static inline cpSpaceHashBin * -getEmptyBin(cpSpaceHash *hash) -{ - cpSpaceHashBin *bin = hash->bins; - - // Make a new one if necessary. - if(bin == NULL) return (cpSpaceHashBin *)malloc(sizeof(cpSpaceHashBin)); - - hash->bins = bin->next; - return bin; -} - -// The hash function itself. -static inline unsigned int -hash_func(unsigned int x, unsigned int y, unsigned int n) -{ - return (x*2185031351ul ^ y*4232417593ul) % n; -} - -static inline void -hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) -{ - // Find the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = bb.l/dim; - int r = bb.r/dim; - int b = bb.b/dim; - int t = bb.t/dim; - - int n = hash->numcells; - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - int index = hash_func(i,j,n); - cpSpaceHashBin *bin = hash->table[index]; - - // Don't add an object twice to the same cell. - if(containsHandle(bin, hand)) continue; - - cpHandleRetain(hand); - // Insert a new bin for the handle in this cell. - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - hash->table[index] = newBin; - } - } -} - -void -cpSpaceHashInsert(cpSpaceHash *hash, void *obj, unsigned int id, cpBB bb) -{ - cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, id, obj, NULL); - hashHandle(hash, hand, bb); -} - -void -cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id) -{ - cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, id, obj); - hashHandle(hash, hand, hash->bbfunc(obj)); -} - -// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?) -static void -handleRehashHelper(void *elt, void *data) -{ - cpHandle *hand = (cpHandle *)elt; - cpSpaceHash *hash = (cpSpaceHash *)data; - - hashHandle(hash, hand, hash->bbfunc(hand->obj)); -} - -void -cpSpaceHashRehash(cpSpaceHash *hash) -{ - clearHash(hash); - - // Rehash all of the handles. - cpHashSetEach(hash->handleSet, &handleRehashHelper, hash); -} - -void -cpSpaceHashRemove(cpSpaceHash *hash, void *obj, unsigned int id) -{ - cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, id, obj); - - if(hand){ - hand->obj = NULL; - cpHandleRelease(hand); - } -} - -// Used by the cpSpaceHashEach() iterator. -typedef struct eachPair { - cpSpaceHashIterator func; - void *data; -} eachPair; - -// Calls the user iterator function. (Gross I know.) -static void -eachHelper(void *elt, void *data) -{ - cpHandle *hand = (cpHandle *)elt; - eachPair *pair = (eachPair *)data; - - pair->func(hand->obj, pair->data); -} - -// Iterate over the objects in the spatial hash. -void -cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data) -{ - // Bundle the callback up to send to the hashset iterator. - eachPair pair = {func, data}; - - cpHashSetEach(hash->handleSet, &eachHelper, &pair); -} - -// Calls the callback function for the objects in a given chain. -static inline void -query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data) -{ - for(; bin; bin = bin->next){ - cpHandle *hand = bin->handle; - void *other = hand->obj; - - // Skip over certain conditions - if( - // Have we already tried this pair in this query? - hand->stamp == hash->stamp - // Is obj the same as other? - || obj == other - // Has other been removed since the last rehash? - || !other - ) continue; - - func(obj, other, data); - - // Stamp that the handle was checked already against this object. - hand->stamp = hash->stamp; - } -} - -void -cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data) -{ - cpFloat dim = hash->celldim; - int index = hash_func((int)(point.x/dim), (int)(point.y/dim), hash->numcells); - - query(hash, hash->table[index], &point, func, data); - - // Increment the stamp. - // Only one cell is checked, but query() requires it anyway. - hash->stamp++; -} - -void -cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data) -{ - // Get the dimensions in cell coordinates. - cpFloat dim = hash->celldim; - int l = bb.l/dim; - int r = bb.r/dim; - int b = bb.b/dim; - int t = bb.t/dim; - - int n = hash->numcells; - - // Iterate over the cells and query them. - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - int index = hash_func(i,j,n); - query(hash, hash->table[index], obj, func, data); - } - } - - // Increment the stamp. - hash->stamp++; -} - -// Similar to struct eachPair above. -typedef struct queryRehashPair { - cpSpaceHash *hash; - cpSpaceHashQueryFunc func; - void *data; -} queryRehashPair; - -// Hashset iterator func used with cpSpaceHashQueryRehash(). -static void -handleQueryRehashHelper(void *elt, void *data) -{ - cpHandle *hand = (cpHandle *)elt; - - // Unpack the user callback data. - queryRehashPair *pair = (queryRehashPair *)data; - cpSpaceHash *hash = pair->hash; - cpSpaceHashQueryFunc func = pair->func; - - cpFloat dim = hash->celldim; - int n = hash->numcells; - - void *obj = hand->obj; - cpBB bb = hash->bbfunc(obj); - - int l = bb.l/dim; - int r = bb.r/dim; - int b = bb.b/dim; - int t = bb.t/dim; - - for(int i=l; i<=r; i++){ - for(int j=b; j<=t; j++){ - int index = hash_func(i,j,n); - cpSpaceHashBin *bin = hash->table[index]; - - if(containsHandle(bin, hand)) continue; - query(hash, bin, obj, func, pair->data); - - cpHandleRetain(hand); - cpSpaceHashBin *newBin = getEmptyBin(hash); - newBin->handle = hand; - newBin->next = bin; - hash->table[index] = newBin; - } - } - - // Increment the stamp for each object we hash. - hash->stamp++; -} - -void -cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data) -{ - clearHash(hash); - - queryRehashPair pair = {hash, func, data}; - cpHashSetEach(hash->handleSet, &handleQueryRehashHelper, &pair); -} diff --git a/chipmunk/cpSpaceHash.h b/chipmunk/cpSpaceHash.h deleted file mode 100644 --- a/chipmunk/cpSpaceHash.h +++ /dev/null @@ -1,100 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// The spatial hash is Chipmunk's default (and currently only) spatial index type. -// Based on a chained hash table. - -// Used internally to track objects added to the hash -typedef struct cpHandle{ - // Pointer to the object - void *obj; - // Retain count - int retain; - // Query stamp. Used to make sure two objects - // aren't identified twice in the same query. - int stamp; -} cpHandle; - -// Linked list element for in the chains. -typedef struct cpSpaceHashBin{ - cpHandle *handle; - struct cpSpaceHashBin *next; -} cpSpaceHashBin; - -// BBox callback. Called whenever the hash needs a bounding box from an object. -typedef cpBB (*cpSpaceHashBBFunc)(void *obj); - -typedef struct cpSpaceHash{ - // Number of cells in the table. - int numcells; - // Dimentions of the cells. - cpFloat celldim; - - // BBox callback. - cpSpaceHashBBFunc bbfunc; - - // Hashset of all the handles. - cpHashSet *handleSet; - - cpSpaceHashBin **table; - // List of recycled bins. - cpSpaceHashBin *bins; - - // Incremented on each query. See cpHandle.stamp. - int stamp; -} cpSpaceHash; - -//Basic allocation/destruction functions. -cpSpaceHash *cpSpaceHashAlloc(void); -cpSpaceHash *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); -cpSpaceHash *cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); - -void cpSpaceHashDestroy(cpSpaceHash *hash); -void cpSpaceHashFree(cpSpaceHash *hash); - -// Resize the hashtable. (Does not rehash! You must call cpSpaceHashRehash() if needed.) -void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); - -// Add an object to the hash. -void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, unsigned int id, cpBB bb); -// Remove an object from the hash. -void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, unsigned int id); - -// Iterator function -typedef void (*cpSpaceHashIterator)(void *obj, void *data); -// Iterate over the objects in the hash. -void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data); - -// Rehash the contents of the hash. -void cpSpaceHashRehash(cpSpaceHash *hash); -// Rehash only a specific object. -void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id); - -// Query callback. -typedef int (*cpSpaceHashQueryFunc)(void *obj1, void *obj2, void *data); -// Point query the hash. A reference to the query point is passed as obj1 to the query callback. -void cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data); -// Query the hash for a given BBox. -void cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); -// Run a query for the object, then insert it. (Optimized case) -void cpSpaceHashQueryInsert(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); -// Rehashes while querying for each object. (Optimized case) -void cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data); diff --git a/chipmunk/cpVect.c b/chipmunk/cpVect.c deleted file mode 100644 --- a/chipmunk/cpVect.c +++ /dev/null @@ -1,63 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -#include "stdio.h" -#include "math.h" - -#include "chipmunk.h" - -cpFloat -cpvlength(const cpVect v) -{ - return sqrtf( cpvdot(v, v) ); -} - -cpFloat -cpvlengthsq(const cpVect v) -{ - return cpvdot(v, v); -} - -cpVect -cpvnormalize(const cpVect v) -{ - return cpvmult( v, 1.0f/cpvlength(v) ); -} - -cpVect -cpvforangle(const cpFloat a) -{ - return cpv(cos(a), sin(a)); -} - -cpFloat -cpvtoangle(const cpVect v) -{ - return atan2(v.y, v.x); -} - -char* -cpvstr(const cpVect v) -{ - static char str[256]; - sprintf(str, "(% .3f, % .3f)", v.x, v.y); - return str; -} diff --git a/chipmunk/cpVect.h b/chipmunk/cpVect.h deleted file mode 100644 --- a/chipmunk/cpVect.h +++ /dev/null @@ -1,106 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -typedef struct cpVect{ - cpFloat x,y; -} cpVect; - -static const cpVect cpvzero={0.0f,0.0f}; - -static inline cpVect -cpv(const cpFloat x, const cpFloat y) -{ - cpVect v = {x, y}; - return v; -} - -static inline cpVect -cpvadd(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x + v2.x, v1.y + v2.y); -} - -static inline cpVect -cpvneg(const cpVect v) -{ - return cpv(-v.x, -v.y); -} - -static inline cpVect -cpvsub(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x - v2.x, v1.y - v2.y); -} - -static inline cpVect -cpvmult(const cpVect v, const cpFloat s) -{ - return cpv(v.x*s, v.y*s); -} - -static inline cpFloat -cpvdot(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.x + v1.y*v2.y; -} - -static inline cpFloat -cpvcross(const cpVect v1, const cpVect v2) -{ - return v1.x*v2.y - v1.y*v2.x; -} - -static inline cpVect -cpvperp(const cpVect v) -{ - return cpv(-v.y, v.x); -} - -static inline cpVect -cpvrperp(const cpVect v) -{ - return cpv(v.y, -v.x); -} - -static inline cpVect -cpvproject(const cpVect v1, const cpVect v2) -{ - return cpvmult(v2, cpvdot(v1, v2)/cpvdot(v2, v2)); -} - -static inline cpVect -cpvrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x - v1.y*v2.y, v1.x*v2.y + v1.y*v2.x); -} - -static inline cpVect -cpvunrotate(const cpVect v1, const cpVect v2) -{ - return cpv(v1.x*v2.x + v1.y*v2.y, v1.y*v2.x - v1.x*v2.y); -} - -cpFloat cpvlength(const cpVect v); -cpFloat cpvlengthsq(const cpVect v); // no sqrt() call -cpVect cpvnormalize(const cpVect v); -cpVect cpvforangle(const cpFloat a); // convert radians to a normalized vector -cpFloat cpvtoangle(const cpVect v); // convert a vector to radians -char *cpvstr(const cpVect v); // get a string representation of a vector diff --git a/chipmunk/prime.h b/chipmunk/prime.h deleted file mode 100644 --- a/chipmunk/prime.h +++ /dev/null @@ -1,68 +0,0 @@ -/* Copyright (c) 2007 Scott Lembcke - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE - * SOFTWARE. - */ - -// Used for resizing hash tables. -// Values approximately double. - -static int primes[] = { - 5, //2^2 + 1 - 11, //2^3 + 3 - 17, //2^4 + 1 - 37, //2^5 + 5 - 67, //2^6 + 3 - 131, //2^7 + 3 - 257, //2^8 + 1 - 521, //2^9 + 9 - 1031, //2^10 + 7 - 2053, //2^11 + 5 - 4099, //2^12 + 3 - 8209, //2^13 + 17 - 16411, //2^14 + 27 - 32771, //2^15 + 3 - 65537, //2^16 + 1 - 131101, //2^17 + 29 - 262147, //2^18 + 3 - 524309, //2^19 + 21 - 1048583, //2^20 + 7 - 2097169, //2^21 + 17 - 4194319, //2^22 + 15 - 8388617, //2^23 + 9 - 16777259, //2^24 + 43 - 33554467, //2^25 + 35 - 67108879, //2^26 + 15 - 134217757, //2^27 + 29 - 268435459, //2^28 + 3 - 536870923, //2^29 + 11 - 1073741827, //2^30 + 3 - 0, -}; - -static int -next_prime(int n) -{ - int i = 0; - while(n > primes[i]){ - i++; - assert(primes[i]); // realistically this should never happen - } - - return primes[i]; -}