packages feed

Hipmunk (empty) → 0.1

raw patch · 39 files changed

+7020/−0 lines, 39 filesdep +arraydep +basedep +containerssetup-changed

Dependencies added: array, base, containers

Files

+ Hipmunk.cabal view
@@ -0,0 +1,81 @@+Cabal-Version: >= 1.2+Build-Type:    Simple+Tested-With:   GHC+Category:      Physics, Game+Name:          Hipmunk+Version:       0.1+Stability:     beta+License:       OtherLicense+License-File:  LICENSE+Copyright:     (c) 2008 Felipe A. Lessa+Author:        Felipe A. Lessa <felipe.lessa@gmail.com>+Maintainer:    Felipe A. Lessa <felipe.lessa@gmail.com>+Synopsis:      A Haskell binding for Chipmunk.+Description:+      Chipmunk is a fast, simple, portable, 2D physics engine+      (<http://wiki.slembcke.net/main/published/Chipmunk>).+      This package contains the Chipmunk rev4 source+      (from <http://chipmunk-physics.googlecode.com/svn/trunk/>)+      and Haskell bindings to all of its functions. It is+      completely self-contained.+      .+      Licensed under the MIT license (like Chipmunk itself).+Extra-Source-Files:+      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,+      Physics/Hipmunk/wrapper.h++Flag small_base+  Description: Choose the new smaller, split-up base package.+++Library+  Exposed-Modules:+      Physics.Hipmunk,+      Physics.Hipmunk.Common,+      Physics.Hipmunk.Body,+      Physics.Hipmunk.Shape,+      Physics.Hipmunk.Joint,+      Physics.Hipmunk.Space+  Other-Modules:+      Physics.Hipmunk.Internal+  Include-Dirs:+      Physics/Hipmunk,+      chipmunk+  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,+      Physics/Hipmunk/wrapper.c+  Build-Depends: base+  if flag(small_base)+    Build-Depends: array, containers+  Extensions:    CPP, ForeignFunctionInterface+  Build-Tools:   hsc2hs+  GHC-Options:   -Wall+  CC-Options:    -O3 -ffast-math -std=gnu99+  Extra-Libraries: m
+ LICENSE view
@@ -0,0 +1,27 @@+Hipmunk, a Haskell binding to the Chipmunk library, is+Copyright (c) 2008 Felipe A. Lessa+and is provided under the same terms as the Chipmunk's+license.++Chipmunk's license:++/* 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.+ */
+ Physics/Hipmunk.hs view
@@ -0,0 +1,35 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Hipmunk.hs+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-- This module re-exports all other Hipmunk modules. It is+-- meant to be imported qualified such as+--+-- @+-- import qualified Physics.Hipmunk as H+-- @+--+-- however it doesn't clash with the 'Prelude'.+--+-----------------------------------------------------------------------------++module Physics.Hipmunk+    (module Physics.Hipmunk.Common,+     module Physics.Hipmunk.Body,+     module Physics.Hipmunk.Shape,+     module Physics.Hipmunk.Joint,+     module Physics.Hipmunk.Space+    )+    where++import Physics.Hipmunk.Common+import Physics.Hipmunk.Body+import Physics.Hipmunk.Shape+import Physics.Hipmunk.Joint+import Physics.Hipmunk.Space
+ Physics/Hipmunk/Body.hsc view
@@ -0,0 +1,351 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Body.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-- Rigid bodies and their properties.+--+-----------------------------------------------------------------------------++module Physics.Hipmunk.Body+    (-- * Creating+     Body,+     newBody,++     -- * Static properties+     -- ** Basic+     -- *** Mass+     Mass,+     getMass,+     setMass,+     -- *** Moment of inertia+     Moment,+     getMoment,+     setMoment,++     -- ** Linear components of motion+     -- *** Position+     getPosition,+     setPosition,+     -- *** Velocity+     Velocity,+     getVelocity,+     setVelocity,+     -- *** Force+     Force,+     getForce,+     setForce,++     -- ** Angular components of motion+     -- *** Angle+     getAngle,+     setAngle,+     -- *** Angular velocity+     AngVel,+     getAngVel,+     setAngVel,+     -- *** Torque+     Torque,+     getTorque,+     setTorque,++     -- * Dynamic properties+     slew,+     updateVelocity,+     updatePosition,+     resetForces,+     applyForce,+     applyOnlyForce,+     applyImpulse,+     dampedSpring,++     -- * Utilities+     localToWorld,+     worldToLocal+    )+    where++import Foreign hiding (rotate, new)+#include "wrapper.h"++import Physics.Hipmunk.Common+import Physics.Hipmunk.Internal++-- | @newBody mass inertia@ creates a new 'Body' with+--   the given mass and moment of inertia.+--+--   It is recommended to call 'setPosition' afterwards.+newBody :: CpFloat -> CpFloat -> IO Body+newBody mass inertia = do+  b <- mallocForeignPtrBytes #{size cpBody}+  withForeignPtr b $ \ptr -> do+    cpBodyInit ptr mass inertia+  return (B b)++foreign import ccall unsafe "wrapper.h"+    cpBodyInit :: BodyPtr -> CpFloat -> CpFloat -> IO ()++++type Mass = CpFloat++getMass :: Body -> IO Mass+getMass (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, m} ptr++setMass :: Body -> Mass -> IO ()+setMass (B b) m = do+  withForeignPtr b $ \ptr -> do+    cpBodySetMass ptr m++foreign import ccall unsafe "wrapper.h"+    cpBodySetMass :: BodyPtr -> Mass -> IO ()+++type Moment = CpFloat++getMoment :: Body -> IO Moment+getMoment (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, i} ptr++setMoment :: Body -> Moment -> IO ()+setMoment (B b) i = do+  withForeignPtr b $ \ptr -> do+    cpBodySetMoment ptr i++foreign import ccall unsafe "wrapper.h"+    cpBodySetMoment :: BodyPtr -> CpFloat -> IO ()++++getAngle :: Body -> IO Angle+getAngle (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, a} ptr++setAngle :: Body -> Angle -> IO ()+setAngle (B b) a = do+  withForeignPtr b $ \ptr -> do+    cpBodySetAngle ptr a++foreign import ccall unsafe "wrapper.h"+    cpBodySetAngle :: BodyPtr -> CpFloat -> IO ()++++getPosition :: Body -> IO Position+getPosition (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, p} ptr++-- | Note that using this function to change the position+--   on every step is not recommended as it may leave+--   the velocity out of sync.+setPosition :: Body -> Position -> IO ()+setPosition (B b) pos = do+  withForeignPtr b $ \ptr -> do+    #{poke cpBody, p} ptr pos+++type Velocity = Vector++getVelocity :: Body -> IO Velocity+getVelocity (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, v} ptr++setVelocity :: Body -> Velocity -> IO ()+setVelocity (B b) v = do+  withForeignPtr b $ \ptr -> do+    #{poke cpBody, v} ptr v++++type Force = Vector++getForce :: Body -> IO Force+getForce (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, f} ptr++setForce :: Body -> Force -> IO ()+setForce (B b) f = do+  withForeignPtr b $ \ptr -> do+    #{poke cpBody, f} ptr f+++type AngVel = CpFloat++getAngVel :: Body -> IO AngVel+getAngVel (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, w} ptr++setAngVel :: Body -> AngVel -> IO ()+setAngVel (B b) w = do+  withForeignPtr b $ \ptr -> do+    #{poke cpBody, w} ptr w+++type Torque = CpFloat++getTorque :: Body -> IO Torque+getTorque (B b) = do+  withForeignPtr b $ \ptr -> do+    #{peek cpBody, t} ptr++setTorque :: Body -> Torque -> IO ()+setTorque (B b) t = do+  withForeignPtr b $ \ptr -> do+    #{poke cpBody, t} ptr t+++-- | @slew b newpos dt@ changes the body @b@'s velocity+--   so that it reaches @newpos@ in @dt@ time.+--+--   It is usually used to change the position of a+--   static body in the world. In that case, remember+--   to reset the velocity to zero afterwards!+slew :: Body -> Position -> Time -> IO ()+slew (B b) newpos dt = do+  withForeignPtr b $ \ptr -> do+    p <- #{peek cpBody, p} ptr+    #{poke cpBody, v} ptr $ (newpos - p) `scale` (recip dt)+++-- | @updateVelocity b gravity damping dt@ redefines body @b@'s+--   linear and angular velocity to account for the force\/torque+--   being applied to it, the gravity and a damping factor+--   during @dt@ time using Euler integration.+--+--   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 (B b) g d dt =+  withForeignPtr b $ \b_ptr ->+  with g $ \g_ptr -> do+    wrBodyUpdateVelocity b_ptr g_ptr d dt++foreign import ccall unsafe "wrapper.h"+    wrBodyUpdateVelocity :: BodyPtr -> VectorPtr+                         -> CpFloat -> Time -> IO ()+++-- | @updatePosition b dt@ redefines the body position like+--   'updateVelocity' (and it also shouldn't be called if you+--   are adding this body to a space).+updatePosition :: Body -> Time -> IO ()+updatePosition (B b) dt = do+  withForeignPtr b $ \ptr -> do+    cpBodyUpdatePosition ptr dt++foreign import ccall unsafe "wrapper.h"+    cpBodyUpdatePosition :: BodyPtr -> Time -> IO ()+++-- | @resetForces b@ redefines as zero all forces and torque+--   acting on body @b@.+resetForces :: Body -> IO ()+resetForces b = do+  setForce b 0+  setTorque b 0+++-- | @applyForce b f r@ applies to the body @b@ the force+--   @f@ with offset @r@, both vectors in world coordinates.+--   This is the most stable way to change a body's velocity.+--+--   Note that the force is accumulated in the body, so you+--   may need to call 'applyOnlyForce'.+applyForce :: Body -> Vector -> Position -> IO ()+applyForce (B b) f p =+  withForeignPtr b $ \b_ptr ->+  with f $ \f_ptr ->+  with p $ \p_ptr -> do+    wrBodyApplyForce b_ptr f_ptr p_ptr++foreign import ccall unsafe "wrapper.h"+    wrBodyApplyForce :: BodyPtr -> VectorPtr -> VectorPtr -> IO ()+++-- | @applyOnlyForce b f r@ applies a force like 'applyForce',+--   but calling 'resetForces' before. Note that using this+--   function is preferable as it is optimized over this common+--   case.+applyOnlyForce :: Body -> Vector -> Position -> IO ()+applyOnlyForce b f p = do+  setForce b f+  setTorque b (p `cross` f)+++-- | @applyImpulse b j r@ applies to the body @b@ the impulse+--   @j@ with offset @r@, both vectors in world coordinates.+applyImpulse :: Body -> Vector -> Position -> IO ()+applyImpulse (B b) j r =+  withForeignPtr b $ \b_ptr ->+  with j $ \j_ptr ->+  with r $ \r_ptr -> do+    wrBodyApplyImpulse b_ptr j_ptr r_ptr++foreign import ccall unsafe "wrapper.h"+    wrBodyApplyImpulse :: BodyPtr -> VectorPtr -> VectorPtr -> IO ()+++-- | @dampedSpring (b1,a1) (b2,a2) rlen k dmp dt@ applies a damped+--   spring force between bodies @b1@ and @b2@ at anchors+--   @a1@ and @a2@, respectively. @k@ is the spring constant+--   (force\/distance), @rlen@ is the rest length of the spring,+--   @dmp@ is the damping constant (force\/velocity), and @dt@+--   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 =+  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++foreign import ccall unsafe "wrapper.h"+    wrDampedSpring :: BodyPtr -> BodyPtr -> VectorPtr -> VectorPtr+                   -> CpFloat -> CpFloat -> CpFloat -> Time -> IO ()+++-- | For a vector @p@ in body @b@'s coordinates,+--   @localToWorld b p@ returns the corresponding vector+--   in world coordinates.+localToWorld :: Body -> Position -> IO Position+localToWorld (B b) p =+  withForeignPtr b $ \b_ptr ->+  with p $ \p_ptr -> do+    wrBodyLocal2World b_ptr p_ptr+    peek p_ptr++foreign import ccall unsafe "wrapper.h"+    wrBodyLocal2World :: BodyPtr -> VectorPtr -> IO ()+++-- | For a vector @p@ in world coordinates,+--   @worldToLocal b p@ returns the corresponding vector+--   in body @b@'s coordinates.+worldToLocal :: Body -> Position -> IO Position+worldToLocal (B b) p =+  withForeignPtr b $ \b_ptr ->+  with p $ \p_ptr -> do+    wrBodyWorld2Local b_ptr p_ptr+    peek p_ptr++foreign import ccall unsafe "wrapper.h"+    wrBodyWorld2Local :: BodyPtr -> VectorPtr -> IO ()
+ Physics/Hipmunk/Common.hsc view
@@ -0,0 +1,290 @@+{-# 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+      Physics/Hipmunk/wrapper.c #-}++-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Common.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-- Functionality used by various modules and routines for+-- initialization and change of global variables.+--+-----------------------------------------------------------------------------++module Physics.Hipmunk.Common+    (-- * Initialization+     initChipmunk,++     -- * Basic data types+     CpFloat,+     infinity,+     Time,+     Angle,++     -- * Global variables+     -- $global_vars++     -- ** Shape counter+     -- $shape_counter+     resetShapeCounter,++     -- ** Contact persistence+     -- $contact_persistence+     getContactPersistence,+     setContactPersistence,++     -- ** Collision slop+     -- $collision_slop+     getCollisionSlop,+     setCollisionSlop,++     -- ** Bias coefficient+     -- $bias_coef+     getBiasCoef,+     setBiasCoef,++     -- ** Joint bias coefficient+     -- $joint_bias_coef+     getJointBiasCoef,+     setJointBiasCoef,++     -- * Vectors+     Vector(..),+     Position,+     fromAngle,+     len,+     normalize,+     scale,+     toAngle,+     dot,+     cross,+     perp,+     project,+     rotate,+     unrotate+    )+    where++import Foreign hiding (rotate)+#include "wrapper.h"++error' :: String -> a+error' = error . ("Physics.Hipmunk.Common: " ++)++-- | Initilizes the Chipmunk library. This should be called+--   once before using any functions of this library.+initChipmunk :: IO ()+initChipmunk = cpInitChipmunk++foreign import ccall unsafe "wrapper.h"+    cpInitChipmunk :: IO ()+++-- | The floating point type used internally in Chipmunk.+type CpFloat = #{type cpFloat}++-- | @infinity@ may be used to create bodies with+--   an infinite mass.+infinity :: CpFloat+infinity = 1e100++-- | Type synonym used to hint that the argument or result+--   represents time.+type Time = CpFloat++-- | Type synonym used to hint that the argument or result+--   represents an angle in radians.+type Angle = 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.++-- $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.+--+--   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.+resetShapeCounter :: IO ()+resetShapeCounter = cpResetShapeIdCounter++foreign import ccall unsafe "wrapper.h"+    cpResetShapeIdCounter :: IO ()+++-- $contact_persistence+--   This variable determines how long contacts should persist.+--   It should be small as the cached contacts will only be+--   close for a short time. (default is 3)++getContactPersistence :: IO #{type int}+getContactPersistence = peek cp_contact_persistence++setContactPersistence :: #{type int} -> IO ()+setContactPersistence = poke cp_contact_persistence++foreign import ccall unsafe "wrapper.h &cp_contact_persistence"+    cp_contact_persistence :: Ptr #{type int}+++-- $collision_slop+--   The collision slop is the amount that shapes are allowed to+--   penetrate. Setting this to zero will work just fine, but using a+--   small positive amount will help prevent oscillating+--   contacts. (default is 0.1)++getCollisionSlop :: IO CpFloat+getCollisionSlop = peek cp_collision_slop++setCollisionSlop :: CpFloat -> IO ()+setCollisionSlop = poke cp_collision_slop++foreign import ccall unsafe "wrapper.h &cp_collision_slop"+    cp_collision_slop :: Ptr CpFloat+++-- $bias_coef+--   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)++getBiasCoef :: IO CpFloat+getBiasCoef = peek cp_bias_coef++setBiasCoef :: CpFloat -> 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)++getJointBiasCoef :: IO CpFloat+getJointBiasCoef = peek cp_joint_bias_coef++setJointBiasCoef :: CpFloat -> IO ()+setJointBiasCoef = poke cp_joint_bias_coef++foreign import ccall unsafe "wrapper.h &cp_joint_bias_coef"+    cp_joint_bias_coef :: Ptr CpFloat++++-- | A two-dimensional vector. It is an instance of 'Num'+--   however the operations 'signum' and '(*)' are not+--   supported.+data Vector = Vector !CpFloat !CpFloat+              deriving (Eq, Show, Ord)++-- | Type synonym used to hint that the argument or result+--   represents a position.+type Position = Vector+++instance Num Vector where+    (Vector x1 y1) + (Vector x2 y2) = Vector (x1+x2) (y1+y2)+    (Vector x1 y1) - (Vector x2 y2) = Vector (x1-x2) (y1-y2)+    negate (Vector x1 y1)           = Vector (-x1) (-y1)+    abs v                           = Vector (len v) 0+    fromInteger n                   = Vector (fromInteger n) 0+    signum _ = error' "signum not supported"+    _ * _    = error' "(*) not supported"++instance Storable Vector where+    sizeOf _    = #{size cpVect}+    alignment _ = alignment (undefined :: CpFloat)+    peek ptr = do+      x <- #{peek cpVect, x} ptr+      y <- #{peek cpVect, y} ptr+      return (Vector x y)+    poke ptr (Vector x y) = do+      #{poke cpVect, x} ptr x+      #{poke cpVect, y} ptr y+++-- | Constructs an unitary vector pointing to the given+--   angle (in radians).+fromAngle :: Angle -> Vector+fromAngle theta = Vector (cos theta) (sin theta)++-- | The length of a vector.+len :: Vector -> CpFloat+len (Vector x y) = sqrt $ x*x + y*y++-- | Normalizes the vector (i.e. divides it by its length).+normalize :: Vector -> Vector+normalize v = v `scale` (recip $ len v)++-- | Scales the components of a vector by the same amount.+scale :: Vector -> CpFloat -> Vector+scale (Vector x y) s = Vector (x*s) (y*s)++-- | @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++-- | @v1 \`dot\` v2@ computes the familiar dot operation.+dot :: Vector -> Vector -> CpFloat+dot (Vector x1 y1) (Vector x2 y2) = x1*x2 + y1*y2++-- | @v1 \`cross\` v2@ computes the familiar cross operation.+cross :: Vector -> Vector -> CpFloat+cross (Vector x1 y1) (Vector x2 y2) = x1*y2 - y1*x2++-- | @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++-- | @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)++-- | @v1 \`rotate\` v2@ uses complex multiplication+--   to rotate (and scale) @v1@ by @v2@.+rotate :: Vector -> Vector -> Vector+rotate (Vector x1 y1) (Vector x2 y2) = Vector x3 y3+    where x3 = x1*x2 - y1*y2+          y3 = x1*y2 + y1*x2++-- | The inverse operation of @rotate@, such that+--   @unrotate (rotate v1 v2) v2@ equals @v1@.+unrotate :: Vector -> Vector -> Vector+unrotate (Vector x1 y1) (Vector x2 y2) = Vector x3 y3+    where x3 = x1*x2 + y1*y2+          y3 = y1*x2 - x1*y2
+ Physics/Hipmunk/Internal.hsc view
@@ -0,0 +1,195 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Internal.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-----------------------------------------------------------------------------++module Physics.Hipmunk.Internal+    (VectorPtr,++     BodyPtr,+     Body(..),+     unB,++     ShapePtr,+     Shape(..),+     unS,++     JointPtr,+     Joint(..),+     unJ,++     SpacePtr,+     Space(..),+     unP,++     Contact(..),+     ContactPtr+    )+    where++import Data.IORef+import Data.Map (Map)+import Foreign+#include "wrapper.h"++import Physics.Hipmunk.Common+++type VectorPtr = Ptr Vector++++-- | 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.+newtype Body = B (ForeignPtr Body)+type BodyPtr = Ptr Body++unB :: Body -> ForeignPtr Body+unB (B b) = b++instance Eq Body where+    B b1 == B b2 = b1 == b2++instance Ord Body where+    B b1 `compare` B b2 = b1 `compare` b2++++-- | 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).+--+--   Note that to have any effect, a 'Shape' must also be+--   added to a 'Space', even if the body was already added.+data Shape = S !(ForeignPtr Shape) !Body+type ShapePtr = Ptr Shape++-- Note also that we have to maintain a reference to the+-- 'Body' to avoid garbage collection in the case that+-- the user doesn't add the body to a space and don't keep+-- a reference (common when adding bodies with infinite mass).+--+-- However, the body doesn't need to keep references to+-- the attached shapes because cpBody do not reference them,+-- so it wouldn't notice at all if they disappeared =).+-- A space would notice, but then the space will keep its+-- own reference the the shape.++unS :: Shape -> ForeignPtr Shape+unS (S s _) = s++instance Eq Shape where+    S s1 _ == S s2 _ = s1 == s2++instance Ord Shape where+    S s1 _ `compare` S s2 _ = s1 `compare` s2++++-- | 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++unJ :: Joint -> ForeignPtr Joint+unJ (J j _ _) = j++instance Eq Joint where+    J j1 _ _ == J j2 _ _ = j1 == j2++instance Ord Joint where+    J j1 _ _ `compare` J j2 _ _ = j1 `compare` j2++++-- | A space is where the simulation really occurs. You add+--   bodies, shapes and joints to a space and then step it+--   to update it as whole.+data Space = P !(ForeignPtr Space)+               !(IORef Entities)   -- Active and static entities+               !(IORef Callbacks)  -- Added callbacks+type SpacePtr  = Ptr Space+type Entities  = Map (Ptr ()) (Either (ForeignPtr ()) Shape)+type Callbacks = (Maybe (FunPtr ()), -- Default+                  Map (#{type unsigned int}, #{type unsigned int})+                      (FunPtr ()))++unP :: Space -> ForeignPtr Space+unP (P sp _ _) = sp++instance Eq Space where+    P s1 _ _ == P s2 _ _ = s1 == s2++instance Ord Space where+    P s1 _ _ `compare` P s2 _ _ = s1 `compare` s2++++-- 'Contact's are an exception to the pattern we've been following+-- as we're going to use StorableArray with them, so we need+-- them to be Storable (like Vector).++-- | A 'Contact' contains information about a collision.+--   It is passed to 'Physics.Hipmunk.Space.Full'.+--+--   The fields 'ctJnAcc' and 'ctJtAcc' do not have any meaningfull+--   value until 'Physics.Hipmunk.Space.step' has returned+--   (i.e. during a call to a callback this information+--   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.+data Contact = Contact {+      ctPos    :: Position,+      -- ^ Position of the collision in world's coordinates.++      ctNormal :: Vector,+      -- ^ Normal of the collision.++      ctDist   :: CpFloat,+      -- ^ Penetration distance of the collision.++      ctJnAcc  :: CpFloat,+      -- ^ Normal component of final impulse applied.+      --   (Valid only after @step@ finishes.)++      ctJtAcc  :: CpFloat+      -- ^ Tangential component of final impulse applied.+      --   (Valid only after @step@ finishes.)+    }+               deriving (Eq, Ord, Show)++type ContactPtr = Ptr Contact++instance Storable Contact where+    sizeOf _    = #{size cpContact}+    alignment _ = alignment (undefined :: Vector)+    peek ptr    = do+      p     <- #{peek cpContact, p} ptr+      n     <- #{peek cpContact, n} ptr+      dist  <- #{peek cpContact, dist} ptr+      jnAcc <- #{peek cpContact, jnAcc} ptr+      jtAcc <- #{peek cpContact, jtAcc} ptr+      return $ Contact {ctPos    = p+                       ,ctNormal = n+                       ,ctDist   = dist+                       ,ctJnAcc  = jnAcc+                       ,ctJtAcc  = jtAcc}+    poke ptr c = do+      #{poke cpContact, p} ptr (ctPos c)+      #{poke cpContact, n} ptr (ctNormal c)+      #{poke cpContact, dist} ptr (ctDist c)+      #{poke cpContact, jnAcc} ptr (ctJnAcc c)+      #{poke cpContact, jtAcc} ptr (ctJtAcc c)
+ Physics/Hipmunk/Joint.hsc view
@@ -0,0 +1,113 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Joint.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- 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 ()+
+ Physics/Hipmunk/Shape.hsc view
@@ -0,0 +1,672 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Shape.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-- Shapes used for collisions, their properties and some useful+-- polygon functions.+--+-----------------------------------------------------------------------------++module Physics.Hipmunk.Shape+    (-- * Shapes+     Shape,+     ShapeType(..),+     newShape,++     -- * Properties+     -- ** Collision type+     CollisionType,+     getCollisionType,+     setCollisionType,+     -- ** Group+     Group,+     getGroup,+     setGroup,+     -- ** Layers+     Layers,+     getLayers,+     setLayers,+     -- ** Elasticity+     Elasticity,+     getElasticity,+     setElasticity,+     -- ** Friction+     Friction,+     getFriction,+     setFriction,+     -- ** Surface velocity+     SurfaceVel,+     getSurfaceVel,+     setSurfaceVel,++     -- * Utilities+     getBody,+     momentForCircle,+     momentForPoly,+     shapeQuery,++     -- ** For polygons+     -- $polygon_util+     Segment,+     Intersection(..),+     epsilon,+     (.==.),+     isLeft,+     isClockwise,+     isConvex,+     intersects,+     polyReduce,+     polyCenter,+     convexHull+    )+    where++import Data.List (foldl', sortBy)+import Foreign hiding (rotate, new)+import Foreign.C+#include "wrapper.h"++import Physics.Hipmunk.Common+import Physics.Hipmunk.Internal++-- | 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}++    -- | A line segment is meant to be used as a static+    --   shape. (It can be used with moving bodies, however+    --   two line segments never generate collisions between+    --   each other.)+  | LineSegment {start     :: !Position,+                 end       :: !Position,+                 thickness :: !CpFloat}++    -- | Polygons are the slowest of all shapes but+    --   the most flexible. The list of vertices must form+    --   a convex hull with clockwise winding.+    --   Note that if you want a non-convex polygon you may+    --   add several convex polygons to the body.+  | Polygon {vertices :: ![Position]}+    deriving (Eq, Ord, Show)+++-- | @newShape b type off@ creates a new shape attached to+--   body @b@ at offset @off@. Note that you have to+--   add the shape to a space otherwise it won't generate+--   collisions.+newShape :: Body -> ShapeType -> Position -> IO Shape+newShape body@(B b) (Circle r) off =+  withForeignPtr b $ \b_ptr ->+  with off $ \off_ptr ->+  mallocForeignPtrBytes #{size cpCircleShape} >>= \shape ->+  withForeignPtr shape $ \shape_ptr -> do+    wrCircleShapeInit shape_ptr b_ptr off_ptr r+    return (S shape body)++newShape body@(B b) (LineSegment p1 p2 r) off =+  withForeignPtr b $ \b_ptr ->+  with (p1+off) $ \p1off_ptr ->+  with (p2+off) $ \p2off_ptr ->+  mallocForeignPtrBytes #{size cpSegmentShape} >>= \shape ->+  withForeignPtr shape $ \shape_ptr -> do+    wrSegmentShapeInit shape_ptr b_ptr p1off_ptr p2off_ptr r+    return (S shape body)++newShape body@(B b) (Polygon verts) off =+  withForeignPtr b $ \b_ptr ->+  with off $ \off_ptr ->+  withArrayLen verts $ \verts_len verts_ptr ->+  mallocForeignPtrBytes #{size cpPolyShape} >>= \shape ->+  withForeignPtr shape $ \shape_ptr -> do+    let verts_len' = fromIntegral verts_len+    wrPolyShapeInit shape_ptr b_ptr verts_len' verts_ptr off_ptr+    addForeignPtrFinalizer cpShapeDestroy shape+    return (S shape body)++foreign import ccall unsafe "wrapper.h"+    wrCircleShapeInit :: ShapePtr -> BodyPtr -> VectorPtr+                      -> CpFloat -> IO ()+foreign import ccall unsafe "wrapper.h"+    wrSegmentShapeInit :: ShapePtr -> BodyPtr -> VectorPtr+                       -> VectorPtr -> CpFloat -> IO ()+foreign import ccall unsafe "wrapper.h"+    wrPolyShapeInit :: ShapePtr -> BodyPtr -> CInt -> VectorPtr+                    -> VectorPtr -> IO ()+foreign import ccall unsafe "wrapper.h &cpShapeDestroy"+    cpShapeDestroy :: FunPtr (ShapePtr -> IO ())+++-- | @getBody s@ is the body that this shape is associated+--   to. Useful especially in 'Physics.Hipmunk.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}+getCollisionType :: Shape -> IO CollisionType+getCollisionType (S shape _) =+  withForeignPtr shape #{peek cpShape, collision_type}+setCollisionType :: Shape -> CollisionType -> IO ()+setCollisionType (S shape _) col =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, collision_type} shape_ptr col++-- | Groups are used to filter collisions between shapes. If+--   the group is zero, then it imposes no restriction+--   to the collisions. However, if the group is non-zero then+--   the shape will not collide with other shapes in the same+--   non-zero group. (default is zero)+--+--   This is primarely used to create multi-body, multi-shape+--   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}+getGroup :: Shape -> IO Group+getGroup (S shape _) =+  withForeignPtr shape #{peek cpShape, group}+setGroup :: Shape -> Group -> IO ()+setGroup (S shape _) gr =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, group} shape_ptr gr++-- | 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@)+--+--   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}+getLayers :: Shape -> IO Layers+getLayers (S shape _) =+  withForeignPtr shape #{peek cpShape, layers}+setLayers :: Shape -> Layers -> IO ()+setLayers (S shape _) lay =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, layers} shape_ptr lay++-- | The elasticity of the shape is such that @0.0@ gives no bounce+--   while @1.0@ give a \"perfect\" bounce. Note that due to+--   inaccuracies using @1.0@ or greater is not recommended.+--+--   The amount of elasticity applied during a collision is+--   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@.+type Elasticity = CpFloat+getElasticity :: Shape -> IO Elasticity+getElasticity (S shape _) =+  withForeignPtr shape #{peek cpShape, e}+setElasticity :: Shape -> Elasticity -> IO ()+setElasticity (S shape _) e =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, e} shape_ptr e++-- | The friction coefficient of the shape according+--   to Coulumb friction model (i.e. @0.0@ is frictionless,+--   iron on iron is around @1.0@, and it could be greater+--   then @1.0@).+--+--   The amount of friction applied during a collision is+--   determined by multiplying the friction coefficient+--   of both shapes. (default is zero)+type Friction = CpFloat+getFriction :: Shape -> IO Friction+getFriction (S shape _) =+  withForeignPtr shape #{peek cpShape, u}+setFriction :: Shape -> Friction -> IO ()+setFriction (S shape _) u =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, u} shape_ptr u++-- | The surface velocity of the shape. Useful to create+--   conveyor belts and players that move around. This+--   value is only used when calculating friction, not+--   collision. (default is zero)+type SurfaceVel = Vector+getSurfaceVel :: Shape -> IO SurfaceVel+getSurfaceVel (S shape _) =+  withForeignPtr shape #{peek cpShape, surface_v}+setSurfaceVel :: Shape -> SurfaceVel -> IO ()+setSurfaceVel (S shape _) sv =+  withForeignPtr shape $ \shape_ptr -> do+    #{poke cpShape, surface_v} shape_ptr sv+++++-- | @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 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.+++-- | @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+--   apply as well).+momentForPoly :: CpFloat -> [Position] -> Position -> CpFloat+momentForPoly m verts off = (m*sum1)/(6*sum2)+  where+    verts' = if off /= 0 then map (+off) verts else verts+    (sum1,sum2) = calc (pairs (,) verts') 0 0++    calc a b c | a `seq` b `seq` c `seq` False = undefined+    calc []           acc1 acc2 = (acc1, acc2)+    calc ((v1,v2):vs) acc1 acc2 =+      let a = v2 `cross` v1+          b = v1 `dot` v1 + v1 `dot` v2 + v2 `dot` v2+      in calc vs (acc1 + a*b) (acc2 + a)+-- We recoded the C function to avoid FFI, unsafePerformIO+-- and a bunch of malloc + poke. Is it worth?++-- | Internal. For @l = [x1,x2,...,xn]@, @pairs f l@ is+--   @[f x1 x2, f x2 x3, ...,f xn x1]@.+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 =+  withForeignPtr shape $ \shape_ptr ->+  with p $ \p_ptr -> do+    i <- wrShapePointQuery shape_ptr p_ptr+    return (i /= 0)++foreign import ccall unsafe "wrapper.h"+    wrShapePointQuery :: ShapePtr -> VectorPtr -> IO CInt++++-- $polygon_util+--   This section is inspired by @pymunk.util@,+--   a Python module made from <http://code.google.com/p/pymunk/>,+--   although implementations are quite different.+--+--   Also, unless noted otherwise all polygons are+--   assumed to be simple (i.e. no overlapping edges).++-- | The epsilon used in the algorithms below when necessary+--   to compare floats for \"equality\".+epsilon :: CpFloat+epsilon = 1e-25++-- | \"Equality\" under 'epsilon'. That is, @a .==. b@+--   if @abs (a - b) <= epsilon@.+(.==.) :: CpFloat -> CpFloat -> Bool+a .==. b = abs (a - b) <= epsilon++-- | A line segment.+type Segment = (Position, Position)++-- | /O(n)/. @isClockwise verts@ is @True@ iff @verts@ form+--   a clockwise polygon.+isClockwise :: [Position] -> Bool+isClockwise = (<= 0) . foldl' (+) 0 . pairs cross++-- | @isLeft (p1,p2) vert@ is+--+--    * @LT@ if @vert@ is at the left of the line defined by @(p1,p2)@.+--+--    * @EQ@ if @vert@ is at the line @(p1,p2)@.+--+--    * @GT@ otherwise.+isLeft :: (Position, Position) -> Position -> Ordering+isLeft (p1,p2) vert = compare 0 $ (p1 - vert) `cross` (p2 - vert)++-- | /O(n)/. @isConvex verts@ is @True@ iff @vers@ form a convex+--   polygon.+isConvex :: [Position] -> Bool+isConvex = foldl1 (==) . map (0 <) . filter (0 /=) . pairs cross . pairs (-)+-- From http://apocalisp.wordpress.com/category/programming/haskell/page/2/++-- | /O(1)/. @intersects seg1 seg2@ is the intersection between+--   the two segments @seg1@ and @seg2@. See 'Intersection'.+intersects :: Segment -> Segment -> Intersection+intersects (a0,a1) (b0,b1) =+    let u                = a1 - a0+        v@(Vector vx vy) = b1 - b0+        w@(Vector wx wy) = a0 - b0+        d = u `cross` v+        parallel = d .==. 0++        -- Parallel case+        collinear = all (.==. 0) [u `cross` w, v `cross` w]+        a_is_point = u `dot` u .==. 0+        b_is_point = v `dot` v .==. 0+        (Vector w2x w2y) = a1 - b0+        (a_in_b, a_in_b') = if vx .==. 0+                             then swap (wy/vy, w2y/vy)+                             else swap (wx/vx, w2x/vx)+            where swap t@(x,y) | x < y     = t+                               | otherwise = (y,x)++        -- Non-parallel case+        sI = v `cross` w / d+        tI = u `cross` w / d++        -- Auxiliary functions+        inSegment p (c0,c1)+            | vertical  = test (gy p) (gy c0, gy c1)+            | otherwise = test (gx p) (gx c0, gx c1)+            where+              vertical = gx c0 .==. gx c1+              (gx, gy) = (\(Vector x _) -> x, \(Vector _ y) -> y)+              test q (d0,d1) = any (inside q) [(d0,d1), (d1,d0)]+        inside n (l,r) = l <= n && n <= r++    in if parallel+       then case (collinear, a_is_point, b_is_point) of+             (False, _, _) ->+                 -- Parallel and non-collinear+                 IntNowhere++             (_, False, False) ->+                 -- Both are parallel, collinear segments+                 case (a_in_b > 1 || a_in_b' < 0,+                       max a_in_b 0, min a_in_b' 1) of+                   (True, _, _) -> IntNowhere+                   (_, i0, i1)+                       | i0 .==. i1 -> IntPoint p0+                       | otherwise  -> IntSegmt (p0,p1)+                       where p0 = b0 + v `scale` i0+                             p1 = b0 + v `scale` i1++             (_, True, True) ->+                 -- Both are points+                 if len (b0-a0) .==. 0+                 then IntPoint a0 else IntNowhere++             _ ->+                 -- One is a point, another is a segment+                 let (point,segment)+                         | a_is_point = (a0, (b0,b1))+                         | otherwise  = (b0, (a0,a1))+                 in if inSegment point segment+                    then IntPoint point else IntNowhere++       else if all (\x -> inside x (0,1)) [sI, tI]+            then IntPoint (a0 + u `scale` sI) else IntNowhere++-- | A possible intersection between two segments.+data Intersection = IntNowhere         -- ^ Don't intercept.+                  | IntPoint !Position -- ^ Intercept in a point.+                  | IntSegmt !Segment  -- ^ Share a segment.+                    deriving (Eq, Ord, Show)+++-- | /O(n)/. @polyReduce delta verts@ removes from @verts@ all+--   points that have less than @delta@ distance+--   in relation to the one preceding it.+--+--   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 delta = go+    where+      go (p1:p2:ps) | len (p2-p1) < delta = go (p1:ps)+                    | otherwise           = p1 : go (p2:ps)+      go other = other++-- | /O(n)/. @polyCenter verts@ is the position in the center+--   of the polygon formed by @verts@.+polyCenter :: [Position] -> Position+polyCenter verts = foldl' (+) 0 verts `scale` s+    where s = recip $ toEnum $ length verts+++-- | /O(n log n)/. @convexHull verts@ is the convex hull of the+--   polygon defined by @verts@. The vertices of the convex+--   hulls are given in clockwise winding. The polygon+--   doesn't have to be simple.+--+--   Implemented using Graham scan, see+--   <http://cgm.cs.mcgill.ca/~beezer/cs507/3coins.html>.+convexHull :: [Position] -> [Position]+convexHull verts =+  let (p0,ps) = takeMinimum verts+      (_:p1:points) = p0 : sortBy (isLeft . (,) p0) ps++      -- points is going counterclockwise now.+      -- In go we use 'hull' with the last added+      -- vertex as the head, so our result is clockwise.++      -- Remove right turns+      go hull@(h1:h2:hs) (q1:qs) =+          case (isLeft (h2,h1) q1, hs) of+            (LT,_) -> go (q1:hull) qs    -- Left turn+            (_,[]) -> go (q1:hull) qs    -- Maintain at least 2 points+            _      -> go (h2:hs) (q1:qs) -- Right turn or straight+      go hull [] = hull+      go _ _     = error "Physics.Hipmunk.Shape.convexHull: never get here"++  in go [p1,p0] points+++-- | Internal. Works like minimum but also returns the+--   list without it. The order of the list may be changed.+--   We have @fst (takeMinimum xs) == minimum xs@ and+--   @sort (uncurry (:) $ takeMinimum xs) == sort xs@+takeMinimum :: Ord a => [a] -> (a, [a])+takeMinimum [] = error "Physics.Hipmunk.Shape.takeMinimum: empty list"+takeMinimum (x:xs) = go x [] xs+    where+      go min_ acc (y:ys) | y < min_  = go y (min_:acc) ys+                         | 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+--   <http://geometryalgorithms.com/Archive/algorithm_0108/algorithm_0108.htm>+--   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 ---------++-}
+ Physics/Hipmunk/Space.hsc view
@@ -0,0 +1,661 @@+-----------------------------------------------------------------------------+-- |+-- Module      :  Physics/Hipmunk/Space.hsc+-- Copyright   :  (c) Felipe A. Lessa 2008+-- License     :  MIT (see LICENSE)+--+-- Maintainer  :  felipe.lessa@gmail.com+-- Stability   :  beta+-- Portability :  portable (needs FFI)+--+-- The space, where the simulation happens and the various entities+-- interact.+--+-----------------------------------------------------------------------------++module Physics.Hipmunk.Space+    (-- * Callbacks problem+     -- $callbacksProblem++     -- * Creating spaces and adding entities+     Space,+     newSpace,+     freeSpace,+     Entity(..),+     StaticShape(..),++     -- * Properties+     -- ** Iterations+     Iterations,+     getIterations,+     setIterations,+     -- ** Elastic iterations+     ElasticIterations,+     getElasticIterations,+     setElasticIterations,+     -- ** Gravity+     Gravity,+     getGravity,+     setGravity,+     -- ** Damping+     Damping,+     getDamping,+     setDamping,+     -- ** Time stamp+     TimeStamp,+     getTimeStamp,++     -- * Spatial hashes+     -- $resizing+     resizeStaticHash,+     resizeActiveHash,+     rehashStatic,+     -- ** Point query+     -- $point_query+     QueryType(..),+     spaceQuery,+     spaceQueryList,++     -- * Stepping+     step,++     -- ** Collision pair functions+     -- $callbacks+     Callback(..),+     setDefaultCallback,+     addCallback,+     removeCallback,++     -- ** Contacts+     Contact(..),+     sumImpulses,+     sumImpulsesWithFriction,+    )+    where++import Control.Exception (bracket)+import Data.Array.Storable+import Data.IORef+import qualified Data.Map as M+import Foreign hiding (new)+#include "wrapper.h"++import Physics.Hipmunk.Common+import Physics.Hipmunk.Internal+import Physics.Hipmunk.Shape+++-- $callbacksProblem+--   We have a huge problem for callbacks: we *have* to call+--   'freeHaskellFunPtr' to every Haskell function that was+--   passed via FFI to C code after we don't need them.+--   However, the 'ForeignPtr' that the 'Space' has can+--   portably have finalizers only in the FFI, never in the+--   Haskell land, so we can't run the Haskell function+--   'freeHaskellFunPtr' from a 'ForeignPtr' finalizer.+--+--   There are two options:+--+--     1. Use "Foreign.Concurrent" to add a Haskell finalizer.+--        Under GHC this is great and adds no overhead (maybe there's+--        even less overhead than calling a C function).+--        However "Foreign.Concurrent" is not portable and+--        works only under GHC.+--+--     2. Require that users of the library (you) call+--        a finalizer function when they plan to stop using+--        the space. This adds some burden to the programmer+--        and somehow defeats the purpose of the GC, however+--        it works everywhere.+--+--   As this is a library that intends to be as portable as+--   possible (like Chipmunk itself), of course I chose+--   to follow the second path. This means that your code will+--   run unchanged on every Haskell environment supporting+--   FFI with C99, but also that you have to take care to+--   avoid memory leaks. You've been warned! :)+++-- | Creates a new, empty space.+--   Some of the memory resources associated with the space+--   must be manually freed through 'freeSpace' when the+--   'Space' is no longer necessary.+newSpace :: IO Space+newSpace =+  mallocForeignPtrBytes #{size cpSpace} >>= \sp ->+  withForeignPtr sp $ \sp_ptr -> do+    cpSpaceInit sp_ptr+    addForeignPtrFinalizer cpSpaceDestroy sp+    entities  <- newIORef M.empty+    callbacks <- newIORef (Nothing, M.empty)+    return (P sp entities callbacks)++foreign import ccall unsafe "wrapper.h"+    cpSpaceInit :: SpacePtr -> IO ()+foreign import ccall unsafe "wrapper.h &cpSpaceDestroy"+    cpSpaceDestroy :: FunPtr (SpacePtr -> IO ())+++-- | @freeSpace sp@ frees some memory resources that can't+--   be automatically deallocated in a portable way.+--   The space @sp@ then becomes invalid and should+--   not be used (passing @sp@ to any other function,+--   including 'freeSpace', results in undefined behavior).+freeSpace :: Space -> IO ()+freeSpace (P _ entities callbacks) = do+  -- The only things we *have* to free are the callbacks,+  -- but we'll release all the IORef contents as well.+  let err :: a+      err = error "Physics.Hipmunk.Space: freeSpace already called here."+  writeIORef entities err+  (def,cbs) <- readIORef callbacks+  writeIORef callbacks err+  maybe (return ()) freeHaskellFunPtr def+  M.fold ((>>) . freeHaskellFunPtr) (return ()) cbs+++-- | Type class implemented by entities that can be+--   added to a space.+class Entity a where+    -- | Add an entity to a 'Space'. Don't add the same+    --   entity twice to a space.+    spaceAdd :: Space -> a -> IO ()+    -- | Remove an entity from a 'Space'. Don't remove+    --   an entity that wasn't add.+    spaceRemove :: Space -> a -> IO ()++spaceAddHelper :: (a -> ForeignPtr b)+               -> (SpacePtr -> Ptr b -> IO ())+               -> (a -> Maybe Shape)+               -> (Space -> a -> IO ())+spaceAddHelper get add toShape =+    \(P sp entities _) new_c ->+        let new  = get new_c+            key  = unsafeForeignPtrToPtr $ castForeignPtr new+            val  = case toShape new_c of+                     Just shape -> Right shape+                     Nothing    -> Left (castForeignPtr new)+        in withForeignPtr sp $ \sp_ptr ->+           withForeignPtr new $ \new_ptr -> do+             add sp_ptr new_ptr+             modifyIORef entities (M.insert key val)++spaceRemoveHelper :: (a -> ForeignPtr b)+                  -> (SpacePtr -> Ptr b -> IO ())+                  -> (Space -> a -> IO ())+spaceRemoveHelper get remove =+    \(P sp entities _) old_c -> do+      let old  = get old_c+          key  = unsafeForeignPtrToPtr $ castForeignPtr old+      modifyIORef entities (M.delete key)+      withForeignPtr sp $ \sp_ptr ->+        withForeignPtr old $ \old_ptr ->+          remove sp_ptr old_ptr++instance Entity Body where+    spaceAdd    = spaceAddHelper    unB cpSpaceAddBody (const Nothing)+    spaceRemove = spaceRemoveHelper unB cpSpaceRemoveBody+foreign import ccall unsafe "wrapper.h"+    cpSpaceAddBody :: SpacePtr -> BodyPtr -> IO ()+foreign import ccall unsafe "wrapper.h"+    cpSpaceRemoveBody :: SpacePtr -> BodyPtr -> IO ()++instance Entity Shape where+    spaceAdd    = spaceAddHelper    unS cpSpaceAddShape Just+    spaceRemove = spaceRemoveHelper unS cpSpaceRemoveShape+foreign import ccall unsafe "wrapper.h"+    cpSpaceAddShape :: SpacePtr -> ShapePtr -> IO ()+foreign import ccall unsafe "wrapper.h"+    cpSpaceRemoveShape :: SpacePtr -> ShapePtr -> IO ()++instance Entity Joint where+    spaceAdd    = spaceAddHelper    unJ cpSpaceAddJoint (const Nothing)+    spaceRemove = spaceRemoveHelper unJ cpSpaceRemoveJoint+foreign import ccall unsafe "wrapper.h"+    cpSpaceAddJoint :: SpacePtr -> JointPtr -> IO ()+foreign import ccall unsafe "wrapper.h"+    cpSpaceRemoveJoint :: SpacePtr -> JointPtr -> IO ()+++-- | A 'StaticShape' is a 'Shape' container that, when added+--   to a space via 'spaceAdd', is added to the static+--   list of shapes.+--+--   A static shape is one assumed not to move. If you move+--   a static shape after adding it, then you need to 'rehashStatic'.+--+--   You should not add the same shape as active and static,+--   nor should you add as active and try to remove as+--   static or vice versa.+newtype StaticShape = Static {unStatic :: Shape}++instance Entity StaticShape where+    spaceAdd    = spaceAddHelper    (unS . unStatic) cpSpaceAddStaticShape (Just . unStatic)+    spaceRemove = spaceRemoveHelper (unS . unStatic) cpSpaceRemoveStaticShape+foreign import ccall unsafe "wrapper.h"+    cpSpaceAddStaticShape :: SpacePtr -> ShapePtr -> IO ()+foreign import ccall unsafe "wrapper.h"+    cpSpaceRemoveStaticShape :: SpacePtr -> ShapePtr -> IO ()++++++-- | The number of iterations to use when solving constraints.+--   (default is 10).+type Iterations = #{type int}+getIterations :: Space -> IO Iterations+getIterations (P sp _ _) =+    withForeignPtr sp #{peek cpSpace, iterations}+setIterations :: Space -> Iterations -> IO ()+setIterations (P sp _ _) it =+    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}+getElasticIterations :: Space -> IO ElasticIterations+getElasticIterations (P sp _ _) =+    withForeignPtr sp #{peek cpSpace, elasticIterations}+setElasticIterations :: Space -> ElasticIterations -> IO ()+setElasticIterations (P sp _ _) it =+    withForeignPtr sp $ \sp_ptr -> do+      #{poke cpSpace, elasticIterations} sp_ptr it++-- | The gravity applied to the system. (default is 0)+type Gravity = Vector+getGravity :: Space -> IO Gravity+getGravity (P sp _ _) =+    withForeignPtr sp #{peek cpSpace, gravity}+setGravity :: Space -> Gravity -> IO ()+setGravity (P sp _ _) g =+    withForeignPtr sp $ \sp_ptr -> do+      #{poke cpSpace, gravity} sp_ptr g++-- | 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}+setDamping :: Space -> Damping -> IO ()+setDamping (P sp _ _) dm =+    withForeignPtr sp $ \sp_ptr -> do+      #{poke cpSpace, damping} sp_ptr dm++-- | The time stamp of the simulation, increased in 1+--   every time 'step' is called.+type TimeStamp = #{type int}+getTimeStamp :: Space -> IO TimeStamp+getTimeStamp (P sp _ _) =+    withForeignPtr sp #{peek cpSpace, stamp}++++++-- $resizing+--   @'resizeStaticHash' sp dim count@ resizes the static+--   hash of space @sp@ to have hash cells of size @dim@+--   and suggested minimum number of cells @count@.+--   @'resizeActiveHash' sp dim count@ works the same way+--   but modifying the active hash of the space.+--+--   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@.+--+--   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 (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 ()++resizeActiveHash :: Space -> CpFloat -> #{type int} -> 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 ()++-- | Rehashes the shapes in the static spatial hash.+--   You only need to call this if you move one of the+--   static shapes.+rehashStatic :: Space -> IO ()+rehashStatic (P sp _ _) =+    withForeignPtr sp cpSpaceRehashStatic++foreign import ccall unsafe "wrapper.h"+    cpSpaceRehashStatic :: SpacePtr -> IO ()+++++-- $point_query+--   Point querying uses the spatial hashes to find out+--   in what shapes a point is contained. It is useful,+--   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+--   shape that+--+--   * Contains point @pos@ (in world's coordinates).+--+--   * Is in the hash selected by @query@ (see 'QueryType').+--+--   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 =+  withForeignPtr sp $ \sp_ptr ->+  bracket (makePointQueryFunc cb) freeHaskellFunPtr $ \cb_ptr ->+  with pos $ \pos_ptr ->+    func sp_ptr pos_ptr 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 ()+type PointQueryFuncPtr = FunPtr PointQueryFunc+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 ()+++-- | @spaceQueryList sp query pos@ 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+  var <- newIORef []+  spaceQuery spce query pos $ modifyIORef var . (:)+  readIORef var+++-- | @step sp dt@ will update the space @sp@ for a @dt@ time+--   step.+--+--   It is highly recommended to use a fixed @dt@ to increase+--   the efficiency of contact persistence. Some tips may be+--   found in <http://www.gaffer.org/game-physics/fix-your-timestep>.+step :: Space -> Time -> IO ()+step (P sp _ _) dt =+  withForeignPtr sp $ \sp_ptr -> do+    cpSpaceStep sp_ptr dt++-- IMPORTANT! This call can (and probably will) callback into Haskell.+foreign import ccall {- !!! -} safe {- !!! -}+    cpSpaceStep :: SpacePtr -> Time -> IO ()+++++-- $callbacks+--   A collision pair function is a callback triggered by 'step'+--   in response to certain collision events. Its return value+--   will determine whether or not the collision will be processed.+--   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+--   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).+++-- | A 'Callback' function can be of three types:+--+--   * A 'Full' callback has access to all parameters passed+--     by Chipmunk, but it is common not to need all of them.+--     The two colliding 'Shape's are passed as arguments with+--     a 'Contact' array and a normal coefficient (this coefficient+--     should be multiplied to the contacts' normals as+--     Chipmunk may have reversed the argument order). See 'Contact'+--     for more information.+--+--   * A 'Basic' callback can't access the 'Contact' information,+--     but incurs a lower overhead per call.+--+--   * A 'Constant' callback always accepts or reject the collision.+--     For example, a @Constant False@ will never accept any+--     collision.+--+--   Although 'Basic' and 'Constant' can be implemented+--   in terms of 'Full', they're optimized to incur less overhead.+--   So try to use the simplest callback type+--   (e.g. @Constant False@ instead of @Basic (\_ _ -> return False)@).+data Callback = Full (Shape -> Shape -> StorableArray Int Contact+                      -> CpFloat -> IO Bool)+              | Basic (Shape -> Shape -> IO Bool)+              | Constant !Bool+++-- | Internal. Type of callback used by Chipmunk.+type ChipmunkCB = ShapePtr -> ShapePtr -> ContactPtr -> #{type int}+                -> CpFloat -> Ptr () -> IO Int+type ChipmunkCBPtr = FunPtr ChipmunkCB+++-- | Internal. Constructs a 'ChipmunkCB' from a 'Callback',+--   returning also the contents of the @data@ pointer.+adaptChipmunkCB :: Space -> Callback+                -> IO (ChipmunkCBPtr, Ptr (), Maybe (FunPtr ()))+adaptChipmunkCB _ (Constant bool) =+  let data_ = intPtrToPtr (if bool then 1 else 0)+  in return (wrConstantCallback, data_, Nothing)+adaptChipmunkCB spce (Basic basic) = makeChipmunkCB' $+  \ptr1 ptr2 _ _ _ _ -> do+    shape1 <- retriveShape spce ptr1+    shape2 <- retriveShape spce ptr2+    okay <- basic shape1 shape2+    return (if okay then 1 else 0)+adaptChipmunkCB spce (Full full) = makeChipmunkCB' $+  \ptr1 ptr2 cont_ptr cont_num normal_coef _ -> do+    shape1 <- retriveShape spce ptr1+    shape2 <- retriveShape spce ptr2++    -- Wrap the pointer in an array. Note that the memory+    -- is managed by Chipmunk, so we don't have finalizers.+    cont_fptr <- newForeignPtr_ cont_ptr+    let bounds = (0, fromIntegral $ cont_num-1)+    array <- unsafeForeignPtrToStorableArray cont_fptr bounds++    okay <- full shape1 shape2 array normal_coef+    return (if okay then 1 else 0)++makeChipmunkCB' :: ChipmunkCB+                -> IO (ChipmunkCBPtr, Ptr (), Maybe (FunPtr ()))+makeChipmunkCB' f = do+  f' <- makeChipmunkCB f+  return (f', nullPtr, Just $ castFunPtr f')++foreign import ccall "wrapper"+    makeChipmunkCB :: ChipmunkCB -> IO ChipmunkCBPtr++foreign import ccall unsafe "wrapper.h &wrConstantCallback"+    wrConstantCallback :: ChipmunkCBPtr++-- | Internal. Retrive a 'Shape' from a 'ShapePtr' and a 'Space'.+retriveShape :: Space -> ShapePtr -> IO Shape+retriveShape (P _ entities _) ptr = do+  ent <- readIORef entities+  Right shape <- M.lookup (castPtr ptr) ent+  return shape+++-- | Defines a new default collision pair function.+--   This callback is called whenever two shapes @a@+--   and @b@ collide such that no other collision+--   pair function was defined to @a@'s and @b@'s+--   collision types. The default is @Constant True@.+setDefaultCallback :: Space -> Callback -> IO ()+setDefaultCallback spce@(P sp _ callbacks) func = do+  -- Find out whats our new function details+  -- (NULL for default means @Constant True@, optimize it)+  (cb,data_,hask) <-+      case func of+        Constant True -> return (nullFunPtr, nullPtr, Nothing)+        _             -> adaptChipmunkCB spce func++  -- Free the previous one+  (def,cbs) <- readIORef callbacks+  case def of+    Nothing  -> return ()+    Just ptr -> freeHaskellFunPtr ptr++  -- Define the new+  writeIORef callbacks (hask,cbs)+  withForeignPtr sp $ \sp_ptr -> do+    cpSpaceSetDefaultCollisionPairFunc sp_ptr cb data_++foreign import ccall unsafe "wrapper.h"+    cpSpaceSetDefaultCollisionPairFunc+        :: SpacePtr -> ChipmunkCBPtr -> Ptr () -> IO ()+++-- | @addCallback sp (cta,ctb) f@ defines @f@ as the callback+--   to be called whenever a collision occurs between+--   a shape of collision type @cta@ and another of+--   collision type @ctb@ (and vice versa). Any other callback+--   already registered to handle @(cta,ctb)@ will be removed.+--+--   Note that you should /not/ add callbacks to both+--   combinations of @(cta,ctb)@ and @(ctb,cta)@. A good rule+--   of thumb is to always use @cta <= ctb@, although this+--   is not necessary.+addCallback :: Space -> (CollisionType, CollisionType) -> Callback -> IO ()+addCallback spce@(P sp _ callbacks) (cta,ctb) func = do+  -- Find out whats our new function details+  -- (NULL for a specific means @Constant False@, optimize it)+  (cb,data_,hask) <-+      case func of+        Constant False -> return (nullFunPtr, nullPtr, Nothing)+        _              -> adaptChipmunkCB spce func++  -- Free the previous one, using+  --   updateLookupWithKey :: Ord k => (k -> a -> Maybe a) -> k+  --                       -> Map k a -> (Maybe a, Map k a)+  (def,cbs) <- readIORef callbacks+  let (old,cbs') = M.updateLookupWithKey (\_ _ -> hask) (cta,ctb) cbs+  case old of+    Nothing  -> return ()+    Just ptr -> freeHaskellFunPtr ptr++  -- Define the new+  writeIORef callbacks (def,cbs')+  withForeignPtr sp $ \sp_ptr -> do+    cpSpaceAddCollisionPairFunc sp_ptr cta ctb cb data_++foreign import ccall unsafe "wrapper.h"+    cpSpaceAddCollisionPairFunc+        :: SpacePtr -> CollisionType -> CollisionType+        -> ChipmunkCBPtr -> Ptr () -> IO ()++-- | @removeCallback sp (cta,ctb)@ removes any callbacks that+--   were registered to handle @(cta,ctb)@ (see 'addCallback').+--   Any collisions that would be handled by the removed+--   callback will be handled by the default one (see+--   'setDefaultCallback').+--+--   Note that you should /always/ use the same order that+--   was passed to 'addCallback'. In other words, after+--   @addCallback sp (cta,ctb) f@ you should use+--   @removeCallback sp (cta,ctb)@, and /never/+--   @removeCallback sp (ctb,cta)@.+--+--   Although pointless, it is harmless to remove a callback+--   that was not added.+removeCallback :: Space -> (CollisionType, CollisionType) -> IO ()+removeCallback (P sp _ callbacks) (cta,ctb) = do+  -- Free the callback+  (def,cbs) <- readIORef callbacks+  let (old,cbs') = M.updateLookupWithKey (\_ _ -> Nothing) (cta,ctb) cbs+  case old of+    Nothing  -> return ()+    Just ptr -> freeHaskellFunPtr ptr++  -- Remove the callback+  --   Note that we need to call Chipmunk even if old is Nothing+  --   because wrConstantCallback is not added to cbs. And+  --   removing what was not added is harmless here.+  writeIORef callbacks (def,cbs')+  withForeignPtr sp $ \sp_ptr -> do+    cpSpaceRemoveCollisionPairFunc sp_ptr cta ctb++foreign import ccall unsafe "wrapper.h"+    cpSpaceRemoveCollisionPairFunc+      :: SpacePtr -> CollisionType -> CollisionType -> IO ()+++-- | Sums the impulses applied to the given contact points.+--   'sumImpulses' sums only the normal components.+--   This function should be called only after 'step'+--   returns.+sumImpulses :: StorableArray Int Contact -> IO Vector+sumImpulses = sumImpulsesInternal wrContactsSumImpulses++foreign import ccall unsafe "wrapper.h"+    wrContactsSumImpulses :: ContactPtr -> #{type int}+                          -> VectorPtr -> IO ()++-- | Sums the impulses applied to the given contact points.+--   This function sums both the normal and tangential components+--   and should be called only after 'step' returns.+sumImpulsesWithFriction :: StorableArray Int Contact -> IO Vector+sumImpulsesWithFriction =+    sumImpulsesInternal wrContactsSumImpulsesWithFriction++foreign import ccall unsafe "wrapper.h"+    wrContactsSumImpulsesWithFriction :: ContactPtr -> #{type int}+                                      -> VectorPtr -> IO ()++sumImpulsesInternal :: (ContactPtr -> #{type int} -> VectorPtr -> IO ())+                    -> StorableArray Int Contact -> IO Vector+sumImpulsesInternal func sa = do+  (i1,i2) <- getBounds sa+  withStorableArray sa $ \sa_ptr ->+    with 0 $ \vec_ptr -> do+      func sa_ptr (fromIntegral $ i2-i1) vec_ptr+      peek vec_ptr+
+ Physics/Hipmunk/wrapper.c view
@@ -0,0 +1,95 @@+#include <stdlib.h>+#include "wrapper.h"++// New functions+int wrConstantCallback(cpShape *a, cpShape *b, cpContact *contacts,+                       int numContacts, cpFloat normal_coef, void *data) {+    return (data == 0 ? 0 : 1);+}++// From cpBody.h+void wrBodyUpdateVelocity(cpBody *b, cpVect *g, cpFloat d, cpFloat dt) {+    cpBodyUpdateVelocity(b, *g, d, dt);+}+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 wrBodyLocal2World(cpBody *b, cpVect *v) {+    cpVect ret = cpBodyLocal2World(b, *v);+    v->x = ret.x;+    v->y = ret.y;+}+void wrBodyWorld2Local(cpBody *b, cpVect *v) {+    cpVect ret = cpBodyWorld2Local(b, *v);+    v->x = ret.x;+    v->y = ret.y;+}+void wrBodyApplyForce(cpBody *b, cpVect *f, cpVect *p) {+    cpBodyApplyForce(b, *f, *p);+}+++// From cpShape.h+void wrCircleShapeInit(cpCircleShape *circle, cpBody *body,+                       cpVect *offset, cpFloat radius) {+    cpCircleShapeInit(circle, body, radius, *offset);+}+void wrSegmentShapeInit(cpSegmentShape *seg, cpBody *body,+                        cpVect *a, cpVect *b, cpFloat r) {+    cpSegmentShapeInit(seg, body, *a, *b, r);+}+int wrShapePointQuery(cpShape *shape, cpVect *p) {+    return cpShapePointQuery(shape, *p);+}+++// 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) {+    cpPinJointInit(joint, a, b, *anchr1, *anchr2);+}+void wrSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b,+                      cpVect *anchr1, cpVect *anchr2,+                      cpFloat min, cpFloat max) {+    cpSlideJointInit(joint, a, b, *anchr1, *anchr2, min, max);+}+void wrPivotJointInit(cpPivotJoint *joint, cpBody *a,+                      cpBody *b, cpVect *pivot) {+    cpPivotJointInit(joint, a, b, *pivot);+}+void wrGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b,+                       cpVect *groove_a, cpVect *groove_b, cpVect *anchr2) {+    cpGrooveJointInit(joint, a, b, *groove_a, *groove_b, *anchr2);+}+++// From cpArbiter.h+void wrContactsSumImpulses(cpContact *contacts, int numContacts, cpVect *ret) {+    *ret = cpContactsSumImpulses(contacts, numContacts);+}+void wrContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts,+                                       cpVect *ret) {+    *ret = cpContactsSumImpulsesWithFriction(contacts, numContacts);+}++// From cpSpace.h+void wrSpaceActiveShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func) {+    cpSpaceShapePointQuery(space, *point, func, NULL);+}+void wrSpaceStaticShapePointQuery(cpSpace *space, cpVect *point, cpSpacePointQueryFunc func) {+    cpSpaceStaticShapePointQuery(space, *point, func, NULL);+}+void wrSpaceBothShapePointQuery(cpSpace  *space, cpVect *point, cpSpacePointQueryFunc func) {+    cpSpaceShapePointQuery(space, *point, func, NULL);+    cpSpaceStaticShapePointQuery(space, *point, func, NULL);+}
+ Physics/Hipmunk/wrapper.h view
@@ -0,0 +1,51 @@+#ifndef WRAPPER_H+#define WRAPPER_H+#include "chipmunk.h"++// New functions+int wrConstantCallback(cpShape *a, cpShape *b, cpContact *contacts,+                       int numContacts, cpFloat normal_coef, void *data);+++// 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);++// 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);++// From cpPolyShape.h+void wrPolyShapeInit(cpPolyShape *poly, cpBody *body,+                     int numVerts, cpVect *verts, cpVect *offset);++// 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 cpArbiter.h+void wrContactsSumImpulses(cpContact *contacts, int numContacts, cpVect *ret);+void wrContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts,+                                       cpVect *ret);++// 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);++#endif
+ Setup.lhs view
@@ -0,0 +1,3 @@+#!runhaskell+> import Distribution.Simple+> main = defaultMain
+ chipmunk/chipmunk.c view
@@ -0,0 +1,69 @@+/* 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<numVerts; i++)+		tVerts[i] = cpvadd(verts[i], offset);+	+	cpFloat sum1 = 0.0f;+	cpFloat sum2 = 0.0f;+	for(int i=0; i<numVerts; i++){+		cpVect v1 = tVerts[i];+		cpVect v2 = tVerts[(i+1)%numVerts];+		+		cpFloat a = cpvcross(v2, v1);+		cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);+		+		sum1 += a*b;+		sum2 += a;+	}+	+	free(tVerts);+	return (m*sum1)/(6.0f*sum2);+}
+ chipmunk/chipmunk.h view
@@ -0,0 +1,86 @@+/* 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.+ */++#ifdef __cplusplus+extern "C" {+#endif+	+typedef float cpFloat;+	+static inline cpFloat+cpfmax(cpFloat a, cpFloat b)+{+	return (a > 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
+ chipmunk/cpArbiter.c view
@@ -0,0 +1,263 @@+/* 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 <math.h>++#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; i<numContacts; i++){+		cpContact *con = &contacts[i];+		cpVect j = cpvmult(con->n, con->jnAcc);+		sum = cpvadd(sum, j);+	}+		+	return sum;+}++cpVect+cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts)+{+	cpVect sum = cpvzero;+	+	for(int i=0; i<numContacts; i++){+		cpContact *con = &contacts[i];+		cpVect t = cpvperp(con->n);+		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; i<arb->numContacts; i++){+		cpContact *old = &arb->contacts[i];+		+		for(int j=0; j<numContacts; j++){+			cpContact *new_contact = &contacts[j];+			+			// This could trigger false possitives.+			if(new_contact->hash == 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; i<arb->numContacts; 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; i<arb->numContacts; 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; i<arb->numContacts; 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);+	}+}
+ chipmunk/cpArbiter.h view
@@ -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.+	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);
+ chipmunk/cpArray.c view
@@ -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 <stdlib.h>+#include <string.h>++#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; i<arr->num; i++){+		if(arr->arr[i] == obj){+			cpArrayDeleteIndex(arr, i);+			return;+		}+	}+}++void+cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data)+{+	for(int i=0; i<arr->num; i++)+		iterFunc(arr->arr[i], data);+}++int+cpArrayContains(cpArray *arr, void *ptr)+{+	for(int i=0; i<arr->num; i++)+		if(arr->arr[i] == ptr) return 1;+	+	return 0;+}
+ chipmunk/cpArray.h view
@@ -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);
+ chipmunk/cpBB.c view
@@ -0,0 +1,46 @@+/* 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 <math.h>++#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);+}
+ chipmunk/cpBB.h view
@@ -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
+ chipmunk/cpBody.c view
@@ -0,0 +1,180 @@+/* 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 <math.h>+#include <float.h>++#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;+//}
+ chipmunk/cpBody.h view
@@ -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.+ */++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{+	// 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 and it's inverse.+	cpFloat m, m_inv;+	// Moment of inertia and it's inverse.+	cpFloat i, i_inv;+	+	// NOTE: v_bias and w_bias are used internally for penetration/joint correction.+	// Linear components of motion (position, velocity, and force)+	cpVect p, v, f, v_bias;+	// Angular components of motion (angle, angular velocity, and torque)+	cpFloat a, w, t, w_bias;+	// Unit length +	cpVect rot; +	+	// User defined data pointer.+	void *data;+//	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 an object so that it will +void cpBodySlew(cpBody *body, cpVect pos, cpFloat dt);++// 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);
+ chipmunk/cpCollision.c view
@@ -0,0 +1,372 @@+/* 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 <math.h>+#include <stdio.h>+#include <assert.h>++#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<num; i++){+		cpFloat dist = cpPolyShapeValueOnAxis(poly, axes[i].n, axes[i].d);+		if(dist > 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; i<poly1->numVerts; i++){+		cpVect v = poly1->tVerts[i];+		if(cpPolyShapeContainsVert(poly2, v))+			cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i));+	}+	+	for(int i=0; i<poly2->numVerts; i++){+		cpVect v = poly2->tVerts[i];+		if(cpPolyShapeContainsVert(poly1, v))+			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; i<poly->numVerts; 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; i<poly->numVerts; 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);+	}++	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; i<poly->numVerts; 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;+}
+ chipmunk/cpCollision.h view
@@ -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);
+ chipmunk/cpHashSet.c view
@@ -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 <stdlib.h>+#include <assert.h>++#include "chipmunk.h"+#include "prime.h"++void+cpHashSetDestroy(cpHashSet *set)+{+	// Free the chains.+	for(int i=0; i<set->size; 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; i<set->size; 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; i<set->size; 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; i<set->size; 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;+		}+	}+}
+ chipmunk/cpHashSet.h view
@@ -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.+	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);
+ chipmunk/cpJoint.c view
@@ -0,0 +1,553 @@+/* 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 <math.h>++#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);+}+
+ chipmunk/cpJoint.h view
@@ -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.+*/++// 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);
+ chipmunk/cpPolyShape.c view
@@ -0,0 +1,139 @@+/* 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 <stdio.h>+#include <math.h>++#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; i<poly->numVerts; 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; i<poly->numVerts; 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; i<poly->numVerts; 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; i<numVerts; i++){+		cpVect a = cpvadd(offset, verts[i]);+		cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);+		cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));++		poly->verts[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);+}
+ chipmunk/cpPolyShape.h view
@@ -0,0 +1,76 @@+/* 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; i<poly->numVerts; 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; i<poly->numVerts; i++){+		cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d;+		if(dist > 0.0) return 0;+	}+	+	return 1;+}
+ chipmunk/cpShape.c view
@@ -0,0 +1,244 @@+/* 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 <assert.h>+#include <stdio.h>++#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);+}
+ chipmunk/cpShape.h view
@@ -0,0 +1,133 @@+/* 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 cpShapeQueryPoint+	int (*pointQuery)(struct cpShape *shape, cpVect p);+} cpShapeClass;++// Basic shape struct that the others inherit from.+typedef struct cpShape{+	const cpShapeClass *klass;+	+	// Unique id used as the hash value.+	unsigned int id;+	// Cached BBox for the shape.+	cpBB bb;+	+	// 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;+	+	// User defined data pointer for the shape.+	void *data;+	+	// cpBody that the shape is attached to.+	cpBody *body;+	+	// Coefficient of restitution. (elasticity)+	cpFloat e;+	// Coefficient of friction.+	cpFloat u;+	// Surface velocity used when solving for friction.+	cpVect surface_v;+} 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 r);+cpShape* cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r);
+ chipmunk/cpSpace.c view
@@ -0,0 +1,530 @@+/* 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 <stdio.h>+#include <math.h>+#include <assert.h>++#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; i<bodies->num; 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; i<bodies->num; 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; i<arbiters->num; i++)+		cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv);++	// Prestep the joints.+	for(int i=0; i<joints->num; i++){+		cpJoint *joint = (cpJoint *)joints->arr[i];+		joint->klass->preStep(joint, dt_inv);+	}++	for(int i=0; i<space->elasticIterations; i++){+		for(int j=0; j<arbiters->num; j++)+			cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f);+			+		for(int j=0; j<joints->num; 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; i<bodies->num; i++){+		cpBody *body = (cpBody *)bodies->arr[i];+		body->velocity_func(body, space->gravity, damping, dt);+	}++	for(int i=0; i<arbiters->num; i++)+		cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]);+	+	// Run the impulse solver.+	for(int i=0; i<space->iterations; i++){+		for(int j=0; j<arbiters->num; j++)+			cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 0.0f);+			+		for(int j=0; j<joints->num; 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; i<bodies->num; i++)+//		cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks);+	+	// Increment the stamp.+	space->stamp++;+}
+ chipmunk/cpSpace.h view
@@ -0,0 +1,113 @@+/* 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{+	// Number of iterations to use in the impulse solver.+	int iterations;+	int elasticIterations;+//	int sleepTicks;+	+	// Self explanatory.+	cpVect gravity;+	cpFloat damping;+	+	// 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);
+ chipmunk/cpSpaceHash.c view
@@ -0,0 +1,455 @@+/* 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 <stdio.h>+#include <math.h>+#include <assert.h>++#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; i<hash->numcells; 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);+}
+ chipmunk/cpSpaceHash.h view
@@ -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, 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);
+ chipmunk/cpVect.c view
@@ -0,0 +1,63 @@+/* 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;+}
+ chipmunk/cpVect.h view
@@ -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.+ */+ +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
+ chipmunk/prime.h view
@@ -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 int+next_prime(int n)+{+	int i = 0;+	while(n > primes[i]){+		i++;+		assert(primes[i]); // realistically this should never happen+	}+	+	return primes[i];+}