/*
 * 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_bar.h"

static rat_constraint_class rat_bar_class=
{
	rat_constraint_bar_destroy,
	rat_constraint_bar_prime,
	rat_constraint_bar_step
};

rat_constraint *rat_constraint_bar_create(rat_body *body_a,rat_body *body_b,vector2 anch_a,vector2 anch_b)
{
	rat_constraint_bar *bar=(rat_constraint_bar *)rat_alloc(sizeof(rat_constraint_bar));

	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(bar,0,sizeof(rat_constraint_bar));

	bar->type=rat_constraint_type_bar;
	bar->clas=&rat_bar_class;

	bar->distance=vector2_distance(world_anch_a,world_anch_b);

	bar->body_a=body_a;
	bar->anch_a=anch_a;

	bar->body_b=body_b;
	bar->anch_b=anch_b;

	bar->norm_acc_j=0.0;

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

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

void rat_constraint_bar_prime(rat_constraint *constraint,rat_real bias_coef)
{
	rat_constraint_bar *bar=(rat_constraint_bar *)constraint;
	
	vector2 delta,impulse,pos_a,pos_b;
	rat_real distance,mass_sum,anch_a_cross_norm;
	rat_real anch_b_cross_norm,angle_a,angle_b;

	pos_a=rat_hull_get_pos(bar->body_a->hull);
	pos_b=rat_hull_get_pos(bar->body_b->hull);
	
	angle_a=rat_hull_get_angle(bar->body_a->hull);
	angle_b=rat_hull_get_angle(bar->body_b->hull);
	
	bar->world_anch_a=vector2_getrotated(bar->anch_a,rat_hull_get_angle(bar->body_a->hull));
	bar->world_anch_b=vector2_getrotated(bar->anch_b,rat_hull_get_angle(bar->body_b->hull));
	
	delta=vector2_sub(vector2_add(pos_b,bar->world_anch_b),vector2_add(pos_a,bar->world_anch_a));
	distance=vector2_magnitude(delta);
	bar->normal=vector2_multf(delta,distance==0?0:1.0/distance);
	
	mass_sum=bar->body_a->inv_mass+bar->body_b->inv_mass;
	anch_a_cross_norm=vector2_cross(bar->world_anch_a,bar->normal);
	anch_b_cross_norm=vector2_cross(bar->world_anch_b,bar->normal);
	bar->mass_norm=1.0f/(mass_sum+
		bar->body_a->inv_moi*anch_a_cross_norm*anch_a_cross_norm+
		bar->body_b->inv_moi*anch_b_cross_norm*anch_b_cross_norm);
	
	bar->bias=-bias_coef*(distance-bar->distance);
	bar->bias_j=0;
	
	impulse=vector2_multf(bar->normal,bar->norm_acc_j);

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

void rat_constraint_bar_step(rat_constraint *constraint)
{
	rat_constraint_bar *bar=(rat_constraint_bar *)constraint;

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

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

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

	vel_ab=vector2_sub(vel_b,vel_a);
	vel_n=vector2_dot(vel_ab,bar->normal);
	
	norm_imp=-vel_n*bar->mass_norm;
	bar->norm_acc_j=+norm_imp;
	
	impulse=vector2_multf(bar->normal,norm_imp);

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