見出し画像

[ベクトル]プログラミングでよー使うベクトル2D（ソース）

アルちゃん

processingで書いてます
前回

左右判定のデモ(p5.js)

交差判定のデモ(p5.js)

参考文献

内積、外積

// 内積
public float Dot(PVector v1, PVector v2)
{
   return (v1.x * v2.x + v1.y * v2.y);
}

//外積、疑似外積、疑スカラー、パープ内積
public float Cross(PVector v1, PVector v2)
{
   return (v1.x * v2.y - v1.y * v2.x);
}

重心

public PVector CenterOfGravity(ArrayList<PVector> vertexes)
{
 PVector sum = new PVector(0,0);
 for(int i = 0; i<vertexes.size(); i++)
 {
   PVector v = vertexes.get(i);
   sum.add(v);
 }
 sum.div(vertexes.size());
 
 return sum;
}

回転

public ArrayList<PVector> RotRadian(ArrayList<PVector> vertex, double radian)
{
 PVector cog = CenterOfGravity(vertex);
 return RotRadian(vertex, cog, radian);
}

public ArrayList<PVector> RotDegree(ArrayList<PVector> vertex, double degree)
{
 PVector cog = CenterOfGravity(vertex);
 return RotDegree(vertex, cog, degree);
}

public ArrayList<PVector> RotRadian(ArrayList<PVector> vertex, PVector center, double radian)
{    
   ArrayList<PVector> ret = new ArrayList<PVector>();
   
   for(int i = 0; i<vertex.size(); i++)
   {
     PVector v = vertex.get(i);
     PVector dv = PVector.sub(v,center);      
     dv = RotRadian(dv, radian);
     ret.add(PVector.add(center,dv));
   }    
   return ret;
}

public ArrayList<PVector> RotDegree(ArrayList<PVector> vertex, PVector center, double degree)
{
   if (degree == 0) { return vertex; }
   double radian = (degree * Math.PI) / 180;
   return RotRadian(vertex, center, radian);
}

public PVector RotRadian(PVector v, double radian)
{
   float x = v.x * cos((float)radian) - v.y * sin((float)radian);
   float y = v.x * sin((float)radian) + v.y * cos((float)radian);
   return new PVector(x,y);
}

public PVector RotDegree(PVector v, double degree)
{
   if (degree == 0) { return new PVector(v.x,v.y); }
   double radian = (degree * Math.PI) / 180;
   return RotRadian(v, radian);
}

public void RotRadianRef(ArrayList<PVector> vertex, PVector center, double radian)
{
   for(int i = 0; i<vertex.size(); i++)
   {
     PVector v = vertex.get(i);
     PVector dv = PVector.sub(v,center);      
     RotRadianRef(dv, radian);
     vertex.get(i).x = center.x+dv.x;
     vertex.get(i).y = center.y+dv.y;
   }    
}

public void RotDegreeRef(ArrayList<PVector> vertex, PVector center, double degree)
{
   if (degree == 0) { return ; }
   double radian = (degree * Math.PI) / 180;
   RotRadianRef(vertex, center, radian);
}

public void RotRadianRef(PVector v, double radian)
{
   float x = v.x * cos((float)radian) - v.y * sin((float)radian);
   float y = v.x * sin((float)radian) + v.y * cos((float)radian);
   v.x = x;
   v.y = y;
}

public void RotDegreeRef(PVector v, double degree)
{
   if (degree == 0) { return; }
   double radian = (degree * Math.PI) / 180;
   RotRadianRef(v, radian);
}

正射影

//Christer p40
//正射影ベクトル(２点u,vが与えられた場合)
public PVector Projection(PVector u, PVector v)
{
 return PVector.mult(u,(PVector.dot(u, v) / PVector.dot(u, u)));
}

//正射影ベクトル(直線上に焼き付ける場合)
public PVector Projection(PVector sv, PVector ev, PVector v)
{
 PVector lv = PVector.sub(ev,sv);
 PVector toV = PVector.sub(v,sv);

 return PVector.mult(lv,(PVector.dot(toV, lv) / PVector.dot(lv, lv)));
}

 //使い方
 PVector proj = Projection(sv,ev,p);
 PVector proj_sv = sv.copy();//絶対座標
 PVector proj_ev = PVector.add(sv,proj);//絶対座標

垂直射影

//垂直射影ベクトル(２点u,vが与えられた場合)
public PVector Perpendicular(PVector u, PVector v)
{
 return PVector.sub(v, Projection(u, v));
}

//垂直射影ベクトル(直線上から射出する場合)
public PVector Perpendicular(PVector sv, PVector ev, PVector v)
{
 PVector lv = PVector.sub(ev,sv);
 PVector toV = PVector.sub(v,sv);

 //このベクトルの先端を絶対座標として取得するためにはsv+projを足さなければならない
 return PVector.sub(toV, Projection(lv, toV));
}

//使い方
 PVector perp = Perpendicular(sv,ev,p);
 PVector perp_sv = proj_ev.copy();//絶対座標
 PVector perp_ev = PVector.add(perp_sv,perp);//絶対座標

鏡映変換

//uの垂直軸に対してvを鏡映変換
public PVector RefrectV(PVector u, PVector v)
{
   //v-2*parpendicular
   return PVector.sub(v,PVector.mult(Perpendicular(u,v),2));
}
public PVector RefrectV(PVector sv, PVector ev, PVector v)
{
   PVector lv = PVector.sub(ev,sv);
   PVector toV = PVector.sub(v,sv);  
   //v-2*parpendicular
   return PVector.sub(toV,PVector.mult(Perpendicular(lv,toV),2));
}

//uに対するvの鏡映変換
public PVector RefrectH(PVector u, PVector v)
{
   //2*parpendicular-v
   return PVector.sub(PVector.mult(Perpendicular(u,v),2), v);
}
public PVector RefrectH(PVector sv, PVector ev, PVector v)
{
   PVector lv = PVector.sub(ev,sv);
   PVector toV = PVector.sub(v,sv);  
   //2*parpendicular-v
   return PVector.sub(PVector.mult(Perpendicular(lv,toV),2), toV);
}

線分の交差判定、及び交点の座標

class VectorLine2D
{
 PVector SV;
 PVector EV;
 
 PVector LV()
 {
   return PVector.sub(this.EV,this.SV);
 }
 
 VectorLine2D(PVector sv, PVector ev)
 {
   this.SV = sv.copy();
   this.EV = ev.copy();
 }
 VectorLine2D(float sx, float sy, float ex, float ey)
 {
   this.SV = new PVector(sx,sy);
   this.EV = new PVector(ex,ey);
 }  
}

//複数線分の交点座標を取得
public ArrayList<PVector> GetIntersection(ArrayList<VectorLine2D> l1, ArrayList<VectorLine2D> l2)
{
 ArrayList<PVector> ret = new ArrayList<PVector>();

 for (int i = 0; i < l1.size(); i++)
 {
   ArrayList<PVector> vs = GetIntersection(l1.get(i), l2);
   if(vs!=null&&vs.size()<1)
   {
     ret.addAll(vs);
   }
 }
 return ret;
}

public ArrayList<PVector> GetIntersection(VectorLine2D l1, ArrayList<VectorLine2D> l2)
{
 ArrayList<PVector> ret = new ArrayList<PVector>();

 for (int i = 0; i < l2.size(); i++)
 {
   PVector v = GetIntersection(l1, l2.get(i));
   if(v!=null)
   {
     ret.add(v);
   }
 }
 return ret;
}

public PVector GetIntersection(PVector sv1, PVector ev1, PVector sv2, PVector ev2)
{
 return GetIntersection(new VectorLine2D(sv1,ev1), new VectorLine2D(sv2, ev2));
}

public PVector GetIntersection(VectorLine2D l1, VectorLine2D l2)
{
 if(IsIntersect(l1,l2))
 {
   PVector v = l1.LV();
   PVector w = l2.LV();
   PVector u = PVector.sub(l2.SV, l1.SV);

   float t1 = (float)(Cross(v, u)/ Cross(w, v));
   return PVector.add(l2.SV, PVector.mult(l2.LV(), t1));
 }
 return null;
}

Straddleは片方の線分からみた相手方の線分を跨ぐかどうかの判定。
交差判定はお互いの線分がStraddleを判定しあわなければならない。

//線分は交差するか
public boolean IsIntersect(VectorLine2D l1, VectorLine2D l2)
{
 if (IsOnStraddle(l1, l2))
 {
   if (IsOnStraddle(l2, l1))
   {
     return true;
   }
 }
 return false;
}

//線分が直線を跨ぐ(直線から線分を見る)
//お互いにお互いを見ると, 平行でない限り必ずどこかで跨ぐ
public boolean IsOnStraddle(VectorLine2D l1, VectorLine2D l2)
{
 PVector lv1 = l1.LV();
 PVector l1SVtol2SV = PVector.sub(l2.SV, l1.SV);
 PVector l1SVtol2EV = PVector.sub(l2.EV, l1.SV);

 //OnLine(外積=0)を入念にとる
 if (Cross(lv1, l1SVtol2SV) == 0 || Cross(lv1, l1SVtol2EV) == 0)
 {
   return true;
 }
 //片方がプラマイどちらかで、片方が外積ゼロだと反応しないケースがでる気がする(未確認)
 if (((int)Cross(lv1, l1SVtol2SV) ^ (int)Cross(lv1, l1SVtol2EV)) < 0)
 {
   return true;
 }
 return false;
}

//線分が直線を跨ぐ(直線から線分を見る)
//お互いにお互いを見ると, 平行でない限り必ずどこかで跨ぐ
public boolean IsStraddle(VectorLine2D l1, VectorLine2D l2)
{
 PVector lv1 = l1.LV();
 PVector l1SVtol2SV = PVector.sub(l2.SV, l1.SV);
 PVector l1SVtol2EV = PVector.sub(l2.EV, l1.SV);

 //OnLineはとらない(false)
 if (Cross(lv1, l1SVtol2SV) == 0 || Cross(lv1, l1SVtol2EV) == 0)
 {
   return false;
 }
 //これだけだとOnLineをとってしまうことがある
 if (((int)Cross(lv1, l1SVtol2SV) ^ (int)Cross(lv1, l1SVtol2EV)) < 0)
 {
   return true;
 }
 return false;
}

最近接点

//Christer p128
//直線に一番近い点の絶対座標：Projection先端の絶対座標に等しい
PVector NearestOnLine(PVector sv, PVector to, PVector v)
{
 //PVector perp = Perpendicular(sv,to,v);
 //perp = Nagate(perp);
 //PVector d = PVector.add(v,perp);
 
 PVector proj = Projection(sv,to,v);
 PVector d = PVector.add(sv,proj);
 
 return d;
}
//Christer p128
//半直線に一番近い点の絶対座標
PVector NearestOnRay(PVector sv, PVector to, PVector v)
{
PVector lv = PVector.sub(to,sv);
PVector toV = PVector.sub(v,sv);
float t = Dot(toV,lv)/Dot(lv,lv);

if(t<0){t=0;} 

return PVector.add(sv, lv.mult(t));
}
//Christer p128
//線分に一番近い点の絶対座標
PVector NearestOnSegment(PVector sv, PVector ev, PVector v)
{
PVector lv = PVector.sub(ev,sv);
PVector toV = PVector.sub(v,sv);
float t = Dot(toV,lv)/Dot(lv,lv);

if(t<0){t=0;}
if(1<t){t=1;}

return PVector.add(sv, lv.mult(t));
}

凸多角形限定の内外判定(Point in Polygon)

//凸のみPointInPolygonの部品
boolean PIP_IsRight(PVector sv, PVector ev, PVector p)
{
 //外積判定のための始点あわせ
 PVector lv = ev.copy().sub(sv);
 PVector p_ = p.copy().sub(sv);  
 if(0<Cross(lv,p_)){return true;}
 return false;
}

//凸のみPointInPolygon
boolean PointInPolygon(ArrayList<PVector> vertex, PVector p)
{
 //最初の一回
 boolean is_right = PIP_IsRight(vertex.get(0),vertex.get(1),p);
 //中間
 for(int i = 1; i<vertex.size(); i++)
 {
   //向き変わった
   if(is_right!=PIP_IsRight(vertex.get(i),vertex.get((i+1)%vertex.size()),p))
   {
     return false;
   }    
 }
 return true;
}

凹含む多角形内外判定(CrossingNumber Point in Polygon)


参考サイト：最も分かり易い
https://www.nttpc.co.jp/technology/number_algorithm.html

//参考：https://www.nttpc.co.jp/technology/number_algorithm.html
boolean CrossingNumber(ArrayList<PVector> vertex, PVector p)
{
 int count = 0;
 for(int i = 0; i < vertex.size(); i++)
 {
   PVector sv = vertex.get(i);
   PVector ev = vertex.get((i+1)%vertex.size());  
   //PVector lv = PVector.sub(ev,sv);
   //PVector toP = PVector.sub(p,sv);
   
   //ルール１，２
   //上向き(sv.y<ev.y 終点含まず、始点ならカウント候補)||下向き(sv.y>ev.y 始点含まず、終点ならカウント候補)
   //ルール３
   //sv.y==ev.yはカウントしない
   if((sv.y <= p.y && p.y < ev.y)||(sv.y > p.y && p.y >= ev.y))
   {
     //ルール４
     //辺は点pよりも絶対座標で右側にあり、辺上ではない
     //点pは辺よりも絶対座標で左側にあり、辺上ではない
     //点p.xと交点.xの比較
     float t = (p.y - sv.y) / (ev.y - sv.y);
     if(p.x < (sv.x + (t * (ev.x - sv.x))))
     {
         count++;
     }
     
     //交点が辺の左
     //外積計算は相対位置で左右を取るため成り立たない
     //上向きの時に入力点を左を取り、下向きの時は入力点を右を取らなければならない
     //というよりも外積計算で左右とったら凸多角形の内外判定と変わらない
     //if(0>Cross(lv,toP)){count++;}
     
   }//if
 }//for
 
 if(count%2==1){return true;}
 return false;
}//CrossingNumber

テスト用のGUIの準備

void copy_vector(ArrayList<PVector> source, ArrayList<PVector> target)
{
 int n = source.size();
 
 for(int i = 0; i<n; i++)
 {
   target.get(i).x=source.get(i).x;
   target.get(i).y=source.get(i).y;
 }
}

void copy_vector(PVector[] source, ArrayList<PVector> target)
{
 target.clear(); 
 for(int i = 0; i<source.length; i++)
 {
   target.add(source[i].copy());
 }
}

class PObject
{
 ArrayList<PVector> Vertexes = new ArrayList<PVector>();
 float r = 30;
 
 PObject(ArrayList<PVector> _vertexes)
 {
   copy_vector(_vertexes, this.Vertexes);
 }
 
 PObject(PVector... _vertexes)
 {
   copy_vector(_vertexes, this.Vertexes);
 }
}

イベント

void mousePressed()
{
 pprev = new PVector(mouseX,mouseY);
 prev = new PVector(mouseX,mouseY);
 current = new PVector(mouseX,mouseY);
 
 for(int i = 0; i<this.PObjects.size(); i++)
 {
   PObject obj = this.PObjects.get(i);
   for(int j = 0; j<obj.Vertexes.size(); j++)
   {      
     PVector v = obj.Vertexes.get(j);
     
     if(v.x-obj.r<mouseX&&mouseX<v.x+obj.r)
     {
       if(v.y-obj.r<mouseY&&mouseY<v.y+obj.r)
       {
         current_vertex_index = j;
         CurrentObject = obj;
         return;
       }
     }
   }  
 }
}

void mouseDragged()
{
 pprev = prev.copy();
 prev = current.copy();
 current.x = mouseX;
 current.y = mouseY;  
 dv = PVector.sub(current,prev);
 
 if(CurrentObject!=null)
 {
   if(0<=current_vertex_index&&current_vertex_index<this.CurrentObject.Vertexes.size())
   {
     PVector v = this.CurrentObject.Vertexes.get(current_vertex_index);
     v.add(dv);
   }
 } 
}

void mouseReleased()
{
 pprev = null;
 prev = null;
 current = null;  
 current_vertex_index=-1;
}

各種描画

void DrawPoint(PVector... vertex)
{
 DrawPoint(Copy(vertex));
}

void DrawPoint(ArrayList<PVector> vertex)
{
 for(int i = 0; i<vertex.size(); i++)
 {
   PVector v = vertex.get(i);
   ellipse(v.x, v.y, 20, 20);
 }
}

//閉じる、塗らない
void DrawLinerClose(PVector... vertex)
{  
 DrawLinerClose(Copy(vertex));
}

//閉じない、塗らない
void DrawLiner(PVector... vertex)
{  
 DrawLiner(Copy(vertex));
}

//閉じる、塗らない
void DrawLinerClose(ArrayList<PVector> vertex)
{  
 for(int i = 0; i<vertex.size()-1; i++)
 {
   PVector sv = vertex.get(i);
   PVector ev = vertex.get(i+1);
   line(sv.x, sv.y, ev.x, ev.y);
 }
 PVector sv = vertex.get(vertex.size()-1);
 PVector ev = vertex.get(0);
 line(sv.x, sv.y, ev.x, ev.y);
}

//閉じない、塗らない
void DrawLiner(ArrayList<PVector> vertex)
{
 for(int i = 0; i<vertex.size()-1; i++)
 {
   PVector sv = vertex.get(i);
   PVector ev = vertex.get(i+1);
   line(sv.x, sv.y, ev.x, ev.y);
 }
}

//閉じる、塗る
void FillLinerClose(PVector... vertex)
{
 beginShape();
 for(int i = 0; i<=vertex.length; i++)
 {
   PVector v = vertex[i%vertex.length];
   vertex(v.x,v.y);
 }
 endShape();
}

//閉じる、塗る
void FillLinerClose(ArrayList<PVector> vertex)
{
 beginShape();
 for(int i = 0; i<=vertex.size(); i++)
 {
   PVector v = vertex.get(i%vertex.size());
   vertex(v.x,v.y);
 }
 endShape();
}

//閉じない、塗る
void FillLiner(PVector... vertex)
{
 beginShape();
 for(int i = 0; i<vertex.length; i++)
 {
   PVector v = vertex[i];
   vertex(v.x,v.y);
 }
 endShape();
}

//閉じない、塗る
void FillLiner(ArrayList<PVector> vertex)
{
 beginShape();
 for(int i = 0; i<vertex.size(); i++)
 {
   PVector v = vertex.get(i);
   vertex(v.x,v.y);
 }
 endShape();
}

//閉じない、塗る
void FillLiner(PGraphics pg, ArrayList<PVector> vertex)
{
 pg.beginShape();
 for(int i = 0; i<vertex.size(); i++)
 {
   PVector v = vertex.get(i);
   pg.vertex(v.x,v.y);
 }
 pg.endShape();
}

void DrawArrow(ArrayList<PVector> vertex)
{
 for(int i = 0; i<vertex.size()-1; i++)
 {
   PVector sv = vertex.get(i);
   PVector ev = vertex.get(i+1);
   DrawArrow(sv,ev);
 }
}

void DrawArrow(PVector sv, PVector ev)
{
 DrawArrow_t(sv,ev,0.2,60);
}

//比率に応じた矢じり t:0-1
void DrawArrow_t(PVector sv, PVector ev, float t, float degree)
{
 PVector rlv = PVector.sub(sv,ev);
 rlv.mult(t);
 
 //矢じりの左端と右端
 PVector lv = RotDegree(rlv, degree/2);
 PVector rv = RotDegree(rlv, -degree/2);
   
 line(sv.x,sv.y,ev.x,ev.y);
 
 beginShape();
 vertex(ev.x,ev.y);
 vertex(ev.x+rv.x,ev.y+rv.y);
 vertex(ev.x+lv.x,ev.y+lv.y);  
 endShape(CLOSE);
}

//矢じりの大きさが一定
void DrawArrow(PVector sv, PVector ev, float l, float degree)
{
 PVector rlv = PVector.sub(sv,ev);
 PVector nrlv = rlv.normalize();
 nrlv.mult(l);
 
 //矢じりの左端と右端
 PVector lv = RotDegree(nrlv, degree/2);
 PVector rv = RotDegree(nrlv, -degree/2);
 
 //PVector b = PVector.add(ev,nrlv);
 
 line(sv.x,sv.y,ev.x,ev.y);
 
 beginShape();
 vertex(ev.x,ev.y);
 vertex(ev.x+rv.x,ev.y+rv.y);
 vertex(ev.x+lv.x,ev.y+lv.y);  
 endShape(CLOSE);
}

左右判定の場合


int window_width = 500;
int window_height = 500;
void settings()
{
 size(window_width, window_height);
}

PVector pprev;
PVector prev;
PVector current;
PVector dv = new PVector(0,0);

ArrayList<PObject> PObjects = new ArrayList<PObject>();
PObject CurrentObject;
int current_vertex_index = -1;

PVector SV;
PVector EV;

PVector P;

void setup()
{
 SV = new PVector(100,100);
 EV = new PVector(200,200);
 
 this.P = new PVector(100,200);

 this.PObjects.add(new PObject(SV,EV));
 this.PObjects.add(new PObject(P));
}

void draw()
{
 background(255);
   
 DrawArrow(PObjects.get(0).Vertexes);
 if(IsRight(PObjects.get(0).Vertexes.get(0),PObjects.get(0).Vertexes.get(1),PObjects.get(1).Vertexes.get(0))){fill(255,0,0);}
 DrawPoint(PObjects.get(1).Vertexes);
 fill(0);
}

boolean IsRight(PVector csv, PVector cev, PVector ev)
{
 PVector toEV = PVector.sub(ev, csv); 
 PVector CLV = PVector.sub(cev,csv);
 if(0<Cross(CLV,toEV))
 {
   return true;
 }  
 return false;
}

boolean IsLeft(PVector csv, PVector cev, PVector ev)
{
 PVector toEV = PVector.sub(ev, csv); 
 PVector CLV = PVector.sub(cev,csv);
 if(0>Cross(CLV,toEV))
 {
   return true;
 }  
 return false;  
}

交差判定の場合

int window_width = 500;
int window_height = 500;
void settings()
{
 size(window_width, window_height);
}

PVector pprev;
PVector prev;
PVector current;
PVector dv = new PVector(0,0);

ArrayList<PObject> PObjects = new ArrayList<PObject>();
PObject CurrentObject;
int current_vertex_index = -1;

PVector v1;
PVector v2;

PVector v3;
PVector v4;

void setup()
{
 this.v1 = new PVector(100,100);
 this.v2 = new PVector(200,200);
 
 this.v3 = new PVector(100,200);
 this.v4 = new PVector(100,300);

 this.PObjects.add(new PObject(v1,v2));
 this.PObjects.add(new PObject(v3,v4));
}

void draw()
{
 background(255);
   
 DrawArrow(PObjects.get(0).Vertexes);
 DrawArrow(PObjects.get(1).Vertexes);
 
 PVector intersection = GetIntersection(
   PObjects.get(0).Vertexes.get(0),PObjects.get(0).Vertexes.get(1),
   PObjects.get(1).Vertexes.get(0),PObjects.get(1).Vertexes.get(1));

 if(intersection!=null)
 {
   fill(255,0,0);
   DrawPoint(intersection);
   fill(0);    
 }
}

ポイントインポリゴンの場合

int window_width = 500;
int window_height = 500;
void settings()
{
 size(window_width, window_height);
}

PVector pprev;
PVector prev;
PVector current;
PVector dv = new PVector(0,0);

ArrayList<PObject> PObjects = new ArrayList<PObject>();
PObject CurrentObject;
int current_vertex_index = -1;

void setup()
{
 PVector v1 = new PVector(100,200);
 PVector v2 = new PVector(300,200);
 PVector v3 = new PVector(250,400);
 
 this.PObjects.add(new PObject(v1,v2,v3));
}

void draw()
{
 background(255);
 
 fill(0);
 if(flag)  {    text("true",10,10);  }
 
 if(PointInPolygon(this.PObjects.get(0).Vertexes, new PVector(mouseX,mouseY)))
 {
   flag = true;
 }
 else
 {
   flag = false;
 }
 
 for(int i = 0; i<this.PObjects.size(); i++)
 {
   PObject obj = this.PObjects.get(i);
   DrawLinerClose(obj.Vertexes);
 }
}







この記事が気に入ったらサポートをしてみませんか？