/*
* Compute the new linear and angular velocities of a bouncing ball.
* Q gives the position of the point of impact and W gives the
* velocity of the object being impacted.
*/
static float sol_bounce(struct v_ball *up,
const float q[3],
const float w[3], float dt)
{
float n[3], r[3], d[3], vn, wn;
float *p = up->p;
float *v = up->v;
/* Find the normal of the impact. */
v_sub(r, p, q);
v_sub(d, v, w);
v_nrm(n, r);
/* Find the new angular velocity. */
v_crs(up->w, d, r);
v_scl(up->w, up->w, -1.0f / (up->r * up->r));
/* Find the new linear velocity. */
vn = v_dot(v, n);
wn = v_dot(w, n);
v_mad(v, v, n, 1.7 * (wn - vn));
v_mad(p, q, n, up->r);
/* Return the "energy" of the impact, to determine the sound amplitude. */
return fabsf(v_dot(n, d));
}
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);
}
static int sol_test_back(float dt,
const struct v_ball *up,
const struct b_side *sp,
const float o[3],
const float w[3])
{
float q[3], d;
/* If the ball is not in front of the plane, the test passes. */
v_sub(q, up->p, o);
d = sp->d;
if (v_dot(q, sp->n) - d - up->r <= 0)
return 1;
/* If it's not in front of the plane after DT seconds, the test passes. */
v_mad(q, q, up->v, dt);
d += v_dot(w, sp->n) * dt;
if (v_dot(q, sp->n) - d - up->r <= 0)
return 1;
/* Else, test fails. */
return 0;
}
void RigidBody::operator()(const RigidBody::InternalState &x, RigidBody::InternalState &dxdt, const double /* t */)
{
Eigen::Vector3d x_dot, v_dot, omega_dot;
Eigen::Vector4d q_dot;
Eigen::Vector4d q(attitude.x(), attitude.y(), attitude.z(), attitude.w());
Eigen::Vector3d omega = angularVelocity;
Eigen::Matrix4d omegaMat = Omega(omega);
x_dot = velocity;
v_dot = force/mass;
q_dot = 0.5*omegaMat*q;
omega_dot = inertia.inverse() *
(torque - omega.cross(inertia*omega));
dxdt[0] = x_dot(0);
dxdt[1] = x_dot(1);
dxdt[2] = x_dot(2);
dxdt[3] = v_dot(0);
dxdt[4] = v_dot(1);
dxdt[5] = v_dot(2);
dxdt[6] = q_dot(0);
dxdt[7] = q_dot(1);
dxdt[8] = q_dot(2);
dxdt[9] = q_dot(3);
dxdt[10] = omega_dot(0);
dxdt[11] = omega_dot(1);
dxdt[12] = omega_dot(2);
}
// works for homogeneous weights only. not clean
float Reaching::GetCosts(joint_vec_t& des_angle,cart_vec_t& des_pos){
joint_vec_t& v1,v2;
cart_vec_t& w1,w2;
float s1,s2;
v4_sub(des_angle,pos_angle,v1);
//m4_diag_v_multiply(weight_angle,v1,v2);
if (weight_angle[0] == 0){
s1 = v4_dot(v1,v1);
return s1;
}
else{
v4_scale(v1,1/weight_angle[0],v2);
s1 = v4_dot(v1,v2);
}
v_sub(des_cart,pos_cart,w1);
if (weight_cart[0] == 0){
s2 = v_dot(w1,w1);
return s2;
}
else{
v4_scale(w1,1/weight_cart[0],w2);
s2 = v_dot(w1,w2);
}
// m3_diag_v_multiply(weight_cart,w1,w2);
return (s1+s2)/(1/weight_cart[0]+1/weight_angle[0]);
}
static float sol_test_side(float dt,
float T[3],
const struct v_ball *up,
const struct s_base *base,
const struct b_lump *lp,
const struct b_side *sp,
const float o[3],
const float w[3])
{
float t = v_side(T, o, w, sp->n, sp->d, up->p, up->v, up->r);
int i;
if (t < dt)
for (i = 0; i < lp->sc; i++)
{
const struct b_side *sq = base->sv + base->iv[lp->s0 + i];
if (sp != sq &&
v_dot(T, sq->n) -
v_dot(o, sq->n) -
v_dot(w, sq->n) * t > sq->d)
return LARGE;
}
return t;
}
/* 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;
}
/*
* Compute appropriate tilt axes from the view basis.
*/
void game_tilt_axes(struct game_tilt *tilt, float view_e[3][3])
{
const float Y[3] = { 0.0f, 1.0f, 0.0f };
v_cpy(tilt->x, view_e[0]);
v_cpy(tilt->z, view_e[2]);
/* Handle possible top-down view. */
if (fabsf(v_dot(view_e[1], Y)) < fabsf(v_dot(view_e[2], Y)))
v_inv(tilt->z, view_e[1]);
}
/*
* Compute the new angular velocity and orientation of a ball pendulum.
* A gives the accelleration of the ball. G gives the gravity vector.
*/
void sol_pendulum(struct v_ball *up,
const float a[3],
const float g[3], float dt)
{
float v[3], A[3], F[3], r[3], Y[3], T[3] = { 0.0f, 0.0f, 0.0f };
const float m = 5.000f;
const float ka = 0.500f;
const float kd = 0.995f;
/* Find the total force over DT. */
v_scl(A, a, ka);
v_mad(A, A, g, -dt);
/* Find the force. */
v_scl(F, A, m / dt);
/* Find the position of the pendulum. */
v_scl(r, up->E[1], -up->r);
/* Find the torque on the pendulum. */
if (fabsf(v_dot(r, F)) > 0.0f)
v_crs(T, F, r);
/* Apply the torque and dampen the angular velocity. */
v_mad(up->W, up->W, T, dt);
v_scl(up->W, up->W, kd);
/* Apply the angular velocity to the pendulum basis. */
sol_rotate(up->E, up->W, dt);
/* Apply a torque turning the pendulum toward the ball velocity. */
v_mad(v, up->v, up->E[1], v_dot(up->v, up->E[1]));
v_crs(Y, v, up->E[2]);
v_scl(Y, up->E[1], 2 * v_dot(Y, up->E[1]));
sol_rotate(up->E, Y, dt);
}
/* 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;
}
static float v_sol(const float p[3], const float v[3], float r)
{
float a = v_dot(v, v);
float b = v_dot(v, p) * 2.0f;
float c = v_dot(p, p) - r * r;
float d = b * b - 4.0f * a * c;
/* Testing for equality against zero is acceptable. */
if (a == 0.0f) return LARGE;
if (d < 0.0f) return LARGE;
else if (d > 0.0f)
{
float t0 = 0.5f * (-b - fsqrtf(d)) / a;
float t1 = 0.5f * (-b + fsqrtf(d)) / a;
float t = (t0 < t1) ? t0 : t1;
return (t < 0.0f) ? LARGE : t;
}
else return -b * 0.5f / a;
}
/*
* Compute the earliest time and position of the intersection of a
* sphere and a plane.
*
* The sphere has radius R and moves along vector V from point P. The
* plane moves along vector W. The plane has normal N and is
* positioned at distance D from the origin O along that normal.
*/
static float v_side(float Q[3],
const float o[3],
const float w[3],
const float n[3], float d,
const float p[3],
const float v[3], float r)
{
float vn = v_dot(v, n);
float wn = v_dot(w, n);
float t = LARGE;
if (vn - wn < 0.0f)
{
float on = v_dot(o, n);
float pn = v_dot(p, n);
float u = (r + d + on - pn) / (vn - wn);
float a = ( d + on - pn) / (vn - wn);
if (0.0f <= u)
{
t = u;
v_mad(Q, p, v, +t);
v_mad(Q, Q, n, -r);
}
else if (0.0f <= a)
{
t = 0;
v_mad(Q, p, v, +t);
v_mad(Q, Q, n, -r);
}
}
return t;
}
/*
double vec1, vec2; // the two vectors
int degf; // degreee/radian flag( 0=rad, 1= deg )
int dim; // vector dimension
*/
double v_angle(double vec1[], double vec2[], int degf, int dim, double zeroTol) {
// Cleaned up by Hamid 11/2006
double mag1, mag2, dprod, cosang, angle;
mag1 = v_magn(vec1, dim);
mag2 = v_magn(vec2, dim);
if (fabs(mag1-0.)<zeroTol || fabs(mag2-0.)<zeroTol) {
angle =0.;
}
else {
dprod = v_dot(vec1, vec2, dim);
cosang = dprod / (mag1 * mag2);
cosang = cosang < -1. ? -1. : cosang;
cosang = cosang > +1. ? +1. : cosang;
angle = acos(cosang);
if (degf == 1)
angle *= RAD_TO_DEG;
}
return angle;
}
/*
* Compute the earliest time and position of the intersection of a
* sphere and a vertex.
*
* The sphere has radius R and moves along vector V from point P. The
* vertex moves along vector W from point Q in a coordinate system
* based at O.
*/
static float v_vert(float Q[3],
const float o[3],
const float q[3],
const float w[3],
const float p[3],
const float v[3], float r)
{
float O[3], P[3], V[3];
float t = LARGE;
v_add(O, o, q);
v_sub(P, p, O);
v_sub(V, v, w);
if (v_dot(P, V) < 0.0f)
{
t = v_sol(P, V, r);
if (t < LARGE)
v_mad(Q, O, w, t);
}
return t;
}
int main(int argc, char **argv)
{
COORD cord[3];
float a,b;
scanf("%f %f %f %f %f %f %f", &cord[0].x, &cord[0].y, &cord[0].z, &cord[1].x, &cord[1].y, &cord[1].z, &a);
printf("\nv1 = ( %f , %f , %f)", cord[0].x, cord[0].y, cord[0].z);
printf("\nv2 = ( %f , %f , %f)", cord[1].x, cord[1].y, cord[1].z);
printf("\na = %f ", a);
cord[2] = v_sum(&cord[0],&cord[1]);
printf("\nv1+v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
cord[2] = v_dif(&cord[0],&cord[1]);
printf("\nv1-v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
b = v_dot(&cord[0],&cord[1]);
printf("\nv1.v2 = %f ", b);
cord[2] = v_prod(&a,&cord[0]);
printf("\na v1 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
cord[2] = v_cross(&cord[0],&cord[1]);
printf("\nv1 x v2 = ( %f , %f , %f )", cord[2].x, cord[2].y, cord[2].z);
b = v_mod_2(&cord[0]);
printf("\nv1^2 = %f ", b);
b = v_mod(&cord[0]);
printf("\n|v1| = %f ", b);
printf("\n\n");
return(0);
}
/* 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;
}
/* Third version of draw triangle uses a z_buffer
And we're using flat shading
clear_z_buffer needs to be run in the main loop before every frame. */
void draw_triangle3(int *a,int *b,int *c,vect_t pos,vect_t rot,rgb color){
int x0,y0,x1,y1,x2,y2;
// this is is the format that the triangles should be passed to us already
vect_t x={a[0],a[1],a[2]+Z};
vect_t y={b[0],b[1],b[2]+Z};
vect_t z={c[0],c[1],c[2]+Z};
// Lighting
vect_t n=v_norm(v_cross(x,y));
// vect_t l=v_sub(cam.pos,x);
vect_t l=cam.pos;
double lum=1.0;
// if( lum < 0 ) return 0;
rgb colors[3];
point p[3];
x=v_rot(x,rot);
y=v_rot(y,rot);
z=v_rot(z,rot);
// should we do the translation here as well does this make sense?
x=v_add(x,pos);
y=v_add(y,pos);
z=v_add(z,pos);
double lums[3];
lums[0]=v_dot(l,x)/(v_len(l)*v_len(x));
lums[1]=v_dot(l,y)/(v_len(l)*v_len(y));
lums[2]=v_dot(l,z)/(v_len(l)*v_len(z));
#if 1
for(int i=0;i<3;i++){
#if 0
lum=lums[i];
if( lum < 0 ) lum=0;
#else
lum=1.0;
#endif
colors[i].r=clip(color.r*lum);
colors[i].g=clip(color.g*lum);
colors[i].b=clip(color.b*lum);
}
#else
for(int i=0;i<3;i++){
colors[i].r=color.r;
colors[i].g=color.g;
colors[i].b=color.b;
}
#endif
int q,w,e;
q=project_coord(x,&p[0].x,&p[0].y);
w=project_coord(y,&p[1].x,&p[1].y);
e=project_coord(z,&p[2].x,&p[2].y);
if(outside_of_screen(p)){
return;
}
rgb ca[3];
ca[0]=ca[1]=ca[2]=color;
vect_t v3[3];
v3[0]=x; v3[1]=y; v3[2]=z;
double distances[3];
// introduce max distance of 5000
distances[0]=clipfloat(v_len(v_sub(x,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[1]=clipfloat(v_len(v_sub(y,cam.pos)),0,MAX_DIST)/MAX_DIST;
distances[2]=clipfloat(v_len(v_sub(z,cam.pos)),0,MAX_DIST)/MAX_DIST;
/*
distances[0]=v_len(v_sub(z,cam.pos));
distances[1]=v_len(v_sub(y,cam.pos));
distances[2]=v_len(v_sub(x,cam.pos));
*/
draw_triangle_gradient_new(p,v3,distances,colors);
#if 0
draw_line_color(p[0].x,p[0].y,p[1].x,p[1].y,black);
draw_line_color(p[1].x,p[1].y,p[2].x,p[2].y,black);
draw_line_color(p[1].x,p[1].y,p[0].x,p[0].y,black);
#endif
}
static void game_update_view(float dt)
{
float dc = view_dc * (jump_b ? 2.0f * fabsf(jump_dt - 0.5f) : 1.0f);
float dx = view_ry * dt * 5.0f;
float k;
view_a += view_ry * dt * 90.f;
/* Center the view about the ball. */
v_cpy(view_c, file.uv->p);
v_inv(view_v, file.uv->v);
switch (config_get_d(CONFIG_CAMERA))
{
case 1: /* Camera 1: Viewpoint chases the ball position. */
v_sub(view_e[2], view_p, view_c);
break;
case 2: /* Camera 2: View vector is given by view angle. */
view_e[2][0] = fsinf(V_RAD(view_a));
view_e[2][1] = 0.f;
view_e[2][2] = fcosf(V_RAD(view_a));
dx = 0.0f;
break;
default: /* Default: View vector approaches the ball velocity vector. */
k = v_dot(view_v, view_v);
v_sub(view_e[2], view_p, view_c);
v_mad(view_e[2], view_e[2], view_v, k * dt / 4);
break;
}
/* Orthonormalize the basis of the view in its new position. */
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* Compute the new view position. */
k = 1.0f + v_dot(view_e[2], view_v) / 10.0f;
view_k = view_k + (k - view_k) * dt;
if (view_k < 0.5) view_k = 0.5;
v_cpy(view_p, file.uv->p);
v_mad(view_p, view_p, view_e[0], dx * view_k);
v_mad(view_p, view_p, view_e[1], view_dp * view_k);
v_mad(view_p, view_p, view_e[2], view_dz * view_k);
/* Compute the new view center. */
v_cpy(view_c, file.uv->p);
v_mad(view_c, view_c, view_e[1], dc);
/* Note the current view angle. */
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
void MoveR::IK4(const point& P)
{
//ROS_INFO("IK\n");
double th0=0,th1=0,th2=0,th3=0;
double x=P.x.data;
double y=P.y.data;
double z=P.z.data;
double p[3]={x,y,z};
double temp1=pow(x,2)+pow(y,2)+pow(z,2);
double norm_p=pow(temp1,0.5);
double b=acos((pow(lup,2)+pow(ldn,2)-pow(norm_p,2))/(2*lup*ldn));
th3=PI-b;
double a=asin((ldn/norm_p)*sin(b));
double s[3]={0,0,0};
double u_z[3]={0,0,1};
double p_s[3];
v_sub(p,s,p_s,3);
//ROS_INFO("p\n");
//v_print(p,3);
//ROS_INFO("s\n");
//v_print(s,3);
//ROS_INFO("p-s\n");
//v_print(p_s,3);
double norm_p_s=pow(pow(p_s[0],2)+pow(p_s[1],2)+pow(p_s[2],2),0.5);
double u_n[3]={p_s[0]/norm_p_s,p_s[1]/norm_p_s,p_s[2]/norm_p_s};
//ROS_INFO("u_n\n");
//v_print(u_n,3);
double tmp[3];
v_scalar_multip(v_dot(u_z,u_n,3),u_n,tmp,3);
double u[3],u_u[3];
//v_add(u_z,tmp,u,3);
//u correct on 2/17
v_cross(u_n,u_z,u);
double norm_u=pow(pow(u[0],2)+pow(u[1],2)+pow(u[2],2),0.5);
v_scalar_multip(1/norm_u,u,u_u,3);
//ROS_INFO("u_u \n");
//v_print(u_u,3);
double v[3],u_v[3];
v_cross(u_n,u_u,v);
double norm_v=pow(pow(v[0],2)+pow(v[1],2)+pow(v[2],2),0.5);
v_scalar_multip(1/norm_v,v,u_v,3);
//ROS_INFO("u_v \n");
//v_print(u_v,3);
double c[3];
v_scalar_multip(cos(a)*lup,u_n,tmp,3);
v_add(s,tmp,c,3);
double r=lup*sin(a);
//double fi=PI/2;
ROS_INFO("b %f\n",b);
ROS_INFO("a %f\n",a);
double tmp1[3],tmp2[3];
v_scalar_multip(cos(fi),u_u,tmp1,3);
v_scalar_multip(sin(fi),u_v,tmp2,3);
v_add(tmp1,tmp2,tmp,3);
v_scalar_multip(r,tmp,tmp,3);
double e[3];
v_add(c,tmp,e,3);
ROS_INFO("u_u \n");
v_print(u_u,3);
double ex=e[0],ey=e[1],ez=e[2];
ROS_INFO("ex %f\n",ex);
ROS_INFO("ey %f\n",ey);
ROS_INFO("ez %f\n",ez);
if(ex>=0&&ey<0)
th0=atan((double)abs(ex)/abs(ey));
else if(ex>=0&&ey>=0)
th0=PI-atan((double)abs(ex)/abs(ey));
else if(ex<0&&ey>=0)
th0=PI+atan((double)abs(ex)/abs(ey));
else if(ex<0&&ey<0)
th0=-atan((double)abs(ex)/abs(ey));
double temp2=pow(pow(ex,2)+pow(ey,2),0.5);
if(ez>=0)
th1=-atan((double)abs(ez)/temp2);
else if(ez<0)
th1=atan((double)abs(ez)/temp2);
//ROS_INFO("IK:th0 %f\n",th0);
//ROS_INFO("IK:th1 %f\n",th1);
//ROS_INFO("th3 %f\n",th3);
th2=atan2(-x*sin(th0)*sin(th1)+y*cos(th0)*sin(th1)-z*cos(th1),x*cos(th0)+y*sin(th0));
ROS_INFO("x %f y %f z %f\n",x,y,z);
ROS_INFO("th0 %f th1 %f th2 %f th3 %f fi% f\n",th0,th1,th2,th3,fi);
if(isnan(th0)||isnan(th1)||isnan(th2)||isnan(th3)||th0>TH0_MAX||th0<TH0_MIN||th1>TH1_MAX||th1<TH1_MIN||th3>TH3_MAX||th3<TH3_MIN){
ROS_INFO("Fail and dont move\n");
}
else{
ROS_INFO("go move move time=%f\n",move_time);
arm_angle.joint0.angle = th0;
arm_angle.joint0.duration = move_time;
arm_angle.joint1.angle = th1;
arm_angle.joint1.duration = move_time;
arm_angle.joint2.angle = th2;
arm_angle.joint2.duration = move_time;
arm_angle.joint3.angle = th3;
arm_angle.joint3.duration = move_time;
}
}
/*
* Compute the earliest time and position of the intersection of a
* sphere and an edge.
*
* The sphere has radius R and moves along vector V from point P. The
* edge moves along vector W from point Q in a coordinate system based
* at O. The edge extends along the length of vector U.
*/
static float v_edge(float Q[3],
const float o[3],
const float q[3],
const float u[3],
const float w[3],
const float p[3],
const float v[3], float r)
{
float d[3], e[3];
float P[3], V[3];
float du, eu, uu, s, t;
v_sub(d, p, o);
v_sub(d, d, q);
v_sub(e, v, w);
/*
* Think projections. Vectors D, extending from the edge vertex Q
* to the sphere, and E, the relative velocity of sphere wrt the
* edge, are made orthogonal to the edge vector U. Division of
* the dot products is required to obtain the true projection
* ratios since U does not have unit length.
*/
du = v_dot(d, u);
eu = v_dot(e, u);
uu = v_dot(u, u);
v_mad(P, d, u, -du / uu);
/* First, test for intersection. */
if (v_dot(P, P) < r * r)
{
/* The sphere already intersects the line of the edge. */
if (du < 0 || du > uu)
{
/*
* The sphere is behind the endpoints of the edge, and
* can't hit the edge without hitting the vertices first.
*/
return LARGE;
}
/* The sphere already intersects the edge. */
if (v_dot(P, e) >= 0)
{
/* Moving apart. */
return LARGE;
}
v_nrm(P, P);
v_mad(Q, p, P, -r);
return 0;
}
v_mad(V, e, u, -eu / uu);
t = v_sol(P, V, r);
s = (du + eu * t) / uu; /* Projection of D + E * t on U. */
if (0.0f <= t && t < LARGE && 0.0f < s && s < 1.0f)
{
v_mad(d, o, w, t);
v_mad(e, q, u, s);
v_add(Q, e, d);
}
else
t = LARGE;
return t;
}
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;
}
float sol_step(struct s_vary *vary, const float *g, float dt, int ui, int *m)
{
float P[3], V[3], v[3], r[3], a[3], d, e, nt, b = 0.0f, tt = dt, t;
int c;
union cmd cmd;
if (ui < vary->uc)
{
struct v_ball *up = vary->uv + ui;
/* If the ball is in contact with a surface, apply friction. */
v_cpy(a, up->v);
v_cpy(v, up->v);
v_cpy(up->v, g);
if (m && sol_test_file(tt, P, V, up, vary) < 0.0005f)
{
v_cpy(up->v, v);
v_sub(r, P, up->p);
t = v_len(r) * v_len(g);
if (t == 0.f)
{
t = SMALL;
}
if ((d = v_dot(r, g) / t) > 0.999f)
{
if ((e = (v_len(up->v) - dt)) > 0.0f)
{
/* Scale the linear velocity. */
v_nrm(up->v, up->v);
v_scl(up->v, up->v, e);
/* Scale the angular velocity. */
v_sub(v, V, up->v);
v_crs(up->w, v, r);
v_scl(up->w, up->w, -1.0f / (up->r * up->r));
}
else
{
/* Friction has brought the ball to a stop. */
up->v[0] = 0.0f;
up->v[1] = 0.0f;
up->v[2] = 0.0f;
(*m)++;
}
}
else v_mad(up->v, v, g, tt);
}
else v_mad(up->v, v, g, tt);
/* Test for collision. */
for (c = 16; c > 0 && tt > 0; c--)
{
float st;
int mi, ms;
/* HACK: avoid stepping across path changes. */
st = tt;
for (mi = 0; mi < vary->mc; mi++)
{
struct v_move *mp = vary->mv + mi;
struct v_path *pp = vary->pv + mp->pi;
if (!pp->f)
continue;
if (mp->tm + ms_peek(st) > pp->base->tm)
st = MS_TO_TIME(pp->base->tm - mp->tm);
}
/* Miss collisions if we reach the iteration limit. */
if (c > 1)
nt = sol_test_file(st, P, V, up, vary);
else
nt = tt;
cmd.type = CMD_STEP_SIMULATION;
cmd.stepsim.dt = nt;
sol_cmd_enq(&cmd);
ms = ms_step(nt);
sol_move_step(vary, nt, ms);
sol_swch_step(vary, nt, ms);
sol_ball_step(vary, nt);
if (nt < st)
if (b < (d = sol_bounce(up, P, V, nt)))
b = d;
tt -= nt;
}
v_sub(a, up->v, a);
sol_pendulum(up, a, g, dt);
}
return b;
}
void game_set_fly(float k)
{
struct s_vary *fp = &file.vary;
float x[3] = { 1.f, 0.f, 0.f };
float y[3] = { 0.f, 1.f, 0.f };
float z[3] = { 0.f, 0.f, 1.f };
float c0[3] = { 0.f, 0.f, 0.f };
float p0[3] = { 0.f, 0.f, 0.f };
float c1[3] = { 0.f, 0.f, 0.f };
float p1[3] = { 0.f, 0.f, 0.f };
float v[3];
if (!state)
return;
v_cpy(view_e[0], x);
v_cpy(view_e[1], y);
if (fp->base->zc > 0)
v_sub(view_e[2], fp->uv[ball].p, fp->base->zv[0].p);
else
v_cpy(view_e[2], z);
if (fabs(v_dot(view_e[1], view_e[2])) > 0.999)
v_cpy(view_e[2], z);
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* k = 0.0 view is at the ball. */
if (fp->uc > 0)
{
v_cpy(c0, fp->uv[ball].p);
v_cpy(p0, fp->uv[ball].p);
}
v_mad(p0, p0, view_e[1], view_dy);
v_mad(p0, p0, view_e[2], view_dz);
/* k = +1.0 view is s_view 0 */
if (k >= 0 && fp->base->wc > 0)
{
v_cpy(p1, fp->base->wv[0].p);
v_cpy(c1, fp->base->wv[0].q);
}
/* k = -1.0 view is s_view 1 */
if (k <= 0 && fp->base->wc > 1)
{
v_cpy(p1, fp->base->wv[1].p);
v_cpy(c1, fp->base->wv[1].q);
}
/* Interpolate the views. */
v_sub(v, p1, p0);
v_mad(view_p, p0, v, k * k);
v_sub(v, c1, c0);
v_mad(view_c, c0, v, k * k);
/* Orthonormalize the view basis. */
v_sub(view_e[2], view_p, view_c);
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
void game_update_view(float dt)
{
const float y[3] = { 0.f, 1.f, 0.f };
float dy;
float dz;
float k;
float e[3];
float d[3];
float s = 2.f * dt;
if (!state)
return;
/* Center the view about the ball. */
v_cpy(view_c, file.vary.uv[ball].p);
v_inv(view_v, file.vary.uv[ball].v);
switch (config_get_d(CONFIG_CAMERA))
{
case 2:
/* Camera 2: View vector is given by view angle. */
view_e[2][0] = fsinf(V_RAD(view_a));
view_e[2][1] = 0.f;
view_e[2][2] = fcosf(V_RAD(view_a));
s = 1.f;
break;
default:
/* View vector approaches the ball velocity vector. */
v_mad(e, view_v, y, v_dot(view_v, y));
v_inv(e, e);
k = v_dot(view_v, view_v);
v_sub(view_e[2], view_p, view_c);
v_mad(view_e[2], view_e[2], view_v, k * dt * 0.1f);
}
/* Orthonormalize the basis of the view in its new position. */
v_crs(view_e[0], view_e[1], view_e[2]);
v_crs(view_e[2], view_e[0], view_e[1]);
v_nrm(view_e[0], view_e[0]);
v_nrm(view_e[2], view_e[2]);
/* The current view (dy, dz) approaches the ideal (view_dy, view_dz). */
v_sub(d, view_p, view_c);
dy = v_dot(view_e[1], d);
dz = v_dot(view_e[2], d);
dy += (view_dy - dy) * s;
dz += (view_dz - dz) * s;
/* Compute the new view position. */
view_p[0] = view_p[1] = view_p[2] = 0.f;
v_mad(view_p, view_c, view_e[1], dy);
v_mad(view_p, view_p, view_e[2], dz);
view_a = V_DEG(fatan2f(view_e[2][0], view_e[2][2]));
}
//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]);
}
static void game_update_view(float dt)
{
float dc = view.dc * (jump_b > 0 ? 2.0f * fabsf(jump_dt - 0.5f) : 1.0f);
float da = input_get_r() * dt * 90.0f;
float k;
float M[16], v[3], Y[3] = { 0.0f, 1.0f, 0.0f };
float view_v[3];
float spd = (float) cam_speed(input_get_c()) / 1000.0f;
/* Track manual rotation time. */
if (da == 0.0f)
{
if (view_time < 0.0f)
{
/* Transition time is influenced by activity time. */
view_fade = CLAMP(VIEW_FADE_MIN, -view_time, VIEW_FADE_MAX);
view_time = 0.0f;
}
/* Inactivity. */
view_time += dt;
}
else
{
if (view_time > 0.0f)
{
view_fade = 0.0f;
view_time = 0.0f;
}
/* Activity (yes, this is negative). */
view_time -= dt;
}
/* Center the view about the ball. */
v_cpy(view.c, vary.uv->p);
view_v[0] = -vary.uv->v[0];
view_v[1] = 0.0f;
view_v[2] = -vary.uv->v[2];
/* Compute view vector. */
if (spd >= 0.0f)
{
/* Viewpoint chases ball position. */
if (da == 0.0f)
{
float s;
v_sub(view.e[2], view.p, view.c);
v_nrm(view.e[2], view.e[2]);
/* Gradually restore view vector convergence rate. */
s = fpowf(view_time, 3.0f) / fpowf(view_fade, 3.0f);
s = CLAMP(0.0f, s, 1.0f);
v_mad(view.e[2], view.e[2], view_v, v_len(view_v) * spd * s * dt);
}
}
else
{
/* View vector is given by view angle. */
view.e[2][0] = fsinf(V_RAD(view.a));
view.e[2][1] = 0.0;
view.e[2][2] = fcosf(V_RAD(view.a));
}
/* Apply manual rotation. */
if (da != 0.0f)
{
m_rot(M, Y, V_RAD(da));
m_vxfm(v, M, view.e[2]);
v_cpy(view.e[2], v);
}
/* Orthonormalize the new view reference frame. */
v_crs(view.e[0], view.e[1], view.e[2]);
v_crs(view.e[2], view.e[0], view.e[1]);
v_nrm(view.e[0], view.e[0]);
v_nrm(view.e[2], view.e[2]);
/* Compute the new view position. */
k = 1.0f + v_dot(view.e[2], view_v) / 10.0f;
view_k = view_k + (k - view_k) * dt;
if (view_k < 0.5) view_k = 0.5;
v_scl(v, view.e[1], view.dp * view_k);
v_mad(v, v, view.e[2], view.dz * view_k);
v_add(view.p, v, vary.uv->p);
/* Compute the new view center. */
v_cpy(view.c, vary.uv->p);
v_mad(view.c, view.c, view.e[1], dc);
/* Note the current view angle. */
view.a = V_DEG(fatan2f(view.e[2][0], view.e[2][2]));
game_cmd_updview();
}
