/' 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.
 '/

type cpBodyPTR as cpBody ptr
type cpShapePTR as cpShape ptr
type cpSpacePTR as cpSpace ptr

type cpBodyVelocityFunc as sub cdecl( byval as cpBodyPTR, byval as cpVect, byval as cpFloat, byval as cpFloat )
type cpBodyPositionFunc as sub cdecl( byval as cpBodyPTR, byval as cpFloat )

extern cpBodyUpdateVelocityDefault alias "cpBodyUpdateVelocityDefault" as cpBodyVelocityFunc
extern cpBodyUpdatePositionDefault alias "cpBodyUpdatePositionDefault" as cpBodyPositionFunc

'' Structure to hold information about the contact graph components
'' when putting groups of objects to sleep.
'' No interesting user accessible fields.
type cpComponentNode
	as cpBodyPTR	parent
	as cpBodyPTR	next
	as integer		rank
	as cpFloat		idleTime
end type

type cpBody
	'' *** Integration Functions

	'' Function that is called to integrate the body's velocity. (Defaults to cpBodyUpdateVelocity)
	as cpBodyVelocityFunc	velocity_func
	
	'' Function that is called to integrate the body's position. (Defaults to cpBodyUpdatePosition)
	as cpBodyPositionFunc	position_func

	'' *** Mass Properties
	
    ' Mass and it's inverse.
    as cpFloat	m, m_inv
    ' Moment of inertia and it's inverse.
    as cpFloat	i, i_inv

	'' *** Positional Properties
	
    ' NOTE: v_bias and w_bias are used internally for penetration/joint correction.
	' Linear components of motion (position, velocity, and force)
	as cpVect	p, v, f

    ' Angular components of motion (angle, angular velocity, and torque)
    as cpFloat	a, w, t
    
    ' Unit length
    as cpVect	rot
	
	'' *** Other Fields
	
	'' Maximum velocities this body can move at after integrating velocity
	as cpFloat	v_limit, w_limit
    
   	'' *** User Definable Fields
	
	'' User defined data pointer.
	as cpDataPointer	data
	
	'' *** Internally Used Fields
	
	'' Velocity bias values used when solving penetrations and correcting constraints.
	as cpVect	v_bias
	as cpFloat	w_bias

	'' Space this body has been added to
	as cpSpacePTR	space
	
	'' Pointer to the shape list.
	'' Shapes form a linked list using cpShape.next when added to a space.
	as cpShapePTR	shapelist
	
	'' Used by cpSpaceStep() to store contact graph information.
	as cpComponentNode	node
end type

' Basic allocation/destruction functions
declare function cpBodyAlloc cdecl alias "cpBodyAlloc" () as cpBody ptr
declare function cpBodyInit cdecl alias "cpBodyInit" ( byval as cpBody ptr, byval as cpFloat, byval as cpFloat ) as cpBody ptr
declare function cpBodyNew cdecl alias "cpBodyNew" ( byval as cpFloat, byval as cpFloat ) as cpBody ptr

declare sub cpBodyDestroy cdecl alias "cpBodyDestroy" ( byval as cpBody ptr )
declare sub cpBodyFree cdecl alias "cpBodyFree" ( byval as cpBody ptr )

'' Wake up a sleeping or idle body.
declare sub cpBodyActivate cdecl alias "cpBodyActivate" ( byval body as cpBody ptr )

function cpBodyIsSleeping( byval body as const cpBody ptr ) as cpBool
	return ( body->node.next <> ( cptr(cpBody ptr, 0) ) )
end function

'' defined in cpSpace.h after the cpSpace type has been defined
declare function cpBodyIsStatic ( byval body as const cpBody ptr ) as cpBool	'' [NOTE] Still inline!

function cpBodyIsRogue( byval body as const cpBody ptr ) as cpBool
	return ( body->space = (cptr( cpSpacePTR, 0) ) )
end function

#define CP_DefineBodyGetter(t, m, n) function cpBodyGet##n( byval body as cpBody ptr ) as t: return body->m : end function
#define CP_DefineBodySetter(t, m, n) sub cpBodySet##n( byval body as cpBody ptr, byval value as t ) : body->m = value : end sub

#macro CP_DefineBodyProperty( type, member, name )
	CP_DefineBodyGetter( type, member, name )
	CP_DefineBodySetter( type, member, name )
#endmacro

'' Accessors for cpBody struct members
CP_DefineBodyGetter(cpFloat, m, Mass)
declare sub cpBodySetMass cdecl alias "cpBodySetMass" ( byval as cpBody ptr, byval as cpFloat )

CP_DefineBodyGetter(cpFloat, i, Moment)
declare sub cpBodySetMoment cdecl alias "cpBodySetMoment" ( byval as cpBody ptr, byval as cpFloat )


CP_DefineBodyProperty(cpVect, p, Pos)
CP_DefineBodyProperty(cpVect, v, Vel)
CP_DefineBodyProperty(cpVect, f, Force)
CP_DefineBodyGetter(cpFloat, a, Angle)
declare sub cpBodySetAngle cdecl alias "cpBodySetAngle" ( byval as cpBody ptr, byval as cpFloat )
CP_DefineBodyProperty(cpFloat, w, AngVel)
CP_DefineBodyProperty(cpFloat, t, Torque)
CP_DefineBodyGetter(cpVect, rot, Rot)
CP_DefineBodyProperty(cpFloat, v_limit, VelLimit)
CP_DefineBodyProperty(cpFloat, w_limit, AngVelLimit)


''  Modify the velocity of the body so that it will move to the specified absolute coordinates in the next timestep.
'' Intended for objects that are moved manually with a custom velocity integration function.
declare sub cpBodySlew cdecl alias "cpBodySlew" ( byval body as cpBody ptr, byval p as cpVect, byval dt as cpFloat )

' Integration functions.
declare sub cpBodyUpdateVelocity cdecl alias "cpBodyUpdateVelocity" ( byval body as cpBody ptr, byval gravity as cpVect, byval damping as cpFloat, byval dt as cpFloat )
declare sub cpBodyUpdatePosition cdecl alias "cpBodyUpdatePosition" ( byval body as cpBody ptr, byval dt as cpFloat )

' Convert body local to world coordinates
function cpBodyLocal2World ( byval body as const cpBody ptr, byval v as const cpVect ) as cpVect
    return cpvadd(body->p, cpvrotate(v, body->rot))
end function

' Convert world to body local coordinates
function cpBodyWorld2Local ( byval body as const cpBody ptr, byval v as const cpVect ) as cpVect
    return cpvunrotate(cpvsub(v, body->p), body->rot)
end function

' Apply an impulse (in world coordinates) to the body.
sub cpBodyApplyImpulse ( byval body as cpBody ptr, byval j as const cpVect, byval r as const cpVect )
	body->v = cpvadd(body->v, cpvmult(j, body->m_inv))
    body->w += body->i_inv*cpvcross(r, j)
end sub

' Zero the forces on a body.
declare sub cpBodyResetForces cdecl alias "cpBodyResetForces" ( byval body as cpBody ptr )
' Apply a force (in world coordinates) to a body.
declare sub cpBodyApplyForce cdecl alias "cpBodyApplyForce" ( byval body as cpBody ptr, byval f as const cpVect, byval r as const cpVect )

function cpBodyKineticEnergy( byval body as cpBody ptr ) as cpFloat
	'' Need to do some fudging to avoid NaNs
	dim as cpFloat vsq, wsq
	vsq = cpvdot(body->v, body->v)
	wsq = body->w * body->w
	return iif(vsq, vsq*body->m, 0.0) + iif(wsq, wsq*body->i, 0.0)
end function

' Apply a damped spring force between two bodies.
declare sub cpDampedSpring cdecl alias "cpDampedSpring" ( byval a as cpBody ptr, byval b as cpBody ptr, byval anchr1 as cpVect, byval anchr2 as cpVect, byval rlen as cpFloat, byval k as cpFloat, byval dmp as cpFloat, byval dt as cpFloat )

