void pl_project_to_plane(Polygon * pl,
const Plane * const pn,
const vec direction)
{
assert(pl);
assert(pn);
double d= v_dot(pn->normal, direction);
// otherwise projection is impossible
assert(fabs(d) > E);
vec tmp;
double * pt;
uint i;
double N;
for (i=0; i < pl->last; i++){
pt= pl->points[i].co;
v_sub(tmp, pt, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pt, pt, tmp);
}
v_sub(tmp, pl->bb.min, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pl->bb.min, pl->bb.min, tmp);
v_sub(tmp, pl->bb.max, pn->point);
N= -v_dot(pn->normal, tmp);
v_scale(tmp, direction, N/d);
v_add(pl->bb.max, pl->bb.max, tmp);
}
//nearest points between two segments given by pi+veci, results given in ratio of vec [0 1]
int Reaching::FindSegmentsNearestPoints(cart_vec_t& p1,cart_vec_t& vec1,cart_vec_t& p2,cart_vec_t& vec2, float *nearest_point1, float *nearest_point2, float *dist){
CMatrix3_t mat;
CMatrix3_t invmat;
cart_vec_t& vec3,tmp,tmp1,tmp2,k,v2;
int i;
m_identity(mat);
v_cross(vec1,vec2,vec3);// check if vec1 and vec2 are not //
if(v_squ_length(vec3)<0.00001){
return 0;
}
v_scale(vec2,-1,v2);
m_set_v3_column(vec1,0,mat);
m_set_v3_column(v2,1,mat);
m_set_v3_column(vec3,2,mat);
m_inverse(mat,invmat);
v_sub(p2,p1,tmp);
v_transform_normal(tmp,invmat,k);
for(i=0;i<2;i++){
k[i] = max(min(1,k[i]),0);
}
v_scale(vec1,k[0],tmp);
v_add(p1,tmp,tmp1);
v_scale(vec2,k[1],tmp);
v_add(p2,tmp,tmp2);
v_sub(tmp2,tmp1,tmp);
*dist=v_length(tmp);
*nearest_point1 = k[0];
*nearest_point2 = k[1];
return 1;
}
void Reaching::IncrementCartWeights(float factor){
if (factor<0) return;
#ifdef DYN_WEIGHTS
v_scale(base_weight_cart,factor,base_weight_cart);
#else
v_scale(weight_cart,factor,weight_cart);
#endif
}
void make_first_shell_centers(cVector *centers, int num_layers, float layer_thickness)
{
// host vars
cVector *ptr_centers;
int x, x_zero, y, j, k, index=0;
// init host vars
x_zero = 15, y = 0;
// allocate memory
// init pointers
ptr_centers = centers;
// do stuff
for(k = 0; k < num_layers; k++)
{
x = (x_zero + 9*k) % num_layers;
y = 0;
for(j = 0; j < num_layers; j++, ptr_centers++, y++)
{
ptr_centers->x = x;
ptr_centers->y = y;
ptr_centers->z = k;
//v_print_t(ptr_centers);
v_scale(ptr_centers, layer_thickness, ptr_centers);
x = (x + 3) % num_layers;
index++;
}
}
// free memory
// return value
}
static void orthonormalize(float dcm[3][3])
{
/* Orthogonalize the i and j unit vectors (DCMDraft2 Eqn. 19). */
dcm_err = v_dotp(dcm[0], dcm[1]);
v_scale(dcm[1], -dcm_err/2, dcm_d[0]); /* i vector correction */
v_scale(dcm[0], -dcm_err/2, dcm_d[1]); /* i vector correction */
v_add(dcm[0], dcm_d[0], dcm[0]);
v_add(dcm[1], dcm_d[1], dcm[1]);
/* k = i x j */
v_crossp(dcm[0], dcm[1], dcm[2]);
/* Normalize all three vectors. */
v_norm(dcm[0]);
v_norm(dcm[1]);
v_norm(dcm[2]);
}
/* Compute the reflection of a vector on an object.
*
* the_object: The object to reflect off of.
* position: The position hit.
* direction: The direction of the vector to be reflected.
*
* Returns: the reflection of direction on the object at position.
*/
float* reflection_vector(object *the_object, float *position, float *direction) {
float *reflection = (float*)malloc(sizeof(float)*3);
float *normal = get_normal(the_object, position);
v_scale(normal, -2*v_dot(direction, normal), reflection);
v_add(reflection, direction, reflection);
v_unit(reflection, reflection);
return reflection;
}
/* Determine whether the given vector created by the point and direction intersects at any point on the given object.
*
* to_check: The sphere to check for intersection.
* origin: The origin of the ray.
* direction: unit vector representing the direction of the ray.
*
* Return: The shortest distance to intersection if it happens, or INFINITY if it does not.
*/
float object_intersect(object *to_check, float *origin, float *direction) {
if (to_check->type == SPHERE_TYPE) {
float a = v_dot(direction, direction);
float *temp = (float*)malloc(sizeof(float)*3);
v_sub(origin, to_check->definition.sphere.center, temp);
v_scale(temp, 2, temp);
float b = v_dot(temp, direction);
v_sub(origin, to_check->definition.sphere.center, temp);
float c = v_dot(temp, temp);
c -= to_check->definition.sphere.radius * to_check->definition.sphere.radius;
float discriminant = b*b - 4*a*c;
if (discriminant < 0) {
return INFINITY;
}
float t1 = (-b + sqrtf(b*b - 4*a*c))/2*a;
float t2 = (-b - sqrtf(b*b - 4*a*c))/2*a;
if (t1 > 0 && t2 > 0) {
return t1 < t2 ? t1 : t2;
}
if (t1 > 0) return t1;
if (t2 > 0) return t2;
else {
return INFINITY;
}
}
else if(to_check->type == PLANE_TYPE) {
// Compute incident angle
float vD = v_dot(to_check->definition.plane.normal, direction);
if (vD == 0) return INFINITY;
float distance = 0;
for (int i = 0; i < 3; i++) {
distance -= to_check->definition.plane.point[i] * to_check->definition.plane.normal[i];
}
float v0 = -(v_dot(to_check->definition.plane.normal, origin) + distance);
float t = v0/vD;
if (t < 0) return INFINITY;
return t;
}
return NAN;
}
void sm_frame(sm_t *sm,int from,int to,float frame) {
int i,frame0,frame1;
if(from == -1) from = 0;
if(to == -1) to = sm->num_frame;
frame0 = (int)frame;
frame -= frame0;
frame0 += from;
if(frame0 >= to) frame0 = ((frame0 - from) % (to - from)) + from;
frame1 = frame0 + 1;
if(frame1 >= to) frame1 = from;
for(i = 0; i < sm->num_bone; i++) {
float m0[16],m1[16];
float xyz0[3],xyz1[3],xyz[3],rot[4];
v_scale(sm->frame[frame0][i].xyz,1.0 - frame,xyz0);
v_scale(sm->frame[frame1][i].xyz,frame,xyz1);
v_add(xyz0,xyz1,xyz);
q_slerp(sm->frame[frame0][i].rot,sm->frame[frame1][i].rot,frame,rot);
m_translate(xyz,m0);
q_to_matrix(rot,m1);
m_multiply(m0,m1,sm->bone[i].matrix);
}
for(i = 0; i < sm->num_surface; i++) {
int j;
sm_surface_t *s = sm->surface[i];
for(j = 0; j < s->num_vertex; j++) {
int k;
v_set(0,0,0,s->vertex[j].xyz);
v_set(0,0,0,s->vertex[j].normal);
for(k = 0; k < s->vertex[j].num_weight; k++) {
float v[3];
sm_weight_t *w = &s->vertex[j].weight[k];
v_transform(w->xyz,sm->bone[w->bone].matrix,v);
v_scale(v,w->weight,v);
v_add(s->vertex[j].xyz,v,s->vertex[j].xyz);
v_transform_normal(w->normal,sm->bone[w->bone].matrix,v);
v_scale(v,w->weight,v);
v_add(s->vertex[j].normal,v,s->vertex[j].normal);
}
}
}
}
static int intersect(float fDst1, float fDst2,
const vec3_t a, const vec3_t b, vec3_t hit)
{
if ((fDst1 * fDst2) >= 0.0f)
return 0;
if (fDst1 == fDst2)
return 0;
v_sub(hit, b, a);
v_add(hit, a);
v_scale(hit, (-fDst1 / (fDst2 - fDst1)));
return 1;
}
int Reaching::LinkDistanceToObject(int link, EnvironmentObject *obj, float *dist,
float *point, cart_vec_t& contact_vector){
cart_vec_t& objPos;
cart_vec_t& lowerLinkExtr;
cart_vec_t& upperLinkExtr;
cart_vec_t& v1,v2,linkv,u,vertexPos,v_tmp,tmp3;
CMatrix4_t ref,tmpmat,tmpmat2;
cart_vec_t& s1,s2;
int i,j,k,dir1,dir2,pindex,min_i;
float alldists[12]; //closest distance to each edge
float allpoints[24]; // closest pair of points on each edge
float min_dist;
int s3[3];
for (i=0;i<12;i++){
alldists[i]=FLT_MAX;
}
switch(link){
case 1:
v_clear(upperLinkExtr);
ElbowPosition(pos_angle,lowerLinkExtr);
break;
case 2:
ElbowPosition(pos_angle,upperLinkExtr);
v_copy(pos_cart,lowerLinkExtr);
break;
default:
cout<<"unvalid link number"<< endl;
}
if(!obj){
return 0;
}
// v_sub(obj->solid->m_position, shoulder_abs_pos,objPos);
Abs2LocalRef(obj->GetPosition(), objPos);
// cout<<"ule: ";coutvec(upperLinkExtr);
// cout<<"lle: ";coutvec(lowerLinkExtr);
// coutvec(objPos);
v_sub(lowerLinkExtr,objPos,v1);
v_sub(upperLinkExtr,objPos,v2);
//m_transpose(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
m_copy(obj->solid->m_ref.m_orient,ref); //stupid to do it each time
v_clear(s3);
//checking when two parallel sides must me checked
for(i=0;i<3;i++){
s1[i] = v_dot(v1, &ref[i*4]);
s2[i] = v_dot(v2, &ref[i*4]);
if (s1[i]*s2[i]>=0){
s3[i] = sign(s1[i]+s2[i]);
}
}
// cout << "s3: ";coutvec(s3);
m_copy(ref,tmpmat);
for(i=0;i<3;i++){
if(s3[i]){
v_sub(lowerLinkExtr,upperLinkExtr,linkv);
v_scale(obj->solid->m_size,-0.5,u);
u[i] *=-s3[i];
v_add(objPos,u,vertexPos);
// cout<<"vPos "<<i<<": ";coutvec(vertexPos);
v_scale(&(ref[i*4]),s3[i],&(tmpmat[i*4]));
// cout<<"norm "<<i<<": ";coutvec((tmpmat+i*4));
m_inverse(tmpmat,tmpmat2);
if(v_dot(&(tmpmat[i*4]),linkv)<0){
v_sub(lowerLinkExtr,vertexPos,v_tmp);
*point = 1;
}
else{
v_sub(upperLinkExtr,vertexPos,v_tmp);
*point = 0;
}
// cout<<"linkv ";coutvec(linkv);
// cout <<"pt "<<*point<<endl;
// #ifdef OLD_AV
// v_copy(linkv,(cart_vec_t&)&tmpmat[i*4]);
// m_inverse(tmpmat,tmpmat2);
// v_sub(upperLinkExtr,vertexPos,tmp2);
// tmp3 should contain the intersection coordinates in
// the referential defined by the edges of the surface
v_transform_normal(v_tmp,tmpmat2,tmp3);
// cout<<"tmp3 ";coutvec(tmp3);
if(tmp3[(i+1)%3]>=0 && tmp3[(i+2)%3]>=0 // the link points to the rectangle
&& tmp3[(i+1)%3]<=obj->solid->m_size[(i+1)%3] &&
tmp3[(i+2)%3]<=obj->solid->m_size[(i+2)%3]){
if(tmp3[i]<0){
return 0; // there is a collision
}
else{
*dist = tmp3[i];
v_copy(&(tmpmat[i*4]),contact_vector);
v_scale(contact_vector,*dist,contact_vector);
return 1;
}
}
}
}
// the link does not point to a rectangle -> look for the edges
v_scale(obj->solid->m_size,-0.5,u);
for(i=0;i<3;i++){// each kind of edge
dir1 = s3[(i+1)%3];
dir2 = s3[(i+2)%3];
for(j=0;j<2;j++){
if(dir1 == 0 || dir1==2*j-1){ //edges of this face must be computed
for(k=0;k<2;k++){
if(dir2 == 0 || dir2==2*k-1){ //edges of this face must be computed
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos);
v_scale(&(ref[4*i]),obj->solid->m_size[i],v1); // edge with the right length
pindex = 4*i+2*j+k;
FindSegmentsNearestPoints(vertexPos,v1,upperLinkExtr,linkv,&(allpoints[2*pindex]),&(allpoints[2*pindex+1]),&(alldists[pindex]));
// cout<<"i:"<<i<<" j:"<<j<<" k:"<<k<<"dist "<<alldists[pindex]<<endl;
// cout<<"v1:";coutvec(v1);
// cout<<"ref:";coutvec((&(ref[4*i])));
// cout<<"vP:";coutvec(vertexPos);
}
}
}
}
}
//looking for the min
min_dist = alldists[0];
min_i = 0;
for(i=1;i<12;i++){
if(alldists[i] < min_dist){
min_dist = alldists[i];
min_i = i;
}
}
// returning the min distance
*dist = min_dist;
*point = allpoints[2*min_i+1];
v_scale(linkv,*point,v1);
v_add(upperLinkExtr,v1,v2); // nearest point on link
k = min_i%2; //retrieving the right edge
j = ((min_i-k)%4)/2;
i = min_i/4;
// cout<<"ijk: "<<i<<" "<<j<<" "<<k<<endl;
v_copy(u,v_tmp);
v_tmp[(i+1)%3]*=-(2*j-1);
v_tmp[(i+2)%3]*=-(2*k-1);
v_add(objPos,v_tmp,vertexPos); // starting vertex of the edge
v_scale(&(ref[3*1]),allpoints[2*min_i]*(obj->solid->m_size[min_i]),v1);
v_add(vertexPos,v1,v1); // nearest point on solid
v_sub(v2,v1,contact_vector);
return 1;
}
void update_ahrs(float dt, float dcm_out[3][3], float gyr_out[3])
{
static uint8_t i;
read_mpu(v_gyr, v_acc);
for (i=0; i<3; i++) {
gyr_out[i] = v_gyr[i];
}
/**
* Accelerometer
* Frame of reference: body
* Units: G (gravitational acceleration)
* Purpose: Measure the acceleration vector v_acc with components
* codirectional with the i, j, and k vectors. Note that the
* gravitational vector is the negative of the K vector.
*/
#ifdef ACC_WEIGHT
// Take weighted average.
#ifdef ACC_SELF_WEIGHT
for (i=0; i<3; i++) {
v_acc[i] = ACC_SELF_WEIGHT * v_acc[i] + (1-ACC_SELF_WEIGHT) * v_acc_last[i];
v_acc_last[i] = v_acc[i];
// Kalman filtering?
//v_acc_last[i] = acc.get(i);
//kalmanUpdate(v_acc[i], accVar, v_acc_last[i], ACC_UPDATE_SIG);
//kalmanPredict(v_acc[i], accVar, 0.0, ACC_PREDICT_SIG);
}
#endif // ACC_SELF_WEIGHT
acc_scale = v_norm(v_acc);
/**
* Reduce accelerometer weight if the magnitude of the measured
* acceleration is significantly greater than or less than 1 g.
*
* TODO: Magnitude of acceleration should be reported over telemetry so
* ACC_SCALE_WEIGHT can be more accurately determined.
*/
#ifdef ACC_SCALE_WEIGHT
acc_scale = (1.0 - MIN(1.0, ACC_SCALE_WEIGHT * ABS(acc_scale - 1.0)));
acc_weight = ACC_WEIGHT * acc_scale;
#else
acc_weight = ACC_WEIGHT;
// Ignore accelerometer if it measures anything 0.5g past gravity.
if (acc_scale > 1.5 || acc_scale < 0.5) {
acc_weight = 0;
}
#endif // ACC_SCALE_WEIGHT
/**
* Express K global unit vector in body frame as k_gb for use in drift
* correction (we need K to be described in the body frame because gravity
* is measured by the accelerometer in the body frame). Technically we
* could just create a transpose of dcm_gyro, but since we don't (yet) have
* a magnetometer, we don't need the first two rows of the transpose. This
* saves a few clock cycles.
*/
for (i=0; i<3; i++) {
k_gb[i] = dcm_gyro[i][2];
}
/**
* Calculate gyro drift correction rotation vector w_a, which will be used
* later to bring KB closer to the gravity vector (i.e., the negative of
* the K vector). Although we do not explicitly negate the gravity vector,
* the cross product below produces a rotation vector that we can later add
* to the angular displacement vector to correct for gyro drift in the
* X and Y axes. Note we negate w_a because our acceleration vector is
* actually the negative of our gravity vector.
*/
v_crossp(k_gb, v_acc, w_a);
v_scale(w_a, -1, w_a);
#endif // ACC_WEIGHT
/**
* Magnetometer
* Frame of reference: body
* Units: N/A
* Purpose: Measure the magnetic north vector v_mag with components
* codirectional with the body's i, j, and k vectors.
*/
#ifdef MAG_WEIGHT
// Express J global unit vectory in body frame as j_gb.
for (i=0; i<3; i++) {
j_gb[i] = dcm_gyro[i][1];
}
// Calculate yaw drift correction vector w_m.
v_crossp(j_gb, v_mag, w_m);
#endif // MAG_WEIGHT
/**
* Gyroscope
* Frame of reference: body
* Units: rad/s
* Purpose: Measure the rotation rate of the body about the body's i,
* j, and k axes.
* ========================================================================
* Scale v_gyr by elapsed time (in seconds) to get angle w*dt in radians,
* then compute weighted average with the accelerometer and magnetometer
* correction vectors to obtain final w*dt.
* TODO: This is still not exactly correct.
*/
for (i=0; i<3; i++) {
w_dt[i] = v_gyr[i] * dt;
#ifdef ACC_WEIGHT
w_dt[i] += acc_weight * w_a[i];
#endif // ACC_WEIGHT
#ifdef MAG_WEIGHT
w_dt[i] += MAG_WEIGHT * w_m[i];
#endif // MAG_WEIGHT
}
/**
* Direction Cosine Matrix
* Frame of reference: global
* Units: None (unit vectors)
* Purpose: Calculate the components of the body's i, j, and k unit
* vectors in the global frame of reference.
* ========================================================================
* Skew the rotation vector and sum appropriate components by combining the
* skew symmetric matrix with the identity matrix. The math can be
* summarized as follows:
*
* All of this is calculated in the body frame. If w is the angular
* velocity vector, let w_dt (w*dt) be the angular displacement vector of
* the DCM over a time interval dt. Let w_dt_i, w_dt_j, and w_dt_k be the
* components of w_dt codirectional with the i, j, and k unit vectors,
* respectively. Also, let dr be the linear displacement vector of the DCM
* and dr_i, dr_j, and dr_k once again be the i, j, and k components,
* respectively.
*
* In very small dt, certain vectors approach orthogonality, so we can
* assume that (draw this out for yourself!):
*
* dr_x = < 0, dw_k, -dw_j>,
* dr_y = <-dw_k, 0, dw_i>, and
* dr_z = < dw_j, -dw_i, 0>,
*
* which can be expressed as the rotation matrix:
*
* [ 0 dw_k -dw_j ]
* dr = [ -dw_k 0 dw_i ]
* [ dw_j -dw_i 0 ].
*
* This can then be multiplied by the current DCM and added to the current
* DCM to update the DCM. To minimize the number of calculations performed
* by the processor, however, we can combine the last two steps by
* combining dr with the identity matrix to produce:
*
* [ 1 dw_k -dw_j ]
* dr + I = [ -dw_k 1 dw_i ]
* [ dw_j -dw_i 1 ],
*
* which we multiply with the current DCM to produce the updated DCM
* directly.
*
* It may be helpful to read the Wikipedia pages on cross products and
* rotation representation.
*/
dcm_d[0][0] = 1;
dcm_d[0][1] = w_dt[2];
dcm_d[0][2] = -w_dt[1];
dcm_d[1][0] = -w_dt[2];
dcm_d[1][1] = 1;
dcm_d[1][2] = w_dt[0];
dcm_d[2][0] = w_dt[1];
dcm_d[2][1] = -w_dt[0];
dcm_d[2][2] = 1;
// Multiply the current DCM with the change in DCM and update.
m_product(dcm_d, dcm_gyro, dcm_gyro);
// Remove any distortions introduced by the small-angle approximations.
orthonormalize(dcm_gyro);
// Apply rotational offset (in case IMU is not installed orthogonally to
// the airframe).
dcm_offset[0][0] = 1;
dcm_offset[0][1] = 0;
dcm_offset[0][2] = -trim_angle[1];
dcm_offset[1][0] = 0;
dcm_offset[1][1] = 1;
dcm_offset[1][2] = trim_angle[0];
dcm_offset[2][0] = trim_angle[1];
dcm_offset[2][1] = -trim_angle[0];
dcm_offset[2][2] = 1;
m_product(dcm_offset, dcm_gyro, dcm_out);
}
/* init
*/
int init(void) {
int i,num_vertex,width,height;
float *vertex;
unsigned char *data;
float rmax;
vec3 min,max;
vec4 plane_s = { 1.0, 0.0, 0.0, 0.0 };
vec4 plane_t = { 0.0, 1.0, 0.0, 0.0 };
vec4 plane_r = { 0.0, 0.0, 1.0, 0.0 };
vec4 plane_q = { 0.0, 0.0, 0.0, 1.0 };
GLenum err;
err = glewInit();
if (GLEW_OK != err)
{
fprintf(stderr, "glewInit() error: %s\n", glewGetErrorString(err));
}
glClearDepth(1);
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_LIGHT0);
glPointSize(4);
glTexGenfv(GL_S,GL_EYE_PLANE,plane_s);
glTexGenfv(GL_T,GL_EYE_PLANE,plane_t);
glTexGenfv(GL_R,GL_EYE_PLANE,plane_r);
glTexGenfv(GL_Q,GL_EYE_PLANE,plane_q);
glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_EYE_LINEAR);
glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_EYE_LINEAR);
glTexGeni(GL_R,GL_TEXTURE_GEN_MODE,GL_EYE_LINEAR);
glTexGeni(GL_Q,GL_TEXTURE_GEN_MODE,GL_EYE_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_CLAMP);
glTexParameteri(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_CLAMP);
glTexImage2D(GL_TEXTURE_2D,0,GL_RGB,SIZE,SIZE,0,GL_RGB,
GL_UNSIGNED_BYTE,NULL);
glGenTextures(1,&texture_id);
glBindTexture(GL_TEXTURE_2D,texture_id);
if((data = load_jpeg("data/ground.jpg",&width,&height))) {
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,
GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,
GL_LINEAR_MIPMAP_LINEAR);
gluBuild2DMipmaps(GL_TEXTURE_2D,4,width,height,GL_RGBA,
GL_UNSIGNED_BYTE,data);
free(data);
}
vertex = load_3ds("data/mesh.3ds",&num_vertex);
if(!vertex) return -1;
v_set(999999,999999,999999,min);
v_set(-999999,-999999,-999999,max);
for(i = 0; i < num_vertex; i++) {
int j;
float *v = &vertex[i << 3];
for(j = 0; j < 3; j++) {
if(min[j] > v[j]) min[j] = v[j];
if(max[j] < v[j]) max[j] = v[j];
}
}
v_add(min,max,min);
v_scale(min,0.5,min);
for(i = 0; i < num_vertex; i++) {
v_sub(&vertex[i << 3],min,&vertex[i << 3]);
}
for(i = 0, rmax = 0; i < num_vertex; i++) {
float r = sqrt(v_dot(&vertex[i << 3],&vertex[i << 3]));
if(r > rmax) rmax = r;
}
rmax = 0.8 / rmax;
for(i = 0; i < num_vertex; i++) {
v_scale(&vertex[i << 3],rmax,&vertex[i << 3]);
}
mesh_id = glGenLists(1);
glNewList(mesh_id,GL_COMPILE);
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertex; i++) {
glNormal3fv((float*)&vertex[i << 3] + 3);
glVertex3fv((float*)&vertex[i << 3]);
}
glEnd();
glEndList();
vertex = load_3ds("data/ground.3ds",&num_vertex);
if(!vertex) return -1;
ground_id = glGenLists(1);
glNewList(ground_id,GL_COMPILE);
glBegin(GL_TRIANGLES);
for(i = 0; i < num_vertex; i++) {
glTexCoord2fv((float*)&vertex[i << 3] + 6);
glNormal3fv((float*)&vertex[i << 3] + 3);
glVertex3fv((float*)&vertex[i << 3]);
}
glEnd();
glEndList();
image = malloc(SIZE * SIZE * 4);
return 0;
}
void lsf_vq(struct melp_param *par)
{
register int16_t i, j, k;
static BOOLEAN firstTime = TRUE;
static int16_t qplsp[LPC_ORD]; /* Q15 */
const int16_t melp_cb_size[4] = { 256, 64, 32, 32 }; /* !!! (12/15/99) */
const int16_t res_cb_size[4] = { 256, 64, 64, 64 };
const int16_t melp_uv_cb_size[1] = { 512 };
int16_t uv_config; /* Bits of uv_config replace uv1, uv2 and cuv. */
int16_t *lsp[NF];
int32_t err, minErr, acc, bcc; /* !!! (12/15/99), Q11 */
int16_t temp1, temp2;
int16_t lpc[LPC_ORD]; /* Q12 */
int16_t wgt[NF][LPC_ORD]; /* Q11 */
int16_t mwgt[2 * LPC_ORD]; /* Q11 */
int16_t bestlsp0[LPC_ORD], bestlsp1[LPC_ORD]; /* Q15 */
int16_t res[2 * LPC_ORD]; /* Q17 */
/* The original program declares lsp_cand[LSP_VQ_CAND][] and */
/* lsp_index_cand[LSP_VQ_CAND*LSP_VQ_STAGES] with LSP_VQ_CAND == 8. The */
/* program only uses up to LSP_INP_CAND == 5 and the declaration is */
/* modified. */
int16_t lsp_cand[LSP_INP_CAND][LPC_ORD]; /* Q15 */
int16_t lsp_index_cand[LSP_INP_CAND * LSP_VQ_STAGES];
int16_t ilsp0[LPC_ORD], ilsp1[LPC_ORD]; /* Q15 */
int16_t cand, inp_index_cand, tos, intfact;
if (firstTime) {
temp2 = melpe_shl(LPC_ORD, 10); /* Q10 */
temp1 = X08_Q10; /* Q10 */
for (i = 0; i < LPC_ORD; i++) {
/* qplsp[i] = (i+1)*0.8/LPC_ORD; */
qplsp[i] = melpe_divide_s(temp1, temp2);
temp1 = melpe_add(temp1, X08_Q10);
}
firstTime = FALSE;
}
/* ==== Compute weights ==== */
for (i = 0; i < NF; i++) {
lsp[i] = par[i].lsf;
lpc_lsp2pred(lsp[i], lpc, LPC_ORD);
vq_lspw(wgt[i], lsp[i], lpc, LPC_ORD);
}
uv_config = 0;
for (i = 0; i < NF; i++) {
uv_config = melpe_shl(uv_config, 1);
if (par[i].uv_flag) {
uv_config |= 0x0001;
/* ==== Adjust weights ==== */
if (i == 0) /* Testing for par[0].uv_flag == 1 */
v_scale(wgt[0], X02_Q15, LPC_ORD);
else if (i == 1)
v_scale(wgt[1], X02_Q15, LPC_ORD);
}
}
/* ==== Quantize the lsp according to the UV decisions ==== */
switch (uv_config) {
case 7: /* 111, all frames are NOT voiced ---- */
lspVQ(lsp[0], wgt[0], lsp[0], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[0], LPC_ORD, FALSE);
lspVQ(lsp[1], wgt[1], lsp[1], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[1], LPC_ORD, FALSE);
lspVQ(lsp[2], wgt[2], lsp[2], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[2], LPC_ORD, FALSE);
break;
case 6: /* 110 */
lspVQ(lsp[0], wgt[0], lsp[0], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[0], LPC_ORD, FALSE);
lspVQ(lsp[1], wgt[1], lsp[1], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[1], LPC_ORD, FALSE);
lspVQ(lsp[2], wgt[2], lsp[2], lsp_v_256x64x32x32, 4, melp_cb_size, /* !!! (12/15/99) */
quant_par.lsf_index[2], LPC_ORD, FALSE);
break;
case 5: /* 101 */
lspVQ(lsp[0], wgt[0], lsp[0], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[0], LPC_ORD, FALSE);
lspVQ(lsp[1], wgt[1], lsp[1], lsp_v_256x64x32x32, 4, melp_cb_size, /* !!! (12/15/99) */
quant_par.lsf_index[1], LPC_ORD, FALSE);
lspVQ(lsp[2], wgt[2], lsp[2], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[2], LPC_ORD, FALSE);
break;
case 3: /* 011 */
lspVQ(lsp[0], wgt[0], lsp[0], lsp_v_256x64x32x32, 4, melp_cb_size, /* !!! (12/15/99) */
quant_par.lsf_index[0], LPC_ORD, FALSE);
lspVQ(lsp[1], wgt[1], lsp[1], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[1], LPC_ORD, FALSE);
lspVQ(lsp[2], wgt[2], lsp[2], lsp_uv_9, 1, melp_uv_cb_size,
quant_par.lsf_index[2], LPC_ORD, FALSE);
break;
default:
if (uv_config == 1) { /* 001 case, if (!uv1 && !uv2 && uv3). */
/* ---- Interpolation [4 inp + (8+6+6+6) res + 9 uv] ---- */
tos = 1;
lspVQ(lsp[2], wgt[2], lsp_cand[0], lsp_uv_9, tos,
melp_uv_cb_size, lsp_index_cand, LPC_ORD, TRUE);
} else {
tos = 4;
lspVQ(lsp[2], wgt[2], lsp_cand[0], lsp_v_256x64x32x32, tos, /* !!! (12/15/99) */
melp_cb_size, lsp_index_cand, LPC_ORD, TRUE);
}
minErr = LW_MAX;
cand = 0;
inp_index_cand = 0;
for (k = 0; k < LSP_INP_CAND; k++) {
for (i = 0; i < 16; i++) {
err = 0;
/* Originally we have two for loops here. One computes */
/* ilsp0[] and ilsp1[] and the other one computes "err". If */
/* "err" already exceeds minErr, we can stop the loop and */
/* there is no need to compute the remaining ilsp0[] and */
/* ilsp1[] entries. Hence the two for loops are joined. */
for (j = 0; j < LPC_ORD; j++) {
/* ilsp0[j] = (inpCoef[i][j] * qplsp[j] +
(1.0 - inpCoef[i][j]) * lsp_cand[k][j]); */
intfact = inpCoef[i][j]; /* Q14 */
acc = melpe_L_mult(intfact, qplsp[j]); /* Q30 */
intfact = melpe_sub(ONE_Q14, intfact); /* Q14 */
acc = melpe_L_mac(acc, intfact, lsp_cand[k][j]); /* Q30 */
ilsp0[j] = melpe_extract_h(melpe_L_shl(acc, 1));
acc =
melpe_L_sub(acc,
melpe_L_shl(melpe_L_deposit_l(lsp[0][j]),
15));
/* ilsp1[j] = inpCoef[i][j + LPC_ORD] * qplsp[j] +
(1.0 - inpCoef[i][j + LPC_ORD]) * lsp_cand[k][j]; */
intfact = inpCoef[i][j + LPC_ORD]; /* Q14 */
bcc = melpe_L_mult(intfact, qplsp[j]);
intfact = melpe_sub(ONE_Q14, intfact);
bcc = melpe_L_mac(bcc, intfact, lsp_cand[k][j]); /* Q30 */
ilsp1[j] = melpe_extract_h(melpe_L_shl(bcc, 1));
bcc =
melpe_L_sub(bcc,
melpe_L_shl(melpe_L_deposit_l(lsp[1][j]),
15));
/* err += wgt0[j]*(lsp0[j] - ilsp0[j])*
(lsp0[j] - ilsp0[j]); */
temp1 = melpe_norm_l(acc);
temp2 = melpe_extract_h(melpe_L_shl(acc, temp1));
if (temp2 == MONE_Q15)
temp2 = -32767;
temp2 = melpe_mult(temp2, temp2);
acc = melpe_L_mult(temp2, wgt[0][j]);
temp1 = melpe_shl(melpe_sub(1, temp1), 1);
acc = melpe_L_shl(acc, melpe_sub(temp1, 3)); /* Q24 */
err = melpe_L_add(err, acc);
/* err += wgt1[j]*(lsp1[j] - ilsp1[j])*
(lsp1[j] - ilsp1[j]); */
temp1 = melpe_norm_l(bcc);
temp2 = melpe_extract_h(melpe_L_shl(bcc, temp1));
if (temp2 == MONE_Q15)
temp2 = -32767;
temp2 = melpe_mult(temp2, temp2);
bcc = melpe_L_mult(temp2, wgt[1][j]);
temp1 = melpe_shl(melpe_sub(1, temp1), 1);
bcc = melpe_L_shl(bcc, melpe_sub(temp1, 3)); /* Q24 */
err = melpe_L_add(err, bcc);
/* computer the err for the last frame */
acc = melpe_L_shl(melpe_L_deposit_l(lsp[2][j]), 15);
acc =
melpe_L_sub(acc,
melpe_L_shl(melpe_L_deposit_l
(lsp_cand[k][j]), 15));
temp1 = melpe_norm_l(acc);
temp2 = melpe_extract_h(melpe_L_shl(acc, temp1));
if (temp2 == MONE_Q15)
temp2 = -32767;
temp2 = melpe_mult(temp2, temp2);
acc = melpe_L_mult(temp2, wgt[2][j]);
temp1 = melpe_shl(melpe_sub(1, temp1), 1);
acc = melpe_L_shl(acc, melpe_sub(temp1, 3)); /* Q24 */
err = melpe_L_add(err, acc);
}
if (err < minErr) {
minErr = err;
cand = k;
inp_index_cand = i;
v_equ(bestlsp0, ilsp0, LPC_ORD);
v_equ(bestlsp1, ilsp1, LPC_ORD);
}
}
}
v_equ(lsp[2], lsp_cand[cand], LPC_ORD);
v_equ(quant_par.lsf_index[0], &(lsp_index_cand[cand * tos]),
tos);
quant_par.lsf_index[1][0] = inp_index_cand;
for (i = 0; i < LPC_ORD; i++) {
temp1 = melpe_sub(lsp[0][i], bestlsp0[i]); /* Q15 */
temp2 = melpe_sub(lsp[1][i], bestlsp1[i]); /* Q15 */
res[i] = melpe_shl(temp1, 2); /* Q17 */
res[i + LPC_ORD] = melpe_shl(temp2, 2); /* Q17 */
}
v_equ(mwgt, wgt[0], LPC_ORD);
v_equ(mwgt + LPC_ORD, wgt[1], LPC_ORD);
/* Note that in the following IF block, the lspVQ() is quantizing on */
/* res[] and res256x64x64x64[], and both of them are Q17 instead of */
/* Q15, unlike the other calling instances in this function. */
if (uv_config == 1) /* if (!uv1 && !uv2 && uv3) */
lspVQ(res, mwgt, res, res256x64x64x64, 4, res_cb_size,
quant_par.lsf_index[2], 2 * LPC_ORD, FALSE);
else
lspVQ(res, mwgt, res, res256x64x64x64, 2, res_cb_size,
quant_par.lsf_index[2], 2 * LPC_ORD, FALSE);
/* ---- reconstruct lsp for later stability check ---- */
for (i = 0; i < LPC_ORD; i++) {
temp1 = melpe_shr(res[i], 2);
lsp[0][i] = melpe_add(temp1, bestlsp0[i]);
temp2 = melpe_shr(res[i + LPC_ORD], 2);
lsp[1][i] = melpe_add(temp2, bestlsp1[i]);
}
break;
}
/* ---- Stability checking ---- */
/* The sortings on lsp[0] and lsp[1] are not necessary because they are */
/* variables local to this function and they are discarded upon exit. */
/* We only check whether they fit the stability test and issue a warning. */
(void)lspStable(lsp[0], LPC_ORD);
(void)lspStable(lsp[1], LPC_ORD);
if (!lspStable(lsp[2], LPC_ORD))
lspSort(lsp[2], LPC_ORD);
v_equ(qplsp, lsp[2], LPC_ORD);
}
/**
* Prepare a mesh for drawing. Compute mesh's final vertex positions
* given a skeleton. Put the vertices in vertex arrays.
*/
static void
PrepareMesh(const struct md5_mesh_part *mesh, const struct md5_joint_t *skeleton)
{
unsigned int i, j, k;
/* Setup vertex indices */
for (k = 0, i = 0; i < mesh->num_tris; ++i) {
for (j = 0; j < 3; ++j, ++k)
vertexIndices[k] = mesh->triangles[i].index[j];
}
/* Setup vertices */
for (i = 0; i < mesh->num_verts; ++i) {
vec3_t vert = { 0.0f, 0.0f, 0.0f };
vec3_t norm = { 0.0f, 0.0f, 0.0f };
// vec3_t tan = { 0.0f, 0.0f, 0.0f };
/* Calculate final vertex to draw with weights */
for (j = 0; j < mesh->vertices[i].count; ++j) {
const struct md5_weight_t *weight
= &mesh->weights[mesh->vertices[i].start + j];
const struct md5_joint_t *joint
= &skeleton[weight->joint];
vec3_t wv;
/* Calculate transformed vertex for this weight */
Quat_rotatePoint(joint->orient, weight->pos, wv);
/* The sum of all weight->bias should be 1.0 */
vert[0] +=
(joint->pos[0] + wv[0]) * weight->bias;
vert[1] +=
(joint->pos[1] + wv[1]) * weight->bias;
vert[2] +=
(joint->pos[2] + wv[2]) * weight->bias;
Quat_rotatePoint(joint->orient, weight->normal, wv);
v_scale(wv, weight->bias);
v_add(norm, norm, wv);
// Quat_rotatePoint(joint->orient, weight->tangent, wv);
// v_scale(wv, weight->bias);
// v_add(tan, tan, wv);
}
v_normalize(norm);
// v_normalize(tan);
vertexArray[i][0] = vert[0];
vertexArray[i][1] = vert[1];
vertexArray[i][2] = vert[2];
normalArray[i][0] = norm[0];
normalArray[i][1] = norm[1];
normalArray[i][2] = norm[2];
#if 0
tangentArray[i][0] = tan[0];
tangentArray[i][1] = tan[1];
tangentArray[i][2] = tan[2];
#endif
texArray[i][0] = mesh->vertices[i].st[0];
texArray[i][1] = 1.0f - mesh->vertices[i].st[1];
}
}
void melp_ana_init()
{
int j;
bpvc_ana_init(FRAME,PITCHMIN,PITCHMAX,NUM_BANDS,2,MINLENGTH);
pitch_ana_init(PITCHMIN,PITCHMAX,FRAME,LPF_ORD,MINLENGTH);
p_avg_init(PDECAY,DEFAULT_PITCH,3);
v_zap(speech,IN_BEG+FRAME);
pitch_avg=DEFAULT_PITCH;
fill(fpitch,DEFAULT_PITCH,2);
v_zap(lpfsp_del,LPF_ORD);
/* Initialize multi-stage vector quantization (read codebook) */
vq_par.num_best = MSVQ_M;
vq_par.num_stages = 4;
vq_par.dimension = 10;
/*
* Allocate memory for number of levels per stage and indices
* and for number of bits per stage
*/
MEM_ALLOC(MALLOC,vq_par.num_levels,vq_par.num_stages,int);
MEM_ALLOC(MALLOC,vq_par.indices,vq_par.num_stages,int);
MEM_ALLOC(MALLOC,vq_par.num_bits,vq_par.num_stages,int);
vq_par.num_levels[0] = 128;
vq_par.num_levels[1] = 64;
vq_par.num_levels[2] = 64;
vq_par.num_levels[3] = 64;
vq_par.num_bits[0] = 7;
vq_par.num_bits[1] = 6;
vq_par.num_bits[2] = 6;
vq_par.num_bits[3] = 6;
vq_par.cb = msvq_cb;
/* Scale codebook to 0 to 1 */
v_scale(vq_par.cb,(2.0/FSAMP),3200);
/* Initialize Fourier magnitude vector quantization (read codebook) */
fs_vq_par.num_best = 1;
fs_vq_par.num_stages = 1;
fs_vq_par.dimension = NUM_HARM;
/*
* Allocate memory for number of levels per stage and indices
* and for number of bits per stage
*/
MEM_ALLOC(MALLOC,fs_vq_par.num_levels,fs_vq_par.num_stages,int);
MEM_ALLOC(MALLOC,fs_vq_par.indices,fs_vq_par.num_stages,int);
MEM_ALLOC(MALLOC,fs_vq_par.num_bits,fs_vq_par.num_stages,int);
fs_vq_par.num_levels[0] = FS_LEVELS;
fs_vq_par.num_bits[0] = FS_BITS;
fs_vq_par.cb = fsvq_cb;
/* Initialize fixed MSE weighting and inverse of weighting */
vq_fsw(w_fs, NUM_HARM, 60.0);
/* Pre-weight codebook (assume single stage only) */
if (fsvq_weighted == 0)
{
fsvq_weighted = 1;
for (j = 0; j < fs_vq_par.num_levels[0]; j++)
window(&fs_vq_par.cb[j*NUM_HARM],w_fs,&fs_vq_par.cb[j*NUM_HARM],
NUM_HARM);
}
}
/**
* ahrs_step() takes the values from the IMU and produces the
* new estimated attitude.
*/
void
ahrs_step(
v_t angles_out,
f_t dt,
f_t p,
f_t q,
f_t r,
f_t ax,
f_t ay,
f_t az,
f_t heading
)
{
m_t C;
m_t A;
m_t AT;
v_t X_dot;
m_t temp;
v_t Xm;
v_t Xe;
/* Construct the quaternion omega matrix */
rotate2omega( A, p, q, r );
/* X_dot = AX */
mv_mult( X_dot, A, X, 4, 4 );
/* X += X_dot dt */
v_scale( X_dot, X_dot, dt, 4 );
v_add( X, X_dot, X, 4 );
v_normq( X, X );
/*
printf( "X =" );
Vprint( X, 4 );
printf( "\n" );
*/
/* AT = transpose(A) */
m_transpose( AT, A, 4, 4 );
/* P = A * P * AT + Q */
m_mult( temp, A, P, 4, 4, 4 );
m_mult( P, temp, AT, 4, 4, 4 );
m_add( P, Q, P, 4, 4 );
compute_c( C, X );
/* Compute the euler angles of the measured attitude */
accel2euler(
Xm,
ax,
ay,
az,
heading
);
/*
* Compute the euler angles of the estimated attitude
* Xe = quat2euler( X )
*/
quat2euler( Xe, X );
/* Kalman filter update */
kalman_update(
P,
R,
C,
X,
Xe,
Xm
);
/* Estimated attitude extraction */
quat2euler( angles_out, X );
}
//TODO: don't pass scene....
void illuminate(vector point, Object* s, vector normal, Scene scene, float initColor[], vector color){
// printf("ic: %f %f %f\n", initColor[0],initColor[1],initColor[2]);
//construct ray from the point in space
//where this collision occured
float diffuse=0;
float specular=0;
for(int i=0; i<scene.numLights; i++){
//TODO: divide by type of light
switch(scene.lights[i]->type){
case DIRECTIONAL:
{
DirLight* dl = static_cast<DirLight*>(scene.lights[i]->li);
vector dir;
v_copy(dir, dl->dir);
v_normalize(dir);
v_scale(dir, dir, -1);
vector re, e;
v_subtract(e, scene.cam.pos, point);
v_normalize(e);
v_scale(re, normal, v_dot(e, normal)*2);
v_subtract(re, re, e);
v_normalize(re);
float dotprod = v_dot(dir, normal);
if(dotprod>0){
Ray rayToLight;
v_copy(rayToLight.o, point);
v_copy(rayToLight.d, dir);
float scale = shadowRay(rayToLight);
diffuse+=dotprod*dl->i;
float x=-1;
if(s->n!=0){
float dotprodSpec = v_dot(re,dir);
if(dotprodSpec<0)
dotprodSpec=0;
x = pow(dotprodSpec, s->n)*dl->i;
}
if(x>0)
specular+=x;
diffuse*=scale;
specular*=scale;
}
}
break;
case SPOT:
{
SpotLight* sl = static_cast<SpotLight*>(scene.lights[i]->li);
vector lightPoint;
v_copy(lightPoint, sl->pos);
vector lightVector;
vector lightToPoint;
v_subtract(lightVector,lightPoint,point);
v_normalize(lightVector);
vector re, e;
v_subtract(e, scene.cam.pos, point);
v_normalize(e);
v_scale(re, normal, v_dot(e, normal)*2);
v_subtract(re, re, e);
v_normalize(re);
float dotprod = v_dot(lightVector,normal);
//if positive, we are facing the light
if(dotprod>0){
Ray rayToLight;
v_copy(rayToLight.o, point);
v_copy(rayToLight.d, lightVector);
float scale = shadowRay(rayToLight);
vector D;
vector dist;
v_subtract(dist,lightPoint,point);
v_copy(D,sl->dir);
v_scale(D,D,-1);
float intensity = pow(v_dot(D,lightVector), sl->falloff);
if(intensity<0)
intensity=0;
diffuse+=dotprod*intensity*sl->i;
float x=-1;
if(s->n!=0){
float dotprodSpec = v_dot(re,lightVector);
if(dotprodSpec<0)
dotprodSpec=0;
x = pow(dotprodSpec, s->n)*intensity*sl->i;
}
if(x>0){
specular+=x;
}
diffuse*=scale;
specular*=scale;
}
}
break;
case POINT:
{
PointLight* pl = static_cast<PointLight*>(scene.lights[i]->li);
vector lightPoint;
v_copy(lightPoint, pl->pos);
vector lightVector;
v_subtract(lightVector,lightPoint,point);
v_normalize(lightVector);
lightVector[3]=0;
point[3]=1;
vector re, e;
v_subtract(e, scene.cam.pos, point);
v_normalize(e);
v_scale(re, normal, v_dot(e, normal)*2);
v_subtract(re, re, e);
v_normalize(re);
float dotprod = v_dot(lightVector,normal);
//if positive, we are facing the light
if(dotprod>0){
Ray rayToLight;
v_copy(rayToLight.o, point);
if(pl->size==0){
v_copy(rayToLight.d, lightVector);
} else {
vector lightTemp;
v_subtract(lightTemp,lightPoint,point);
// printf("Pre Jitter:\n");
// v_print(lightTemp);
// printf("size: %f\n",pl->size);
float sx=randFloat(-pl->size,pl->size);
float sy=randFloat(-pl->size,pl->size);
float sz=randFloat(-pl->size,pl->size);
vector jitter;
jitter[0]=sx;
jitter[1]=sy;
jitter[2]=sz;
// v_print(jitter);
v_add(lightTemp, lightTemp, jitter);
// printf("Post Jitter:\n");
// v_print(lightTemp);
v_normalize(lightTemp);
lightTemp[3]=0;
v_copy(rayToLight.d, lightTemp);
}
float scale = shadowRay(rayToLight);
diffuse+=dotprod*pl->i;
float x=-1;
if(s->n!=0){
float dotprodSpec = v_dot(re,lightVector);
if(dotprodSpec<0)
dotprodSpec=0;
x = pow(dotprodSpec, s->n)*pl->i;
}
if(x>0){
specular+=x;
}
diffuse*=scale;
specular*=scale;
}
}
break;
}
}
color[0]=s->kd*(scene.amb + diffuse)*initColor[0];
color[0]+=s->ks*specular;
color[1]=s->kd*(scene.amb + diffuse)*initColor[1];
color[1]+=s->ks*specular;
color[2]=s->kd*(scene.amb + diffuse)*initColor[2];
color[2]+=s->ks*specular;
//printf("c %f\n",color[0]);
}