void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
Matrix3d rotation;
rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
rotation(1,0), rotation(1,1), rotation(1,2),
rotation(2,0), rotation(2,1), rotation(2,2));
Vector3d mid_point = (start_pos + end_pos)/2.0;
origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}
/* Get grad & const for perpend. line to given points */
static void
line_definition (gdouble x1,
gdouble y1,
gdouble x2,
gdouble y2,
gdouble *lgrad,
gdouble *lconst)
{
double grad1;
double midx, midy;
grad1 = line_grad (x1, y1, x2, y2);
if (grad1 == 0.0)
{
#ifdef DEBUG
printf ("Infinite grad....\n");
#endif /* DEBUG */
return;
}
mid_point (x1, y1, x2, y2, &midx, &midy);
/* Invert grad for perpen gradient */
*lgrad = -1.0 / grad1;
*lconst = line_cons (midx, midy,*lgrad);
}
void Tree::Create(const std::vector<sf::Vector3f>& tile_vertices)
{
if(tile_vertices.size() != 4)
{
throw std::invalid_argument("tile_vertices must have 4 elements");
}
sf::Vector3f mid_point((tile_vertices[0].x + tile_vertices[2].x) / 2.0,
(tile_vertices[0].y + tile_vertices[2].y) / 2.0,
(tile_vertices[0].z + tile_vertices[2].z) / 2.0);
std::vector<sf::Vector3f> quad_vertices(4);
this->quads.resize(4);
quad_vertices[0] = tile_vertices[0];
quad_vertices[1] = mid_point;
quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
quad_vertices[3] = tile_vertices[0] + sf::Vector3f(0.0, 0.0, 1.0);
this->quads[0].create(quad_vertices);
quad_vertices[0] = tile_vertices[1];
quad_vertices[1] = mid_point;
quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
quad_vertices[3] = tile_vertices[1] + sf::Vector3f(0.0, 0.0, 1.0);
this->quads[1].create(quad_vertices);
quad_vertices[0] = tile_vertices[2];
quad_vertices[1] = mid_point;
quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
quad_vertices[3] = tile_vertices[2] + sf::Vector3f(0.0, 0.0, 1.0);
this->quads[2].create(quad_vertices);
quad_vertices[0] = tile_vertices[3];
quad_vertices[1] = mid_point;
quad_vertices[2] = mid_point + sf::Vector3f(0.0, 0.0, 1.0);
quad_vertices[3] = tile_vertices[3] + sf::Vector3f(0.0, 0.0, 1.0);
this->quads[3].create(quad_vertices);
this->type = rand() % 2;
}
double capsuleBoxDistance(const Vector3d& a_start, const Vector3d& a_end, const double a_radius, const Vector3d& b_center, const Vector3d& b_half_length, Vector3d& direction)
{
btTransform tr[2];
Matrix3d rotationA;
rotation_from_tangent((a_start - a_end).normalized(), rotationA);
btMatrix3x3 basisA;
basisA.setValue(rotationA(0,1), rotationA(0,0), rotationA(0,2),
rotationA(1,1), rotationA(1,0), rotationA(1,2),
rotationA(2,1), rotationA(2,0), rotationA(2,2));
btMatrix3x3 basisB;
basisB.setIdentity();
tr[0].setBasis(basisA);
tr[1].setBasis(basisB);
Vector3d mid_point = (a_start + a_end)/2.0;
btVector3 originA(mid_point(0), mid_point(1), mid_point(2));
btVector3 originB(b_center(0), b_center(1), b_center(2));
tr[0].setOrigin(originA);
tr[1].setOrigin(originB);
btCollisionShape* shapePtr[2];
btCapsuleShape capsule(btScalar(a_radius), btScalar((a_start-a_end).norm()));
btBoxShape box(btVector3(b_half_length(0), b_half_length(1), b_half_length(2)));
shapePtr[0] = &capsule;
shapePtr[1] = &box;
btDefaultCollisionConfiguration collisionConfiguration;
btCollisionDispatcher dispatcher(&collisionConfiguration);
btDbvtBroadphase pairCache;
btCollisionWorld world (&dispatcher,&pairCache,&collisionConfiguration);
world.getDispatchInfo().m_convexMaxDistanceUseCPT = true;
MyContactResultCallback result;
btCollisionObject obA;
obA.setCollisionShape(shapePtr[0]);
obA.setWorldTransform(tr[0]);
btCollisionObject obB;
obB.setCollisionShape(shapePtr[1]);
obB.setWorldTransform(tr[1]);
world.contactPairTest(&obA,&obB,result);
direction = result.positionWorldOnB - result.positionWorldOnA;
return result.distance;
}
static void
calc_section (spline *s,
double cminus1x, double cminus1y,
double cx, double cy,
double cplus1x, double cplus1y,
double cplus2x, double cplus2y)
{
double p0x, p1x, p2x, p3x, tempx,
p0y, p1y, p2y, p3y, tempy;
third_point (cx, cy, cplus1x, cplus1y, &p1x, &p1y);
third_point (cplus1x, cplus1y, cx, cy, &p2x, &p2y);
third_point (cx, cy, cminus1x, cminus1y, &tempx, &tempy);
mid_point (tempx, tempy, p1x, p1y, &p0x, &p0y);
third_point (cplus1x, cplus1y, cplus2x, cplus2y, &tempx, &tempy);
mid_point (tempx, tempy, p2x, p2y, &p3x, &p3y);
add_bezier_arc (s, p0x, p0y, p1x, p1y, p2x, p2y, p3x, p3y);
}
CubitVector RefEdge::curve_center()
{
CubitVector p1 = start_vertex()->coordinates();
CubitVector p2 = end_vertex()->coordinates();
if ( start_vertex() == end_vertex() )
{
mid_point(p2);
}
p1 += p2;
p1 /= 2.0;
return p1;
}
static void
add_bezier_arc (spline *s,
double x0, double y0,
double x1, double y1,
double x2, double y2,
double x3, double y3)
{
double midx01, midx12, midx23, midlsegx, midrsegx, cx,
midy01, midy12, midy23, midlsegy, midrsegy, cy;
mid_point (x0, y0, x1, y1, &midx01, &midy01);
mid_point (x1, y1, x2, y2, &midx12, &midy12);
mid_point (x2, y2, x3, y3, &midx23, &midy23);
mid_point (midx01, midy01, midx12, midy12, &midlsegx, &midlsegy);
mid_point (midx12, midy12, midx23, midy23, &midrsegx, &midrsegy);
mid_point (midlsegx, midlsegy, midrsegx, midrsegy, &cx, &cy);
if (can_approx_with_line (x0, y0, midlsegx, midlsegy, cx, cy))
add_line (s, x0, y0, cx, cy);
else if ((midx01 != x1) || (midy01 != y1) || (midlsegx != x2)
|| (midlsegy != y2) || (cx != x3) || (cy != y3))
add_bezier_arc (s, x0, y0, midx01, midy01, midlsegx, midlsegy, cx, cy);
if (can_approx_with_line (cx, cy, midx23, midy23, x3, y3))
add_line (s, cx, cy, x3, y3);
else if ((cx != x0) || (cy != y0) || (midrsegx != x1) || (midrsegy != y1)
|| (midx23 != x2) || (midy23 != y2))
add_bezier_arc (s, cx, cy, midrsegx, midrsegy, midx23, midy23, x3, y3);
}
void CombatCamera::ViewportRay(double screen_x, double screen_y,
double out_ray_origin[3], double out_ray_direction[3]) const
{
Matrix projection(4, 4);
std::copy(m_camera.getProjectionMatrix()[0], m_camera.getProjectionMatrix()[0] + 16,
projection.data().begin());
Matrix view(4, 4);
std::copy(m_camera.getViewMatrix(true)[0], m_camera.getViewMatrix(true)[0] + 16, view.data().begin());
Matrix inverse_vp = Inverse4(prod(projection, view));
double nx = (2.0 * screen_x) - 1.0;
double ny = 1.0 - (2.0 * screen_y);
Matrix near_point(3, 1);
near_point(0, 0) = nx;
near_point(1, 0) = ny;
near_point(2, 0) = -1.0;
// Use mid_point rather than far point to avoid issues with infinite projection
Matrix mid_point(3, 1);
mid_point(0, 0) = nx;
mid_point(1, 0) = ny;
mid_point(2, 0) = 0.0;
// Get ray origin and ray target on near plane in world space
Matrix ray_origin = Matrix4xVector3(inverse_vp, near_point);
Matrix ray_target = Matrix4xVector3(inverse_vp, mid_point);
Matrix ray_direction = ray_target - ray_origin;
ray_direction /=
ray_direction(0, 0) * ray_direction(0, 0) +
ray_direction(1, 0) * ray_direction(1, 0) +
ray_direction(2, 0) * ray_direction(2, 0);
std::copy(ray_origin.data().begin(), ray_origin.data().end(), out_ray_origin);
std::copy(ray_direction.data().begin(), ray_direction.data().end(), out_ray_direction);
}
void line_arc_line(double tooth_angle)
{
double gap = gap_width();
double radius = gap/3;
double clearance = gear_for_point->GetClearanceMM();
double line_length = clearance - radius;
if(line_length < 0.0)
{
line_length = 0.0;
radius = clearance;
}
gp_Pnt p1, p2, p1B, p2B;
get_clearance_points(p1, p2, tooth_angle, 0.0);
get_clearance_points(p1B, p2B, tooth_angle, line_length);
gp_Pnt pm = mid_point(tooth_angle, line_length);
gp_Circ c(gp_Ax2(pm, gp_Dir(0, 0, -1)), radius);
if(line_length >= wxGetApp().m_geom_tol)
sketch_for_gear->Add(new HLine(p1, p1B, &wxGetApp().current_color), NULL);
double mid_span_line_length = gap - 2*radius;
if(mid_span_line_length >= wxGetApp().m_geom_tol)
{
gp_Vec v(p1, p2);
v.Normalize();
gp_Pnt p1C, p2C;
get_clearance_points(p1C, p2C, tooth_angle, clearance);
gp_Pnt two_arc_p1 = gp_Pnt(p1C.XYZ() + v.XYZ() * radius);
gp_Pnt two_arc_p2 = gp_Pnt(two_arc_p1.XYZ() + v.XYZ() * mid_span_line_length);
gp_Pnt pm1 = gp_Pnt(p1B.XYZ() + v.XYZ() * radius);
gp_Pnt pm2 = gp_Pnt(pm1.XYZ() + v.XYZ() * mid_span_line_length);
gp_Circ c1(gp_Ax2(pm1, gp_Dir(0, 0, -1)), radius);
gp_Circ c2(gp_Ax2(pm2, gp_Dir(0, 0, -1)), radius);
sketch_for_gear->Add(new HArc(p1B, two_arc_p1, c1, &wxGetApp().current_color), NULL);
sketch_for_gear->Add(new HLine(two_arc_p1, two_arc_p2, &wxGetApp().current_color), NULL);
sketch_for_gear->Add(new HArc(two_arc_p2, p2B, c2, &wxGetApp().current_color), NULL);
}
else
{
sketch_for_gear->Add(new HArc(p1B, p2B, c, &wxGetApp().current_color), NULL);
}
if(line_length >= wxGetApp().m_geom_tol)
sketch_for_gear->Add(new HLine(p2B, p2, &wxGetApp().current_color), NULL);
spline_points_for_gear.clear();
spline_points_for_gear.push_back(p2);
}
// need code that binary searches and figures out where to insert
// need to account for the possibility of the difference being less
// than 0.001
int binary_search(int* ids, int id, double* values, double value, int min_index, int max_index){
if (max_index < min_index){
return min_index;
} else {
int mid_index = mid_point(min_index , max_index);
double of_interest = values[ids[mid_index]];
double diff = abs(of_interest-value);
int same = FALSE;
if (diff < 0.001){
same = TRUE;
}
if ((of_interest > value && !same) || (same && id > ids[mid_index])){
return binary_search(ids, id, values, value, min_index, mid_index-1);
} else {
return binary_search(ids, id, values, value, mid_index+1, max_index);
}
}
}
void Points(double *matrix, int *csize, double *tMatrix)
{
int j;
for(j=0;j<*csize-1;j++)
{
if(matrix[j+1]==matrix[j])
{
do
{
tMatrix[j]=0.0;
j++;
} while(matrix[j+1]==matrix[j]);
}
tMatrix[j] = mid_point(matrix[j], matrix[j+1]);
}
tMatrix[(*csize)-1]=0.0;
}
void orderPointsCounterClockWise(std::vector<vpImagePoint> &p)
{
// Now we create a map of VpPoint in order to order the points
std::map< double, vpImagePoint> poly_verteces;
vpImagePoint mid_point(0, 0);
for(unsigned int i=0; i < p.size(); i++)
mid_point += p[i];
mid_point /= p.size();
double theta;
for(unsigned int i=0; i < p.size(); i++)
{
theta = atan2(p[i].get_j() - mid_point.get_j(), p[i].get_i() - mid_point.get_i());
// Insert the vertexes in the map (ordered)
poly_verteces.insert ( std::pair<double, vpImagePoint>(theta, p[i]) );
}
// Now we create a Vector containing the ordered vertexes
std::vector<vpImagePoint> p_ordered;
for( std::map<double, vpImagePoint>::iterator it = poly_verteces.begin(); it!=poly_verteces.end(); ++it )
{
p_ordered.push_back( it->second );
}
p = p_ordered;
}
void clearance_point_mid(double tooth_angle)
{
gp_Pnt m = mid_point(tooth_angle, 1.0);
point(m.X(), m.Y());
}