/*
 * Copyright (c) 2008-2009 Bill Whitacre http://rampancy.g0dsoft.com
 * 
 * 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 "constraint_slider.h"

static rat_constraint_class rat_slider_class=
{
	rat_constraint_slider_destroy,
	rat_constraint_slider_prime,
	rat_constraint_slider_step
};

rat_constraint *rat_constraint_slider_create(rat_body *body_a,rat_body *body_b,vector2 anch_a,vector2 anch_b,rat_real min_dist,rat_real max_dist)
{
	rat_constraint_slider *slider=(rat_constraint_slider *)rat_alloc(sizeof(rat_constraint_slider));

	vector2 world_anch_a=vector2_add(rat_hull_get_pos(body_a->hull),
		vector2_getrotated(anch_a,rat_hull_get_angle(body_a->hull)));
	vector2 world_anch_b=vector2_add(rat_hull_get_pos(body_b->hull),
		vector2_getrotated(anch_b,rat_hull_get_angle(body_b->hull)));

	memset(slider,0,sizeof(rat_constraint_slider));

	slider->type=rat_constraint_type_slider;
	slider->clas=&rat_slider_class;

	slider->min_dist=min_dist;
	slider->max_dist=max_dist;

	slider->body_a=body_a;
	slider->anch_a=anch_a;

	slider->body_b=body_b;
	slider->anch_b=anch_b;

	slider->norm_acc_j=0.0;

	if (body_a) rat_body_add_constraint(body_a,(rat_constraint *)slider);
	if (body_b) rat_body_add_constraint(body_b,(rat_constraint *)slider);
	
	return (rat_constraint *)slider;
}

void rat_constraint_slider_destroy(rat_constraint *constraint)
{
	rat_dealloc((void *)constraint);
}

void rat_constraint_slider_prime(rat_constraint *constraint,rat_real bias_coef)
{
	rat_constraint_slider *slider=(rat_constraint_slider *)constraint;
	
	vector2 delta,impulse;
	vector2 pos_a=rat_hull_get_pos(slider->body_a->hull);
	vector2 pos_b=rat_hull_get_pos(slider->body_b->hull);
	
	rat_real distance,bias_dist,mass_sum,anch_a_cross_norm,anch_b_cross_norm;
	rat_real angle_a=rat_hull_get_angle(slider->body_a->hull);
	rat_real angle_b=rat_hull_get_angle(slider->body_b->hull);
	
	slider->world_anch_a=vector2_getrotated(slider->anch_a,rat_hull_get_angle(slider->body_a->hull));
	slider->world_anch_b=vector2_getrotated(slider->anch_b,rat_hull_get_angle(slider->body_b->hull));
	
	delta=vector2_sub(vector2_add(pos_b,slider->world_anch_b),vector2_add(pos_a,slider->world_anch_a));
	distance=vector2_magnitude(delta);
	if (distance>slider->max_dist)
		bias_dist=(bias_dist=distance-slider->max_dist)<0?0.0:bias_dist;
	else if (distance<slider->min_dist)
	{
		bias_dist=(bias_dist=slider->min_dist-distance)<0?0.0:bias_dist;
		distance=-distance;
	}
	else
	{
		slider->bias=0;
		slider->norm_acc_j=0.0;
		return;
	}

	slider->normal=vector2_multf(delta,distance==0?0:1.0/distance);
	
	mass_sum=slider->body_a->inv_mass+slider->body_b->inv_mass;
	anch_a_cross_norm=vector2_cross(slider->world_anch_a,slider->normal);
	anch_b_cross_norm=vector2_cross(slider->world_anch_b,slider->normal);
	slider->mass_norm=1.0f/(mass_sum+
		slider->body_a->inv_moi*anch_a_cross_norm*anch_a_cross_norm+
		slider->body_b->inv_moi*anch_b_cross_norm*anch_b_cross_norm);
	
	slider->bias=-bias_coef*bias_dist;
	slider->bias_j=0;
	
	impulse=vector2_multf(slider->normal,slider->norm_acc_j);

	rat_body_apply_offset_impulse(slider->body_a,vector2_negative(impulse),slider->world_anch_a);
	rat_body_apply_offset_impulse(slider->body_b,impulse,slider->world_anch_b);
}

void rat_constraint_slider_step(rat_constraint *constraint)
{
	rat_constraint_slider *slider=(rat_constraint_slider *)constraint;

	vector2 bias_imp,vel_a,vel_b,vel_ab,impulse;
	vector2 bias_vel_a,bias_vel_b,bias_vel_ab;
	rat_real vel_n,norm_imp,bias_vel_n,bias_norm_imp;

	if (slider->bias==0) return;

	bias_vel_a=vector2_add(slider->body_a->velocity_bias,
		vector2_multf(vector2_perp_left(slider->world_anch_a),slider->body_a->omega_bias));
	bias_vel_b=vector2_add(slider->body_b->velocity_bias,
		vector2_multf(vector2_perp_left(slider->world_anch_b),slider->body_b->omega_bias));

	bias_vel_ab=vector2_sub(bias_vel_b,bias_vel_a);
	bias_vel_n=vector2_dot(bias_vel_ab,slider->normal);
	
	bias_norm_imp=(slider->bias-bias_vel_n)*slider->mass_norm;
	slider->bias_j+=bias_norm_imp;
	
	bias_imp=vector2_multf(slider->normal,bias_norm_imp);
	rat_body_apply_offset_bias_impulse(slider->body_a,vector2_negative(bias_imp),slider->world_anch_a);
	rat_body_apply_offset_bias_impulse(slider->body_b,bias_imp,slider->world_anch_b);
	
	vel_a=vector2_add(slider->body_a->velocity,
		vector2_multf(vector2_perp_left(slider->world_anch_a),slider->body_a->omega));
	vel_b=vector2_add(slider->body_b->velocity,
		vector2_multf(vector2_perp_left(slider->world_anch_b),slider->body_b->omega));

	vel_ab=vector2_sub(vel_b,vel_a);
	vel_n=vector2_dot(vel_ab,slider->normal);
	
	norm_imp=(slider->bias<0)?
		((norm_imp=-vel_n*slider->mass_norm)>0?0.0:norm_imp):
		((norm_imp=-vel_n*slider->mass_norm)<0?0.0:norm_imp);
	slider->norm_acc_j=+norm_imp;
	
	impulse=vector2_multf(slider->normal,norm_imp);

	rat_body_apply_offset_impulse(slider->body_a,vector2_negative(impulse),slider->world_anch_a);
	rat_body_apply_offset_impulse(slider->body_b,impulse,slider->world_anch_b);
}