diff --git a/Hipmunk.cabal b/Hipmunk.cabal new file mode 100644 --- /dev/null +++ b/Hipmunk.cabal @@ -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 +Maintainer: Felipe A. Lessa +Synopsis: A Haskell binding for Chipmunk. +Description: + Chipmunk is a fast, simple, portable, 2D physics engine + (). + This package contains the Chipmunk rev4 source + (from ) + 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 diff --git a/LICENSE b/LICENSE new file mode 100644 --- /dev/null +++ b/LICENSE @@ -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. + */ diff --git a/Physics/Hipmunk.hs b/Physics/Hipmunk.hs new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk.hs @@ -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 diff --git a/Physics/Hipmunk/Body.hsc b/Physics/Hipmunk/Body.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Body.hsc @@ -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 () diff --git a/Physics/Hipmunk/Common.hsc b/Physics/Hipmunk/Common.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Common.hsc @@ -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 diff --git a/Physics/Hipmunk/Internal.hsc b/Physics/Hipmunk/Internal.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Internal.hsc @@ -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) diff --git a/Physics/Hipmunk/Joint.hsc b/Physics/Hipmunk/Joint.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Joint.hsc @@ -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 () + diff --git a/Physics/Hipmunk/Shape.hsc b/Physics/Hipmunk/Shape.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Shape.hsc @@ -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 , +-- 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 +-- . +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 +-- +-- for example), however intersection points are \"returned\" +-- as soon as they are found. That is, the WHNF of @intersections segs@ +-- only needs to calculate the necessary to find the first +-- intersection, so if you want to know only if there is a +-- an intersection or not then you only need /O(n log n)/ time. +-- +-- (Note that the @segs@ does not need to be a polygon at all.) +intersections :: [Segment] -> [InterIndexes] +intersections = bentleyOttmann . zip [0..] + +type InterIndexes = (Intersection, SegmentIndex, SegmentIndex) + + +--- Basic data types +type SegmentIndex = Int +type SegmentArray = Array SegmentIndex Segment +type IndSeg = (SegmentIndex, Segment) +type Neighbors = (SegmentIndex, SegmentIndex) +data Event = EvStart !Position !SegmentIndex + | EvEnd !Position !SegmentIndex + | EvInter !Position !SegmentIndex !SegmentIndex + deriving (Eq) + +instance Ord Event where + e1 `compare` e2 = case evPos e1 `compare` evPos e2 of + EQ -> evIdent e1 `compare` evIdent e2 + ot -> ot + where + evIdent (EvStart _ i) = (-2, i) + evIdent (EvEnd _ i) = (-1, i) + evIdent (EvInter _ i j) = (i, j) + +evPos :: Event -> Position +evPos (EvStart p _) = p +evPos (EvEnd p _) = p +evPos (EvInter p _ _) = p + +interErr :: a +interErr = error . ("Physics.Hipmunk.Shape.intersections: " ++) + +interDebug :: Bool -> Bool +--interDebug = const False +interDebug = id + +--- Event queue (a priority queue) +data EventQueue = EQNil + | EQBranch !Event EventQueue EventQueue + +eqSingle :: Event -> EventQueue +eqSingle ev = EQBranch ev EQNil EQNil + +eqGet :: EventQueue -> (Event, EventQueue) +eqGet EQNil = interErr "[eqGet] never get here" +eqGet (EQBranch ev q1 q2) = (ev, eqMerge q1 q2) + +eqMerge :: EventQueue -> EventQueue -> EventQueue +eqMerge EQNil other = other +eqMerge other EQNil = other +eqMerge left@(EQBranch evL _ _) right@(EQBranch evR r1 r2) + = case ev `compare` ev' of + LT -> helper left right + GT -> helper right left + EQ -> eqMerge left (eqMerge r1 r2) -- Discard ev'! + | ev <= ev' = helper left right + | otherwise = helper right left + where + helper (EQBranch ev EQNil q2) r = EQBranch ev r q2 + helper (EQBranch ev q1 q2) r = EQBranch ev q2 (eqMerge q1 r) + +eqInsert :: Event -> EventQueue -> EventQueue +eqInsert ev q = eqMerge q (eqSingle ev) + +eqFromList :: [Event] -> EventQueue +eqFromList evs = foldr eqMerge EQNil . map eqSingle + +eqRun :: (Event -> EventQueue -> ([a], EventQueue)) -> EventQueue -> [a] +eqRun _ EQNil = [] +eqRun f q = case uncurry f $ eqGet q of + (xs, q') -> xs ++ eqRun f q' + +--- Sweep line +type SweepElemRef = IORef (Maybe SweepElem) +data SweepElem = SE !SegmentIndex !SweepElemRef !SweepElemRef +type SweepLine = (IM.IntMap Position, SweepElem) + +slEmpty :: SweepLine +slEmpty = IM.empty + +slInsert :: SegmentIndex -> SweepLine -> IO (SweepLine, [Neighbors]) +slInsert s sl = do + newLeft <- newIORef Nothing + newRight <- newIORef Nothing + let newElem = SE s newLeft newRight + + let update _ set | IM.null set = return [] + update left set = do + let (extrm, oldElem@(SE _ l r)) = f set + where f = if left then IM.findMax else IM.findMin + let newRef = (if left then newLeft else newRight) + oldRef = (if left then r else l) + old <- readIORef oldRef + writeIORef oldRef newElem + writeIORef newRef oldElem + return [extrm] + + let (ltSet, gtSet) = IM.split s sl + l <- update True ltSet + r <- update False gtSet + return (IM.insert s newElem sl, l ++ r) + +slSwap :: Neighbors -> SweepLine -> (SweepLine, [Neighbors]) +slSwap (s1,s2) sl = + let (s1L, s1R) = sl IM.! s1 + (s2L, s2R) = sl IM.! s2 + + newSl = IM.insert s1 (s2L, s2R) $ + IM.insert s2 (s1L, s1R) $ sl + changes = (if s1L == siNone then id else ((s1L,s2):)) + (if s2R == siNone then [] else [(s1,s2R)]) + in if interDebug (s1R /= s2 || s2L /= s1) + then interErr "[slSwap] they're not neighbors!" + else (newSl, changes) + + + + +-- -- This looks like Data.Set, however with some peculiarities. +-- type SLSize = Int +-- data SweepLine = SLNil +-- | SLBranch !SLSize !SegmentIndex SweepLine SweepLine + +-- slSize :: SweepLine -> SLSize +-- slSize SLNil = 0 +-- slSize (SLBranch size _ _ _) = size + +-- slSingle :: SegmentIndex -> SweepLine +-- slSingle s = SLBranch 1 s SLNil SLNil + +-- slInsert :: SegmentIndex -> SweepLine -> SweepLine +-- slInsert s SLNil = slSingle s +-- slInsert s (SLBranch size t l r) = +-- case s `compare` t of +-- LT -> slBalance t (slInsert s l) r +-- GT -> slBalance t l (slInsert s r) +-- EQ -> interErr "[slInsert] segment already on sweepline" + +-- slRemove :: SegmentIndex -> SweepLine -> SweepLine +-- slRemove s SLNil = interErr "[slRemove] segment not on sweepline" +-- slRemove s (SLBranch size t l r) = +-- case s `compare` t of +-- LT -> slBalance t (slRemove s l) r +-- GT -> slBalance t l (slRemove s r) + +-- slBalance + +--- Bentley-Ottmann functions +boEvents :: IndSeg -> [Event] +boEvents (index, a0,a1) = [EvStart start index, EvEnd end index] + where (start,end) | a0 < a1 = (a0,a1) + | otherwise = (a1,a0) + +bentleyOttmann :: [IndSeg] -> [InterIndexes] +bentleyOttmann isegs = eqRun go . eqFromList . concatMap boEvents + where + segArray = array (0, length isegs - 1) isegs + + go EQNil = [] + go + + +--------- END OF BENTLEY-OTTMANN --------- + +-} diff --git a/Physics/Hipmunk/Space.hsc b/Physics/Hipmunk/Space.hsc new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/Space.hsc @@ -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 . +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 + diff --git a/Physics/Hipmunk/wrapper.c b/Physics/Hipmunk/wrapper.c new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/wrapper.c @@ -0,0 +1,95 @@ +#include +#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); +} diff --git a/Physics/Hipmunk/wrapper.h b/Physics/Hipmunk/wrapper.h new file mode 100644 --- /dev/null +++ b/Physics/Hipmunk/wrapper.h @@ -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 diff --git a/Setup.lhs b/Setup.lhs new file mode 100644 --- /dev/null +++ b/Setup.lhs @@ -0,0 +1,3 @@ +#!runhaskell +> import Distribution.Simple +> main = defaultMain diff --git a/chipmunk/chipmunk.c b/chipmunk/chipmunk.c new file mode 100644 --- /dev/null +++ b/chipmunk/chipmunk.c @@ -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 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 diff --git a/chipmunk/cpArbiter.c b/chipmunk/cpArbiter.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpArbiter.c @@ -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 +#include + +#include "chipmunk.h" + +cpFloat cp_bias_coef = 0.1f; +cpFloat cp_collision_slop = 0.1f; + +cpContact* +cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int hash) +{ + con->p = p; + con->n = n; + con->dist = dist; + + con->jnAcc = 0.0f; + con->jtAcc = 0.0f; + con->jBias = 0.0f; + + con->hash = hash; + + return con; +} + +cpVect +cpContactsSumImpulses(cpContact *contacts, int numContacts) +{ + cpVect sum = cpvzero; + + for(int i=0; in, con->jnAcc); + sum = cpvadd(sum, j); + } + + return sum; +} + +cpVect +cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts) +{ + cpVect sum = cpvzero; + + for(int i=0; in); + cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc)); + sum = cpvadd(sum, j); + } + + return sum; +} + +cpArbiter* +cpArbiterAlloc(void) +{ + return (cpArbiter *)calloc(1, sizeof(cpArbiter)); +} + +cpArbiter* +cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp) +{ + arb->numContacts = 0; + arb->contacts = NULL; + + arb->a = a; + arb->b = b; + + arb->stamp = stamp; + + return arb; +} + +cpArbiter* +cpArbiterNew(cpShape *a, cpShape *b, int stamp) +{ + return cpArbiterInit(cpArbiterAlloc(), a, b, stamp); +} + +void +cpArbiterDestroy(cpArbiter *arb) +{ + free(arb->contacts); +} + +void +cpArbiterFree(cpArbiter *arb) +{ + if(arb) cpArbiterDestroy(arb); + free(arb); +} + +void +cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts) +{ + // Iterate over the possible pairs to look for hash value matches. + for(int i=0; inumContacts; i++){ + cpContact *old = &arb->contacts[i]; + + for(int j=0; jhash == old->hash){ + // Copy the persistant contact information. + new_contact->jnAcc = old->jnAcc; + new_contact->jtAcc = old->jtAcc; + } + } + } + + free(arb->contacts); + + arb->contacts = contacts; + arb->numContacts = numContacts; +} + +void +cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv) +{ + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; + + cpFloat e = shapea->e * shapeb->e; + arb->u = shapea->u * shapeb->u; + arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); + + cpBody *a = shapea->body; + cpBody *b = shapeb->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + + // Calculate the offsets. + con->r1 = cpvsub(con->p, a->p); + con->r2 = cpvsub(con->p, b->p); + + // Calculate the mass normal. + cpFloat mass_sum = a->m_inv + b->m_inv; + + cpFloat r1cn = cpvcross(con->r1, con->n); + cpFloat r2cn = cpvcross(con->r2, con->n); + cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; + con->nMass = 1.0f/kn; + + // Calculate the mass tangent. + cpVect t = cpvperp(con->n); + cpFloat r1ct = cpvcross(con->r1, t); + cpFloat r2ct = cpvcross(con->r2, t); + cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct; + con->tMass = 1.0f/kt; + + // Calculate the target bias velocity. + con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop); + con->jBias = 0.0f; + + // Calculate the target bounce velocity. + cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w)); + cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w)); + con->bounce = cpvdot(con->n, cpvsub(v2, v1))*e; + } +} + +void +cpArbiterApplyCachedImpulse(cpArbiter *arb) +{ + cpShape *shapea = arb->a; + cpShape *shapeb = arb->b; + + arb->u = shapea->u * shapeb->u; + arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v); + + cpBody *a = shapea->body; + cpBody *b = shapeb->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + + cpVect t = cpvperp(con->n); + cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc)); + cpBodyApplyImpulse(a, cpvneg(j), con->r1); + cpBodyApplyImpulse(b, j, con->r2); + } +} + +void +cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef) +{ + cpBody *a = arb->a->body; + cpBody *b = arb->b->body; + + for(int i=0; inumContacts; i++){ + cpContact *con = &arb->contacts[i]; + cpVect n = con->n; + cpVect r1 = con->r1; + cpVect r2 = con->r2; + + // Calculate the relative bias velocities. + cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias)); + cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias)); + cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n); + + // Calculate and clamp the bias impulse. + cpFloat jbn = (con->bias - vbn)*con->nMass; + cpFloat jbnOld = con->jBias; + con->jBias = cpfmax(jbnOld + jbn, 0.0f); + jbn = con->jBias - jbnOld; + + // Apply the bias impulse. + cpVect jb = cpvmult(n, jbn); + cpBodyApplyBiasImpulse(a, cpvneg(jb), r1); + cpBodyApplyBiasImpulse(b, jb, r2); + + // Calculate the relative velocity. + cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + cpVect vr = cpvsub(v2, v1); + cpFloat vrn = cpvdot(vr, n); + + // Calculate and clamp the normal impulse. + cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass; + cpFloat jnOld = con->jnAcc; + con->jnAcc = cpfmax(jnOld + jn, 0.0f); + jn = con->jnAcc - jnOld; + + // Calculate the relative tangent velocity. + cpVect t = cpvperp(n); + cpFloat vrt = cpvdot(cpvadd(vr, arb->target_v), t); + + // Calculate and clamp the friction impulse. + cpFloat jtMax = arb->u*con->jnAcc; + cpFloat jt = -vrt*con->tMass; + cpFloat jtOld = con->jtAcc; + con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax); + jt = con->jtAcc - jtOld; + + // Apply the final impulse. + cpVect j = cpvadd(cpvmult(n, jn), cpvmult(t, jt)); + cpBodyApplyImpulse(a, cpvneg(j), r1); + cpBodyApplyImpulse(b, j, r2); + } +} diff --git a/chipmunk/cpArbiter.h b/chipmunk/cpArbiter.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpArbiter.h @@ -0,0 +1,85 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Determines how fast penetrations resolve themselves. +extern cpFloat cp_bias_coef; +// Amount of allowed penetration. Used to reduce vibrating contacts. +extern cpFloat cp_collision_slop; + +// Data structure for contact points. +typedef struct cpContact{ + // Contact point and normal. + cpVect p, n; + // Penetration distance. + cpFloat dist; + + // Calculated by cpArbiterPreStep(). + cpVect r1, r2; + cpFloat nMass, tMass, bounce; + + // Persistant contact information. + cpFloat jnAcc, jtAcc, jBias; + cpFloat bias; + + // Hash value used to (mostly) uniquely identify a contact. + unsigned int hash; +} cpContact; + +// Contacts are always allocated in groups. +cpContact* cpContactInit(cpContact *con, cpVect p, cpVect n, cpFloat dist, unsigned int hash); + +// Sum the contact impulses. (Can be used after cpSpaceStep() returns) +cpVect cpContactsSumImpulses(cpContact *contacts, int numContacts); +cpVect cpContactsSumImpulsesWithFriction(cpContact *contacts, int numContacts); + +// Data structure for tracking collisions between shapes. +typedef struct cpArbiter{ + // Information on the contact points between the objects. + int numContacts; + cpContact *contacts; + + // The two shapes involved in the collision. + cpShape *a, *b; + + // Calculated by cpArbiterPreStep(). + cpFloat u; + cpVect target_v; + + // Time stamp of the arbiter. (from cpSpace) + int stamp; +} cpArbiter; + +// Basic allocation/destruction functions. +cpArbiter* cpArbiterAlloc(void); +cpArbiter* cpArbiterInit(cpArbiter *arb, cpShape *a, cpShape *b, int stamp); +cpArbiter* cpArbiterNew(cpShape *a, cpShape *b, int stamp); + +void cpArbiterDestroy(cpArbiter *arb); +void cpArbiterFree(cpArbiter *arb); + +// These functions are all intended to be used internally. +// Inject new contact points into the arbiter while preserving contact history. +void cpArbiterInject(cpArbiter *arb, cpContact *contacts, int numContacts); +// Precalculate values used by the solver. +void cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv); +void cpArbiterApplyCachedImpulse(cpArbiter *arb); +// Run an iteration of the solver on the arbiter. +void cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef); diff --git a/chipmunk/cpArray.c b/chipmunk/cpArray.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpArray.c @@ -0,0 +1,114 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" + + +//#define CP_ARRAY_INCREMENT 10 + +// NOTE: cpArray is rarely used and will probably go away. + +cpArray* +cpArrayAlloc(void) +{ + return (cpArray *)calloc(1, sizeof(cpArray)); +} + +cpArray* +cpArrayInit(cpArray *arr, int size) +{ + arr->num = 0; + + size = (size ? size : 4); + arr->max = size; + arr->arr = (void **)malloc(size*sizeof(void**)); + + return arr; +} + +cpArray* +cpArrayNew(int size) +{ + return cpArrayInit(cpArrayAlloc(), size); +} + +void +cpArrayDestroy(cpArray *arr) +{ + free(arr->arr); +} + +void +cpArrayFree(cpArray *arr) +{ + if(!arr) return; + cpArrayDestroy(arr); + free(arr); +} + +void +cpArrayPush(cpArray *arr, void *object) +{ + if(arr->num == arr->max){ + arr->max *= 2; + arr->arr = (void **)realloc(arr->arr, arr->max*sizeof(void**)); + } + + arr->arr[arr->num] = object; + arr->num++; +} + +void +cpArrayDeleteIndex(cpArray *arr, int index) +{ + int last = --arr->num; + arr->arr[index] = arr->arr[last]; +} + +void +cpArrayDeleteObj(cpArray *arr, void *obj) +{ + for(int i=0; inum; i++){ + if(arr->arr[i] == obj){ + cpArrayDeleteIndex(arr, i); + return; + } + } +} + +void +cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data) +{ + for(int i=0; inum; i++) + iterFunc(arr->arr[i], data); +} + +int +cpArrayContains(cpArray *arr, void *ptr) +{ + for(int i=0; inum; i++) + if(arr->arr[i] == ptr) return 1; + + return 0; +} diff --git a/chipmunk/cpArray.h b/chipmunk/cpArray.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpArray.h @@ -0,0 +1,45 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// NOTE: cpArray is rarely used and will probably go away. + +typedef struct cpArray{ + int num, max; + void **arr; +} cpArray; + +typedef void (*cpArrayIter)(void *ptr, void *data); + +cpArray *cpArrayAlloc(void); +cpArray *cpArrayInit(cpArray *arr, int size); +cpArray *cpArrayNew(int size); + +void cpArrayDestroy(cpArray *arr); +void cpArrayFree(cpArray *arr); + +void cpArrayClear(cpArray *arr); + +void cpArrayPush(cpArray *arr, void *object); +void cpArrayDeleteIndex(cpArray *arr, int index); +void cpArrayDeleteObj(cpArray *arr, void *obj); + +void cpArrayEach(cpArray *arr, cpArrayIter iterFunc, void *data); +int cpArrayContains(cpArray *arr, void *ptr); diff --git a/chipmunk/cpBB.c b/chipmunk/cpBB.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpBB.c @@ -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 + +#include "chipmunk.h" + +cpVect +cpBBClampVect(const cpBB bb, const cpVect v) +{ + cpFloat x = cpfmin(cpfmax(bb.l, v.x), bb.r); + cpFloat y = cpfmin(cpfmax(bb.b, v.y), bb.t); + return cpv(x, y); +} + +cpVect +cpBBWrapVect(const cpBB bb, const cpVect v) +{ + cpFloat ix = fabsf(bb.r - bb.l); + cpFloat modx = fmodf(v.x - bb.l, ix); + cpFloat x = (modx > 0.0f) ? modx : modx + ix; + + cpFloat iy = fabsf(bb.t - bb.b); + cpFloat mody = fmodf(v.y - bb.b, iy); + cpFloat y = (mody > 0.0f) ? mody : mody + iy; + + return cpv(x + bb.l, y + bb.b); +} diff --git a/chipmunk/cpBB.h b/chipmunk/cpBB.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpBB.h @@ -0,0 +1,53 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +typedef struct cpBB{ + cpFloat l, b, r ,t; +} cpBB; + +static inline cpBB +cpBBNew(const cpFloat l, const cpFloat b, + const cpFloat r, const cpFloat t) +{ + cpBB bb = {l, b, r, t}; + return bb; +} + +static inline int +cpBBintersects(const cpBB a, const cpBB b) +{ + return (a.l<=b.r && b.l<=a.r && a.b<=b.t && b.b<=a.t); +} + +static inline int +cpBBcontainsBB(const cpBB bb, const cpBB other) +{ + return (bb.l < other.l && bb.r > other.r && bb.b < other.b && bb.t > other.t); +} + +static inline int +cpBBcontainsVect(const cpBB bb, const cpVect v) +{ + return (bb.l < v.x && bb.r > v.x && bb.b < v.y && bb.t > v.y); +} + +cpVect cpBBClampVect(const cpBB bb, const cpVect v); // clamps the vector to lie within the bbox +cpVect cpBBWrapVect(const cpBB bb, const cpVect v); // wrap a vector to a bbox diff --git a/chipmunk/cpBody.c b/chipmunk/cpBody.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpBody.c @@ -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 +#include +#include + +#include "chipmunk.h" + +cpBody* +cpBodyAlloc(void) +{ + return (cpBody *)malloc(sizeof(cpBody)); +} + +cpBody* +cpBodyInit(cpBody *body, cpFloat m, cpFloat i) +{ + body->velocity_func = cpBodyUpdateVelocity; + body->position_func = cpBodyUpdatePosition; + + cpBodySetMass(body, m); + cpBodySetMoment(body, i); + + body->p = cpvzero; + body->v = cpvzero; + body->f = cpvzero; + + cpBodySetAngle(body, 0.0f); + body->w = 0.0f; + body->t = 0.0f; + + body->v_bias = cpvzero; + body->w_bias = 0.0f; + + body->data = NULL; +// body->active = 1; + + return body; +} + +cpBody* +cpBodyNew(cpFloat m, cpFloat i) +{ + return cpBodyInit(cpBodyAlloc(), m, i); +} + +void cpBodyDestroy(cpBody *body){} + +void +cpBodyFree(cpBody *body) +{ + if(body) cpBodyDestroy(body); + free(body); +} + +void +cpBodySetMass(cpBody *body, cpFloat m) +{ + body->m = m; + body->m_inv = 1.0f/m; +} + +void +cpBodySetMoment(cpBody *body, cpFloat i) +{ + body->i = i; + body->i_inv = 1.0f/i; +} + +void +cpBodySetAngle(cpBody *body, cpFloat a) +{ + body->a = fmod(a, (cpFloat)M_PI*2.0f); + body->rot = cpvforangle(a); +} + +void +cpBodySlew(cpBody *body, cpVect pos, cpFloat dt) +{ + cpVect delta = cpvsub(pos, body->p); + body->v = cpvmult(delta, 1.0/dt); +} + +void +cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) +{ + body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)); + body->w = body->w*damping + body->t*body->i_inv*dt; +} + +void +cpBodyUpdatePosition(cpBody *body, cpFloat dt) +{ + body->p = cpvadd(body->p, cpvmult(cpvadd(body->v, body->v_bias), dt)); + cpBodySetAngle(body, body->a + (body->w + body->w_bias)*dt); + + body->v_bias = cpvzero; + body->w_bias = 0.0f; +} + +void +cpBodyResetForces(cpBody *body) +{ + body->f = cpvzero; + body->t = 0.0f; +} + +void +cpBodyApplyForce(cpBody *body, cpVect f, cpVect r) +{ + body->f = cpvadd(body->f, f); + body->t += cpvcross(r, f); +} + +void +cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt) +{ + // Calculate the world space anchor coordinates. + cpVect r1 = cpvrotate(anchr1, a->rot); + cpVect r2 = cpvrotate(anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1)); + cpFloat dist = cpvlength(delta); + cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero; + + cpFloat f_spring = (dist - rlen)*k; + + // Calculate the world relative velocities of the anchor points. + cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w)); + cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w)); + + // Calculate the damping force. + // This really should be in the impulse solver and can produce problems when using large damping values. + cpFloat vrn = cpvdot(cpvsub(v2, v1), n); + cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv))); + + // Apply! + cpVect f = cpvmult(n, f_spring + f_damp); + cpBodyApplyForce(a, f, r1); + cpBodyApplyForce(b, cpvneg(f), r2); +} + +//int +//cpBodyMarkLowEnergy(cpBody *body, cpFloat dvsq, int max) +//{ +// cpFloat ke = body->m*cpvdot(body->v, body->v); +// cpFloat re = body->i*body->w*body->w; +// +// if(ke + re > body->m*dvsq) +// body->active = 1; +// else if(body->active) +// body->active = (body->active + 1)%(max + 1); +// else { +// body->v = cpvzero; +// body->v_bias = cpvzero; +// body->w = 0.0f; +// body->w_bias = 0.0f; +// } +// +// return body->active; +//} diff --git a/chipmunk/cpBody.h b/chipmunk/cpBody.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpBody.h @@ -0,0 +1,110 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +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); diff --git a/chipmunk/cpCollision.c b/chipmunk/cpCollision.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpCollision.c @@ -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 +#include +#include +#include + +#include "chipmunk.h" + +typedef int (*collisionFunc)(cpShape*, cpShape*, cpContact**); + +static collisionFunc *colfuncs = NULL; + +// Add contact points for circle to circle collisions. +// Used by several collision tests. +static int +circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact **con) +{ + cpFloat mindist = r1 + r2; + cpVect delta = cpvsub(p2, p1); + cpFloat distsq = cpvlengthsq(delta); + if(distsq >= mindist*mindist) return 0; + + cpFloat dist = sqrtf(distsq); + // To avoid singularities, do nothing in the case of dist = 0. + cpFloat non_zero_dist = (dist ? dist : INFINITY); + + // Allocate and initialize the contact. + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvadd(p1, cpvmult(delta, 0.5 + (r1 - 0.5*mindist)/non_zero_dist)), + cpvmult(delta, 1.0/non_zero_dist), + dist - mindist, + 0 + ); + + return 1; +} + +// Collide circle shapes. +static int +circle2circle(cpShape *shape1, cpShape *shape2, cpContact **arr) +{ + cpCircleShape *circ1 = (cpCircleShape *)shape1; + cpCircleShape *circ2 = (cpCircleShape *)shape2; + + return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr); +} + +// Collide circles to segment shapes. +static int +circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con) +{ + cpCircleShape *circ = (cpCircleShape *)circleShape; + cpSegmentShape *seg = (cpSegmentShape *)segmentShape; + + // Radius sum + cpFloat rsum = circ->r + seg->r; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn); + cpFloat dist = fabs(dn) - rsum; + if(dist > 0.0f) return 0; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, circ->tc); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt < dtMin){ + if(dt < (dtMin - rsum)){ + return 0; + } else { + return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con); + } + } else { + if(dt < dtMax){ + cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn); + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)), + n, + dist, + 0 + ); + return 1; + } else { + if(dt < (dtMax + rsum)) { + return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con); + } else { + return 0; + } + } + } + + return 1; +} + +// Helper function for allocating contact point lists. +static cpContact * +addContactPoint(cpContact **arr, int *max, int *num) +{ + if(*arr == NULL){ + // Allocate the array if it hasn't been done. + (*max) = 2; + (*num) = 0; + (*arr) = (cpContact *)malloc((*max)*sizeof(cpContact)); + } else if(*num == *max){ + // Extend it if necessary. + (*max) *= 2; + (*arr) = (cpContact *)realloc(*arr, (*max)*sizeof(cpContact)); + } + + cpContact *con = &(*arr)[*num]; + (*num)++; + + return con; +} + +// Find the minimum separating axis for the give poly and axis list. +static inline int +findMSA(cpPolyShape *poly, cpPolyShapeAxis *axes, int num, cpFloat *min_out) +{ + int min_index = 0; + cpFloat min = cpPolyShapeValueOnAxis(poly, axes->n, axes->d); + if(min > 0.0) return -1; + + for(int i=1; i 0.0) { + return -1; + } else if(dist > min){ + min = dist; + min_index = i; + } + } + + (*min_out) = min; + return min_index; +} + +// Add contacts for penetrating vertexes. +static inline int +findVerts(cpContact **arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist) +{ + int max = 0; + int num = 0; + + for(int i=0; inumVerts; i++){ + cpVect v = poly1->tVerts[i]; + if(cpPolyShapeContainsVert(poly2, v)) + cpContactInit(addContactPoint(arr, &max, &num), v, n, dist, CP_HASH_PAIR(poly1, i)); + } + + for(int i=0; inumVerts; i++){ + cpVect v = poly2->tVerts[i]; + if(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; inumVerts; i++){ + cpVect v = poly->tVerts[i]; + if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){ + cpFloat dt = cpvcross(seg->tn, v); + if(dta >= dt && dt >= dtb){ + cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly, i)); + } + } + } +} + +// This one is complicated and gross. Just don't go there... +// TODO: Comment me! +static int +seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + cpFloat segD = cpvdot(seg->tn, seg->ta); + cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r; + cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r; + if(minNeg > 0.0f || minNorm > 0.0f) return 0; + + int mini = 0; + cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d); + if(poly_min > 0.0f) return 0; + for(int i=0; inumVerts; i++){ + cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d); + if(dist > 0.0f){ + return 0; + } else if(dist > poly_min){ + poly_min = dist; + mini = i; + } + } + + int max = 0; + int num = 0; + + cpVect poly_n = cpvneg(axes[mini].n); + + cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r)); + cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r)); + if(cpPolyShapeContainsVert(poly, va)) + cpContactInit(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg, 0)); + if(cpPolyShapeContainsVert(poly, vb)) + cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg, 1)); + + // Floating point precision problems here. + // This will have to do for now. + poly_min -= cp_collision_slop; + if(minNorm >= poly_min || minNeg >= poly_min) { + if(minNorm > minNeg) + findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f); + else + findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f); + } + + return num; +} + +// This one is less gross, but still gross. +// TODO: Comment me! +static int +circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con) +{ + cpCircleShape *circ = (cpCircleShape *)shape1; + cpPolyShape *poly = (cpPolyShape *)shape2; + cpPolyShapeAxis *axes = poly->tAxes; + + int mini = 0; + cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r; + for(int i=0; inumVerts; i++){ + cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r; + if(dist > 0.0){ + return 0; + } else if(dist > min) { + min = dist; + mini = i; + } + } + + cpVect n = axes[mini].n; + cpVect a = poly->tVerts[mini]; + cpVect b = poly->tVerts[(mini + 1)%poly->numVerts]; + cpFloat dta = cpvcross(n, a); + cpFloat dtb = cpvcross(n, b); + cpFloat dt = cpvcross(n, circ->tc); + + if(dt < dtb){ + return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con); + } else if(dt < dta) { + (*con) = (cpContact *)malloc(sizeof(cpContact)); + cpContactInit( + (*con), + cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)), + cpvneg(n), + min, + 0 + ); + + return 1; + } else { + return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con); + } +} + +static void +addColFunc(cpShapeType a, cpShapeType b, collisionFunc func) +{ + colfuncs[a + b*CP_NUM_SHAPES] = func; +} + +#ifdef __cplusplus +extern "C" { +#endif + // Initializes the array of collision functions. + // Called by cpInitChipmunk(). + void + cpInitCollisionFuncs(void) + { + if(!colfuncs) + colfuncs = (collisionFunc *)calloc(CP_NUM_SHAPES*CP_NUM_SHAPES, sizeof(collisionFunc)); + + addColFunc(CP_CIRCLE_SHAPE, CP_CIRCLE_SHAPE, circle2circle); + addColFunc(CP_CIRCLE_SHAPE, CP_SEGMENT_SHAPE, circle2segment); + addColFunc(CP_SEGMENT_SHAPE, CP_POLY_SHAPE, seg2poly); + addColFunc(CP_CIRCLE_SHAPE, CP_POLY_SHAPE, circle2poly); + addColFunc(CP_POLY_SHAPE, CP_POLY_SHAPE, poly2poly); + } +#ifdef __cplusplus +} +#endif + +int +cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr) +{ + // Their shape types must be in order. + assert(a->klass->type <= b->klass->type); + + collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES]; + return (cfunc) ? cfunc(a, b, arr) : 0; +} diff --git a/chipmunk/cpCollision.h b/chipmunk/cpCollision.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpCollision.h @@ -0,0 +1,23 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Collides two cpShape structures. (this function is lonely :( ) +int cpCollideShapes(cpShape *a, cpShape *b, cpContact **arr); diff --git a/chipmunk/cpHashSet.c b/chipmunk/cpHashSet.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpHashSet.c @@ -0,0 +1,219 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +#include +#include + +#include "chipmunk.h" +#include "prime.h" + +void +cpHashSetDestroy(cpHashSet *set) +{ + // Free the chains. + for(int i=0; isize; i++){ + // Free the bins in the chain. + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + free(bin); + bin = next; + } + } + + // Free the table. + free(set->table); +} + +void +cpHashSetFree(cpHashSet *set) +{ + if(set) cpHashSetDestroy(set); + free(set); +} + +cpHashSet * +cpHashSetAlloc(void) +{ + return (cpHashSet *)calloc(1, sizeof(cpHashSet)); +} + +cpHashSet * +cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) +{ + set->size = next_prime(size); + set->entries = 0; + + set->eql = eqlFunc; + set->trans = trans; + + set->default_value = NULL; + + set->table = (cpHashSetBin **)calloc(set->size, sizeof(cpHashSetBin *)); + + return set; +} + +cpHashSet * +cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans) +{ + return cpHashSetInit(cpHashSetAlloc(), size, eqlFunc, trans); +} + +static int +setIsFull(cpHashSet *set) +{ + return (set->entries >= set->size); +} + +static void +cpHashSetResize(cpHashSet *set) +{ + // Get the next approximate doubled prime. + int newSize = next_prime(set->size + 1); + // Allocate a new table. + cpHashSetBin **newTable = (cpHashSetBin **)calloc(newSize, sizeof(cpHashSetBin *)); + + // Iterate over the chains. + for(int i=0; isize; i++){ + // Rehash the bins into the new table. + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + int index = bin->hash%newSize; + bin->next = newTable[index]; + newTable[index] = bin; + + bin = next; + } + } + + free(set->table); + + set->table = newTable; + set->size = newSize; +} + +void * +cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data) +{ + int index = hash%set->size; + + // Find the bin with the matching element. + cpHashSetBin *bin = set->table[index]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + // Create it necessary. + if(!bin){ + bin = (cpHashSetBin *)malloc(sizeof(cpHashSetBin)); + bin->hash = hash; + bin->elt = set->trans(ptr, data); // Transform the pointer. + + bin->next = set->table[index]; + set->table[index] = bin; + + set->entries++; + + // Resize the set if it's full. + if(setIsFull(set)) + cpHashSetResize(set); + } + + return bin->elt; +} + +void * +cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr) +{ + int index = hash%set->size; + + // Pointer to the previous bin pointer. + cpHashSetBin **prev_ptr = &set->table[index]; + // Pointer the the current bin. + cpHashSetBin *bin = set->table[index]; + + // Find the bin + while(bin && !set->eql(ptr, bin->elt)){ + prev_ptr = &bin->next; + bin = bin->next; + } + + // Remove it if it exists. + if(bin){ + // Update the previos bin pointer to point to the next bin. + (*prev_ptr) = bin->next; + set->entries--; + + void *return_value = bin->elt; + free(bin); + return return_value; + } + + return NULL; +} + +void * +cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr) +{ + int index = hash%set->size; + cpHashSetBin *bin = set->table[index]; + while(bin && !set->eql(ptr, bin->elt)) + bin = bin->next; + + return (bin ? bin->elt : set->default_value); +} + +void +cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data) +{ + for(int i=0; isize; i++){ + cpHashSetBin *bin; + for(bin = set->table[i]; bin; bin = bin->next) + func(bin->elt, data); + } +} + +void +cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data) +{ + // Iterate over all the chains. + for(int i=0; isize; i++){ + // The rest works similarly to cpHashSetRemove() above. + cpHashSetBin **prev_ptr = &set->table[i]; + cpHashSetBin *bin = set->table[i]; + while(bin){ + cpHashSetBin *next = bin->next; + + if(func(bin->elt, data)){ + prev_ptr = &bin->next; + } else { + (*prev_ptr) = next; + + set->entries--; + free(bin); + } + + bin = next; + } + } +} diff --git a/chipmunk/cpHashSet.h b/chipmunk/cpHashSet.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpHashSet.h @@ -0,0 +1,79 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// cpHashSet uses a chained hashtable implementation. +// Other than the transformation functions, there is nothing fancy going on. + +// cpHashSetBin's form the linked lists in the chained hash table. +typedef struct cpHashSetBin { + // Pointer to the element. + void *elt; + // Hash value of the element. + unsigned int hash; + // Next element in the chain. + struct cpHashSetBin *next; +} cpHashSetBin; + +// Equality function. Returns true if ptr is equal to elt. +typedef int (*cpHashSetEqlFunc)(void *ptr, void *elt); +// Used by cpHashSetInsert(). Called to transform the ptr into an element. +typedef void *(*cpHashSetTransFunc)(void *ptr, void *data); +// Iterator function for a hashset. +typedef void (*cpHashSetIterFunc)(void *elt, void *data); +// Reject function. Returns true if elt should be dropped. +typedef int (*cpHashSetRejectFunc)(void *elt, void *data); + +typedef struct cpHashSet { + // Number of elements stored in the table. + int entries; + // Number of cells in the table. + int size; + + cpHashSetEqlFunc eql; + cpHashSetTransFunc trans; + + // Default value returned by cpHashSetFind() when no element is found. + // Defaults to NULL. + void *default_value; + + cpHashSetBin **table; +} cpHashSet; + +// Basic allocation/destruction functions. +void cpHashSetDestroy(cpHashSet *set); +void cpHashSetFree(cpHashSet *set); + +cpHashSet *cpHashSetAlloc(void); +cpHashSet *cpHashSetInit(cpHashSet *set, int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); +cpHashSet *cpHashSetNew(int size, cpHashSetEqlFunc eqlFunc, cpHashSetTransFunc trans); + +// Insert an element into the set, returns the element. +// If it doesn't already exist, the transformation function is applied. +void *cpHashSetInsert(cpHashSet *set, unsigned int hash, void *ptr, void *data); +// Remove and return an element from the set. +void *cpHashSetRemove(cpHashSet *set, unsigned int hash, void *ptr); +// Find an element in the set. Returns the default value if the element isn't found. +void *cpHashSetFind(cpHashSet *set, unsigned int hash, void *ptr); + +// Iterate over a hashset. +void cpHashSetEach(cpHashSet *set, cpHashSetIterFunc func, void *data); +// Iterate over a hashset while rejecting certain elements. +void cpHashSetReject(cpHashSet *set, cpHashSetRejectFunc func, void *data); diff --git a/chipmunk/cpJoint.c b/chipmunk/cpJoint.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpJoint.c @@ -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 +#include + +#include "chipmunk.h" + +// TODO: Comment me! + +cpFloat cp_joint_bias_coef = 0.1f; + +void cpJointDestroy(cpJoint *joint){} + +void +cpJointFree(cpJoint *joint) +{ + if(joint) cpJointDestroy(joint); + free(joint); +} + +static void +cpJointInit(cpJoint *joint, const cpJointClass *klass, cpBody *a, cpBody *b) +{ + joint->klass = klass; + joint->a = a; + joint->b = b; +} + + +static inline cpVect +relative_velocity(cpVect r1, cpVect v1, cpFloat w1, cpVect r2, cpVect v2, cpFloat w2){ + cpVect v1_sum = cpvadd(v1, cpvmult(cpvperp(r1), w1)); + cpVect v2_sum = cpvadd(v2, cpvmult(cpvperp(r2), w2)); + + return cpvsub(v2_sum, v1_sum); +} + +static inline cpFloat +scalar_k(cpBody *a, cpBody *b, cpVect r1, cpVect r2, cpVect n) +{ + cpFloat mass_sum = a->m_inv + b->m_inv; + cpFloat r1cn = cpvcross(r1, n); + cpFloat r2cn = cpvcross(r2, n); + + return mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn; +} + +static inline void +apply_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyImpulse(a, cpvneg(j), r1); + cpBodyApplyImpulse(b, j, r2); +} + +static inline void +apply_bias_impulses(cpBody *a , cpBody *b, cpVect r1, cpVect r2, cpVect j) +{ + cpBodyApplyBiasImpulse(a, cpvneg(j), r1); + cpBodyApplyBiasImpulse(b, j, r2); +} + + +static void +pinJointPreStep(cpJoint *joint, cpFloat dt_inv) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + cpPinJoint *jnt = (cpPinJoint *)joint; + + jnt->r1 = cpvrotate(jnt->anchr1, a->rot); + jnt->r2 = cpvrotate(jnt->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); + cpFloat dist = cpvlength(delta); + jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + // calculate mass normal + jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n); + + // calculate bias velocity + jnt->bias = -cp_joint_bias_coef*dt_inv*(dist - jnt->dist); + jnt->jBias = 0.0f; + + // apply accumulated impulse + cpVect j = cpvmult(jnt->n, jnt->jnAcc); + apply_impulses(a, b, jnt->r1, jnt->r2, j); +} + +static void +pinJointApplyImpulse(cpJoint *joint) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + + cpPinJoint *jnt = (cpPinJoint *)joint; + cpVect n = jnt->n; + cpVect r1 = jnt->r1; + cpVect r2 = jnt->r2; + + //calculate bias impulse + cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); + cpFloat vbn = cpvdot(vbr, n); + + cpFloat jbn = (jnt->bias - vbn)*jnt->nMass; + jnt->jBias += jbn; + + cpVect jb = cpvmult(n, jbn); + apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); + + // compute relative velocity + cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); + cpFloat vrn = cpvdot(vr, n); + + // compute normal impulse + cpFloat jn = -vrn*jnt->nMass; + jnt->jnAcc =+ jn; + + // apply impulse + cpVect j = cpvmult(n, jn); + apply_impulses(a, b, jnt->r1, jnt->r2, j); +} + +static const cpJointClass pinJointClass = { + CP_PIN_JOINT, + pinJointPreStep, + pinJointApplyImpulse, +}; + +cpPinJoint * +cpPinJointAlloc(void) +{ + return (cpPinJoint *)malloc(sizeof(cpPinJoint)); +} + +cpPinJoint * +cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + cpJointInit((cpJoint *)joint, &pinJointClass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + + cpVect p1 = cpvadd(a->p, cpvrotate(anchr1, a->rot)); + cpVect p2 = cpvadd(b->p, cpvrotate(anchr2, b->rot)); + joint->dist = cpvlength(cpvsub(p2, p1)); + + joint->jnAcc = 0.0; + + return joint; +} + +cpJoint * +cpPinJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2) +{ + return (cpJoint *)cpPinJointInit(cpPinJointAlloc(), a, b, anchr1, anchr2); +} + + + + +static void +slideJointPreStep(cpJoint *joint, cpFloat dt_inv) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + cpSlideJoint *jnt = (cpSlideJoint *)joint; + + jnt->r1 = cpvrotate(jnt->anchr1, a->rot); + jnt->r2 = cpvrotate(jnt->anchr2, b->rot); + + cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); + cpFloat dist = cpvlength(delta); + cpFloat pdist = 0.0; + if(dist > jnt->max) { + pdist = dist - jnt->max; + } else if(dist < jnt->min) { + pdist = jnt->min - dist; + dist = -dist; + } + jnt->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY)); + + // calculate mass normal + jnt->nMass = 1.0f/scalar_k(a, b, jnt->r1, jnt->r2, jnt->n); + + // calculate bias velocity + jnt->bias = -cp_joint_bias_coef*dt_inv*(pdist); + jnt->jBias = 0.0f; + + // apply accumulated impulse + if(!jnt->bias) //{ + // if bias is 0, then the joint is not at a limit. + jnt->jnAcc = 0.0f; +// } else { + cpVect j = cpvmult(jnt->n, jnt->jnAcc); + apply_impulses(a, b, jnt->r1, jnt->r2, j); +// } +} + +static void +slideJointApplyImpulse(cpJoint *joint) +{ + cpSlideJoint *jnt = (cpSlideJoint *)joint; + if(!jnt->bias) return; // early exit + + cpBody *a = joint->a; + cpBody *b = joint->b; + + cpVect n = jnt->n; + cpVect r1 = jnt->r1; + cpVect r2 = jnt->r2; + + //calculate bias impulse + cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); + cpFloat vbn = cpvdot(vbr, n); + + cpFloat jbn = (jnt->bias - vbn)*jnt->nMass; + cpFloat jbnOld = jnt->jBias; + jnt->jBias = cpfmin(jbnOld + jbn, 0.0f); + jbn = jnt->jBias - jbnOld; + + cpVect jb = cpvmult(n, jbn); + apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); + + // compute relative velocity + cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); + cpFloat vrn = cpvdot(vr, n); + + // compute normal impulse + cpFloat jn = -vrn*jnt->nMass; + cpFloat jnOld = jnt->jnAcc; + jnt->jnAcc = cpfmin(jnOld + jn, 0.0f); + jn = jnt->jnAcc - jnOld; + + // apply impulse + cpVect j = cpvmult(n, jn); + apply_impulses(a, b, jnt->r1, jnt->r2, j); +} + +static const cpJointClass slideJointClass = { + CP_SLIDE_JOINT, + slideJointPreStep, + slideJointApplyImpulse, +}; + +cpSlideJoint * +cpSlideJointAlloc(void) +{ + return (cpSlideJoint *)malloc(sizeof(cpSlideJoint)); +} + +cpSlideJoint * +cpSlideJointInit(cpSlideJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + cpJointInit((cpJoint *)joint, &slideJointClass, a, b); + + joint->anchr1 = anchr1; + joint->anchr2 = anchr2; + joint->min = min; + joint->max = max; + + joint->jnAcc = 0.0; + + return joint; +} + +cpJoint * +cpSlideJointNew(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat min, cpFloat max) +{ + return (cpJoint *)cpSlideJointInit(cpSlideJointAlloc(), a, b, anchr1, anchr2, min, max); +} + + + + +static void +pivotJointPreStep(cpJoint *joint, cpFloat dt_inv) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + cpPivotJoint *jnt = (cpPivotJoint *)joint; + + jnt->r1 = cpvrotate(jnt->anchr1, a->rot); + jnt->r2 = cpvrotate(jnt->anchr2, b->rot); + + // calculate mass matrix + // If I wasn't lazy, this wouldn't be so gross... + cpFloat k11, k12, k21, k22; + + cpFloat m_sum = a->m_inv + b->m_inv; + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + cpFloat r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv; + cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv; + cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv; + cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv; + cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + cpFloat det_inv = 1.0f/(k11*k22 - k12*k21); + jnt->k1 = cpv( k22*det_inv, -k12*det_inv); + jnt->k2 = cpv(-k21*det_inv, k11*det_inv); + + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); + jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv); + jnt->jBias = cpvzero; + + // apply accumulated impulse + apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc); +} + +static void +pivotJointApplyImpulse(cpJoint *joint) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + + cpPivotJoint *jnt = (cpPivotJoint *)joint; + cpVect r1 = jnt->r1; + cpVect r2 = jnt->r2; + cpVect k1 = jnt->k1; + cpVect k2 = jnt->k2; + + //calculate bias impulse + cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); + vbr = cpvsub(jnt->bias, vbr); + + cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2)); + jnt->jBias = cpvadd(jnt->jBias, jb); + + apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); + + // compute relative velocity + cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); + + // compute normal impulse + cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2)); + jnt->jAcc = cpvadd(jnt->jAcc, j); + + // apply impulse + apply_impulses(a, b, jnt->r1, jnt->r2, j); +} + +static const cpJointClass pivotJointClass = { + CP_PIVOT_JOINT, + pivotJointPreStep, + pivotJointApplyImpulse, +}; + +cpPivotJoint * +cpPivotJointAlloc(void) +{ + return (cpPivotJoint *)malloc(sizeof(cpPivotJoint)); +} + +cpPivotJoint * +cpPivotJointInit(cpPivotJoint *joint, cpBody *a, cpBody *b, cpVect pivot) +{ + cpJointInit((cpJoint *)joint, &pivotJointClass, a, b); + + joint->anchr1 = cpvunrotate(cpvsub(pivot, a->p), a->rot); + joint->anchr2 = cpvunrotate(cpvsub(pivot, b->p), b->rot); + + joint->jAcc = cpvzero; + + return joint; +} + +cpJoint * +cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot) +{ + return (cpJoint *)cpPivotJointInit(cpPivotJointAlloc(), a, b, pivot); +} + + + + +static void +grooveJointPreStep(cpJoint *joint, cpFloat dt_inv) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + cpGrooveJoint *jnt = (cpGrooveJoint *)joint; + + // calculate endpoints in worldspace + cpVect ta = cpBodyLocal2World(a, jnt->grv_a); + cpVect tb = cpBodyLocal2World(a, jnt->grv_b); + + // calculate axis + cpVect n = cpvrotate(jnt->grv_n, a->rot); + cpFloat d = cpvdot(ta, n); + + jnt->grv_tn = n; + jnt->r2 = cpvrotate(jnt->anchr2, b->rot); + + // calculate tangential distance along the axis of r2 + cpFloat td = cpvcross(cpvadd(b->p, jnt->r2), n); + // calculate clamping factor and r2 + if(td <= cpvcross(ta, n)){ + jnt->clamp = 1.0f; + jnt->r1 = cpvsub(ta, a->p); + } else if(td >= cpvcross(tb, n)){ + jnt->clamp = -1.0f; + jnt->r1 = cpvsub(tb, a->p); + } else { + jnt->clamp = 0.0f; + jnt->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p); + } + + // calculate mass matrix + // If I wasn't lazy and wrote a proper matrix class, this wouldn't be so gross... + cpFloat k11, k12, k21, k22; + cpFloat m_sum = a->m_inv + b->m_inv; + + // start with I*m_sum + k11 = m_sum; k12 = 0.0f; + k21 = 0.0f; k22 = m_sum; + + // add the influence from r1 + cpFloat r1xsq = jnt->r1.x * jnt->r1.x * a->i_inv; + cpFloat r1ysq = jnt->r1.y * jnt->r1.y * a->i_inv; + cpFloat r1nxy = -jnt->r1.x * jnt->r1.y * a->i_inv; + k11 += r1ysq; k12 += r1nxy; + k21 += r1nxy; k22 += r1xsq; + + // add the influnce from r2 + cpFloat r2xsq = jnt->r2.x * jnt->r2.x * b->i_inv; + cpFloat r2ysq = jnt->r2.y * jnt->r2.y * b->i_inv; + cpFloat r2nxy = -jnt->r2.x * jnt->r2.y * b->i_inv; + k11 += r2ysq; k12 += r2nxy; + k21 += r2nxy; k22 += r2xsq; + + // invert + cpFloat det_inv = 1.0f/(k11*k22 - k12*k21); + jnt->k1 = cpv( k22*det_inv, -k12*det_inv); + jnt->k2 = cpv(-k21*det_inv, k11*det_inv); + + + // calculate bias velocity + cpVect delta = cpvsub(cpvadd(b->p, jnt->r2), cpvadd(a->p, jnt->r1)); + jnt->bias = cpvmult(delta, -cp_joint_bias_coef*dt_inv); + jnt->jBias = cpvzero; + + // apply accumulated impulse + apply_impulses(a, b, jnt->r1, jnt->r2, jnt->jAcc); +} + +static inline cpVect +grooveConstrain(cpGrooveJoint *jnt, cpVect j){ + cpVect n = jnt->grv_tn; + cpVect jn = cpvmult(n, cpvdot(j, n)); + + cpVect t = cpvperp(n); + cpFloat coef = (jnt->clamp*cpvcross(j, n) > 0.0f) ? 1.0f : 0.0f; + cpVect jt = cpvmult(t, cpvdot(j, t)*coef); + + return cpvadd(jn, jt); +} + +static void +grooveJointApplyImpulse(cpJoint *joint) +{ + cpBody *a = joint->a; + cpBody *b = joint->b; + + cpGrooveJoint *jnt = (cpGrooveJoint *)joint; + cpVect r1 = jnt->r1; + cpVect r2 = jnt->r2; + cpVect k1 = jnt->k1; + cpVect k2 = jnt->k2; + + //calculate bias impulse + cpVect vbr = relative_velocity(r1, a->v_bias, a->w_bias, r2, b->v_bias, b->w_bias); + vbr = cpvsub(jnt->bias, vbr); + + cpVect jb = cpv(cpvdot(vbr, k1), cpvdot(vbr, k2)); + cpVect jbOld = jnt->jBias; + jnt->jBias = grooveConstrain(jnt, cpvadd(jbOld, jb)); + jb = cpvsub(jnt->jBias, jbOld); + + apply_bias_impulses(a, b, jnt->r1, jnt->r2, jb); + + // compute impulse + cpVect vr = relative_velocity(r1, a->v, a->w, r2, b->v, b->w); + + cpVect j = cpv(-cpvdot(vr, k1), -cpvdot(vr, k2)); + cpVect jOld = jnt->jAcc; + jnt->jAcc = grooveConstrain(jnt, cpvadd(jOld, j)); + j = cpvsub(jnt->jAcc, jOld); + + // apply impulse + apply_impulses(a, b, jnt->r1, jnt->r2, j); +} + +static const cpJointClass grooveJointClass = { + CP_GROOVE_JOINT, + grooveJointPreStep, + grooveJointApplyImpulse, +}; + +cpGrooveJoint * +cpGrooveJointAlloc(void) +{ + return (cpGrooveJoint *)malloc(sizeof(cpGrooveJoint)); +} + +cpGrooveJoint * +cpGrooveJointInit(cpGrooveJoint *joint, cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + cpJointInit((cpJoint *)joint, &grooveJointClass, a, b); + + joint->grv_a = groove_a; + joint->grv_b = groove_b; + joint->grv_n = cpvperp(cpvnormalize(cpvsub(groove_b, groove_a))); + joint->anchr2 = anchr2; + + joint->jAcc = cpvzero; + + return joint; +} + +cpJoint * +cpGrooveJointNew(cpBody *a, cpBody *b, cpVect groove_a, cpVect groove_b, cpVect anchr2) +{ + return (cpJoint *)cpGrooveJointInit(cpGrooveJointAlloc(), a, b, groove_a, groove_b, anchr2); +} + diff --git a/chipmunk/cpJoint.h b/chipmunk/cpJoint.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpJoint.h @@ -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); diff --git a/chipmunk/cpPolyShape.c b/chipmunk/cpPolyShape.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpPolyShape.c @@ -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 +#include +#include + +#include "chipmunk.h" + +cpPolyShape * +cpPolyShapeAlloc(void) +{ + return (cpPolyShape *)calloc(1, sizeof(cpPolyShape)); +} + +static void +cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpVect *src = poly->verts; + cpVect *dst = poly->tVerts; + + for(int i=0; inumVerts; i++) + dst[i] = cpvadd(p, cpvrotate(src[i], rot)); +} + +static void +cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot) +{ + cpPolyShapeAxis *src = poly->axes; + cpPolyShapeAxis *dst = poly->tAxes; + + for(int i=0; inumVerts; i++){ + cpVect n = cpvrotate(src[i].n, rot); + dst[i].n = n; + dst[i].d = cpvdot(p, n) + src[i].d; + } +} + +static cpBB +cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + cpFloat l, b, r, t; + + cpPolyShapeTransformAxes(poly, p, rot); + cpPolyShapeTransformVerts(poly, p, rot); + + cpVect *verts = poly->tVerts; + l = r = verts[0].x; + b = t = verts[0].y; + + // TODO do as part of cpPolyShapeTransformVerts? + for(int i=1; inumVerts; i++){ + cpVect v = verts[i]; + + l = cpfmin(l, v.x); + r = cpfmax(r, v.x); + + b = cpfmin(b, v.y); + t = cpfmax(t, v.y); + } + + return cpBBNew(l, b, r, t); +} + +static void +cpPolyShapeDestroy(cpShape *shape) +{ + cpPolyShape *poly = (cpPolyShape *)shape; + + free(poly->verts); + free(poly->tVerts); + + free(poly->axes); + free(poly->tAxes); +} + +static int +cpPolyShapePointQuery(cpShape *shape, cpVect p){ + // TODO Check against BB first? + return cpPolyShapeContainsVert((cpPolyShape *)shape, p); +} + +static const cpShapeClass polyClass = { + CP_POLY_SHAPE, + cpPolyShapeCacheData, + cpPolyShapeDestroy, + cpPolyShapePointQuery, +}; + +cpPolyShape * +cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + poly->numVerts = numVerts; + + poly->verts = (cpVect *)calloc(numVerts, sizeof(cpVect)); + poly->tVerts = (cpVect *)calloc(numVerts, sizeof(cpVect)); + poly->axes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); + poly->tAxes = (cpPolyShapeAxis *)calloc(numVerts, sizeof(cpPolyShapeAxis)); + + for(int i=0; iverts[i] = a; + poly->axes[i].n = n; + poly->axes[i].d = cpvdot(n, a); + } + + cpShapeInit((cpShape *)poly, &polyClass, body); + + return poly; +} + +cpShape * +cpPolyShapeNew(cpBody *body, int numVerts, cpVect *verts, cpVect offset) +{ + return (cpShape *)cpPolyShapeInit(cpPolyShapeAlloc(), body, numVerts, verts, offset); +} diff --git a/chipmunk/cpPolyShape.h b/chipmunk/cpPolyShape.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpPolyShape.h @@ -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; inumVerts; i++) + min = cpfmin(min, cpvdot(n, verts[i])); + + return min - d; +} + +// Returns true if the polygon contains the vertex. +static inline int +cpPolyShapeContainsVert(cpPolyShape *poly, cpVect v) +{ + cpPolyShapeAxis *axes = poly->tAxes; + + int i; + for(i=0; inumVerts; i++){ + cpFloat dist = cpvdot(axes[i].n, v) - axes[i].d; + if(dist > 0.0) return 0; + } + + return 1; +} diff --git a/chipmunk/cpShape.c b/chipmunk/cpShape.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpShape.c @@ -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 +#include +#include + +#include "chipmunk.h" +#include "math.h" + +unsigned int SHAPE_ID_COUNTER = 0; + +void +cpResetShapeIdCounter(void) +{ + SHAPE_ID_COUNTER = 0; +} + + +cpShape* +cpShapeInit(cpShape *shape, const cpShapeClass *klass, cpBody *body) +{ + shape->klass = klass; + + shape->id = SHAPE_ID_COUNTER; + SHAPE_ID_COUNTER++; + + assert(body != NULL); + shape->body = body; + + shape->e = 0.0f; + shape->u = 0.0f; + shape->surface_v = cpvzero; + + shape->collision_type = 0; + shape->group = 0; + shape->layers = 0xFFFF; + + shape->data = NULL; + + cpShapeCacheBB(shape); + + return shape; +} + +void +cpShapeDestroy(cpShape *shape) +{ + if(shape->klass->destroy) shape->klass->destroy(shape); +} + +void +cpShapeFree(cpShape *shape) +{ + if(shape) cpShapeDestroy(shape); + free(shape); +} + +cpBB +cpShapeCacheBB(cpShape *shape) +{ + cpBody *body = shape->body; + + shape->bb = shape->klass->cacheData(shape, body->p, body->rot); + return shape->bb; +} + +int +cpShapePointQuery(cpShape *shape, cpVect p){ + return shape->klass->pointQuery(shape, p); +} + + + +cpCircleShape * +cpCircleShapeAlloc(void) +{ + return (cpCircleShape *)calloc(1, sizeof(cpCircleShape)); +} + +static inline cpBB +bbFromCircle(const cpVect c, const cpFloat r) +{ + return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r); +} + +static cpBB +cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpCircleShape *circle = (cpCircleShape *)shape; + + circle->tc = cpvadd(p, cpvrotate(circle->c, rot)); + return bbFromCircle(circle->tc, circle->r); +} + +static int +cpCircleShapePointQuery(cpShape *shape, cpVect p){ + cpCircleShape *circle = (cpCircleShape *)shape; + + cpFloat distSQ = cpvlengthsq(cpvsub(circle->tc, p)); + return distSQ <= (circle->r*circle->r); +} + +static const cpShapeClass circleClass = { + CP_CIRCLE_SHAPE, + cpCircleShapeCacheData, + NULL, + cpCircleShapePointQuery, +}; + +cpCircleShape * +cpCircleShapeInit(cpCircleShape *circle, cpBody *body, cpFloat radius, cpVect offset) +{ + circle->c = offset; + circle->r = radius; + + cpShapeInit((cpShape *)circle, &circleClass, body); + + return circle; +} + +cpShape * +cpCircleShapeNew(cpBody *body, cpFloat radius, cpVect offset) +{ + return (cpShape *)cpCircleShapeInit(cpCircleShapeAlloc(), body, radius, offset); +} + +cpSegmentShape * +cpSegmentShapeAlloc(void) +{ + return (cpSegmentShape *)calloc(1, sizeof(cpSegmentShape)); +} + +static cpBB +cpSegmentShapeCacheData(cpShape *shape, cpVect p, cpVect rot) +{ + cpSegmentShape *seg = (cpSegmentShape *)shape; + + seg->ta = cpvadd(p, cpvrotate(seg->a, rot)); + seg->tb = cpvadd(p, cpvrotate(seg->b, rot)); + seg->tn = cpvrotate(seg->n, rot); + + cpFloat l,r,s,t; + + if(seg->ta.x < seg->tb.x){ + l = seg->ta.x; + r = seg->tb.x; + } else { + l = seg->tb.x; + r = seg->ta.x; + } + + if(seg->ta.y < seg->tb.y){ + s = seg->ta.y; + t = seg->tb.y; + } else { + s = seg->tb.y; + t = seg->ta.y; + } + + cpFloat rad = seg->r; + return cpBBNew(l - rad, s - rad, r + rad, t + rad); +} + +static int +cpSegmentShapePointQuery(cpShape *shape, cpVect p){ + cpSegmentShape *seg = (cpSegmentShape *)shape; + + // Calculate normal distance from segment. + cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn); + cpFloat dist = fabs(dn) - seg->r; + if(dist > 0.0f) return 0; + + // Calculate tangential distance along segment. + cpFloat dt = -cpvcross(seg->tn, p); + cpFloat dtMin = -cpvcross(seg->tn, seg->ta); + cpFloat dtMax = -cpvcross(seg->tn, seg->tb); + + // Decision tree to decide which feature of the segment to collide with. + if(dt <= dtMin){ + if(dt < (dtMin - seg->r)){ + return 0; + } else { + return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r); + } + } else { + if(dt < dtMax){ + return 1; + } else { + if(dt < (dtMax + seg->r)) { + return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r); + } else { + return 0; + } + } + } + + return 1; +} + +static const cpShapeClass segmentClass = { + CP_SEGMENT_SHAPE, + cpSegmentShapeCacheData, + NULL, + cpSegmentShapePointQuery, +}; + +cpSegmentShape * +cpSegmentShapeInit(cpSegmentShape *seg, cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + seg->a = a; + seg->b = b; + seg->n = cpvperp(cpvnormalize(cpvsub(b, a))); + + seg->r = r; + + cpShapeInit((cpShape *)seg, &segmentClass, body); + + return seg; +} + +cpShape* +cpSegmentShapeNew(cpBody *body, cpVect a, cpVect b, cpFloat r) +{ + return (cpShape *)cpSegmentShapeInit(cpSegmentShapeAlloc(), body, a, b, r); +} diff --git a/chipmunk/cpShape.h b/chipmunk/cpShape.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpShape.h @@ -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); diff --git a/chipmunk/cpSpace.c b/chipmunk/cpSpace.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpSpace.c @@ -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 +#include +#include +#include + +#include "chipmunk.h" + +int cp_contact_persistence = 3; + +// Equal function for contactSet. +static int +contactSetEql(void *ptr, void *elt) +{ + cpShape **shapes = (cpShape **)ptr; + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + cpArbiter *arb = (cpArbiter *)elt; + + return ((a == arb->a && b == arb->b) || (b == arb->a && a == arb->b)); +} + +// Transformation function for contactSet. +static void * +contactSetTrans(void *ptr, void *data) +{ + cpShape **shapes = (cpShape **)ptr; + cpShape *a = shapes[0]; + cpShape *b = shapes[1]; + + cpSpace *space = (cpSpace *)data; + + return cpArbiterNew(a, b, space->stamp); +} + +// Collision pair function wrapper struct. +typedef struct collFuncData { + cpCollFunc func; + void *data; +} collFuncData; + +// Equals function for collFuncSet. +static int +collFuncSetEql(void *ptr, void *elt) +{ + unsigned int *ids = (unsigned int *)ptr; + unsigned int a = ids[0]; + unsigned int b = ids[1]; + + cpCollPairFunc *pair = (cpCollPairFunc *)elt; + + return ((a == pair->a && b == pair->b) || (b == pair->a && a == pair->b)); +} + +// Transformation function for collFuncSet. +static void * +collFuncSetTrans(void *ptr, void *data) +{ + unsigned int *ids = (unsigned int *)ptr; + collFuncData *funcData = (collFuncData *)data; + + cpCollPairFunc *pair = (cpCollPairFunc *)malloc(sizeof(cpCollPairFunc)); + pair->a = ids[0]; + pair->b = ids[1]; + pair->func = funcData->func; + pair->data = funcData->data; + + return pair; +} + +// Default collision pair function. +static int +alwaysCollide(cpShape *a, cpShape *b, cpContact *arr, int numCon, cpFloat normal_coef, void *data) +{ + return 1; +} + +// BBfunc callback for the spatial hash. +static cpBB +bbfunc(void *ptr) +{ + cpShape *shape = (cpShape *)ptr; + return shape->bb; +} + +// Iterator functions for destructors. +static void freeWrap(void *ptr, void *unused){ free( ptr);} +static void shapeFreeWrap(void *ptr, void *unused){ cpShapeFree((cpShape *) ptr);} +static void arbiterFreeWrap(void *ptr, void *unused){ cpArbiterFree((cpArbiter *)ptr);} +static void bodyFreeWrap(void *ptr, void *unused){ cpBodyFree((cpBody *) ptr);} +static void jointFreeWrap(void *ptr, void *unused){ cpJointFree((cpJoint *) ptr);} + +cpSpace* +cpSpaceAlloc(void) +{ + return (cpSpace *)calloc(1, sizeof(cpSpace)); +} + +#define DEFAULT_DIM_SIZE 100.0f +#define DEFAULT_COUNT 1000 +#define DEFAULT_ITERATIONS 10 +#define DEFAULT_ELASTIC_ITERATIONS 0 + +cpSpace* +cpSpaceInit(cpSpace *space) +{ + space->iterations = DEFAULT_ITERATIONS; + space->elasticIterations = DEFAULT_ELASTIC_ITERATIONS; +// space->sleepTicks = 300; + + space->gravity = cpvzero; + space->damping = 1.0f; + + space->stamp = 0; + + space->staticShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc); + space->activeShapes = cpSpaceHashNew(DEFAULT_DIM_SIZE, DEFAULT_COUNT, &bbfunc); + + space->bodies = cpArrayNew(0); + space->arbiters = cpArrayNew(0); + space->contactSet = cpHashSetNew(0, contactSetEql, contactSetTrans); + + space->joints = cpArrayNew(0); + + cpCollPairFunc pairFunc = {0, 0, alwaysCollide, NULL}; + space->defaultPairFunc = pairFunc; + space->collFuncSet = cpHashSetNew(0, collFuncSetEql, collFuncSetTrans); + space->collFuncSet->default_value = &space->defaultPairFunc; + + return space; +} + +cpSpace* +cpSpaceNew(void) +{ + return cpSpaceInit(cpSpaceAlloc()); +} + +void +cpSpaceDestroy(cpSpace *space) +{ + cpSpaceHashFree(space->staticShapes); + cpSpaceHashFree(space->activeShapes); + + cpArrayFree(space->bodies); + + cpArrayFree(space->joints); + + if(space->contactSet) + cpHashSetEach(space->contactSet, &arbiterFreeWrap, NULL); + cpHashSetFree(space->contactSet); + cpArrayFree(space->arbiters); + + if(space->collFuncSet) + cpHashSetEach(space->collFuncSet, &freeWrap, NULL); + cpHashSetFree(space->collFuncSet); +} + +void +cpSpaceFree(cpSpace *space) +{ + if(space) cpSpaceDestroy(space); + free(space); +} + +void +cpSpaceFreeChildren(cpSpace *space) +{ + cpSpaceHashEach(space->staticShapes, &shapeFreeWrap, NULL); + cpSpaceHashEach(space->activeShapes, &shapeFreeWrap, NULL); + cpArrayEach(space->bodies, &bodyFreeWrap, NULL); + cpArrayEach(space->joints, &jointFreeWrap, NULL); +} + +void +cpSpaceAddCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b, + cpCollFunc func, void *data) +{ + unsigned int ids[] = {a, b}; + unsigned int hash = CP_HASH_PAIR(a, b); + // Remove any old function so the new one will get added. + cpSpaceRemoveCollisionPairFunc(space, a, b); + + collFuncData funcData = {func, data}; + cpHashSetInsert(space->collFuncSet, hash, ids, &funcData); +} + +void +cpSpaceRemoveCollisionPairFunc(cpSpace *space, unsigned int a, unsigned int b) +{ + unsigned int ids[] = {a, b}; + unsigned int hash = CP_HASH_PAIR(a, b); + cpCollPairFunc *old_pair = (cpCollPairFunc *)cpHashSetRemove(space->collFuncSet, hash, ids); + free(old_pair); +} + +void +cpSpaceSetDefaultCollisionPairFunc(cpSpace *space, cpCollFunc func, void *data) +{ + cpCollPairFunc pairFunc = {0, 0, (func ? func : alwaysCollide), (func ? data : NULL)}; + space->defaultPairFunc = pairFunc; +} + +void +cpSpaceAddShape(cpSpace *space, cpShape *shape) +{ + cpSpaceHashInsert(space->activeShapes, shape, shape->id, shape->bb); +} + +void +cpSpaceAddStaticShape(cpSpace *space, cpShape *shape) +{ + cpShapeCacheBB(shape); + cpSpaceHashInsert(space->staticShapes, shape, shape->id, shape->bb); +} + +void +cpSpaceAddBody(cpSpace *space, cpBody *body) +{ + cpArrayPush(space->bodies, body); +} + +void +cpSpaceAddJoint(cpSpace *space, cpJoint *joint) +{ + cpArrayPush(space->joints, joint); +} + +void +cpSpaceRemoveShape(cpSpace *space, cpShape *shape) +{ + cpSpaceHashRemove(space->activeShapes, shape, shape->id); +} + +void +cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) +{ + cpSpaceHashRemove(space->staticShapes, shape, shape->id); +} + +void +cpSpaceRemoveBody(cpSpace *space, cpBody *body) +{ + cpArrayDeleteObj(space->bodies, body); +} + +void +cpSpaceRemoveJoint(cpSpace *space, cpJoint *joint) +{ + cpArrayDeleteObj(space->joints, joint); +} + +void +cpSpaceEachBody(cpSpace *space, cpSpaceBodyIterator func, void *data) +{ + cpArray *bodies = space->bodies; + + for(int i=0; inum; i++) + func((cpBody *)bodies->arr[i], data); +} + +// Iterator function used for updating shape BBoxes. +static void +updateBBCache(void *ptr, void *unused) +{ + cpShape *shape = (cpShape *)ptr; + cpShapeCacheBB(shape); +} + +void +cpSpaceResizeStaticHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpaceHashResize(space->staticShapes, dim, count); + cpSpaceHashRehash(space->staticShapes); +} + +void +cpSpaceResizeActiveHash(cpSpace *space, cpFloat dim, int count) +{ + cpSpaceHashResize(space->activeShapes, dim, count); +} + +void +cpSpaceRehashStatic(cpSpace *space) +{ + cpSpaceHashEach(space->staticShapes, &updateBBCache, NULL); + cpSpaceHashRehash(space->staticShapes); +} + +typedef struct pointQueryFuncPair { + cpSpacePointQueryFunc func; + void *data; +} pointQueryFuncPair; + +static int +pointQueryHelper(void *point, void *obj, void *data) +{ + cpShape *shape = (cpShape *)obj; + pointQueryFuncPair *pair = (pointQueryFuncPair *)data; + + if(cpShapePointQuery(shape, *((cpVect *)point))) + pair->func(shape, pair->data); + + return 1; // return value needed for historical reasons (value is ignored) +} + +static void +pointQuery(cpSpaceHash *hash, cpVect point, cpSpacePointQueryFunc func, void *data) +{ + pointQueryFuncPair pair = {func, data}; + cpSpaceHashPointQuery(hash, point, pointQueryHelper, &pair); +} + +void +cpSpaceShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data) +{ + pointQuery(space->activeShapes, point, func, data); +} + +void +cpSpaceStaticShapePointQuery(cpSpace *space, cpVect point, cpSpacePointQueryFunc func, void *data) +{ + pointQuery(space->staticShapes, point, func, data); +} + +static inline int +queryReject(cpShape *a, cpShape *b) +{ + return + // BBoxes must overlap + !cpBBintersects(a->bb, b->bb) + // Don't collide shapes attached to the same body. + || a->body == b->body + // Don't collide objects in the same non-zero group + || (a->group && b->group && a->group == b->group) + // Don't collide objects that don't share at least on layer. + || !(a->layers & b->layers); +} + +// Callback from the spatial hash. +// TODO: Refactor this into separate functions? +static int +queryFunc(void *p1, void *p2, void *data) +{ + // Cast the generic pointers from the spatial hash back to usefull types + cpShape *a = (cpShape *)p1; + cpShape *b = (cpShape *)p2; + cpSpace *space = (cpSpace *)data; + + // Reject any of the simple cases + if(queryReject(a,b)) return 0; + + // Shape 'a' should have the lower shape type. (required by cpCollideShapes() ) + if(a->klass->type > b->klass->type){ + cpShape *temp = a; + a = b; + b = temp; + } + + // Find the collision pair function for the shapes. + unsigned int ids[] = {a->collision_type, b->collision_type}; + unsigned int hash = CP_HASH_PAIR(a->collision_type, b->collision_type); + cpCollPairFunc *pairFunc = (cpCollPairFunc *)cpHashSetFind(space->collFuncSet, hash, ids); + if(!pairFunc->func) return 0; // A NULL pair function means don't collide at all. + + // Narrow-phase collision detection. + cpContact *contacts = NULL; + int numContacts = cpCollideShapes(a, b, &contacts); + if(!numContacts) return 0; // Shapes are not colliding. + + // The collision pair function requires objects to be ordered by their collision types. + cpShape *pair_a = a; + cpShape *pair_b = b; + cpFloat normal_coef = 1.0f; + + // Swap them if necessary. + if(pair_a->collision_type != pairFunc->a){ + cpShape *temp = pair_a; + pair_a = pair_b; + pair_b = temp; + normal_coef = -1.0f; + } + + if(pairFunc->func(pair_a, pair_b, contacts, numContacts, normal_coef, pairFunc->data)){ + // The collision pair function OKed the collision. Record the contact information. + + // Get an arbiter from space->contactSet for the two shapes. + // This is where the persistant contact magic comes from. + cpShape *shape_pair[] = {a, b}; + cpArbiter *arb = (cpArbiter *)cpHashSetInsert(space->contactSet, CP_HASH_PAIR(a, b), shape_pair, space); + + // Timestamp the arbiter. + arb->stamp = space->stamp; + arb->a = a; arb->b = b; // TODO: Investigate why this is still necessary? + // Inject the new contact points into the arbiter. + cpArbiterInject(arb, contacts, numContacts); + + // Add the arbiter to the list of active arbiters. + cpArrayPush(space->arbiters, arb); + + return numContacts; + } else { + // The collision pair function rejected the collision. + + free(contacts); + return 0; + } +} + +// Iterator for active/static hash collisions. +static void +active2staticIter(void *ptr, void *data) +{ + cpShape *shape = (cpShape *)ptr; + cpSpace *space = (cpSpace *)data; + cpSpaceHashQuery(space->staticShapes, shape, shape->bb, &queryFunc, space); +} + +// Hashset reject func to throw away old arbiters. +static int +contactSetReject(void *ptr, void *data) +{ + cpArbiter *arb = (cpArbiter *)ptr; + cpSpace *space = (cpSpace *)data; + + if((space->stamp - arb->stamp) > cp_contact_persistence){ + cpArbiterFree(arb); + return 0; + } + + return 1; +} + +void +cpSpaceStep(cpSpace *space, cpFloat dt) +{ + if(!dt) return; // prevents div by zero. + cpFloat dt_inv = 1.0f/dt; + + cpArray *bodies = space->bodies; + cpArray *arbiters = space->arbiters; + cpArray *joints = space->joints; + + // Empty the arbiter list. + cpHashSetReject(space->contactSet, &contactSetReject, space); + space->arbiters->num = 0; + + // Integrate positions. + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->position_func(body, dt); + } + + // Pre-cache BBoxes and shape data. + cpSpaceHashEach(space->activeShapes, &updateBBCache, NULL); + + // Collide! + cpSpaceHashEach(space->activeShapes, &active2staticIter, space); + cpSpaceHashQueryRehash(space->activeShapes, &queryFunc, space); + + // Prestep the arbiters. + for(int i=0; inum; i++) + cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt_inv); + + // Prestep the joints. + for(int i=0; inum; i++){ + cpJoint *joint = (cpJoint *)joints->arr[i]; + joint->klass->preStep(joint, dt_inv); + } + + for(int i=0; ielasticIterations; i++){ + for(int j=0; jnum; j++) + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 1.0f); + + for(int j=0; jnum; j++){ + cpJoint *joint = (cpJoint *)joints->arr[j]; + joint->klass->applyImpulse(joint); + } + } + + // Integrate velocities. + cpFloat damping = pow(1.0f/space->damping, -dt); + for(int i=0; inum; i++){ + cpBody *body = (cpBody *)bodies->arr[i]; + body->velocity_func(body, space->gravity, damping, dt); + } + + for(int i=0; inum; i++) + cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i]); + + // Run the impulse solver. + for(int i=0; iiterations; i++){ + for(int j=0; jnum; j++) + cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j], 0.0f); + + for(int j=0; jnum; j++){ + cpJoint *joint = (cpJoint *)joints->arr[j]; + joint->klass->applyImpulse(joint); + } + } + +// cpFloat dvsq = cpvdot(space->gravity, space->gravity); +// dvsq *= dt*dt * space->damping*space->damping; +// for(int i=0; inum; i++) +// cpBodyMarkLowEnergy(bodies->arr[i], dvsq, space->sleepTicks); + + // Increment the stamp. + space->stamp++; +} diff --git a/chipmunk/cpSpace.h b/chipmunk/cpSpace.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpSpace.h @@ -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); diff --git a/chipmunk/cpSpaceHash.c b/chipmunk/cpSpaceHash.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpSpaceHash.c @@ -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 +#include +#include +#include + +#include "chipmunk.h" +#include "prime.h" + +static cpHandle* +cpHandleAlloc(void) +{ + return (cpHandle *)malloc(sizeof(cpHandle)); +} + +static cpHandle* +cpHandleInit(cpHandle *hand, void *obj) +{ + hand->obj = obj; + hand->retain = 0; + hand->stamp = 0; + + return hand; +} + +static cpHandle* +cpHandleNew(void *obj) +{ + return cpHandleInit(cpHandleAlloc(), obj); +} + +static inline void +cpHandleRetain(cpHandle *hand) +{ + hand->retain++; +} + +static inline void +cpHandleFree(cpHandle *hand) +{ + free(hand); +} + +static inline void +cpHandleRelease(cpHandle *hand) +{ + hand->retain--; + if(hand->retain == 0) + cpHandleFree(hand); +} + + +cpSpaceHash* +cpSpaceHashAlloc(void) +{ + return (cpSpaceHash *)calloc(1, sizeof(cpSpaceHash)); +} + +// Frees the old table, and allocates a new one. +static void +cpSpaceHashAllocTable(cpSpaceHash *hash, int numcells) +{ + free(hash->table); + + hash->numcells = numcells; + hash->table = (cpSpaceHashBin **)calloc(numcells, sizeof(cpSpaceHashBin *)); +} + +// Equality function for the handleset. +static int +handleSetEql(void *obj, void *elt) +{ + cpHandle *hand = (cpHandle *)elt; + return (obj == hand->obj); +} + +// Transformation function for the handleset. +static void * +handleSetTrans(void *obj, void *unused) +{ + cpHandle *hand = cpHandleNew(obj); + cpHandleRetain(hand); + + return hand; +} + +cpSpaceHash* +cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int numcells, cpSpaceHashBBFunc bbfunc) +{ + cpSpaceHashAllocTable(hash, next_prime(numcells)); + hash->celldim = celldim; + hash->bbfunc = bbfunc; + + hash->bins = NULL; + hash->handleSet = cpHashSetNew(0, &handleSetEql, &handleSetTrans); + + hash->stamp = 1; + + return hash; +} + +cpSpaceHash* +cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc) +{ + return cpSpaceHashInit(cpSpaceHashAlloc(), celldim, cells, bbfunc); +} + +static inline void +clearHashCell(cpSpaceHash *hash, int index) +{ + cpSpaceHashBin *bin = hash->table[index]; + while(bin){ + cpSpaceHashBin *next = bin->next; + + // Release the lock on the handle. + cpHandleRelease(bin->handle); + // Recycle the bin. + bin->next = hash->bins; + hash->bins = bin; + + bin = next; + } + + hash->table[index] = NULL; +} + +// Clear all cells in the hashtable. +static void +clearHash(cpSpaceHash *hash) +{ + for(int i=0; inumcells; i++) + clearHashCell(hash, i); +} + +// Free the recycled hash bins. +static void +freeBins(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->bins; + while(bin){ + cpSpaceHashBin *next = bin->next; + free(bin); + bin = next; + } +} + +// Hashset iterator function to free the handles. +static void +handleFreeWrap(void *elt, void *unused) +{ + cpHandle *hand = (cpHandle *)elt; + cpHandleFree(hand); +} + +void +cpSpaceHashDestroy(cpSpaceHash *hash) +{ + clearHash(hash); + freeBins(hash); + + // Free the handles. + cpHashSetEach(hash->handleSet, &handleFreeWrap, NULL); + cpHashSetFree(hash->handleSet); + + free(hash->table); +} + +void +cpSpaceHashFree(cpSpaceHash *hash) +{ + if(!hash) return; + cpSpaceHashDestroy(hash); + free(hash); +} + +void +cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells) +{ + // Clear the hash to release the old handle locks. + clearHash(hash); + + hash->celldim = celldim; + cpSpaceHashAllocTable(hash, next_prime(numcells)); +} + +// Return true if the chain contains the handle. +static inline int +containsHandle(cpSpaceHashBin *bin, cpHandle *hand) +{ + while(bin){ + if(bin->handle == hand) return 1; + bin = bin->next; + } + + return 0; +} + +// Get a recycled or new bin. +static inline cpSpaceHashBin * +getEmptyBin(cpSpaceHash *hash) +{ + cpSpaceHashBin *bin = hash->bins; + + // Make a new one if necessary. + if(bin == NULL) return (cpSpaceHashBin *)malloc(sizeof(cpSpaceHashBin)); + + hash->bins = bin->next; + return bin; +} + +// The hash function itself. +static inline unsigned int +hash_func(unsigned int x, unsigned int y, unsigned int n) +{ + return (x*2185031351ul ^ y*4232417593ul) % n; +} + +static inline void +hashHandle(cpSpaceHash *hash, cpHandle *hand, cpBB bb) +{ + // Find the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + int n = hash->numcells; + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int index = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[index]; + + // Don't add an object twice to the same cell. + if(containsHandle(bin, hand)) continue; + + cpHandleRetain(hand); + // Insert a new bin for the handle in this cell. + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[index] = newBin; + } + } +} + +void +cpSpaceHashInsert(cpSpaceHash *hash, void *obj, unsigned int id, cpBB bb) +{ + cpHandle *hand = (cpHandle *)cpHashSetInsert(hash->handleSet, id, obj, NULL); + hashHandle(hash, hand, bb); +} + +void +cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id) +{ + cpHandle *hand = (cpHandle *)cpHashSetFind(hash->handleSet, id, obj); + hashHandle(hash, hand, hash->bbfunc(obj)); +} + +// Hashset iterator function for rehashing the spatial hash. (hash hash hash hash?) +static void +handleRehashHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + cpSpaceHash *hash = (cpSpaceHash *)data; + + hashHandle(hash, hand, hash->bbfunc(hand->obj)); +} + +void +cpSpaceHashRehash(cpSpaceHash *hash) +{ + clearHash(hash); + + // Rehash all of the handles. + cpHashSetEach(hash->handleSet, &handleRehashHelper, hash); +} + +void +cpSpaceHashRemove(cpSpaceHash *hash, void *obj, unsigned int id) +{ + cpHandle *hand = (cpHandle *)cpHashSetRemove(hash->handleSet, id, obj); + + if(hand){ + hand->obj = NULL; + cpHandleRelease(hand); + } +} + +// Used by the cpSpaceHashEach() iterator. +typedef struct eachPair { + cpSpaceHashIterator func; + void *data; +} eachPair; + +// Calls the user iterator function. (Gross I know.) +static void +eachHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + eachPair *pair = (eachPair *)data; + + pair->func(hand->obj, pair->data); +} + +// Iterate over the objects in the spatial hash. +void +cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data) +{ + // Bundle the callback up to send to the hashset iterator. + eachPair pair = {func, data}; + + cpHashSetEach(hash->handleSet, &eachHelper, &pair); +} + +// Calls the callback function for the objects in a given chain. +static inline void +query(cpSpaceHash *hash, cpSpaceHashBin *bin, void *obj, cpSpaceHashQueryFunc func, void *data) +{ + for(; bin; bin = bin->next){ + cpHandle *hand = bin->handle; + void *other = hand->obj; + + // Skip over certain conditions + if( + // Have we already tried this pair in this query? + hand->stamp == hash->stamp + // Is obj the same as other? + || obj == other + // Has other been removed since the last rehash? + || !other + ) continue; + + func(obj, other, data); + + // Stamp that the handle was checked already against this object. + hand->stamp = hash->stamp; + } +} + +void +cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data) +{ + cpFloat dim = hash->celldim; + int index = hash_func((int)(point.x/dim), (int)(point.y/dim), hash->numcells); + + query(hash, hash->table[index], &point, func, data); + + // Increment the stamp. + // Only one cell is checked, but query() requires it anyway. + hash->stamp++; +} + +void +cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data) +{ + // Get the dimensions in cell coordinates. + cpFloat dim = hash->celldim; + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + int n = hash->numcells; + + // Iterate over the cells and query them. + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int index = hash_func(i,j,n); + query(hash, hash->table[index], obj, func, data); + } + } + + // Increment the stamp. + hash->stamp++; +} + +// Similar to struct eachPair above. +typedef struct queryRehashPair { + cpSpaceHash *hash; + cpSpaceHashQueryFunc func; + void *data; +} queryRehashPair; + +// Hashset iterator func used with cpSpaceHashQueryRehash(). +static void +handleQueryRehashHelper(void *elt, void *data) +{ + cpHandle *hand = (cpHandle *)elt; + + // Unpack the user callback data. + queryRehashPair *pair = (queryRehashPair *)data; + cpSpaceHash *hash = pair->hash; + cpSpaceHashQueryFunc func = pair->func; + + cpFloat dim = hash->celldim; + int n = hash->numcells; + + void *obj = hand->obj; + cpBB bb = hash->bbfunc(obj); + + int l = bb.l/dim; + int r = bb.r/dim; + int b = bb.b/dim; + int t = bb.t/dim; + + for(int i=l; i<=r; i++){ + for(int j=b; j<=t; j++){ + int index = hash_func(i,j,n); + cpSpaceHashBin *bin = hash->table[index]; + + if(containsHandle(bin, hand)) continue; + query(hash, bin, obj, func, pair->data); + + cpHandleRetain(hand); + cpSpaceHashBin *newBin = getEmptyBin(hash); + newBin->handle = hand; + newBin->next = bin; + hash->table[index] = newBin; + } + } + + // Increment the stamp for each object we hash. + hash->stamp++; +} + +void +cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data) +{ + clearHash(hash); + + queryRehashPair pair = {hash, func, data}; + cpHashSetEach(hash->handleSet, &handleQueryRehashHelper, &pair); +} diff --git a/chipmunk/cpSpaceHash.h b/chipmunk/cpSpaceHash.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpSpaceHash.h @@ -0,0 +1,100 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// The spatial hash is Chipmunk's default (and currently only) spatial index type. +// Based on a chained hash table. + +// Used internally to track objects added to the hash +typedef struct cpHandle{ + // Pointer to the object + void *obj; + // Retain count + int retain; + // Query stamp. Used to make sure two objects + // aren't identified twice in the same query. + int stamp; +} cpHandle; + +// Linked list element for in the chains. +typedef struct cpSpaceHashBin{ + cpHandle *handle; + struct cpSpaceHashBin *next; +} cpSpaceHashBin; + +// BBox callback. Called whenever the hash needs a bounding box from an object. +typedef cpBB (*cpSpaceHashBBFunc)(void *obj); + +typedef struct cpSpaceHash{ + // Number of cells in the table. + int numcells; + // Dimentions of the cells. + cpFloat celldim; + + // BBox callback. + cpSpaceHashBBFunc bbfunc; + + // Hashset of all the handles. + cpHashSet *handleSet; + + cpSpaceHashBin **table; + // List of recycled bins. + cpSpaceHashBin *bins; + + // Incremented on each query. See cpHandle.stamp. + int stamp; +} cpSpaceHash; + +//Basic allocation/destruction functions. +cpSpaceHash *cpSpaceHashAlloc(void); +cpSpaceHash *cpSpaceHashInit(cpSpaceHash *hash, cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); +cpSpaceHash *cpSpaceHashNew(cpFloat celldim, int cells, cpSpaceHashBBFunc bbfunc); + +void cpSpaceHashDestroy(cpSpaceHash *hash); +void cpSpaceHashFree(cpSpaceHash *hash); + +// Resize the hashtable. (Does not rehash! You must call cpSpaceHashRehash() if needed.) +void cpSpaceHashResize(cpSpaceHash *hash, cpFloat celldim, int numcells); + +// Add an object to the hash. +void cpSpaceHashInsert(cpSpaceHash *hash, void *obj, unsigned int id, cpBB bb); +// Remove an object from the hash. +void cpSpaceHashRemove(cpSpaceHash *hash, void *obj, unsigned int id); + +// Iterator function +typedef void (*cpSpaceHashIterator)(void *obj, void *data); +// Iterate over the objects in the hash. +void cpSpaceHashEach(cpSpaceHash *hash, cpSpaceHashIterator func, void *data); + +// Rehash the contents of the hash. +void cpSpaceHashRehash(cpSpaceHash *hash); +// Rehash only a specific object. +void cpSpaceHashRehashObject(cpSpaceHash *hash, void *obj, unsigned int id); + +// Query callback. +typedef int (*cpSpaceHashQueryFunc)(void *obj1, void *obj2, void *data); +// Point query the hash. A reference to the query point is passed as obj1 to the query callback. +void cpSpaceHashPointQuery(cpSpaceHash *hash, cpVect point, cpSpaceHashQueryFunc func, void *data); +// Query the hash for a given BBox. +void cpSpaceHashQuery(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); +// Run a query for the object, then insert it. (Optimized case) +void cpSpaceHashQueryInsert(cpSpaceHash *hash, void *obj, cpBB bb, cpSpaceHashQueryFunc func, void *data); +// Rehashes while querying for each object. (Optimized case) +void cpSpaceHashQueryRehash(cpSpaceHash *hash, cpSpaceHashQueryFunc func, void *data); diff --git a/chipmunk/cpVect.c b/chipmunk/cpVect.c new file mode 100644 --- /dev/null +++ b/chipmunk/cpVect.c @@ -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; +} diff --git a/chipmunk/cpVect.h b/chipmunk/cpVect.h new file mode 100644 --- /dev/null +++ b/chipmunk/cpVect.h @@ -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 diff --git a/chipmunk/prime.h b/chipmunk/prime.h new file mode 100644 --- /dev/null +++ b/chipmunk/prime.h @@ -0,0 +1,68 @@ +/* Copyright (c) 2007 Scott Lembcke + * + * Permission is hereby granted, free of charge, to any person obtaining a copy + * of this software and associated documentation files (the "Software"), to deal + * in the Software without restriction, including without limitation the rights + * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + * copies of the Software, and to permit persons to whom the Software is + * furnished to do so, subject to the following conditions: + * + * The above copyright notice and this permission notice shall be included in + * all copies or substantial portions of the Software. + * + * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE + * SOFTWARE. + */ + +// Used for resizing hash tables. +// Values approximately double. + +static int primes[] = { + 5, //2^2 + 1 + 11, //2^3 + 3 + 17, //2^4 + 1 + 37, //2^5 + 5 + 67, //2^6 + 3 + 131, //2^7 + 3 + 257, //2^8 + 1 + 521, //2^9 + 9 + 1031, //2^10 + 7 + 2053, //2^11 + 5 + 4099, //2^12 + 3 + 8209, //2^13 + 17 + 16411, //2^14 + 27 + 32771, //2^15 + 3 + 65537, //2^16 + 1 + 131101, //2^17 + 29 + 262147, //2^18 + 3 + 524309, //2^19 + 21 + 1048583, //2^20 + 7 + 2097169, //2^21 + 17 + 4194319, //2^22 + 15 + 8388617, //2^23 + 9 + 16777259, //2^24 + 43 + 33554467, //2^25 + 35 + 67108879, //2^26 + 15 + 134217757, //2^27 + 29 + 268435459, //2^28 + 3 + 536870923, //2^29 + 11 + 1073741827, //2^30 + 3 + 0, +}; + +static int +next_prime(int n) +{ + int i = 0; + while(n > primes[i]){ + i++; + assert(primes[i]); // realistically this should never happen + } + + return primes[i]; +}