//********************************************
// Collinear
//********************************************
int CVector3d::Collinear(CVector3d &vector)
{
float x = vector.x() / m_x;
float y = vector.y() / m_y;
float z = vector.z() / m_z;
return ((x == y) && (y == z));
}
//********************************************
// Cross
//********************************************
CVector3d
CVector3d::Cross(const CVector3d& v) const
{
return CVector3d(y() * v.z() - z() * v.y(),
z() * v.x() - x() * v.z(),
x() * v.y() - y() * v.x());
}
//********************************************
// Equals
// Determines if two vectors are equal
// within a tolerence (squared distance).
//********************************************
int
CVector3d::Equals(const CVector3d& v, double tolerence) const
{
CVector3d diff = *this - v;
return diff.LengthSquared() <= tolerence;
}
//********************************************
// IsCollinear
//********************************************
int
CVector3d::IsCollinear(CVector3d &vector) const
{
double x = vector.x() / vec[0];
double y = vector.y() / vec[1];
double z = vector.z() / vec[2];
return ((x == y) && (y == z));
}
//********************************************
// Inner
//********************************************
CVector3d CVector3d::Inner(CVector3d* u,
CVector3d* v)
{
// w = u ^ v
CVector3d w;
w.Set(u->y()*v->z()-u->z()*v->y(),
u->z()*v->x()-u->x()*v->z(),
u->x()*v->y()-u->y()*v->x());
return w;
}
/** Compute the distance from a point pta
* to the line defined by the 2 points ptb and ptc
* The two points ptb and ptc defining the baseline also define
* its positive direction from point ptb toward point ptc.
* It is assumed that the 3 points pta, ptb, ptc define a plane
* not far from the X-Y plane.
* The sign of d is positive if the point pta
* is left of the line ptb=>ptc
* The sign of d is negative if pta is right of the line.
*
* @author Robert Laine alias Sailcuter
*/
real Distance3d(const CPoint3d &pta, const CPoint3d &ptb, const CPoint3d &ptc)
{
real d;
CVector3d Va = CVector3d( pta - ptb );
CVector3d Vb = CVector3d( ptc - ptb).normalized();
CVector3d Vd = CVector3d::crossProduct(Vb, Va);
d = Vd.length();
if ( Vd.z() < 0 )
d = -d;
//
return d;
}
//********************************************
// Operator -
//********************************************
CVector3d operator-(CVector3d& u,
CVector3d& v)
{
CVector3d w;
w.Set(u.x()-v.x(),u.y()-v.y(),u.z()-v.z());
return w;
}
//********************************************
// Rotate this vector around pAround by angle
// by Haeyoung Lee
//********************************************
CVector3d CVector3d::Rotate(double angle,
CVector3d Around)
{
double f1, f2, f3;
CVector3d t1, t2;
f1 = (double)cos((double)angle);
f2 = (double)sin((double)angle);
t1 = Projection(&Around);
t2 = Around.Cross(this);
f3 = Dot(Around);
return CVector3d((double)(f1*t1.x()+f2*t2.x()+f3*Around.x()),
(double)(f1*t1.y()+f2*t2.y()+f3*Around.y()),
(double)(f1*t1.z()+f2*t2.z()+f3*Around.z()));
}
//********************************************
// Inner
//********************************************
void CVector3d::Inner(CVector3d& vector)
{
double x = (double)m_y*(double)vector.z()-(double)m_z*(double)vector.y();
double y = (double)m_z*(double)vector.x()-(double)m_x*(double)vector.z();
double z = (double)m_x*(double)vector.y()-(double)m_y*(double)vector.x();
Set((float)x,(float)y,(float)z);
}
//********************************************
// Operator ^
//********************************************
CVector3d operator^(CVector3d& u,
CVector3d& v)
{
// w = u ^ v
CVector3d w;
w.Set(u.y()*v.z()-u.z()*v.y(),
u.z()*v.x()-u.x()*v.z(),
u.x()*v.y()-u.y()*v.x());
return w;
}
//********************************************
// Operator =
//********************************************
CVector3d CVector3d::operator=(CVector3d& vector)
{
CVector3d v;
v.Set(vector);
return v;
}
//********************************************
// Set
//********************************************
void CVector3d::Set(CVector3d &vector)
{
Set(vector.x(),vector.y(),vector.z());
}
//Pileline�쐬�i2014/07/18�j
void create_pileline_01(int* VoxSize)
{
Pileline P;
edge E;
CVector3d temp;
vector<Pileline> CT;//connection
vector<edge> Et;//edge cost
int count=0;
for(int z(1); z<VoxSize[2]-1; z++)
for(int x(1); x<VoxSize[0]-1; x++)
{
for(int y(1); y<VoxSize[1]-1; y++)
{
if(block[x][y][z].fill == false)continue;
//###################################
//�P�~�P�̏ꍇ
//###################################
if(block[x][y][z].flag == 1)
{
temp.Set(x,y,z);
P.Point.push_back(temp);
P.num=1;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
block[x][y][z].BrickN=count;
count++;
}
else if(block[x][y][z].flag > 1)
{
if(block[x][y][z].corner != 1)
continue;
//###################################
//�P�~�Q�̏ꍇ
//###################################
if(block[x][y][z].flag == 12)
{
//�P�~�Q���������̏ꍇ
if(block[x][y+1][z].flag == 12 && block[x][y][z].ID == block[x][y+1][z].ID && block[x][y+1][z].corner == 2)
{
for(int i(0);i<2;i++){
temp.Set(x,y+i,z);
P.Point.push_back(temp);
block[x][y+i][z].BrickN=count;
}
P.num=2;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
continue;
}
//�P�~�Q���������̏ꍇ
if(block[x+1][y][z].flag == 12 && block[x][y][z].ID == block[x+1][y][z].ID && block[x+1][y][z].corner == 2)
{
for(int i(0);i<2;i++){
temp.Set( x+i , y ,z );
P.Point.push_back(temp);
block[x+i][y][z].BrickN=count;
}
P.num=2;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
//###################################
//�P�~�R�̏ꍇ
//###################################
//else if(block[x][y][z].flag == 13 && block[x][y][z].corner == 1)
else if(block[x][y][z].flag == 13)
{
//�P�~�R���������̏ꍇ
if(y+2 < VoxSize[1])
{
if(block[x][y+2][z].flag == 13 && block[x][y][z].ID == block[x][y+2][z].ID && block[x][y+2][z].corner == 2)
{
for(int i(0);i<3;i++){
temp.Set( x , y+i ,z );
P.Point.push_back(temp);
block[x][y+i][z].BrickN=count;
}
P.num=3;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
continue;
}
}
//�P�~�R���������̏ꍇ
if(x+2 < VoxSize[0])
{
if(block[x+2][y][z].flag == 13 && block[x][y][z].ID == block[x+2][y][z].ID && block[x+2][y][z].corner == 2)
{
for(int i(0);i<3;i++){
temp.Set( x+i , y ,z );
P.Point.push_back(temp);
block[x+i][y][z].BrickN=count;
}
P.num=3;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
}
//###################################
//�P�~�S�̏ꍇ
//###################################
//else if(block[x][y][z].flag == 14 && block[x][y][z].corner == 1)
else if(block[x][y][z].flag == 14)
{
//�P�~�S���������̏ꍇ
if(y+3 < VoxSize[1])
{
if(block[x][y+3][z].flag == 14 && block[x][y][z].ID == block[x][y+3][z].ID && block[x][y+3][z].corner == 2)
{
for(int i(0);i<4;i++){
temp.Set( x , y+i ,z );
P.Point.push_back(temp);
block[x][y+i][z].BrickN=count;
}
P.num=4;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
continue;
}
}
//�P�~�S���������̏ꍇ
if(x+3 < VoxSize[0])
{
if(block[x+3][y][z].flag == 14 && block[x][y][z].ID == block[x+3][y][z].ID && block[x+3][y][z].corner == 2)
{
for(int i(0);i<4;i++){
temp.Set( x+i , y ,z );
P.Point.push_back(temp);
block[x+i][y][z].BrickN=count;
}
P.num=4;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
}
//###################################
//�Q�~�Q�̏ꍇ
//###################################
//else if(block[x][y][z].flag == 22 && block[x][y][z].corner == 1)
else if(block[x][y][z].flag == 22)
{
if(block[x+1][y+1][z].flag == 22 && block[x][y][z].ID == block[x+1][y+1][z].ID && block[x+1][y+1][z].corner == 2)
{
for(int i(0) ; i<2;++i)
for(int j(0);j<2;++j){
temp.Set(x+i,y+j,z);
P.Point.push_back(temp);
block[x+i][y+j][z].BrickN=count;
}
P.num=4;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
//###################################
//�Q�~�R�̏ꍇ
//###################################
//else if(block[x][y][z].flag == 23 && block[x][y][z].corner == 1)
else if(block[x][y][z].flag == 23)
{
//�Q�~�R���������̏ꍇ
if(y+2 < VoxSize[1])
{
if(block[x+1][y+2][z].flag == 23 && block[x][y][z].ID == block[x+1][y+2][z].ID && block[x+1][y+2][z].corner == 2)
{
for(int i(0) ; i<2;++i)
for(int j(0);j<3;++j){
temp.Set(x+i,y+j,z);
P.Point.push_back(temp);
block[x+i][y+j][z].BrickN=count;
}
P.num=6;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
continue;
}
}
//�Q�~�R���������̏ꍇ
if(x+2 < VoxSize[0])
{
if(block[x+2][y+1][z].flag == 23 && block[x][y][z].ID == block[x+2][y+1][z].ID && block[x+2][y+1][z].corner == 2)
{
for(int i(0) ; i<3;++i)
for(int j(0);j<2;++j){
temp.Set(x+i,y+j,z);
P.Point.push_back(temp);
block[x+i][y+j][z].BrickN=count;
}
P.num=6;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
}
//###################################
//�Q�~�S�̏ꍇ
//###################################
//else if(block[x][y][z].flag == 24 && block[x][y][z].corner == 1)
else if(block[x][y][z].flag == 24)
{
//�Q�~�S���������̏ꍇ
if(y+3 < VoxSize[1])
{
if(block[x+1][y+3][z].flag == 24 && block[x][y][z].ID == block[x+1][y+3][z].ID && block[x+1][y+3][z].corner == 2)
{
for(int i(0) ; i<2;++i)
for(int j(0);j<4;++j){
temp.Set(x+i,y+j,z);
block[x+i][y+j][z].BrickN=count;
P.Point.push_back(temp);
}
P.num=8;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
continue;
}
}
//�Q�~�S���������̏ꍇ
if(x+3 < VoxSize[0])
{
if(block[x+3][y+1][z].flag == 24 && block[x][y][z].ID == block[x+3][y+1][z].ID && block[x+3][y+1][z].corner == 2)
{
for(int i(0) ; i<4;++i)
for(int j(0);j<2;++j){
temp.Set(x+i,y+j,z);
P.Point.push_back(temp);
block[x+i][y+j][z].BrickN=count;
}
P.num=8;
P.gravity.Clear();
for(int i(0); i <(int)P.Point.size();i++)
P.gravity+=P.Point[i];
P.gravity.Set(P.gravity.x/P.num , P.gravity.y/P.num , P.gravity.z/P.num);
if((zcood-1)==z)
P.ground=true;
if(block[x][y][z].in_surf)
P.in_surf=true;
CT.push_back(P);
P.init();
temp.Clear();
P.Point.clear();
P.Point.shrink_to_fit();
count++;
}
}
}
}
}
}
//for(int z(1); z<VoxSize[2]-1; z++)
// for(int x(1); x<VoxSize[0]-1; x++)
// {
// for(int y(1); y<VoxSize[1]-1; y++)
// {
// if(block[x][y][z].fill == false)continue;
//
//
//
//
// }
// }
vector<int> Nc;
//�ӂƃR�X�g�v�Z
for(int i (0); i < (int) CT.size();++i){
for(int c(0); c< CT[i].num; c++){
if(block[(int)CT[i].Point[c].x][(int)CT[i].Point[c].y][(int)CT[i].Point[c].z+1].fill){
Nc.push_back(block[(int)CT[i].Point[c].x][(int)CT[i].Point[c].y][(int)CT[i].Point[c].z+1].BrickN);
if(block[(int)CT[i].Point[c].x][(int)CT[i].Point[c].y][(int)CT[i].Point[c].z+1].BrickN==0)
cout<<"���������̂� "<<(int)CT[i].Point[c].z<<" "<<block[(int)CT[i].Point[c].x][(int)CT[i].Point[c].y][(int)CT[i].Point[c].z+1].flag<<endl;
}
}
//int number=0;
//cout<<endl;
//for(int a(0); a <(int) Nc.size(); a++){
//cout<<Nc[a]<<" ";
// if(Nc[a]==-10)continue;
// int count=1;
// number=Nc[a];
// for(int k = a+1; k <(int) Nc.size();k++){
// if(Nc[k] == number){
// count++;
// Nc[k]=-10;
// }
// E.cost=count;
// E.PN[0]=i;
// E.PN[1]=number;
// Et.push_back(E);
// }
// }
//Nc.clear();
//Nc.shrink_to_fit();
//�����ԍ����ǂ������邩
int num=0;
int cou=0;
for(int a(0) ; a < (int)Nc.size();a++){
if(Nc[a]<0)continue;
cou=0;
num=Nc[a];
for(int k(a+1); k < (int)Nc.size(); k++){
if(Nc[k]==num){
cou++;
Nc[k]=-10;
}
}
E.cost=cou+1;
E.PN[0]=i;
E.PN[1]=num;
Et.push_back(E);
}
Nc.clear();
Nc.shrink_to_fit();
}
ofstream Ffout("Pileline_gravity01.txt");
Ffout<<CT.size()<<" "<<Et.size()<<endl;
for(int i (0); i < (int) CT.size();++i){
//CT[i].gravity.Print();
//Ffout<<CT[i].gravity.x<<" "<<CT[i].gravity.y<<" "<<CT[i].gravity.z*1.2<<endl;
//LEGO�d�l�ɕύX
//Ffout<<CT[i].gravity.y<<" "<<VoxSize[2]-CT[i].gravity.z<<" "<<(VoxSize[0]-CT[i].gravity.x)*1.2<<endl;
Ffout<<CT[i].gravity.x<<" "<<CT[i].gravity.y<<" "<<CT[i].gravity.z*1.2<<" "<<CT[i].num<<" "<<CT[i].ground
<<" "<<CT[i].in_surf<<endl;
}
for(int i(0) ; i < (int)Et.size(); i++)
{
Ffout<<Et[i].PN[0]<<" "<<Et[i].PN[1]<<" "<<Et[i].cost<<endl;
}
Ffout.close();
CT.clear();
CT.shrink_to_fit();
//Pileline�A���S���Y�����O���̒���z�u����
//���I�������A����
int ID,flag ,Zcoord,Xcoord,Ycoord;
//double Zcoord,Xcoord,Ycoord;
string line;
stringstream ssline;
fstream fin("Prop.txt");
for(int i (0) ; i < 10 ; i++){
getline(fin , line);
ssline.str(line);
ssline>>Xcoord>>Ycoord>>Zcoord;
ssline.clear();
//�R�����z��ɑΉ������A�������
ID=block[Xcoord][Ycoord][Zcoord].ID;
flag=block[Xcoord][Ycoord][Zcoord].flag;
//for(int j = Zcoord+1; j < zcood ; j++){
if(flag==24){
if(block[Xcoord-2][(int)(Ycoord)-1][Zcoord].ID==ID&&
block[Xcoord-2][(int)(Ycoord)-1][Zcoord].flag==flag&&
block[Xcoord-2][(int)(Ycoord)-1][Zcoord].corner==1){
for(int j = Zcoord+1; j < zcood ; j++){
ID24++;change_bricks(block,Xcoord-2,Ycoord-1,j,4,2,ID24,flag);
}
}
else if(block[Xcoord-1][Ycoord-2][Zcoord].ID==ID&&
block[Xcoord-1][Ycoord-2][Zcoord].flag==flag&&
block[Xcoord-1][Ycoord-2][Zcoord].corner==1){
for(int j = Zcoord+1; j < zcood ; j++){
ID24++;change_bricks(block,Xcoord-1,Ycoord-2,j,2,4,ID24,flag);
}
}
}
else if(flag==23){
if(block[Xcoord][Ycoord+1][Zcoord].ID==ID&&
block[Xcoord][Ycoord+1][Zcoord].flag==flag){
for(int j = Zcoord+1; j < zcood ; j++){
ID23++;change_bricks(block,Xcoord-1,Ycoord-1,j,3,2,ID23,flag);
}
}
else {
for(int j = Zcoord+1; j < zcood ; j++){
ID23++;change_bricks(block,Xcoord-1,Ycoord-1,j,2,3,ID23,flag);
}
}
}
else if(flag==22){
for(int j = Zcoord+1; j < zcood ; j++){
ID22++;change_bricks(block,Xcoord-1,Ycoord-1,j,2,2,ID22,flag);
}
}
else if(flag==14){
if(block[Xcoord-2][Ycoord][Zcoord].ID==ID&&
block[Xcoord-2][Ycoord][Zcoord].flag==flag){
for(int j = Zcoord+1; j < zcood ; j++){
ID14++;change_bricks(block,Xcoord-2,Ycoord,j,4,1,ID14,flag);
}
}
else {
for(int j = Zcoord+1; j < zcood ; j++){
ID14++;change_bricks(block,Xcoord,Ycoord-2,j,1,4,ID14,flag);
}
}
}
else if(flag==13){
if(block[Xcoord-1][Ycoord][Zcoord].ID==ID&&
block[Xcoord-1][Ycoord][Zcoord].flag==flag){
for(int j = Zcoord+1; j < zcood ; j++){
ID13++;change_bricks(block,Xcoord-1,Ycoord,j,3,1,ID13,flag);
}
}
else {
for(int j = Zcoord+1; j < zcood ; j++){
ID14++;change_bricks(block,Xcoord,Ycoord-1,j,1,3,ID13,flag);
}
}
}
else if(flag==12){
if(block[Xcoord-1][Ycoord][Zcoord].ID==ID&&
block[Xcoord-1][Ycoord][Zcoord].flag==flag){
for(int j = Zcoord+1; j < zcood ; j++){
ID12++;change_bricks(block,Xcoord-1,Ycoord,j,2,1,ID12,flag);
}
}
else {
for(int j = Zcoord+1; j < zcood ; j++){
ID12++;change_bricks(block,Xcoord,Ycoord-1,j,1,2,ID12,flag);
}
}
}
else if(flag==1){
for(int j = Zcoord+1; j < zcood ; j++){
change_bricks(block,Xcoord,Ycoord,j,1,1,0,flag);
}
}
//}
}
}
