void QTriangulatingStroker::moveTo(const qreal *pts)
{
m_cx = pts[0];
m_cy = pts[1];
float x2 = pts[2];
float y2 = pts[3];
normalVector(m_cx, m_cy, x2, y2, &m_nvx, &m_nvy);
// To acheive jumps we insert zero-area tringles. This is done by
// adding two identical points in both the end of previous strip
// and beginning of next strip
bool invisibleJump = m_vertices.size();
switch (m_cap_style) {
case Qt::FlatCap:
if (invisibleJump) {
m_vertices.add(m_cx + m_nvx);
m_vertices.add(m_cy + m_nvy);
}
break;
case Qt::SquareCap: {
float sx = m_cx - m_nvy;
float sy = m_cy + m_nvx;
if (invisibleJump) {
m_vertices.add(sx + m_nvx);
m_vertices.add(sy + m_nvy);
}
emitLineSegment(sx, sy, m_nvx, m_nvy);
break; }
case Qt::RoundCap: {
QVarLengthArray<float> points;
arcPoints(m_cx, m_cy, m_cx + m_nvx, m_cy + m_nvy, m_cx - m_nvx, m_cy - m_nvy, points);
m_vertices.resize(m_vertices.size() + points.size() + 2 * int(invisibleJump));
int count = m_vertices.size();
int front = 0;
int end = points.size() / 2;
while (front != end) {
m_vertices.at(--count) = points[2 * end - 1];
m_vertices.at(--count) = points[2 * end - 2];
--end;
if (front == end)
break;
m_vertices.at(--count) = points[2 * front + 1];
m_vertices.at(--count) = points[2 * front + 0];
++front;
}
if (invisibleJump) {
m_vertices.at(count - 1) = m_vertices.at(count + 1);
m_vertices.at(count - 2) = m_vertices.at(count + 0);
}
break; }
default: break; // ssssh gcc...
}
emitLineSegment(m_cx, m_cy, m_nvx, m_nvy);
}
double
NBHeightMapper::Triangle::getZ(const Position& geo) const {
// en.wikipedia.org/wiki/Line-plane_intersection
Position p0 = myCorners.front();
Position line(0, 0, 1);
p0.sub(geo); // p0 - l0
Position normal = normalVector();
return p0.dotProduct(normal) / line.dotProduct(normal);
}
inline
Circle3d
Circle3d::transformedBy(const TTransformation& transformation) const {
return Circle3d(
centerPoint().transformedBy(transformation),
normalVector().transformedBy(transformation),
transformation.scale() * radius()
);
}
void GLC_MeshData::setVboUsage(bool usage)
{
if (usage && (m_PositionSize != -1) && (!m_Positions.isEmpty()) && (!m_VertexBuffer.isCreated()))
{
createVBOs();
fillVbo(GLC_MeshData::GLC_Vertex);
fillVbo(GLC_MeshData::GLC_Normal);
fillVbo(GLC_MeshData::GLC_Texel);
fillVbo(GLC_MeshData::GLC_Color);
useVBO(false, GLC_MeshData::GLC_Color);
const int lodCount= m_LodList.count();
for (int i= 0; i < lodCount; ++i)
{
m_LodList.at(i)->setIboUsage(usage);
}
}
else if (!usage && m_VertexBuffer.isCreated())
{
m_Positions= positionVector();
m_PositionSize= m_Positions.size();
m_VertexBuffer.destroy();
m_Normals= normalVector();
m_NormalBuffer.destroy();
if (m_TexelBuffer.isCreated())
{
m_Texels= texelVector();
m_TexelsSize= m_Texels.size();
m_TexelBuffer.destroy();
}
if (m_ColorBuffer.isCreated())
{
m_Colors= colorVector();
m_ColorSize= m_Colors.size();
m_ColorBuffer.destroy();
}
const int lodCount= m_LodList.count();
for (int i= 0; i < lodCount; ++i)
{
m_LodList.at(i)->setIboUsage(usage);
}
}
m_UseVbo= usage;
}
void GLC_MeshData::copyVboToClientSide()
{
if (m_VertexBuffer.isCreated() && m_Positions.isEmpty())
{
Q_ASSERT(m_NormalBuffer.isCreated());
m_Positions= positionVector();
m_Normals= normalVector();
if (m_TexelBuffer.isCreated())
{
m_Texels= texelVector();
}
if (m_ColorBuffer.isCreated())
{
m_Colors= colorVector();
}
}
}
/*!
Compute the angle that the vector normal to the plane forms with xy plane
\return
The angle that the vector normal to this forms with the xy plane, return a value in the interval [0 ; 2*PI],
positive angles is counterclockwise, or DBL_MAX if this is not valid
*/
double GM_3dPlane::xyAngle() const {
double ret = DBL_MAX;
if (!isValid())
return ret;
GM_3dVector normVector = normalVector();
double dz = normVector.z();
double dxy = sqrt(normVector.x()*normVector.x() + normVector.y()*normVector.y());
double tanAng = atan2(dz, dxy);
if (tanAng < 0.0) {
ret = (2.0 * GM_PI) + tanAng;
}
else {
ret = tanAng;
}
return ret;
}
Box3d
Circle3d::bounds() const {
double nx2 = normalVector().x() * normalVector().x();
double ny2 = normalVector().y() * normalVector().y();
double nz2 = normalVector().z() * normalVector().z();
double dx = radius() * sqrt(ny2 + nz2);
double dy = radius() * sqrt(nx2 + nz2);
double dz = radius() * sqrt(nx2 + ny2);
return Box3d(
Interval(centerPoint().x() - dx, centerPoint().x() + dx),
Interval(centerPoint().y() - dy, centerPoint().y() + dy),
Interval(centerPoint().z() - dz, centerPoint().z() + dz)
);
}
// Return the color Vector
GLfloatVector GLC_MeshData::colorVector() const
{
if (m_ColorBuffer.isCreated())
{
// VBO created get data from VBO
const int sizeOfVbo= m_ColorSize;
const GLsizeiptr dataSize= sizeOfVbo * sizeof(GLfloat);
GLfloatVector normalVector(sizeOfVbo);
const_cast<QGLBuffer&>(m_ColorBuffer).bind();
GLvoid* pVbo = const_cast<QGLBuffer&>(m_ColorBuffer).map(QGLBuffer::ReadOnly);
memcpy(normalVector.data(), pVbo, dataSize);
const_cast<QGLBuffer&>(m_ColorBuffer).unmap();
const_cast<QGLBuffer&>(m_ColorBuffer).release();
return normalVector;
}
else
{
return m_Colors;
}
}
void QTriangulatingStroker::cubicTo(const qreal *pts)
{
const QPointF *p = (const QPointF *) pts;
QBezier bezier = QBezier::fromPoints(*(p - 1), p[0], p[1], p[2]);
QRectF bounds = bezier.bounds();
float rad = qMax(bounds.width(), bounds.height());
int threshold = qMin<float>(64, (rad + m_curvyness_add) * m_curvyness_mul);
if (threshold < 4)
threshold = 4;
qreal threshold_minus_1 = threshold - 1;
float vx, vy;
float cx = m_cx, cy = m_cy;
float x, y;
for (int i=1; i<threshold; ++i) {
qreal t = qreal(i) / threshold_minus_1;
QPointF p = bezier.pointAt(t);
x = p.x();
y = p.y();
normalVector(cx, cy, x, y, &vx, &vy);
emitLineSegment(x, y, vx, vy);
cx = x;
cy = y;
}
m_cx = cx;
m_cy = cy;
m_nvx = vx;
m_nvy = vy;
}
void QTriangulatingStroker::join(const qreal *pts)
{
// Creates a join to the next segment (m_cx, m_cy) -> (pts[0], pts[1])
normalVector(m_cx, m_cy, pts[0], pts[1], &m_nvx, &m_nvy);
switch (m_join_style) {
case Qt::BevelJoin:
break;
case Qt::SvgMiterJoin:
case Qt::MiterJoin: {
// Find out on which side the join should be.
int count = m_vertices.size();
float prevNvx = m_vertices.at(count - 2) - m_cx;
float prevNvy = m_vertices.at(count - 1) - m_cy;
float xprod = prevNvx * m_nvy - prevNvy * m_nvx;
float px, py, qx, qy;
// If the segments are parallel, use bevel join.
if (qFuzzyIsNull(xprod))
break;
// Find the corners of the previous and next segment to join.
if (xprod < 0) {
px = m_vertices.at(count - 2);
py = m_vertices.at(count - 1);
qx = m_cx - m_nvx;
qy = m_cy - m_nvy;
} else {
px = m_vertices.at(count - 4);
py = m_vertices.at(count - 3);
qx = m_cx + m_nvx;
qy = m_cy + m_nvy;
}
// Find intersection point.
float pu = px * prevNvx + py * prevNvy;
float qv = qx * m_nvx + qy * m_nvy;
float ix = (m_nvy * pu - prevNvy * qv) / xprod;
float iy = (prevNvx * qv - m_nvx * pu) / xprod;
// Check that the distance to the intersection point is less than the miter limit.
if ((ix - px) * (ix - px) + (iy - py) * (iy - py) <= m_miter_limit * m_miter_limit) {
m_vertices.add(ix);
m_vertices.add(iy);
m_vertices.add(ix);
m_vertices.add(iy);
}
// else
// Do a plain bevel join if the miter limit is exceeded or if
// the lines are parallel. This is not what the raster
// engine's stroker does, but it is both faster and similar to
// what some other graphics API's do.
break; }
case Qt::RoundJoin: {
QVarLengthArray<float> points;
int count = m_vertices.size();
float prevNvx = m_vertices.at(count - 2) - m_cx;
float prevNvy = m_vertices.at(count - 1) - m_cy;
if (m_nvx * prevNvy - m_nvy * prevNvx < 0) {
arcPoints(0, 0, m_nvx, m_nvy, -prevNvx, -prevNvy, points);
for (int i = points.size() / 2; i > 0; --i)
emitLineSegment(m_cx, m_cy, points[2 * i - 2], points[2 * i - 1]);
} else {
arcPoints(0, 0, -prevNvx, -prevNvy, m_nvx, m_nvy, points);
for (int i = 0; i < points.size() / 2; ++i)
emitLineSegment(m_cx, m_cy, points[2 * i + 0], points[2 * i + 1]);
}
break; }
default: break; // gcc warn--
}
emitLineSegment(m_cx, m_cy, m_nvx, m_nvy);
}
void GLC_Cone::createMeshAndWire()
{
Q_ASSERT(GLC_Mesh::isEmpty());
Q_ASSERT(m_WireData.isEmpty());
// Create cosinus and sinus array according to the discretion and radius
const int vertexNumber= m_Discret + 1;
// Normals values
QVector<float> cosNormalArray(vertexNumber);
QVector<float> sinNormalArray(vertexNumber);
QVector<float> cosArray(vertexNumber);
QVector<float> sinArray(vertexNumber);
const double angle= (2.0 * glc::PI) / static_cast<double>(m_Discret);
// Normal Z value
GLC_Vector3d normalVector(1.0, 0.0, 0.0);
GLC_Matrix4x4 rotation(glc::Y_AXIS, -atan(m_Radius / m_Length));
normalVector= rotation * normalVector;
const float normalZ= static_cast<float>(normalVector.z());
const double factor= normalVector.x(); // Normailsation factor
for (int i= 0; i < vertexNumber; ++i)
{
const double cosValue= cos(static_cast<double>(i) * angle);
const double sinValue= sin(static_cast<double>(i) * angle);
cosNormalArray[i]= static_cast<GLfloat>(factor * cosValue);
sinNormalArray[i]= static_cast<GLfloat>(factor * sinValue);
cosArray[i]= static_cast<GLfloat>(m_Radius * cosValue);
sinArray[i]= static_cast<GLfloat>(m_Radius * sinValue);
}
// Mesh Data
GLfloatVector verticeVector;
GLfloatVector normalsVector;
GLfloatVector texelVector;
// Wire Data
GLfloatVector bottomWireData(vertexNumber * 3);
const int size= vertexNumber * 3;
verticeVector.resize(3 * size);
normalsVector.resize(3 * size);
texelVector.resize(2 * size);
for (int i= 0; i < vertexNumber; ++i)
{
// Bottom Mesh
verticeVector[3 * i]= cosArray[i];
verticeVector[3 * i + 1]= sinArray[i];
verticeVector[3 * i + 2]= 0.0f;
normalsVector[3 * i]= cosNormalArray[i];
normalsVector[3 * i + 1]= sinNormalArray[i];
normalsVector[3 * i + 2]= normalZ;
texelVector[2 * i]= static_cast<float>(i) / static_cast<float>(m_Discret);
texelVector[2 * i + 1]= 0.0f;
// Bottom Wire
bottomWireData[3 * i]= cosArray[i];
bottomWireData[3 * i + 1]= sinArray[i];
bottomWireData[3 * i + 2]= 0.0f;
// Top
verticeVector[3 * i + 3 * vertexNumber]= 0.0f;
verticeVector[3 * i + 1 + 3 * vertexNumber]= 0.0f;
verticeVector[3 * i + 2 + 3 * vertexNumber]= static_cast<float>(m_Length);
normalsVector[3 * i + 3 * vertexNumber]= cosNormalArray[i];
normalsVector[3 * i + 1 + 3 * vertexNumber]= sinNormalArray[i];
normalsVector[3 * i + 2 + 3 * vertexNumber]= normalZ;
texelVector[2 * i + 2 * vertexNumber]= texelVector[i];
texelVector[2 * i + 1 + 2 * vertexNumber]= 1.0f;
// Bottom Cap ends
verticeVector[3 * i + 2 * 3 * vertexNumber]= cosArray[i];
verticeVector[3 * i + 1 + 2 * 3 * vertexNumber]= sinArray[i];
verticeVector[3 * i + 2 + 2 * 3 * vertexNumber]= 0.0f;
normalsVector[3 * i + 2 * 3 * vertexNumber]= 0.0f;
normalsVector[3 * i + 1 + 2 * 3 * vertexNumber]= 0.0f;
normalsVector[3 * i + 2 + 2 * 3 * vertexNumber]= -1.0f;
texelVector[2 * i + 2 * 2 * vertexNumber]= texelVector[i];
texelVector[2 * i + 1 + 2 * 2 * vertexNumber]= 0.0f;
}
// Add bulk data in to the mesh
GLC_Mesh::addVertice(verticeVector);
GLC_Mesh::addNormals(normalsVector);
GLC_Mesh::addTexels(texelVector);
// Add polyline to wire data
GLC_Geometry::addPolyline(bottomWireData);
// Set the material to use
GLC_Material* pCylinderMaterial;
if (hasMaterial())
{
pCylinderMaterial= this->firstMaterial();
}
else
{
pCylinderMaterial= new GLC_Material();
}
IndexList circumferenceStrips;
// Create the index
for (int i= 0; i < vertexNumber; ++i)
{
circumferenceStrips.append(i + vertexNumber);
circumferenceStrips.append(i);
}
addTrianglesStrip(pCylinderMaterial, circumferenceStrips);
{
IndexList bottomCap;
IndexList topCap;
int id1= 0;
int id2= m_Discret - 1;
const int size= m_Discret / 2 + (m_Discret % 2);
for (int i= 0; i < size; ++i)
{
bottomCap.append(id1 + 2 * vertexNumber);
bottomCap.append(id2 + 2 * vertexNumber);
id1+= 1;
id2-= 1;
}
addTrianglesStrip(pCylinderMaterial, bottomCap);
}
finish();
}
void xaeBlockScene::setRebound(coor c)
{
/** 法线向量 */
coor cNormal = c;
cNormal.x -= m_fBallX;
cNormal.y -= m_fBallY;
/** 速度向量 */
coor cSpeed(m_fBallSpeedX, m_fBallSpeedY);
switch(g_nReboundAlgorithm)
{
case 1:
{
/** 许波同学的算法 */
float a = cNormal.y / cNormal.x;
float m = cSpeed.x;
float n = cSpeed.y;
float x = ((1 - a * a) * m + 2 * a * n) / (a * a + 1);
float y = ((a * a - 1) * n + 2 * a * m) / (a * a + 1);
m_fBallSpeedX = x;
m_fBallSpeedY = y;
break;
}
case 2:
{
/** 宋莹莹的算法 */
float x1 = cSpeed.x, y1 = cSpeed.y;
float x2 = cNormal.x, y2 = cNormal.y;
float x3 = 2 * (x2 * (y1 * y2 + x1 * x2) / (x2 * x2 + y2 * y2)) - x1;
float y3 = 2 * (y2 * (y1 * y2 + x1 * x2) / (x2 * x2 + y2 * y2)) - y1;
m_fBallSpeedX = x3;
m_fBallSpeedY = y3;
break;
}
case 3:
{
/** 垃圾的算法 */
m_fBallSpeedX = -m_fBallSpeedX;
m_fBallSpeedY = -m_fBallSpeedY;
m_fBallSpeedX += ((rand() % 10) - 5);
m_fBallSpeedY += ((rand() % 10) - 5);
break;
}
case 100:
{
/** 重写的算法 */
int x = c.x - m_fBallX;
int y = c.y - m_fBallY;
/** 上下方碰撞 */
if(abs(y) >= abs(x))
{
m_fBallSpeedY = -m_fBallSpeedY;
}
else
/** 左右方碰撞 */
{
m_fBallSpeedX = -m_fBallSpeedX;
}
break;
}
case 4:
default:
{
/** 陈洁操的算法 */
hgeVector normalVector(cNormal.x, cNormal.y);
hgeVector speedVector(cSpeed.x, cSpeed.y);
float sink = ((normalVector.x * speedVector.y) + (normalVector.y * speedVector.x)) / (normalVector.Length() * speedVector.Length());
float cosk = normalVector.Dot(&speedVector) / (normalVector.Length() * speedVector.Length());
float newx = normalVector.x * cosk + normalVector.y * sink;
float newy = normalVector.y * sink - normalVector.x * sink;
hgeVector newSpeedVector(newx, newy);
newSpeedVector = (newSpeedVector / newSpeedVector.Length()) * speedVector.Length();
m_fBallSpeedX = newSpeedVector.x;
m_fBallSpeedY = newSpeedVector.y;
break;
}
}
}
inline
Axis3d
Circle3d::axis() const {
return Axis3d(centerPoint(), normalVector());
}
inline
Plane3d
Circle3d::plane() const {
return Plane3d(centerPoint(), normalVector());
}
void Orbit::integrate(PotentialField& potentialField, ElectricField& electricField,
Field<int>& faceTypeField, CodeField& vertexTypeField,
ShortestEdgeField shortestEdgeField, FILE *outFile, double orbitLabelValue) {
// TODO: need some clever way to set tMax and/or detect trapped orbits
// double dt=min(0.005,0.005/initialVelocity.norm()), tMax=100;
// double dt=min(0.01,0.01/initialVelocity.norm()), tMax=100;
double dt=min(0.02,0.02/initialVelocity.norm()), tMax=100;
// vect3d currentPosition = initialPosition;
// TODO: should be consistent about starting position
vect3d currentPosition = initialPosition + (initialVelocity+extern_VEXB)*SMALL_TIME;
vect3d currentVelocity = initialVelocity;
// TODO: shouldn't hard-code quasi-neutral operation
double phiSurface = -4;
vertexType = vertexTypeField.getField(initialNode);
vect3d vertexNormalVector;
vect3d initialNormalVelocity;
if (vertexType==4 && charge<0.) {
vertexNormalVector =
mesh_ptr->getVertexNormalVector(initialNode, faceTypeField);
vect3d coords = mesh_ptr->getCoordinates(initialNode);
initialNormalVelocity =
currentVelocity.dot(vertexNormalVector)*vertexNormalVector;
// TODO: could do something like below to get distribution at surface
// currentVelocity += sqrt(2.*charge*phiSurface)*vertexNormalVector;
}
// initialEnergy = 0.5*pow(initialVelocity.norm(),2.)
// + charge*potentialField.getField(initialNode);
// Don't integrate orbit if doesn't have enough energy to escape potential
// TODO: this should be refined
// if (0.5*pow(initialVelocity.norm(),2.)+0.22 <
// if ((0.5*pow(initialVelocity.norm(),2.) +
// if ((0.5*pow(initialVelocity[2],2.) +
// charge*potentialField.getField(initialNode)) < 0.) {
// negativeEnergy = true;
// tMax = 0.;
// } else {
negativeEnergy = false;
// }
// TODO: treat inwards electrons in a better way?
if (vertexType==4 &&
0.5*pow(initialNormalVelocity.norm(),2.)<charge*phiSurface &&
currentVelocity.dot(vertexNormalVector)<0.) {
currentVelocity -= 2.*initialNormalVelocity;
}
int nSteps=0;
double potential=0.;
bool endLoop=false;
bool foundTet=false;
boost::array<Eigen::Matrix<double,NDIM,1>, 2> positionAndVelocity;
boost::array<Eigen::Matrix<double,NDIM,1>, 2> positionAndVelocityOut;
positionAndVelocity[0] = currentPosition;
positionAndVelocity[1] = currentVelocity;
// VelocityVerletStepper<NDIM> timestepper;
// CyclotronicStepper timestepper;
// TaylorStepper timestepper;
DriftStepper timestepper;
// boost::numeric::odeint::runge_kutta4<boost::array<vect3d,2> >
// timestepper;
VelocityAndAcceleration<NDIM> velocityAndAcceleration(potentialField,
// electricField, charge, initialNode, initialVelocity, false);
electricField, charge, initialNode, initialVelocity, true);
foundTet = velocityAndAcceleration.foundTet;
// assert(foundTet);
// TODO: do this more cleanly
if (foundTet) {
initialPotential = velocityAndAcceleration.currentPotential;
double driftPotential=extern_E.dot(initialPosition);
initialPotential += driftPotential;
initialEnergy = 0.5*pow((initialVelocity+extern_VEXB).norm(),2.)
// initialEnergy = 0.5*pow(initialVelocity.norm(),2.)
+ charge*initialPotential;
double currentPotential=initialPotential;
double currentEnergy=initialEnergy;
MinimumBasisFunction minimumBasisFunction(mesh_ptr, &positionAndVelocity,
&velocityAndAcceleration, ×tepper);
// // For second order leap-frog, offset position from velocity in time
// currentPosition -= currentVelocity*dt/2.;
// for (double t=0.; t<tMax; t+=dt) {
double t=0;
double numberOfStepsPerRegion = 3.;
int numberOfSteps = 100000*numberOfStepsPerRegion;
// TODO: set max number of steps more cleverly (since also need to limit by accel)
for (int iT=0; iT<numberOfSteps && !negativeEnergy; iT++) {
dt = shortestEdgeField[velocityAndAcceleration.currentRegionIndex]
/(fabs(currentVelocity[2])+extern_VEXB.norm()+SMALL_VELOCITY)/
// /(fabs(currentVelocity[2])+DELTA_LENGTH)/
numberOfStepsPerRegion;
// /currentVelocity.norm()/5.;
// TODO: Need acceleration info as well, since v_z changes during time-step
// dt = min(0.01,dt);
// dt = 0.01;
// TODO: can this fail (need to ensure minimumBasisFunction(0.)>0.?
dt = SMALL_TIME;
t += dt;
try {
timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, dt);
} catch (...) {
cout << "Failed to do SMALL_TIME step into interior." << endl;
}
// TODO: optimise this or replace with better way?
double dtMultiplier=10.;
double dtAtWhichNegative=sqrt(SMALL_TIME)/dtMultiplier;
// TODO: minimumBasisFunction() should not throw provided positionAndVelocity[0] is within tet
try {
while (dtAtWhichNegative<tMax && minimumBasisFunction(dtAtWhichNegative)>0.) {
dtAtWhichNegative *= dtMultiplier;
}
} catch (...) {
cout << "Failed to evaluate minimumBasisFunction(" << dtAtWhichNegative << ")" << endl;
}
// // TODO: debugging
//// cout << minimumBasisFunction(-dtAtWhichNegative) << endl;
// cout << minimumBasisFunction(SMALL_TIME) << endl;
// cout << minimumBasisFunction(dtAtWhichNegative) << endl << endl;
//// cout << mesh_ptr->minimumBasisFunction(positionAndVelocity[0]+dtAtWhichNegative*(
//// positionAndVelocity[1]+VEXB),
//// velocityAndAcceleration.currentRegionIndex) << endl;
// if (extern_orbitNumber==64313 7328) {
// if (extern_orbitNumber==64313) {
// cout << velocityAndAcceleration.currentPosition.transpose() << endl;
// cout << dtAtWhichNegative << endl;
// cout << minimumBasisFunction(0.) << " " << minimumBasisFunction(dtAtWhichNegative) << endl;
//// cout << minimumBasisFunction(SMALL_TIME) << " " << minimumBasisFunction(dtAtWhichNegative) << endl;
// }
boost::uintmax_t max_iter=500;
// tolerance is number of bits
boost::math::tools::eps_tolerance<double> tol(8);
std::pair<double, double> dtInterval;
dtInterval.first=0.;
dtInterval.second=SMALL_TIME;
try {
dtInterval = boost::math::tools::toms748_solve(minimumBasisFunction, 0.,
dtAtWhichNegative, tol, max_iter);
// boost::math::tools::toms748_solve(minimumBasisFunction, SMALL_TIME,
// dtAtWhichNegative, tol, max_iter);
} catch (...) {
cout << "toms748 failed to find root." << endl;
}
// dt = dtInterval.first;
// TODO: find better way to ensure next point is not in same region
dt = dtInterval.second+SMALL_TIME;
// double additionalDtToExitRegion = (dtInterval.second-dtInterval.first);
double additionalDtToExitRegion = 0.;
// TODO: debugging
// cout << minimumBasisFunction(dt) << endl;
// cout << minimumBasisFunction(dt+additionalDtToExitRegion) << endl << endl;
t += dt;
nSteps++;
// // TODO: debugging
// cout << nSteps << " : " << t << endl;
// cout << positionAndVelocity[0].transpose() << endl;
// TODO: DriftStepper::do_step() should not throw, but others might...
try {
timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, dt);
} catch (...) {
cout << "Failed to step out of tet." << endl;
}
currentPosition = positionAndVelocity[0];
currentVelocity = positionAndVelocity[1];
// // TODO: debugging
// cout << positionAndVelocity[0].transpose() << endl;
// timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, -dt);
// cout << positionAndVelocity[0].transpose() << endl;
// timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, dt/2.);
// cout << positionAndVelocity[0].transpose() << endl;
// timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, dt/2.);
// cout << positionAndVelocity[0].transpose() << endl;
dt = additionalDtToExitRegion;
t+=dt;
if (isnan(currentPosition.norm()))
throw string("currentPosition is NaN in Orbit.cpp");
vect3d previousPosition = currentPosition;
vect3d previousVelocity = currentVelocity;
entHandle previousElement = velocityAndAcceleration.currentElement;
// currentPosition += dt*currentVelocity;
// vect3d currentAcceleration(0.,0.,0.);
// // TODO: replace this hack
// if (t==0.) {
// currentElement = mesh_ptr->findTet(previousPosition,
// currentPosition, initialNode, &foundTet, false);
// // TODO: figure out why sometimes throw here (grazing orbits?)
//// if (!foundTet)
//// throw;
// int dimension=mesh_ptr->getEntityDimension(currentElement);
// if (dimension!=iBase_REGION)
// foundTet = false;
// }
//// if (!foundTet)
//// break;
// if (foundTet) {
try {
positionAndVelocity[0] = currentPosition;
positionAndVelocity[1] = currentVelocity;
// TODO: debugging
// cout << minimumBasisFunction(SMALL_TIME) << endl;
// cout << minimumBasisFunction(dt) << endl << endl;
timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t, dt);
// // TODO: always exit with small time-step to minimize energy error...do better?
// timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t+dt,
// additionalDtToExitRegion);
// TODO: replace this hack to update currentElement?
timestepper.do_step(boost::ref(velocityAndAcceleration), positionAndVelocity, t+dt, SMALL_TIME);
// // TODO: figure out why inout arg leads to problems
// // (Eigen not liking certain optimization tricks in odeint?.
// Actually, just missing initialization)
// timestepper.do_step(boost::ref(velocityAndAcceleration),
// positionAndVelocity, t, positionAndVelocityOut, dt);
// positionAndVelocity[0] = positionAndVelocityOut[0];
// positionAndVelocity[1] = positionAndVelocityOut[1];
// timestepper.do_step(velocityAndAcceleration, positionAndVelocity, t, dt);
currentPosition = positionAndVelocity[0];
currentVelocity = positionAndVelocity[1];
currentElement = velocityAndAcceleration.currentElement;
if (currentElement==previousElement)
throw string("Did not step to new element...something is wrong.");
// TODO: change this to use regionIndex
foundTet = mesh_ptr->checkIfInTet(currentPosition, currentElement);
if (!foundTet) {
// TODO: figure out why sometimes lose track of tet
throw int(OUTSIDE_DOMAIN);
// currentElement = mesh_ptr->findTet(previousPosition, currentPosition,
// currentElement, &foundTet);
}
// finalEnergy = 0.5*pow(currentVelocity.norm(),2.)
// + charge*potentialField.getField(currentPosition);
// TODO: this doesn't account for final step, but as long as boundary
// is at potential zero it shouldn't matter for orbits with non-zero weight
// (actually, am using finalPotential later)
driftPotential = extern_E.dot(currentPosition);
currentPotential = velocityAndAcceleration.currentPotential;
currentPotential += driftPotential;
currentEnergy = 0.5*pow((currentVelocity+extern_VEXB).norm(),2.)
// currentEnergy = 0.5*pow(currentVelocity.norm(),2.)
// + charge*velocityAndAcceleration.currentPotential;
+ charge*(currentPotential);
// // TODO: set order through input parameter?
// int interpolationOrder = INTERPOLATIONORDER;
//// currentAcceleration = charge*
//// electricField.getField(currentPosition, ¤tElement);
// potential = potentialField.getField(currentPosition, ¤tElement,
// interpolationOrder);
// if (isnan(potential))
// potential = potentialField.getField(currentPosition,
// ¤tElement, 1);
// // TODO: hard-coding dimension here...
// for (int i=0; i<3; i++) {
// vect3d perturbedPosition = currentPosition +
// vect3d::Unit(i)*DELTA_LENGTH;
// double perturbedPotential = potentialField.getField(
// perturbedPosition, ¤tElement, interpolationOrder);
// // TODO: track down why perturbedPotential sometimes is NaN
//// cout << "calculation of error term succeeded." <<
//// i << " " << currentElement << endl;
// if (isnan(perturbedPotential)) {
// perturbedPotential = potentialField.getField(
// perturbedPosition, ¤tElement, 1);
// cout << "calculation of error term failed." <<
// i << " " << currentElement << endl;
// }
//// if (isnan(potential) || isnan(perturbedPotential)) {
//// cout << potential << " " << perturbedPotential <<
//// " " << currentPosition.transpose() << endl;
//// throw;
//// }
// currentAcceleration[i] =
// -charge*(perturbedPotential-potential)/DELTA_LENGTH;
// }
} catch (int signal) {
switch (signal) {
case OUTSIDE_DOMAIN:
foundTet = false;
// TODO: not guaranteed that outside domain (since different than stepper)
// TODO: Revisit this in magnetized case
// currentPosition += dt*currentVelocity;
currentPosition = positionAndVelocity[0];
currentVelocity = positionAndVelocity[1];
break;
default:
// TODO: handle other exceptions?
throw;
break;
}
} catch (...) {
// TODO: handle other exceptions?
throw;
}
// }
if (foundTet==false) {
// TODO: if near vertex could leave domain through non-boundary face
// by crossing sliver of other tet in one time-step
// entHandle faceCrossed = mesh_ptr->findFaceCrossed(
// previousElement, previousPosition, currentPosition);
int faceCrossedIndex = mesh_ptr->findBoundaryFaceCrossed(
mesh_ptr->indicesOfEntities[previousElement],
previousPosition, currentPosition,
faceTypeField, vertexTypeField);
entHandle faceCrossed = NULL;
if (faceCrossedIndex>=0) {
faceCrossed = mesh_ptr->entitiesVectors[iBase_FACE][faceCrossedIndex];
}
// TODO: grazing orbits don't enter domain in first time-step
int faceType;
vect3d normalVector(0.,0.,0.), normalVelocity(0.,0.,0.);
if (faceCrossed!=NULL) {
faceType = faceTypeField.getField(faceCrossed);
normalVector = mesh_ptr->getNormalVector(faceCrossed,
previousPosition);
normalVelocity =
currentVelocity.dot(normalVector)*normalVector;
// // TODO: debugging
// if (extern_orbitNumber==15) {
// cout << currentPotential << endl;
// }
// finalPotential = 0.;
// vector<entHandle> vertices =
// mesh_ptr->getVertices(faceCrossed);
// for (int i=0; i<vertices.size(); i++) {
// // TODO: should use point where left domain here
// finalPotential += 1./3.*potentialField.getField(vertices[i]);
// }
// // TODO: debugging
// if (extern_orbitNumber==15) {
// cout << finalPotential << endl;
// }
vect3d exitPosition;
vector<vect3d> vertexVectors =
mesh_ptr->getVertexVectors(faceCrossed);
// TODO: straight line intersection may not be accurate for magnetized orbits
mesh_ptr->checkIfIntersectsTriangle(previousPosition,
currentPosition, vertexVectors, &exitPosition);
vect3d centroid(0.,0.,0.);
// vector<vect3d> vVs = mesh_ptr->getVertexVectors(i,iBase_REGION);
// centroid = (vVs[0]+vVs[1]+vVs[2]+vVs[3])/4.;
// vect3d interiorDirection=centroid-exitPosition;
// interiorDirection /= interiorDirection.norm();
// TODO: Don't hard-code this correction
// exitPosition += sqrt(LENGTH_TOLERANCE)*interiorDirection;
exitPosition -= sqrt(LENGTH_TOLERANCE)*normalVector;
// TODO: figure out why this fails with interpolationorder=1
// currentPotential = potentialField.getField(exitPosition);
// TODO: for consistency should strictly evaluate everything at exitPosition
// driftPotential = E.dot(currentPosition);
// currentPotential += driftPotential;
// currentEnergy = 0.5*pow((currentVelocity+VEXB).norm(),2.)
//// currentEnergy = 0.5*pow(currentVelocity.norm(),2.)
// + charge*currentPotential;
finalPotential = currentPotential;
finalEnergy = currentEnergy;
// // TODO: debugging
// if (extern_orbitNumber==15) {
// cout << finalPotential << endl;
//// cout << previousPosition.transpose() << endl;
//// cout << exitPosition.transpose() << endl;
//// cout << currentPosition.transpose() << endl;
//// cout << potentialField.getField(previousPosition) << endl;
//// cout << potentialField.getField(exitPosition) << endl;
// }
// TODO: shouldn't hard-code boundary code
} else {
faceType = 0;
}
// if (faceType==0) {
// cout << previousElement << " " <<
// faceCrossed << " " << currentPosition.norm() <<
// " " << previousPosition.norm() << endl;
// }
finalFaceType = faceType;
// TODO: need to account for shielding potential here
if (faceType==4 && 0.5*pow(normalVelocity.norm(),2.)<charge*phiSurface) {
currentElement = previousElement;
foundTet = true;
// TODO: resetting position isn't quite right
currentPosition = previousPosition;
// TODO: revisit this in magnetized case
// TODO: calc this from previousVelocity?
currentVelocity -= 2.*normalVelocity;
} else {
// if (faceType==0)
// cout << "last face crossed was an interior one" << endl;
endLoop=true;
}
}
// if (nSteps==1 && !foundTet) {
// cout << currentPosition.transpose() << " " <<
// currentVelocity.transpose() << " " <<
// currentPosition.dot(currentVelocity) << endl;
// }
// double eFieldR = currentAcceleration.dot(currentPosition)/
// currentPosition.norm();
//// // TODO: comment out this
//// currentAcceleration = -charge*currentPosition/
//// pow(currentPosition.norm(),3.);
// currentVelocity += dt*currentAcceleration;
// vect3d velocityAtPosition = currentVelocity - 1./2.*dt*currentAcceleration;
// double energy = 1./2.*pow(velocityAtPosition.norm(),2.) + charge*potential;
// if (foundTet) {
// potential = potentialField.getField(currentPosition,
// &velocityAndAcceleration.currentElement,
// velocityAndAcceleration.interpolationOrder);
// } else {
potential = 0.;
// }
// assert(currentPosition.norm()<10.);
double energy = 0.;
// double energy = 1./2.*pow(currentVelocity.norm(),2.) + charge*finalPotential;
if (outFile) {
// if (outFile && (extern_orbitNumber==15 || extern_orbitNumber==790)) {
// fprintf(outFile, "%f %f %f %p\n", currentPosition[0], currentPosition[1],
// currentPosition[2], (void*)currentElement);
// TODO: fix occasional very large coordinate values
// // TODO: comment this out unless using spheres mesh
// if (currentPosition.norm()<=5.)
// fprintf(outFile, "%f %f %f %d\n", currentPosition[0], currentPosition[1],
// currentPosition[2], extern_orbitNumber);
fprintf(outFile, "%f %f %f %f\n", currentPosition[0], currentPosition[1],
// currentPosition[2], currentEnergy);
// currentPosition[2], currentPotential);
// currentPosition[2], 0.5*pow((currentVelocity+VEXB).norm(),2.));
// currentPosition[2], eFieldR);
currentPosition[2], orbitLabelValue);
}
if (endLoop)
break;
}
} else { // !foundTet
// TODO: check that only get here if starting from node on boundary
finalFaceType = vertexTypeField.getField(initialNode);
finalPotential = potentialField.getField(initialNode);
}
// cout << "Final radius=" << currentPosition.norm() << " nSteps=" <<
// nSteps << "faceType =" << finalFaceType << endl;
finalPosition = currentPosition;
// TODO: correct for time-step offset?
finalVelocity = currentVelocity;
// TODO: debugging
double fractionalEnergyChange = (finalEnergy-initialEnergy)/initialEnergy;
// if (fabs(fractionalEnergyChange) > 0.0 && finalPotential-driftPotential > -1.) {
// if (fabs(fractionalEnergyChange) > 0.0 && finalPotential > -1.) {
// cout << fractionalEnergyChange << " energy change for orbit " <<
// extern_orbitNumber << " : " << finalPotential << " / " <<
// initialPotential << " " << finalVelocity.norm() << " / " <<
// initialVelocity.norm() << " " << finalEnergy << " / " <<
// initialEnergy << endl;
// }
}
void
Lattice2d_mt :: computeInternalSourceRhsVectorAt(FloatArray &answer, TimeStep *atTime, ValueModeType mode)
{
int i, j, n, nLoads;
double dV;
bcGeomType ltype;
Load *load;
IntegrationRule *iRule = integrationRulesArray [ 0 ];
GaussPoint *gp;
Node *nodeA, *nodeB;
FloatArray deltaX(3), normalVector(3);
FloatArray val, helpLoadVector, globalIPcoords;
FloatMatrix nm;
double k;
answer.resize(0);
FloatArray gravityHelp(2);
nLoads = this->giveBodyLoadArray()->giveSize();
for ( i = 1; i <= nLoads; i++ ) {
n = bodyLoadArray.at(i);
load = ( Load * ) domain->giveLoad(n);
ltype = load->giveBCGeoType();
if ( ltype == GravityPressureBGT ) {
//Compute change of coordinates
nodeA = this->giveNode(1);
nodeB = this->giveNode(2);
deltaX.at(1) = nodeB->giveCoordinate(1) - nodeA->giveCoordinate(1);
deltaX.at(2) = nodeB->giveCoordinate(2) - nodeA->giveCoordinate(2);
deltaX.at(3) = nodeB->giveCoordinate(2) - nodeA->giveCoordinate(2);
//Compute the local coordinate system
gp = iRule->getIntegrationPoint(0);
gravityHelp.at(1) = 1.;
gravityHelp.at(2) = -1.;
dV = this->computeVolumeAround(gp);
load->computeValueAt(val, atTime, deltaX, mode);
k = static_cast< TransportMaterial * >( this->giveMaterial() )->giveCharacteristicValue(Conductivity_hh, gp, atTime);
double helpFactor = val.at(1) * k * dV;
helpFactor /= pow(this->giveLength(), 2.);
gravityHelp.times(helpFactor);
if ( helpLoadVector.isEmpty() ) {
helpLoadVector.resize( gravityHelp.giveSize() );
}
for ( j = 1; j <= gravityHelp.giveSize(); j++ ) {
helpLoadVector.at(j) += gravityHelp.at(j);
}
}
answer.add(helpLoadVector);
}
return;
}
