/*
 * ODE::Geometry::Placeable#body
 * --
 * Fetch the body associated with the receiving geometry, if any. If no body is
 * associated, returns <tt>nil</tt>.
 */
static VALUE
ode_geometry_placeable_body( self )
	 VALUE self;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	dBodyID			body = dGeomGetBody( ptr->id );

	if ( body ) {
		ode_BODY *bodyPtr = (ode_BODY *)dBodyGetData( body );

		if ( ! bodyPtr ) rb_bug( "dBodyID with null data pointer in placeable geom." );
		return bodyPtr->object;
	}

	else {
		return Qnil;
	}
}

/*
 * ODE::Geometry::Placeable#body=( ODE::Body )
 * --
 * Set the body associated with the receiving geometry. Setting a body on a
 * geometry automatically combines the position vector and rotation matrix of
 * the body and geom, so that setting the position or orientation of one will
 * set the value for both objects.
 *
 * Setting a geometry's body to <tt>nil</tt> gives it its own position and
 * rotation, independent from any body. If the geometry was previously connected
 * to a body then its new independent position/rotation is set to the last
 * position/rotation of the body.
 */
static VALUE
ode_geometry_placeable_body_eq( self, body )
	 VALUE self, body;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	ode_BODY		*bodyPtr = ode_get_body( body );

	dGeomSetBody( ptr->id, bodyPtr->id );

	return body;
}

/*
 * ODE::Geometry::Placeable#position
 * --
 * 
 */
static VALUE
ode_geometry_placeable_position( self )
	 VALUE self;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	const dReal		*vec = dGeomGetPosition( ptr->id );
	VALUE			position;
	
	Vec3ToOdePosition( vec, position );
	return position;
}

/*
 * ODE::Geometry::Placeable#position=
 * --
 * 
 */
static VALUE
ode_geometry_placeable_position_eq( self, position )
	 VALUE self, position;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	VALUE			posArray;

	/* Normalize the argument/s into a 3rd-order vector */
	if ( RARRAY(position)->len == 1 ) 
		posArray = ode_obj_to_ary3( *(RARRAY(position)->ptr), "position" );
	else
		posArray = ode_obj_to_ary3( position, "position" );

	/* Set the position from the normalized array */
	dGeomSetPosition( ptr->id,
					  (dReal)NUM2DBL(*(RARRAY(posArray)->ptr    )),
					  (dReal)NUM2DBL(*(RARRAY(posArray)->ptr + 1)),
					  (dReal)NUM2DBL(*(RARRAY(posArray)->ptr + 2)) );
	
	return Qtrue;
}

/*
 * ODE::Geometry::Placeable#rotation
 * --
 * 
 */
static VALUE
ode_geometry_placeable_rotation( self )
	 VALUE self;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	const dReal		*quat;
	VALUE			args[4];

	/* Fetch the rotation quaternion for the geom. */
	quat = dGeomGetRotation( ptr->id );

	args[0] = rb_float_new( *(quat  ) );
	args[1] = rb_float_new( *(quat+1) );
	args[2] = rb_float_new( *(quat+2) );
	args[3] = rb_float_new( *(quat+3) );
	
	/* Create a new ODE::Quaternion object from the quaternion and return it */
	return rb_class_new_instance( 4, args, ode_cOdeQuaternion );
	
	
}

/*
 * ODE::Geometry::Placeable#rotation=
 * --
 * 
 */
static VALUE
ode_geometry_placeable_rotation_eq( self, rotation )
	 VALUE self, rotation;
{
	ode_GEOMETRY	*ptr = get_geom( self );
	VALUE			ary;
	dQuaternion		quat;
	dMatrix3		R;
	int				i;

	/* Call to_ary on whatever we got, and make sure it's an Array with four elements. */
	if ( RARRAY(rotation)->len == 1 ) 
		ary = ode_obj_to_ary4( *(RARRAY(rotation)->ptr), "rotation" );
	else
		ary = ode_obj_to_ary4( rotation, "rotation" );

	/* Copy the values in the array into the quaternion */
	for ( i = 0 ; i <= 3 ; i++ )
		quat[i] = NUM2DBL( *(RARRAY(ary)->ptr + i) );
	dQtoR( quat, R );
  
	/* Get the body and set its rotation */
	dGeomSetRotation( ptr->id, R );

	return Qtrue;
	
	
}