--This example constrains the feet of a creature to the main body. The legs will move along
--with the feet automatically because they are connected with joints.

function init()	
	--Find handle the the head body
	mainBody = FindBody("main")
	headBody = FindBody("head")

	local mainTransform = GetBodyTransform(mainBody)

	--Find the feet and their initial relative transform and animation offset
	footBodies = FindBodies("foot")
	footTransforms = {}
	footOffsets = {}
	for i=1, #footBodies do
		local t = GetBodyTransform(footBodies[i])
		--Store the transform of each foot relative the main body
		footTransforms[i] = TransformToLocalTransform(mainTransform, t)               
		--Also store the offset angle in the animation (provided for each foot with a tag)
		local tagval = tonumber(GetTagValue(footBodies[i], "offset")) or 0
		footOffsets[i] = tagval / 180 * math.pi
	end
end


function update(dt)
	--Maximum speed and strength that can be applied to the head
	local maxSpeed = 10
	local strength = 50

	--Current head transform
	local mainTransform = GetBodyTransform(mainBody)

	--Constrain each foot to an animated target on the main body 
	for i=1, #footBodies do
		local a = (GetTime() + footOffsets[i]) * -3.0
		local current = GetBodyTransform(footBodies[i])

		--Compute foot target in main body coordinates
		local x = 0
		local y = math.max(math.sin(a)* 0.2, 0.0)
		local z = math.cos(a) * 0.4
		local t = TransformCopy(footTransforms[i])
		t.pos = VecAdd(t.pos, Vec(x, y, z))
	
		--Transform to world space
		local target = TransformToParentTransform(mainTransform, t)

		--Insert the constraints
		ConstrainPosition(footBodies[i], mainBody, current.pos, target.pos, maxSpeed, strength) 
		ConstrainOrientation(footBodies[i], mainBody, current.rot, target.rot, maxSpeed, strength)
	end

	--Help the main body stay upright and walk forward in a straight line
	--This is not strickly necessary, but a good way to help the main body without being too obvious
	ConstrainOrientation(mainBody, 0, mainTransform.rot, Quat(), 10, 8)

	--Constrain the orientation of the head to be the same as the main body, but only allow
	--a small amount of force in doing so, letting the head flap a little bit
	local headTransform = GetBodyTransform(headBody)
	ConstrainOrientation(headBody, mainBody, headTransform.rot, mainTransform.rot, 10, 1)
end