/**
* The assumption is that p0 corresponds to the from vertex. Probably true.
* I might check it again later though
* @brief Geom_2D::mesh_to_plane
* @param heh
* @param samp
* @param p0
* @param p1
*/
void Geom_2D::mesh_to_plane(Mesh::HalfedgeHandle heh, Mesh::Point &samp, Point_2D &p0, Point_2D &p1, Point_2D &dest){
Mesh::Point from = mesh->point(mesh->from_vertex_handle(heh));
Mesh::Scalar dist = distance3d(from, samp);
if (p0[0]<p1[0]){
dest[0] = p0[0]+dist;
}else{
dest[0] = p0[0]-dist;
}
dest[1] = 0.0;
}
boost::optional<std::size_t> find_armor_upgrade(entity_t &E, const int range) {
boost::optional<std::size_t> result;
auto my_pos = E.component<position_t>();
if (range != -1 && !my_pos) return result;
boost::container::flat_map<item_location_t, float> ac_by_loc;
each<item_t, item_carried_t>([&ac_by_loc, &result, &E] (entity_t &e, item_t &i, item_carried_t &c) {
if (c.carried_by == E.id && i.type == CLOTHING) {
auto finder = get_clothing_by_tag(i.item_tag);
if (finder) {
ac_by_loc[c.location] = finder->armor_class;
}
}
});
// Loop over all items on the ground
each<item_t, position_t>([&result, &ac_by_loc, &my_pos, &range] (entity_t &e, item_t &i, position_t &pos) {
if (!i.claimed && i.type == CLOTHING) {
if (is_better_armor(i.item_tag, ac_by_loc) && (range==-1 || distance3d(my_pos->x, my_pos->y, my_pos->z, pos.x, pos.y, pos.z) < range)) result = e.id;
}
});
// ditto for all items in containers
if (!result) {
each<item_t, item_stored_t>([&result, &ac_by_loc, &my_pos, &range] (entity_t &e, item_t &i, item_stored_t &pos) {
if (!i.claimed && i.type == CLOTHING) {
auto POS = e.component<position_t>();
if (is_better_armor(i.item_tag, ac_by_loc) && (range == -1 || (POS && distance3d(my_pos->x, my_pos->y, my_pos->z, POS->x, POS->y, POS->z) < range))) result = e.id;
}
});
}
if (result) {
entity(result.get())->component<item_t>()->claimed = true;
}
return result;
}
void buildRepelWalls(int num, int vert2[][3], vec3d &boidpos, vec3d &boidvel, float mass, float force, RepelAttract_t rep) {
vec3d vrtx;
for(int j=0; j<num; j++) {
vrtx.x = vert2[j][0];
vrtx.y = vert2[j][1];
vrtx.z = vert2[j][2];
if(distance3d(vrtx, boidpos) < 45.0) {
repelAttractVelocity(vrtx, raMass, boidpos, raForce, rep, boidvel);
repelAttractVelocity(vrtx, raMass, boidpos, raForce, rep, boidvel);
}
}
}
void handleObstacles() {
for (int i = 0; i < objcount; i++) {
// Save old position
vec3d pStart = boid[i].position;
//vec3d pStartVel = boid[i].velocity;
// Compute total acceleration and then integrate
setAcceleration(boid[i], xgravity, ygravity, zgravity, windvelocity, k);
eulerIntegrate(boid[i], dt);
// Save new position for use in collision detection later
vec3d pDest = boid[i].position;
//vec3d pDestVel = boid[i].velocity;
//vec3d pDestAcc = boid[i].acceleration;
// Collision with predator's habitat
for(int pp=0; pp<habcount; pp++) {
detectAndReflect(habitat[pp], pStart, pDest, boid[i].velocity, e);
}
buildRepelWalls(num_repelvert, repelvert, boid[i].position, boid[i].velocity, raMass, raForce, REPEL);
//////////////////////////////////////////////////////
// Check if there are obstacles like a polygon along the way
// ///////////////////////////////////////////////////
for(int p=0; p<tricount; p++) {
// Check how far boids are to obstacle before reacting.
// If I don't have this, boids react too prematurely.
if(distance3d(triangle[p].topleft, boid[i].position) < 45.0) {
repelAttractVelocity(triangle[p].topleft, raMass, boid[i].position,
raForce,
REPEL,
boid[i].velocity);
repelAttractVelocity(triangle[p].topright,
raMass, boid[i].position,
raForce,
REPEL,
boid[i].velocity);
}
// Collision with other polygons
detectAndReflect(triangle[p], pStart, pDest, boid[i].velocity, e);
}
} // end for loop
}
void spheremove (int iterations, int delay)
{
fill(0x00);
float origin_x, origin_y, origin_z, distance, diameter;
origin_x = 0;
origin_y = 3.5;
origin_z = 3.5;
diameter = 3;
int x, y, z, i;
for (i=0; i<iterations; i++)
{
origin_x = 3.5+sin((float)i/50)*2.5;
origin_y = 3.5+cos((float)i/50)*2.5;
origin_z = 3.5+cos((float)i/30)*2;
diameter = 2+sin((float)i/150);
for (x=0; x<8; x++)
{
for (y=0; y<8; y++)
{
for (z=0; z<8; z++)
{
distance = distance3d(x,y,z, origin_x, origin_y, origin_z);
//printf("Distance: %f \n", distance);
if (distance>diameter && distance<diameter+1)
{
setvoxel(x,y,z);
}
}
}
}
delay_ms(delay);
fill(0x00);
}
}
void eggManagement2() {
///////////////////////////////////
// Leader checks if there's food(egg)
///////////////////////////////////
for(int o=0; o<objcount; o++) {
for(int f=0; f<foodcount; f++) {
setAccel(food[f].acceleration, food[f].velocity, food[f].mass, 0, 0, 0, windvelocity, 0);
eulerInt(food[f].position, food[f].velocity, food[f].acceleration, dt);
repelAttractVelocity(food[f].position,
food[f].mass,
boid[o].position,
food[f].gravity,
ATTRACT,
boid[o].velocity);
if (food[f].stolen) {
boidVelocityMatch(boid[2].velocity, food[f].velocity, 0.5);
boidCentering(boid[2].position, food[f].position, food[f].velocity, 0.05);
boidAvoidance(boid[2].position, food[f].position, food[f].velocity, 0.5);
//boidAvoidance(food[f].position, food[f+1].position, food[f+1].velocity, 0.5);
}
/////////////////////////////////////////////////////////
// if close enough steal food
/////////////////////////////////////////////////////////
if(distance3d(food[f].position, boid[o].position) < 0.5) {
// how long to linger around food before actually taking it
linger -= 0.0005;
if(linger < 0.0) {
IsFoodPresent = false;
revballgravity = revball_init;
food[f].stolen = true;
food[f].thief = 1;
food[f].gravity = 0.0;
food[f].love = 0.0;
}
}
}
}
}
void predatorPolygonCollision() {
for (int i=0; i<predcount; i++) {
// Save old position for use in collision detection later
vec3d pStart = predator[i].position;
//vec3d Pos, Vel;
setAccel(predator[i].acceleration, predator[i].velocity, predator[i].mass, xgravity, -ygravity, zgravity, windvelocity, k);
eulerInt(predator[i].position, predator[i].velocity, predator[i].acceleration, dt);
// Save new position for use in collision detection later
vec3d pDest = predator[i].position;
// Collision with own habitat
for(int p=0; p<habcount; p++) {
detectAndReflect(habitat[p], pStart, pDest, predator[i].velocity, e);
}
// Collision with other polygons
for(int p=0; p<tricount; p++) {
detectAndReflect(triangle[p], pStart, pDest, predator[i].velocity, e);
// Repellance with other polygons
if(distance3d(triangle[p].topleft, predator[i].position) < 35.0) {
repelAttractVelocity(triangle[p].topleft,
raMass,
predator[i].position,
raForce,
REPEL,
predator[i].velocity);
repelAttractVelocity(triangle[p].topright,
raMass,
predator[i].position,
raForce,
REPEL,
predator[i].velocity);
}
}
}
}
int main(int argc,char** argv)
{
cv::Mat src = cv::imread("input.jpg");
IplImage* img=cvLoadImage("input.jpg");
IplImage* image = cvCloneImage(img);
IplImage* temp=cvCloneImage(image);
cvNamedWindow("Draw Line Example", CV_WINDOW_NORMAL);
cvSetMouseCallback(
"Draw Line Example",
my_callback,
(void*)image);
while(1)
{
cvCopyImage(image,temp);
if(drawing_line) draw_line(temp,pt1,pt2);
cvShowImage("Draw Line Example",temp);
int c = cv::waitKey(20);
if ( c == 'q' )
{
break;
}
switch(c){
case 'x':
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'y':
x_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'z':
y_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 's':
z_point_vector = point_vector;
point_vector.clear();
pt1 = cvPoint(0,0);
pt2 = cvPoint(0,0);
cvCopyImage(img,image);
break;
case 'h':
point_vector.clear();
drawing_point = true;
cvCopyImage(img,image);
break;
default:
break;
}
}
float w = (img->width + img->height)/4.0;
// calculate vanishing points
for (std::vector<int>::size_type i=0;i!= x_point_vector.size();++i){
x_point_vector[i].x -= img->width/2;
x_point_vector[i].y -= img->height/2;
}
for (std::vector<int>::size_type i=0;i!= y_point_vector.size();++i){
y_point_vector[i].x -= img->width/2;
y_point_vector[i].y -= img->height/2;
}
for (std::vector<int>::size_type i=0;i!= z_point_vector.size();++i){
z_point_vector[i].x -= img->width/2;
z_point_vector[i].y -= img->height/2;
}
CvPoint3D32f x_vanish = calc_vanishing_point(x_point_vector,w);
CvPoint3D32f y_vanish = calc_vanishing_point(y_point_vector,w);
CvPoint3D32f z_vanish = calc_vanishing_point(z_point_vector,w);
point_vector.clear();
point_vector.push_back(cvPoint(2316, 3681));
point_vector.push_back(cvPoint(1711, 3372));
point_vector.push_back(cvPoint(2226, 3300));
point_vector.push_back(cvPoint(2942, 3570));
point_vector.push_back(cvPoint(2326, 3092));
// calculate homography of x-y plane and scale in xyz axis
if (point_vector.size() < 5)
{
std::cout << "Less than 5 points are clicked!\n";
return -1;
}
double scale_x = 300, scale_y = 600, scale_z = 300;
std::vector <cv::Point2d> target_point_vector;
target_point_vector.push_back(cv::Point2d(0,0));
target_point_vector.push_back(cv::Point2d(0, scale_y));
target_point_vector.push_back(cv::Point2d(scale_x, scale_y));
target_point_vector.push_back(cv::Point2d(scale_x, 0));
std::vector <cv::Point2d> src_point_vector;
src_point_vector.push_back(cv::Point2d(point_vector[0].x, point_vector[0].y));
src_point_vector.push_back(cv::Point2d(point_vector[1].x, point_vector[1].y));
src_point_vector.push_back(cv::Point2d(point_vector[2].x, point_vector[2].y));
src_point_vector.push_back(cv::Point2d(point_vector[3].x, point_vector[3].y));
cv::Vec3d origin(point_vector[0].x, point_vector[0].y, 1);
cv::Mat Hxy = cv::findHomography(src_point_vector, target_point_vector);
cv::Vec3d z_scale(point_vector[4].x, point_vector[4].y, 1);
point_vector.clear();
//cv::Vec3d v_x(x_vanish.x + image->width / 2, x_vanish.y + image->height / 2, 1);
//cv::Vec3d v_y(y_vanish.x + image->width / 2, y_vanish.y + image->height / 2, 1);
//cv::Vec3d v_z(z_vanish.x + image->width / 2, z_vanish.y + image->height / 2, 1);
cv::Vec3d v_x(9563.7, 2403.48, 1);
cv::Vec3d v_y(-86.75, 2409.9, 1);
cv::Vec3d v_z(5534.12, 530677, 1);
std::cout << "origin: " << origin << "\n";
std::cout << "vanishing x: " << v_x << "\n";
std::cout << "vanishing y: " << v_y << "\n";
std::cout << "vanishing z: " << v_z << "\n";
std::cout << "homography: " << Hxy << "\n";
// calculate scale of z axis
double z_temp = cal3dZ(v_x, v_y, v_z, z_scale, origin, origin, 1);
scale_z = scale_z / z_temp;
double x_coord, y_coord, z_coord;
cv::Vec3d pt0, pt1, pt2, pt3;
cv::Vec3d top0, bottom0, top1, bottom1, top2, bottom2, top3, bottom3;
std::vector<cv::Vec3d> src_text_vector;
std::vector<cv::Vec3d> target_text_vector;
int cnt = 111;
while (1)
{
cvCopyImage(image, temp);
int c = cv::waitKey(20);
if (c == 27)
{
break;
}
if (c == 'p')
{
if (point_vector.size() < 4) { break; }
top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);
pt1 = cal3dXYZ(v_x, v_y, v_z, bottom0, bottom0, origin, scale_z, Hxy);
top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
pt3 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);
pt2 = cal3dXYZ(v_x, v_y, v_z, bottom1, bottom1, origin, scale_z, Hxy);
std::cout << "3D coordinate 0: " << pt0 << "\n";
std::cout << "3D coordinate 1: " << pt1 << "\n";
std::cout << "3D coordinate 2: " << pt2 << "\n";
std::cout << "3D coordinate 3: " << pt3 << "\n";
double text_w = distance3d(pt0, pt3);
double text_h = distance3d(pt0, pt1);
std::cout << "Save texture...\n";
src_text_vector.clear();
target_text_vector.clear();
src_text_vector.push_back(top0);
src_text_vector.push_back(bottom0);
src_text_vector.push_back(bottom1);
src_text_vector.push_back(top1);
target_text_vector.push_back(cv::Vec3d(0, 0, 1));
target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));
cv::Mat text = getHomo(target_text_vector, src_text_vector);
std::cout << "texture: " << text_w << ", " << text_h << "\n";
cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
getTexture(texture, src, text);
std::string text_file = std::to_string(cnt) + ".jpg";
cv::imwrite(text_file, texture);
cnt++;
std::string textures[] = { text_file };
std::cout << textures[0] << std::endl;
std::vector<cv::Vec3d> text_points;
text_points.push_back(pt1);
text_points.push_back(pt0);
text_points.push_back(pt3);
text_points.push_back(pt2);
std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
create_crml_file(text_points, fout, textures);
fout.close();
point_vector.clear();
}
else if (c == 'v')
{
if (point_vector.size() < 8) { break; }
top0 = cv::Vec3d(point_vector[0].x, point_vector[0].y, 1);
bottom0 = cv::Vec3d(point_vector[1].x, point_vector[1].y, 1);
pt0 = cal3dXYZ(v_x, v_y, v_z, top0, bottom0, origin, scale_z, Hxy);
top1 = cv::Vec3d(point_vector[2].x, point_vector[2].y, 1);
bottom1 = cv::Vec3d(point_vector[3].x, point_vector[3].y, 1);
pt1 = cal3dXYZ(v_x, v_y, v_z, top1, bottom1, origin, scale_z, Hxy);
top2 = cv::Vec3d(point_vector[4].x, point_vector[4].y, 1);
bottom2 = cv::Vec3d(point_vector[5].x, point_vector[5].y, 1);
pt2 = cal3dXYZ(v_x, v_y, v_z, top2, bottom2, origin, scale_z, Hxy);
top3 = cv::Vec3d(point_vector[6].x, point_vector[6].y, 1);
bottom3 = cv::Vec3d(point_vector[7].x, point_vector[7].y, 1);
pt3 = cal3dXYZ(v_x, v_y, v_z, top3, bottom3, origin, scale_z, Hxy);
std::cout << "3D coordinate 0: " << pt0 << "\n";
std::cout << "3D coordinate 1: " << pt1 << "\n";
std::cout << "3D coordinate 2: " << pt2 << "\n";
std::cout << "3D coordinate 3: " << pt3 << "\n";
double text_w = distance3d(pt0, pt3);
double text_h = distance3d(pt0, pt1);
std::cout << "Save texture...\n";
src_text_vector.clear();
target_text_vector.clear();
src_text_vector.push_back(top0);
src_text_vector.push_back(top1);
src_text_vector.push_back(top2);
src_text_vector.push_back(top3);
target_text_vector.push_back(cv::Vec3d(0, 0, 1));
target_text_vector.push_back(cv::Vec3d(0, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, text_h, 1));
target_text_vector.push_back(cv::Vec3d(text_w, 0, 1));
cv::Mat text = getHomo(target_text_vector, src_text_vector);
std::cout << "texture: " << text_w << ", " << text_h << "\n";
cv::Mat texture(text_h, text_w, src.type(), Scalar(0, 0, 0));
getTexture(texture, src, text);
std::string text_file = std::to_string(cnt) + ".jpg";
cv::imwrite(text_file, texture);
cnt++;
std::string textures[] = { text_file };
std::cout << textures[0] << std::endl;
std::vector<cv::Vec3d> text_points;
text_points.push_back(pt1);
text_points.push_back(pt0);
text_points.push_back(pt3);
text_points.push_back(pt2);
std::ofstream fout("livingroom.wrl", std::ofstream::out | std::ofstream::app);
create_crml_file(text_points, fout, textures);
fout.close();
point_vector.clear();
}
}
cvReleaseImage(&img);
cvReleaseImage(&image);
cvReleaseImage(&temp);
cvDestroyAllWindows();
return 0;
}
void drawPredator() {
for(int i = 0; i<predcount; i++) {
glPushMatrix();
glColor3ub( (char) rand()%256, (char) rand()%256, (char) rand()%256);
glTranslatef(predator[i].position.x, predator[i].position.y, predator[i].position.z);
glPushMatrix();
glScalef(1.0, 1.0,2.2);
glutWireSphere(OBJECTRADIUS,20,20);
glPopMatrix();
glTranslatef(0.0, 0.0, 2.0);
glScalef(1.2, 1.2,1.2);
glutWireSphere(OBJECTRADIUS,10,10);
// Draw Question Mark
if(angry > 0 && distance3d(revpoint1.position, predator[i].position) < 5) {
glPushMatrix();
glTranslatef(0.0, 6.0, 0.0);
glutSolidSphere(1.5,20,20);
glRotatef(0, 0,1,0);
glScalef(1.5,1.5,1.5);
glBegin(GL_TRIANGLES);
//glColor4f(0.2, 0.2, 0.7, 1.0);
glVertex3f(-3, 15, -1);
glVertex3f(3, 15, -1);
glVertex3f(0, 2, -1);
glEnd();
glPopMatrix();
}
glPushMatrix();
glPushMatrix();
glTranslatef (1.5, 0.5, -0.4);
glRotatef ((GLfloat) rightflap, 0.0, 0.3, 1.0);
glTranslatef (1.0, 0.0, 0.0);
glPushMatrix();
glScalef (4.0, 0.3, 1.5);
glColor4f(0.8,0.5,0,0.1);
glutSolidCube (1.5);
glPopMatrix();
glPopMatrix();
glPushMatrix();
glTranslatef (1.5, 0.5, 0.4);
glRotatef ((GLfloat) rightflap, 0.0, -0.1, 1.0);
glTranslatef (1.0, 0.0, 0.0);
glPushMatrix();
glScalef (4.0, 0.4, 1.5);
glColor4f(0.8,0.5,0, 0.1);
glutSolidCube (1.2);
glPopMatrix();
glPopMatrix();
glPushMatrix();
glTranslatef (-1.5, 0.5, -0.4);
glRotatef ((GLfloat) leftflap, 0.0, 0.3, 1.0);
glTranslatef (-1.0, 0.0, 0.0);
glPushMatrix();
glScalef (4.0, 0.3, 1.5);
glColor4f(0.8,0.5,0,0.1);
glutSolidCube (1.2);
glPopMatrix();
glPopMatrix();
glPushMatrix();
glTranslatef (-1.5, 0.5, 0.4);
glRotatef ((GLfloat) leftflap, 0.0, -0.1, 1.0);
glTranslatef (-1.0, 0.0, 0.0);
glPushMatrix();
glScalef (4.0, 0.4, 1.5);
glColor4f(0.8,0.5,0, 0.1);
glutSolidCube (1.2);
glPopMatrix();
glPopMatrix();
glPopMatrix();
glPopMatrix();
}
}
void handlePredatoryBehavior2(){
for(int pr=0; pr<predcount; pr++) {
//////////////////////////////////////////////////
// Predator is of course attracted to its own egg
// but not for eating. AND only if the egg has
// not been stolen yet
/////////////////////////////////////////////////
for(int f=0; f<foodcount; f++) {
repelAttractVelocity(food[f].position,
food[f].mass,
predator[pr].position,
food[f].love,
ATTRACT,
predator[pr].velocity);
}
for(int ob=0; ob<objcount; ob++) {
////////////////////////////////////////////////////////////
// compute attract velocity of predator toward leader ONLY
// and only if at a certain distance to its egg
//////////////////////////////////////////////////////////////
for(int f=0; f<foodcount; f++) {
if( (distance3d(food[f].position, boid[ob].position) < 35)
&& !(food[f].stolen) ){
repelAttractVelocity(boid[ob].position,
boid[ob].mass,
predator[pr].position,
objgravity,
ATTRACT,
predator[pr].velocity);
}
}
////////////////////////////////////////////////////////////////
// Check how close boids are to predator before they react.
////////////////////////////////////////////////////////////////
if(distance3d(predator[pr].position, boid[ob].position) < 45.0) {
// At the same time, compute repel velocity of prey
// during predator presence
if(ob==0)
repelAttractVelocity(predator[pr].position,
predator[pr].mass,
boid[ob].position,
predgravity+5,
REPEL,
boid[ob].velocity);
else
repelAttractVelocity(predator[pr].position,
predator[pr].mass,
boid[ob].position,
predgravity,
REPEL,
boid[ob].velocity);
}
///////////////////////////////////////////////////////////
// This makes boids loose their cohesiveness when
// attacker is really close. Imitates real situation better
///////////////////////////////////////////////////////////
if(distance3d(predator[pr].position, boid[ob].position) < 25.0) {
kcentering = 0.0;
kmatching = 0.0;
}
else {
kcentering = kcenter_init;
kmatching = kmatch_init;
}
}
}
}
