void SRS::SolveAim(float psi_angle, Matrix R1)
{
float h1[3], N[3], angle;
Matrix S0, S1;
// Get the final hand position
evalcircle(c, u, v, radius, psi_angle, h1);
// Rotate ee_r1 to h1
crossproduct(N, ee_r1, h1);
unitize(N);
angle = angle_between_vectors(ee_r1, h1, N);
rotation_axis_to_matrix(N, angle, S0);
// Now rotate a0 to a
float a[3], a0[3];
vecsub(a, (float*)ee, h1);
unitize(a);
hmatmult(S1,Ry,S0);
vecmult0(a0, (float*)axis, S1);
cpvector(N, h1);
unitize(N);
angle = angle_between_vectors(a0, a, N);
rotation_axis_to_matrix(N, angle, S1);
hmatmult(R1, S0, S1);
}
MxQuadric::MxQuadric(const MxVector& p1,const MxVector& p2,const MxVector& p3,
double area)
: A(p1.dim()), b(p1.dim())
{
VERIFY( p1.dim()==p2.dim() && p1.dim()==p3.dim() );
MxVector e1=p2; e1-=p1; unitize(e1); // e1 = p2-p1; unitize
MxVector e2=p3; e2-=p1; // e2 = p3-p1
MxVector t = e1;
t*=e1*e2; e2-=t; unitize(e2); // e2 = p3-p1-e1*(e1*(p3-p1)); unitize
double p1e1 = p1*e1;
double p1e2 = p1*e2;
mxm_identity(A, A.dim());
symmetric_subfrom(A, e1,e1);
symmetric_subfrom(A, e2,e2);
// b = e1*p1e1 + e2*p1e2 - p1
b=e1; b*=p1e1; t=e2; t*=p1e2; b += t; b -= p1;
c = p1*p1 - p1e1*p1e1 - p1e2*p1e2;
r = area;
}
float get_circle_equation(const float ee[3],
const float axis[3],
const float pos_axis[3],
float upper_len,
float lower_len,
float c[3],
float u[3],
float v[3],
float n[3])
{
float wn = norm((float *)ee);
float radius;
cpvector(n, ee);
unitize(n);
// Use law of cosines to get angle between first spherical joint
// and revolute joint
float alpha;
if (!law_of_cosines(wn, upper_len, lower_len, alpha))
return 0;
// center of circle (origin is location of first S joint)
vecscalarmult(c, n, _cos(alpha) * upper_len);
radius = _sin(alpha) * upper_len;
float temp[3];
//
// A little kludgy. If the goal is behind the joint instead
// of in front of it, we reverse the angle measurement by
// inverting the normal vector
//
if (DOT(n,pos_axis) < 0.0)
vecscalarmult(n,n,-1.0);
vecscalarmult(temp, n, DOT(axis,n));
vecsub(u, (float *)axis, temp);
unitize(u);
crossproduct(v, n, u);
#if 0
printf("Circle equation\n");
printf("c = [%lf,%lf,%lf]\n", c[0], c[1], c[2]);
printf("u = [%lf,%lf,%lf]\n", u[0], u[1], u[2]);
printf("v = [%lf,%lf,%lf]\n", v[0], v[1], v[2]);
printf("n = [%lf,%lf,%lf]\n", n[0], n[1], n[2]);
printf("r = %lf\n", radius);
#endif
return radius;
}
bool eigenvectors(const Mat2& M, const Vec2& evals, Vec2 evecs[2])
{
evecs[0] = Vec2(-M(0,1), M(0,0)-evals[0]);
evecs[1] = Vec2(-M(0,1), M(0,0)-evals[1]);
unitize(evecs[0]);
unitize(evecs[1]);
return true;
}
void MxPropSlim::discontinuity_constraint(MxVertexID i, MxVertexID j, MxFaceID f)
{
Vec3 org(m->vertex(i)), dest(m->vertex(j));
Vec3 e = dest - org;
Vec3 v1(m->vertex(m->face(f)(0)));
Vec3 v2(m->vertex(m->face(f)(1)));
Vec3 v3(m->vertex(m->face(f)(2)));
Vec3 n = triangle_normal(v1,v2,v3);
Vec3 n2 = e ^ n;
unitize(n2);
MxQuadric3 Q3(n2, -(n2*org));
Q3 *= boundary_weight;
if( weighting_policy == MX_WEIGHT_AREA ||
weighting_policy == MX_WEIGHT_AREA_AVG )
{
Q3.set_area(norm2(e));
Q3 *= Q3.area();
}
MxQuadric Q(Q3, dim());
quadric(i) += Q;
quadric(j) += Q;
}
//
// p = Projection of u onto v
//
void project(float p[3], const float u[3], const float v[3])
{
float vnorm[3];
cpvector(vnorm, v);
unitize(vnorm);
vecscalarmult(p, vnorm, DOT(u,vnorm));
}
int hitRing(entity_t * ent, point_t base, vector_t dir, hitinfo_t * hit) {
if(hitPlane(ent, base, dir, hit) != 1) return 0;
assert(ent->magic == ENTITY_T);
sobj_t *sobj = ent->entDerived;
assert(sobj->magic == SCENEOBJ_T);
plane_t *plane = sobj->sobjDerived;
assert(plane->magic == PLANE_T);
ring_t * ring = plane->planeDerived;
assert(ring->magic == RING_T);
tuple_t r_centerTOhit = ray(plane->point, hit->hitpoint);
double d_hitpoint = length(r_centerTOhit);
double d_inner = length(scale(unitize(r_centerTOhit), ring->radius1));
double d_outer = length(scale(unitize(r_centerTOhit), ring->radius2));
return ((d_hitpoint > d_inner) && (d_hitpoint < d_outer));
}
//
// Return the angle between two vectors u,v about the axis n
//
float angle_between_vectors(float u[3], float v[3], float n[3])
{
#if 0
float temp[3];
float up[3];
float vp[3];
cpvector(up, u);
cpvector(vp, v);
unitize(up);
unitize(vp);
crossproduct(temp, up, vp);
float mag = DOT(temp,n);
// Vectors are parallel at 0 or 180
if (mag*mag < 1e-8)
{
if (DOT(up,vp) < 0)
return M_PI;
else
return 0;
}
int sign = (mag > 0) ? 1 : -1;
float t = DOT(up,vp);
if (t > 1.0)
t = 1.0;
else if (t < -1.0)
t = -1.0;
return sign*acos(t);
#else
float up[3];
float vp[3];
float uv[3];
project_plane(up, u, n);
project_plane(vp, v, n);
crossproduct(uv, up, vp);
return atan2(DOT(n, uv), DOT(up, vp));
#endif
}
Mat4 quadrix_discontinuity_constraint(Edge *edge, const Vec3& n)
{
Vec3& org = *edge->org();
Vec3& dest = *edge->dest();
Vec3 e = dest - org;
Vec3 n2 = e ^ n;
unitize(n2);
real d = -n2 * org;
return quadrix_plane_constraint(n2, d);
}
/** completePlane **/
void completePlane(scene_t *scene, entity_t *ent) {
sobj_t *sobj;
plane_t *plane;
assert(scene->magic == SCENE_T);
assert(ent->magic == ENTITY_T);
sobj = ent->entDerived;
assert(sobj->magic == SCENEOBJ_T);
plane = sobj->sobjDerived;
assert(plane->magic == PLANE_T);
plane->normal = unitize(cross(plane->orient1, plane->orient2));
completeSceneObj(scene, ent);
}
/** rayTrace **/
intensity_t rayTrace(scene_t *scene, point_t base, vector_t unitDir,
double total_dist, entity_t *self) {
intensity_t intensity = ((intensity_t){0, 0, 0});
entity_t * closestEnt;
hitinfo_t * hit = malloc(sizeof(hitinfo_t));
closestEnt = closest(scene, base, unitDir, self, hit);
if (closestEnt == NULL) {
free(hit);
return (intensity);
}
total_dist += hit->distance;
window_t * window = ((window_t *)(scene->window->entDerived));
sobj_t * sobj = ((sobj_t *)(closestEnt->entDerived));
intensity = ((tuple_t){window->ambient.x * sobj->color.r,
window->ambient.y * sobj->color.g,
window->ambient.z * sobj->color.b});
intensity_t light = lighting(scene, closestEnt, hit);
intensity.x += light.x;
intensity.y += light.y;
intensity.z += light.z;
intensity.x /= 255;
intensity.y /= 255;
intensity.z /= 255;
intensity.x /= total_dist;
intensity.y /= total_dist;
intensity.z /= total_dist;
sobj_t * closestSobj = closestEnt->entDerived;
if (length(((tuple_t)(closestSobj->reflective))) != 0) {
vector_t U = scale(unitDir, -1);
vector_t N = hit->normal;
vector_t V = unitize(add(scale(N, (2 * dot(U, N))), unitDir));
intensity_t reflection;
reflection = rayTrace(scene, hit->hitpoint, V, total_dist, closestEnt);
multiply(reflection, closestSobj->reflective);
intensity = add(intensity, reflection);
}
free(hit);
return (intensity);
} /* End rayTrace */
void ParticleSystem::simulate(float DeltaT)
{
//need to initialize force every iteration
for (unsigned i=0;i<f.size() ;i++)
{
f[i]={0.0,-9.0,0.0};
}
// f[0]={-.5,-0.1,-0.1};
// f[6]={ .5, 0.1,0.1};
//for all spring related particle calculate the force
double const K=10.0; //spring constant
double const zeta=0.05; //damping ratio
for (map< pair<int,int>, float >::iterator it=springConnection.begin();it!=springConnection.end();it++)
{
float rest = it->second;
Vector3D particleDistance= vertices[ it->first.first]->co - vertices[it->first.second]->co;
double l = norm(particleDistance);
unitize(particleDistance);
Vector3D force = K*( rest- l)*particleDistance ;
f[it->first.first]=f[it->first.first]+force-v[it->first.first]*zeta;
f[it->first.second]=f[it->first.second]-force-v[it->first.second]*zeta;
}
// #pragma omp parallel for
for (unsigned i=0; i<v.size();i++)
{
v[i] = ((f[i]/m[i])*DeltaT)+v[i];
if (vertices[i]->co[1]<=0)
{
v[i]=-v[i];
}
}
// #pragma omp parallel for
for (unsigned i=0; i<v.size(); i++)
{
Vector3D offset=DeltaT*v[i];
vertices[i]->co=vertices[i]->co+offset;
}
}
Quaternion::Quaternion( const Vec3& axis, double radians )
{
radians *= 0.5;
Vec3 naxis = axis;
unitize( naxis );
double sine = sin( radians );
w = cos( radians );
x = sine * naxis[0];
y = sine * naxis[1];
z = sine * naxis[2];
make_unit( *this );
}
intensity_t processPointLight(scene_t *scene, entity_t *ent, entity_t *light, hitinfo_t *hit) {
assert(ent->magic == ENTITY_T);
tuple_t surfaceNormal = unitize(hit->normal);
tuple_t testRay = ray(hit->hitpoint,((pointlight_t *)(light->entDerived))->center);
testRay = unitize(testRay);
intensity_t returnIntensity = {0,0,0};
hitinfo_t *occlude = malloc(sizeof(hitinfo_t));
pointlight_t *pointlight = light->entDerived;
assert(pointlight->magic == POINTLIGHT_T);
sobj_t *obj = ent->entDerived;
assert(obj->magic == SCENEOBJ_T);
intensity_t diffuse = obj->diffuse;
intensity_t intensity = {pointlight->color.r * pointlight->brightness,pointlight->color.g*pointlight->brightness,pointlight->color.b*pointlight->brightness};
double lightDistance = length(ray(pointlight->center,hit->hitpoint));
if( dot(testRay,surfaceNormal) > 0 ) {
entity_t *occludingObject = closest(scene,pointlight->center,scale(testRay, -1.0),NULL, occlude);
free(occlude);
if(occludingObject == ent){
returnIntensity.x = diffuse.x * intensity.x * dot(testRay,surfaceNormal);
returnIntensity.y = diffuse.y*intensity.y * dot(testRay,surfaceNormal);
returnIntensity.z = diffuse.z*intensity.z * dot(testRay,surfaceNormal);
returnIntensity = scale(returnIntensity,1.0/lightDistance);
return(returnIntensity);
} else {
return(returnIntensity);
}
} else {
free(occlude);
return(returnIntensity);
}
}
//
// Form local coordinate system {x,y} from points p,q relative to the
// implicit origin 0. pscale is the reciprocal length of the p vector
// which as it turns out is already known. If the invert flag is true
// construct the transpose of the rotation matrix instead
//
inline void make_frame(const float p[3],
float p_scale,
const float q[3],
Matrix R,
int invert = 0)
{
float x[3], y[3], t[3];
// x vector is unit vector from origin to p
vecscalarmult(x, (float *)p, p_scale);
// y vector is unit perpendicular projection of q onto x
vecscalarmult(t, x, DOT(q,x));
vecsub(y, (float *) q, t);
unitize(y);
// z vector is x cross y
if (invert)
{
R[0][0] = x[0]; R[1][0] = x[1]; R[2][0] = x[2];
R[0][1] = y[0]; R[1][1] = y[1]; R[2][1] = y[2];
R[0][2] = x[1]*y[2] - x[2]*y[1];
R[1][2] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
else
{
R[0][0] = x[0]; R[0][1] = x[1]; R[0][2] = x[2];
R[1][0] = y[0]; R[1][1] = y[1]; R[1][2] = y[2];
R[2][0] = x[1]*y[2] - x[2]*y[1];
R[2][1] = x[2]*y[0] - x[0]*y[2];
R[2][2] = x[0]*y[1] - x[1]*y[0];
}
R[3][0] = R[3][1] = R[3][2] =
R[0][3] = R[1][3] = R[2][3] = 0;
R[3][3] = 1.0;
}
//
// To extract axis and angle from R use the formulas (murray, pg 414)
//
// 2 * cos(theta) - 1 = trace(R)
// and
// axis = vector associated with skew symmetric matrix (R-R')/(2*sin(theta))
//
//
// By our convention always return 0 <= angle < M_PI
//
void rotation_matrix_to_axis(const Matrix R, float axis[], float &angle)
{
const float eps = 1e-7f;
angle = acos((R[0][0] + R[1][1] + R[2][2] - 1) / 2.0f);
// Close to identity. Arbitrarily set solution to z axis rotation of 0
if (_abs(angle) < eps || _abs(angle - M_PI) < eps)
{
angle = 0.0;
axis[0] = axis[1] = 0.0; axis[2] = 1.0;
}
else
{
axis[0] = R[1][2] - R[2][1];
axis[1] = R[2][0] - R[0][2];
axis[2] = R[0][1] - R[1][0];
unitize(axis);
}
}
int hitPlane(entity_t *ent, point_t base, vector_t dir, hitinfo_t *hit) {
assert(ent->magic == ENTITY_T);
sobj_t *sobj = ent->entDerived;
assert(sobj->magic == SCENEOBJ_T);
plane_t *planePtr = sobj->sobjDerived;
assert(planePtr->magic == PLANE_T);
point_t Q = planePtr->point; /* Point data */
vector_t N = planePtr->normal; /* Normal data */
vector_t D = dir; /* Direction vector*/
point_t V = base; /* Base coordinates*/
point_t H; /* Hit point */
double t; /* Distance */
if (dot(N, D) == 0)
return 0; // parallel
t = (dot(N, Q) - dot(N, V))/
dot(N, D);
if (t < 0)
return 0; // behind me
H = scale(D, t);
H = add(H, V);
if (H.z > 0)
return 0; // between the "screen" and me
hit->hitpoint = H;
hit->normal = unitize(N);
hit->distance = t;
/* Adjust the normal depending on whether ray hit from front
or back of plane */
if (dot(D, hit->normal) > 0) {
hit->normal = scale(hit->normal, -1);
}
return 1;
}
/** genRay **/
vector_t genRay(scene_t *scene, int column, int row) {
vector_t direction; // Directior vector
entity_t *ent;
window_t *window;
assert(scene->magic == SCENE_T);
ent = scene->window;
window = ent->entDerived;
assert(window->magic == WINDOW_T);
/* Computer the pixel's real scene coordinates */
direction.x = ((double)(column)/
(double)(scene->picture->columns-1))*window->windowWidth;
direction.x -= window->windowWidth/2.0;
direction.y = ((double)(row)/
(double)(scene->picture->rows-1))*window->windowHeight;
direction.y -= window->windowHeight/2.0;
direction.z = 0;
/* And now construct a unit vector from the view point to the pixel */
direction = ray(window->viewPoint, direction);
direction = unitize(direction);
return(direction);
} /* End genRay */
void find_normal_vector(float v[3], float n[3])
{
int num_zero;
float min, temp;
int min_i;
min_i = 0;
min = _abs(v[0]);
num_zero = (min < 1e-8f);
temp = _abs(v[1]);
if (temp < 1e-8f)
num_zero++;
if (temp < min)
{
min = temp;
min_i = 1;
}
temp = _abs(v[2]);
if (temp < 1e-8)
num_zero++;
if (temp < min)
{
min = temp;
min_i = 2;
}
n[0] = n[1] = n[2] = 0.0;
switch (num_zero)
{
case 3:
// Vector is zero so there is no soln
break;
case 2:
// Vector has only one nonzero component. Set any of the
// other to 1.0 and 0 to the others
n[min_i] = 1.0;
break;
// Vector has at least two nonzero components
case 1:
default:
if (min_i == 0)
{
n[1] = -v[2];
n[2] = v[1];
}
else if (min_i == 1)
{
n[0] = -v[2];
n[2] = v[0];
}
else
{
n[0] = -v[1];
n[1] = v[0];
}
unitize(n);
break;
}
}
void C3DPlotCanvas::OnMouse( wxMouseEvent& event )
{
wxPoint pt(event.GetPosition());
wxClientDC dc(this);
PrepareDC(dc);
wxPoint point(event.GetLogicalPosition(dc));
if (event.RightDown()) {
m_bRButton = true;
int where[2];
where[0] = point.x;
where[1] = point.y;
last[0] = point.x;
last[1] = point.y;
ball->mouse_down(where,3);
}
if (event.RightUp()) {
m_bRButton = false;
int where[2];
where[0] = point.x;
where[1] = point.y;
ball->mouse_up(where,3);
}
if (event.LeftDown()) {
if ((event.CmdDown()) && this->m_d && this->b_select) {
m_brush = true;
last[0] = point.x;
last[1] = point.y;
} else {
m_bLButton = true;
int where[2];
where[0] = point.x;
where[1] = point.y;
last[0] = point.x;
last[1] = point.y;
ball->mouse_down(where,1);
}
}
if (event.LeftUp()) {
if (bSelect && this->m_d ) {
bSelect = false;
SelectByRect();
} else if (m_brush) {
m_brush = false;
} else {
m_bLButton = false;
int where[2];
where[0] = point.x;
where[1] = point.y;
ball->mouse_up(where,1);
}
}
if (event.Dragging()) {
int where[2];
where[0] = point.x;
where[1] = point.y;
if (m_brush) {
float vp[4];
glGetFloatv(GL_VIEWPORT, vp);
float W=vp[2], H=vp[3];
float diam = 2*(ball->radius);
ball->apply_transform();
int pix1[2], pix2[2], pix3[2];
pix1[0] = (int)(W/2);
pix1[1] = (int)(H/2);
pix2[0] = (int)(W/2-1);
pix2[1] = (int)(H/2);
pix3[0] = (int)(W/2);
pix3[1] = (int)(H/2-1);
double world1[3], world2[3],world3[3];
unproject_pixel(pix1, world1, 0.0);
unproject_pixel(pix2, world2, 0.0);
unproject_pixel(pix3, world3, 0.0);
ball->unapply_transform();
Vec3f w1(world1);
Vec3f w2(world2);
Vec3f w3(world3);
Vec3f screen_x = w1-w2;
unitize(screen_x);
Vec3f screen_y = w3-w1;
unitize(screen_y);
Vec3f XX(1,0,0);
Vec3f YY(0,1,0);
Vec3f ZZ(0,0,1);
xp += diam * (where[0] - last[0]) / W *(XX*screen_x);
yp += diam * (where[0] - last[0]) / W *(YY*screen_x);
zp += diam * (where[0] - last[0]) / W *(ZZ*screen_x);
xp += diam * (last[1] - where[1]) / H *(XX*screen_y);
yp += diam * (last[1] - where[1]) / H *(YY*screen_y);
zp += diam * (last[1] - where[1]) / H *(ZZ*screen_y);
if (xp < -1.0) xp = -1.0;
if (xp > 1.0) xp = 1.0;
if (yp < -1.0) yp = -1.0;
if (yp > 1.0) yp = 1.0;
if (zp < -1.0) zp = -1.0;
if (zp > 1.0) zp = 1.0;
last[0] = where[0];
last[1] = where[1];
c3d_plot_frame->control->m_xp->SetValue((int)((xp+1)*10000));
c3d_plot_frame->control->m_yp->SetValue((int)((yp+1)*10000));
c3d_plot_frame->control->m_zp->SetValue((int)((zp+1)*10000));
this->UpdateSelect();
} else {
bSelect = false;
if (m_bLButton & m_bRButton) {
ball->mouse_drag(where,last,2);
last[0] = where[0];
last[1] = where[1];
} else if (m_bLButton) {
ball->mouse_drag(where,last,1);
last[0] = where[0];
last[1] = where[1];
} else if (m_bRButton) {
ball->mouse_drag(where,last,3);
last[0] = where[0];
last[1] = where[1];
}
}
}
Refresh();
}
static void get_aim_circle_equation(const float g[3],
const float a[3],
const float ta[3],
const float tb[3],
const float proj_axis[3],
float theta4,
float center[3],
float u[3],
float v[3],
float &radius)
{
float L1 = DOT(ta,ta);
float L2 = DOT(tb,tb);
Matrix Ry, Ryt;
rotation_principal_axis_to_matrix('y', theta4, Ry);
invertrmatrix(Ryt, Ry);
// Compute distance of hand to shoulder
float t1[3], t2[3];
vecmult(t1, (float *) tb, Ry);
vecmult(t2, (float *) ta, Ryt);
float L3 = _sqrt(L1 + L2 + DOT(ta,t1) + DOT(tb,t2));
// Lengths of upper and lower arms
L1 = _sqrt(L1);
L2 = _sqrt(L2);
// Compute angle between a and shoulder-to-hand vector
// This is done assuming R1 = I since the angle does
// not depend on the shoulder joints
//
// h = Ry*tb + ta
// a = Ry*a
vecadd(t2, t1, (float *) ta);
unitize(t2);
vecmult(t1, (float *) a, Ry);
float alpha = acos(DOT(t1,t2));
//
// Compute the angles of the triangle s,h,g
//
float L4 = _sqrt(DOT(g,g));
float beta = M_PI - alpha;
float delta = asin(_sin(beta)*L3/L4);
if (delta < 0)
delta = - delta;
float gamma = M_PI - delta - beta;
float c_gamma = _cos(gamma);
float n[3];
cpvector(n, g);
unitize(n);
vecscalarmult(center, n, c_gamma*L3);
radius = _sqrt(1-c_gamma*c_gamma)*L3;
project_plane(u, (float *) proj_axis, n);
unitize(u);
crossproduct(v, n, u);
}
