inline Vector2f VectorLocalization2D::attractorFunction(line2f l, Vector2f p, float attractorRange, float margin)
{
static const bool debug = false;
Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1()));
float location = (p-p0).dot(dir);
if(location<-margin || location>l.Length()+margin){
return attraction;
}
attraction = (p-p0) - dir*location ;
float d = attraction.norm();
/*
if(d>0.5*attractorRange){
float d2 = max(0.0f, attractorRange - d);
attraction *= 2.0f*d2/attractorRange;
}
*/
if(d>attractorRange){
attraction = Vector2f::Zero();
}
if(debug){
debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) );
}
return attraction;
}
// 布の形状の更新
void updateCloth(void) {
// ★ 次の手順で質点の位置を決定する
//clothのみがグローバル変数
// 1. 質点に働く力を求める
// 質点のfの定義
for(int y = 0; y < POINT_NUM; y++) {
for(int x = 0; x < POINT_NUM; x++) {
cloth->points[x][y].f.set(0,0,0);
}
}
//バネによる力を考える
for(int i = 0; i < cloth->springs.size(); i++) {
Vector3d nowlength(cloth->springs[i]->p0->p - cloth->springs[i]->p1->p);
double gap = nowlength.length() - cloth->springs[i]->restLength;
double strpower = Ks * gap;
//p1-p0
Vector3d attraction(cloth->springs[i]->p1->p.x - cloth->springs[i]->p0->p.x,
cloth->springs[i]->p1->p.y - cloth->springs[i]->p0->p.y,
cloth->springs[i]->p1->p.z - cloth->springs[i]->p0->p.z);
attraction.normalize();
attraction.scale(strpower);
cloth->springs[i]->p0->f += attraction;
//向きを反転
attraction.scale(-1);
cloth->springs[i]->p1->f += attraction;
}
//重力M*gを加える
//ついでに空気抵抗を考える
for(int y = 0; y < POINT_NUM; y++) {
for(int x = 0; x < POINT_NUM; x++) {
Vector3d grav(Mass*gravity.x,Mass*gravity.y,Mass*gravity.z);
cloth->points[x][y].f += grav;
Vector3d airresister(Dk*cloth->points[x][y].v.x,Dk*cloth->points[x][y].v.y,Dk*cloth->points[x][y].v.z);
cloth->points[x][y].f -= airresister;
}
}
// 2. 質点の加速度を求める
// 3. 質点の速度を更新する
// 4. 質点の位置を更新する
for(int y = 0; y < POINT_NUM; y++){
for(int x = 0; x < POINT_NUM; x++) {
//加速度accelを求めてdTを掛けた
if(!(cloth->points[x][y].bFixed)){
Vector3d accel(cloth->points[x][y].f.x/Mass*dT,cloth->points[x][y].f.y/Mass*dT,cloth->points[x][y].f.z/Mass*dT);
//質点の速度
cloth->points[x][y].v += accel;
//質点の位置
Vector3d xx(cloth->points[x][y].v.x*dT,cloth->points[x][y].v.y*dT,cloth->points[x][y].v.z*dT);
cloth->points[x][y].p += xx;
}
}
}
}
inline Vector2f VectorLocalization2D::observationFunction(line2f l, Vector2f p) const
{
static const bool debug = false;
Vector2f attraction(0.0,0.0), dir(V2COMP(l.Dir())), p0(V2COMP(l.P0())), p1(V2COMP(l.P1()));
float location = (p-p0).dot(dir);
attraction = (p-p0) - dir*location ;
if(debug){
debugLines.push_back( line2f( vector2f(p.x(),p.y()) ,vector2f((p-attraction).x(),(p-attraction).y()) ) );
const float crossSize = 0.002;
debugLines.push_back( line2f( vector2f(p.x()+crossSize,p.y()) , vector2f(p.x()-crossSize,p.y())) );
debugLines.push_back( line2f( vector2f(p.x(),p.y()+crossSize) , vector2f(p.x(),p.y()-crossSize)) );
}
return attraction;
}
int
DomainPartitioner::releaseVertex(int from,
int vertexTag,
Graph &theWeightedPartitionGraph,
bool mustReleaseToLighter,
double factorGreater,
bool adjacentVertexNotInOther)
{
// check that the object did the partitioning
if (partitionFlag == false) {
opserr << "DomainPartitioner::balance(const Vector &load)";
opserr << " - not partitioned or DomainPartitioner did not partition\n";
return -1;
}
// we first check the vertex is on the fromBoundary
Subdomain *fromSubdomain = myDomain->getSubdomainPtr(from);
if (fromSubdomain == 0) {
opserr << "DomainPartitioner::swapVertex - No from Subdomain: ";
opserr << from << " exists\n";
return -1;
}
// get the vertex from the boundary vertices of from
// Graph &theEleGraph = myDomain->getElementGraph();
Graph *fromBoundary = theBoundaryElements[from-1];
Vertex *vertexPtr = fromBoundary->getVertexPtr(vertexTag);
if (vertexPtr == 0)
vertexPtr = theElementGraph->getVertexPtr(vertexTag);
if (vertexPtr == 0) // if still 0 no vertex given by tag exists
return -3;
ID attraction(numPartitions+1);
attraction.Zero();
// determine the attraction to the other partitions
const ID &adjacent = vertexPtr->getAdjacency();
int numAdjacent = adjacent.Size();
for (int i=0; i<numAdjacent; i++) {
int otherTag = adjacent(i);
Vertex *otherVertex = theElementGraph->getVertexPtr(otherTag);
int otherPartition = otherVertex->getColor();
if (otherPartition != from)
attraction(otherPartition) += 1;
}
// determine the other partition the vertex is most attracted to
int partition = 1;
int maxAttraction = attraction(1);
for (int j=2; j<=numPartitions; j++)
if (attraction(j) > maxAttraction) {
partition = j;
maxAttraction = attraction(j);
}
// swap the vertex
if (mustReleaseToLighter == false)
return swapVertex(from, partition, vertexTag, adjacentVertexNotInOther);
else { // check the other partition has a lighter load
Vertex *fromVertex = theWeightedPartitionGraph.getVertexPtr(from);
Vertex *toVertex = theWeightedPartitionGraph.getVertexPtr(partition);
double fromWeight = fromVertex->getWeight();
double toWeight = toVertex->getWeight();
if (fromWeight == toWeight)
opserr << "DomainPartitioner::releaseVertex - TO CHANGE >= to >\n";
if (fromWeight >= toWeight) {
if (toWeight == 0.0)
return swapVertex(from,partition,vertexTag,adjacentVertexNotInOther);
if (fromWeight/toWeight > factorGreater)
return swapVertex(from,partition,vertexTag,adjacentVertexNotInOther);
}
}
return 0;
}
