void RandomModel::surfacesAndEdges(double margin[2],
double bH[2],
double bL[2],
double bInterval[2]) {
double totalLength = -bInterval[1];
// 道路右侧情况，x>0
while (totalLength <= this->length) {
double x0 = (width / 2) + this->randomNum(margin);
double y0 = totalLength + this->randomNum(bInterval);
double z0 = this->randomNum(bH);
Point p0(x0, y0, z0);
// 面
double length = this->randomNum(bL);
Surface s = this->surfaceProduce(p0, length, this->materials);
s.init();
this->surfaces.push_back(s);
// 刃形，p2p0,p1p3
Point p1 = s.getP1();
Point p2 = s.getP2();
Point p3 = s.getP3();
Point pb0(p0.x + BUILDING_WIDTH, p0.y, p0.z);
Point pb1(p3.x + BUILDING_WIDTH, p3.y, p3.z);
Edge e0(p2, p0, pb0, p3);
Edge e1(p1, p3, pb1, p0);
this->edges.push_back(e0);
this->edges.push_back(e1);
totalLength = y0 + length ;
}
// 道路左侧情况，x<0
totalLength = -bInterval[1];
while (totalLength <= this->length) {
double x0 = -((width / 2) + this->randomNum(margin));
double y0 = totalLength + this->randomNum(bInterval);
double z0 = this->randomNum(bH);
Point p0(x0, y0, z0);
// 面
double length = this->randomNum(bL);
Surface s = this->surfaceProduce(p0, length, this->materials);
s.init();
this->surfaces.push_back(s);
// 刃形，p2p0,p1p3
Point p1 = s.getP1();
Point p2 = s.getP2();
Point p3 = s.getP3();
Point pb0(p0.x - BUILDING_WIDTH, p0.y, p0.z);
Point pb1(p3.x - BUILDING_WIDTH, p3.y, p3.z);
Edge e0(p2, p0, pb0, p3);
Edge e1(p1, p3, pb1, p0);
this->edges.push_back(e0);
this->edges.push_back(e1);
totalLength = y0 + length;
}
}
int main()
{
void h0(float, float);
void e0(float, float);
void s0(float, float);
float a, b, c;
printf("请输入a,b,c:");
scanf("%f,%f,%f", &a, &b, &c);
printf("方程为：%5.2fx*x+%5.2fx+%5.2f=0\n", a, b, c);
Delta = b*b - 4 * a*c;
printf("结果是：\n");
if (Delta > 0)
{
h0(a, b);
printf("x1=%f\nx2=%f\n", x1, x2);
}
else if (Delta < 0)
{
s0(a, b);
printf("x1=%f+%fi\nx2=%f-%fi\n", m, n, m, n);
}
else
{
e0(a, b);
printf("x1=x2=%f\n", x1);
}
return 0;
}
bool Matrix2d::isConformal(float& scaleX, float& scaleY, float& angle,
bool& isMirror, Vector2d& reflex) const
{
Vector2d e0 (m11, m12);
Vector2d e1 (m21, m22);
if (!e0.isPerpendicularTo(e1))
return false;
scaleX = e0.length();
scaleY = e1.length();
e0 /= scaleX;
e1 /= scaleY;
if (mgIsZero(e0.x - e1.y) && mgIsZero(e0.y + e1.x))
{
isMirror = false;
angle = e0.angle2();
}
else
{
isMirror = true;
angle = e0.angle2() / 2.f;
reflex.x = cosf(angle);
reflex.y = sinf(angle);
angle = 0.f;
}
return true;
}
void Shape::computeBaryCentreCoord(float pt[3], float v0[3], float v1[3], float v2[3], float lambd[3])
{
// v presents triangle vertex, pt locate inside triangle
// pt[0]->x pt[1]->y pt[2]->z
// v0[0]->x1 v0[1]->y1 v0[2]->z1
// v1[0]->x2 v1[1]->y2 v1[2]->z2
// v2[0]->x3 v2[1]->y3 v2[2]->z3
Eigen::Vector3f e0(v1[0] - v0[0], v1[1] - v0[1], v1[2] - v0[2]);
Eigen::Vector3f e1(v2[0] - v0[0], v2[1] - v0[1], v2[2] - v0[2]);
Eigen::Vector3f e2(pt[0] - v0[0], pt[1] - v0[1], pt[2] - v0[2]);
float d00 = e0.dot(e0);
float d01 = e0.dot(e1);
float d11 = e1.dot(e1);
float d20 = e2.dot(e0);
float d21 = e2.dot(e1);
float denom = d00*d11 - d01*d01;
lambd[1] = (d11*d20 - d01*d21) / denom;
lambd[2] = (d00*d21 - d01*d20) / denom;
lambd[0] = 1.0f - lambd[1] - lambd[2];
}
int HE_Mesh::createFace(int v0_idx, int v1_idx, int v2_idx)
{
HE_Face f;
int f_idx = faces.size();
HE_Edge e0 (v0_idx, f_idx);
HE_Edge e1 (v1_idx, f_idx);
HE_Edge e2 (v2_idx, f_idx);
int e0_idx = edges.size() + 0;
int e1_idx = edges.size() + 1;
int e2_idx = edges.size() + 2;
edges.push_back(e0);
edges.push_back(e1);
edges.push_back(e2);
connectEdgesPrevNext(e0_idx, e1_idx);
connectEdgesPrevNext(e1_idx, e2_idx);
connectEdgesPrevNext(e2_idx, e0_idx);
vertices[v0_idx].someEdge_idx = e0_idx;
vertices[v1_idx].someEdge_idx = e1_idx;
vertices[v2_idx].someEdge_idx = e2_idx;
f.edge_idx[0] = e0_idx;
f.edge_idx[1] = e1_idx;
f.edge_idx[2] = e2_idx;
faces.push_back(f);
return f_idx;
}
/**
* Perform a not equals operation when all the elements are numbers.
*
* @param parms -- The parameters to the operation.
* @return the new total.
*
* @version
* - JR Lewis 2012.03.07
* - Initial version.
*/
bool NotEqualsComparisonBuiltinsImplementation::InterpretAllNumbers_(std::vector<Element> const& parms) const
{
bool success = false;
if (parms.size() >= 2)
{
Element e0(parms[0]);
Number_* n0 = static_cast<Number_*>(e0.ElementHandle());
bool isANumber = false;
for(size_t i=1; i< parms.size(); ++i)
{
Number n1 = CastToNumber(parms[i], isANumber);
if ( n1.IsInt() != n0->IsInt()
|| n1.IntValue() != n0->IntValue()
|| n1.DoubleValue() != n0->DoubleValue() )
{
success = true;
break;
}
}
}
return success;
}
void dgPolygonSoupDatabaseBuilder::End(bool optimize)
{
Optimize(optimize);
// build the normal array and adjacency array
// calculate all face the normals
hacd::HaI32 indexCount = 0;
m_normalPoints[m_faceCount].m_x = hacd::HaF64 (0.0f);
for (hacd::HaI32 i = 0; i < m_faceCount; i ++) {
hacd::HaI32 faceIndexCount = m_faceVertexCount[i];
hacd::HaI32* const ptr = &m_vertexIndex[indexCount + 1];
dgBigVector v0 (&m_vertexPoints[ptr[0]].m_x);
dgBigVector v1 (&m_vertexPoints[ptr[1]].m_x);
dgBigVector e0 (v1 - v0);
dgBigVector normal (hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f), hacd::HaF32 (0.0f));
for (hacd::HaI32 j = 2; j < faceIndexCount - 1; j ++) {
dgBigVector v2 (&m_vertexPoints[ptr[j]].m_x);
dgBigVector e1 (v2 - v0);
normal += e0 * e1;
e0 = e1;
}
normal = normal.Scale (dgRsqrt (normal % normal));
m_normalPoints[i].m_x = normal.m_x;
m_normalPoints[i].m_y = normal.m_y;
m_normalPoints[i].m_z = normal.m_z;
indexCount += faceIndexCount;
}
// compress normals array
m_normalIndex[m_faceCount] = 0;
m_normalCount = dgVertexListToIndexList(&m_normalPoints[0].m_x, sizeof (dgBigVector), 3, m_faceCount, &m_normalIndex[0], hacd::HaF32 (1.0e-4f));
}
static void dec_body(rdp_tree_node_data* rdp_tree)
{
char* name;
{
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, SCAN_P_ID, &dec_body_stop);
name = SCAN_CAST->id;
scan_();
if (scan_test(NULL, RDP_T_61 /* = */, NULL))
{ /* Start of rdp_dec_body_1 */
while (1)
{
{
scan_test(NULL, RDP_T_61 /* = */, &dec_body_stop);
scan_();
e0(rdp_add_child("e0", rdp_tree));
}
break; /* hi limit is 1! */
}
} /* end of rdp_dec_body_1 */
else
{
/* default action processing for rdp_dec_body_1*/
}
symbol_insert_key(mini, &name, sizeof(char*), sizeof(mini_data)); \
if (*name == '_' && *(name+1) == '_')\
text_message(TEXT_ERROR_ECHO, "variable names must not begin with two underscores\n");\
scan_test_set(NULL, &dec_body_stop, &dec_body_stop);
}
}
static void
sha512_transform(u64 *state, const u8 *input)
{
u64 a, b, c, d, e, f, g, h, t1, t2;
int i;
u64 W[16];
/* load the state into our registers */
a=state[0]; b=state[1]; c=state[2]; d=state[3];
e=state[4]; f=state[5]; g=state[6]; h=state[7];
/* now iterate */
for (i=0; i<80; i+=8) {
if (!(i & 8)) {
int j;
if (i < 16) {
/* load the input */
for (j = 0; j < 16; j++)
LOAD_OP(i + j, W, input);
} else {
for (j = 0; j < 16; j++) {
BLEND_OP(i + j, W);
}
}
}
t1 = h + e1(e) + Ch(e,f,g) + sha512_K[i ] + W[(i & 15)];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + sha512_K[i+1] + W[(i & 15) + 1];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + sha512_K[i+2] + W[(i & 15) + 2];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + sha512_K[i+3] + W[(i & 15) + 3];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + sha512_K[i+4] + W[(i & 15) + 4];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + sha512_K[i+5] + W[(i & 15) + 5];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + sha512_K[i+6] + W[(i & 15) + 6];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + sha512_K[i+7] + W[(i & 15) + 7];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
}
state[0] += a; state[1] += b; state[2] += c; state[3] += d;
state[4] += e; state[5] += f; state[6] += g; state[7] += h;
/* erase our data */
a = b = c = d = e = f = g = h = t1 = t2 = 0;
}
void eval_precedence()
{
QgsExpression e0( "1+2*3" );
QCOMPARE( e0.evaluate().toInt(), 7 );
QgsExpression e1( "(1+2)*(3+4)" );
QCOMPARE( e1.evaluate().toInt(), 21 );
QgsExpression e2( e1.dump() );
QCOMPARE( e2.evaluate().toInt(), 21 );
}
bool Matrix2d::hasMirror(Vector2d& reflex) const
{
Vector2d e0 (m11, m12);
Vector2d e1 (m21, m22);
if (e0.normalize() && e1.normalize() && e0.isPerpendicularTo(e1)) {
if (!mgIsZero(e0.x - e1.y) || !mgIsZero(e0.y + e1.x)) {
reflex.setAngleLength(e0.angle2() / 2.f, 1.f);
return true;
}
}
return false;
}
int main()
{
incomparable i0, i1;
equal_comparable e0(0), e1(1);
test_comparable t0(0), t1(1);
return cbt::main_test({ cbt::TestEqual(std::cout, "incomparable", i0, i1)
, cbt::TestEqual(std::cout, "equal_comparable", e0, e0)
, !cbt::TestEqual(std::cout, "equal_comparable", e0, e1)
, cbt::TestEqual(std::cout, "test_comparable", t0, t0)
, !cbt::TestEqual(std::cout, "test_comparable", t0, t1)
, true
});
}
EdgeMap compute_edge_map(const MatrixIr& faces) {
assert(faces.cols() == 3);
EdgeMap result;
const size_t num_faces = faces.rows();
for (size_t i=0; i<num_faces; i++) {
const Vector3I& f = faces.row(i);
Triplet e0(f[1], f[2]);
Triplet e1(f[2], f[0]);
Triplet e2(f[0], f[1]);
result.insert(e0, i);
result.insert(e1, i);
result.insert(e2, i);
}
return result;
}
TEST(pd_next_impl, TestEltwiseImpl) {
auto eng = engine(get_test_engine_kind(), 0);
memory::desc md({8, 32, 4, 4}, memory::data_type::f32, memory::format_tag::nChw8c);
eltwise_forward::desc ed(prop_kind::forward_training,
algorithm::eltwise_relu, md, 0, 0);
eltwise_forward::primitive_desc epd(ed, eng);
std::string impl0(epd.impl_info_str());
eltwise_forward e0(epd);
while (epd.next_impl()) {
std::string impl1(epd.impl_info_str());
eltwise_forward e1(epd);
EXPECT_NE(impl0, impl1);
impl0 = impl1;
}
}
dVector dPolygonNormal (int indexCount, const dFloat* const vertex, int strideInBytes, const int* const indices)
{
int stride = strideInBytes / sizeof (dFloat);
int index = indices[0] * stride;
dVector p0 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
index = indices[1] * stride;
dVector p1 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dVector e0 (p1 - p0);
dVector normal (0.0, 0.0f, 0.0f);
for (int i = 2; i < indexCount; i ++) {
index = indices[i] * stride;
dVector p2 (vertex[index], vertex[index + 1], vertex[index + 2], 0.0f);
dVector e1 (p2 - p0);
normal += e0 * e1;
e0 = e1;
}
return normal;
}
int main(int argc, char const *argv[]) {
if (argc == 2) {
strcpy(f, argv[1]);
}
p = 0;
e0();
if (!aceito) {
printf("%s\n", argv[1]);
if (p == 0) {
printf("^\n");
} else {
printf("%*s" "%s\n", p - 1, " ", "^");
}
}
return 0;
}
dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const
{
const dgEdge* edge = face;
dgBigVector p0 (pool[edge->m_incidentVertex]);
edge = edge->m_next;
dgBigVector p1 (pool[edge->m_incidentVertex]);
dgBigVector e1 (p1 - p0);
dgBigVector normal (dgFloat32 (0.0f));
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
dgBigVector p2 (pool[edge->m_incidentVertex]);
dgBigVector e2 (p2 - p0);
dgBigVector n1 (e1.CrossProduct(e2));
#ifdef _DEBUG
dgAssert(n1.m_w == dgFloat32(0.0f));
dgFloat64 mag = normal.DotProduct(n1).GetScalar();
dgAssert ( mag >= -dgFloat32 (0.1f));
#endif
normal += n1;
e1 = e2;
}
dgFloat64 den = sqrt (normal.DotProduct(normal).GetScalar()) + dgFloat64 (1.0e-24f);
normal = normal.Scale (dgFloat64 (1.0f)/ den);
#ifdef _DEBUG
edge = face;
dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
do {
dgBigVector de1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
dgBigVector dn1 (e0.CrossProduct(de1));
dgFloat64 x = normal.DotProduct(dn1).GetScalar();
dgAssert (x > -dgFloat64 (0.01f));
e0 = de1;
edge = edge->m_next;
} while (edge != face);
#endif
return normal;
}
void dgCollisionDeformableMesh::UpdateVisualNormals()
{
for (dgInt32 i = 0; i < m_trianglesCount; i ++) {
dgInt32 i0 = m_indexList[i * 3];
dgInt32 i1 = m_indexList[i * 3 + 1];
dgInt32 i2 = m_indexList[i * 3 + 2];
dgVector e0 (m_particles.m_posit[i1] - m_particles.m_posit[i0]);
dgVector e1 (m_particles.m_posit[i2] - m_particles.m_posit[i0]);
dgVector n = e0 * e1;
n = n.Scale3(dgRsqrt (n % n));
m_faceNormals[i] = n;
}
dgAssert (m_visualVertexData);
for (dgInt32 i = 0; i < m_visualVertexCount; i ++) {
m_visualVertexData[i].m_normals[0] = dgFloat32 (0.0f);
m_visualVertexData[i].m_normals[1] = dgFloat32 (0.0f);
m_visualVertexData[i].m_normals[2] = dgFloat32 (0.0f);
}
for (dgList<dgMeshSegment>::dgListNode* node = m_visualSegments.GetFirst(); node; node = node->GetNext() ) {
const dgMeshSegment& segment = node->GetInfo();
for (dgInt32 i = 0; i < segment.m_indexCount; i ++) {
dgInt32 index = segment.m_indexList[i];
dgInt32 faceIndexNormal = i / 3;
m_visualVertexData[index].m_normals[0] += m_faceNormals[faceIndexNormal].m_x;
m_visualVertexData[index].m_normals[1] += m_faceNormals[faceIndexNormal].m_y;
m_visualVertexData[index].m_normals[2] += m_faceNormals[faceIndexNormal].m_z;
}
}
for (dgInt32 i = 0; i < m_visualVertexCount; i ++) {
dgVector n (m_visualVertexData[i].m_normals[0], m_visualVertexData[i].m_normals[1], m_visualVertexData[i].m_normals[2], dgFloat32 (0.0f));
n = n.Scale3(dgRsqrt (n % n));
m_visualVertexData[i].m_normals[0] = n.m_x;
m_visualVertexData[i].m_normals[1] = n.m_y;
m_visualVertexData[i].m_normals[2] = n.m_z;
}
}
//===========================================================================
//Function Name: closest_point
//
//Member Type: PUBLIC
//Description: return the closest point on segment defined by the edge
//===========================================================================
CubitStatus CubitFacetEdge::closest_point(const CubitVector &point,
CubitVector &closest_point )
{
//CubitStatus rv = CUBIT_SUCCESS;
CubitPoint *pt0 = this->point(0);
CubitPoint *pt1 = this->point(1);
// the edge vector
CubitVector e0 ( pt1->x() - pt0->x(),
pt1->y() - pt0->y(),
pt1->z() - pt0->z() );
double elen = e0.normalize();
// vector from vert0 to point
CubitVector v0 ( point.x() - pt0->x(),
point.y() - pt0->y(),
point.z() - pt0->z() );
// project to edge
double len = v0%e0;
if (len <= 0.0)
{
closest_point = pt0->coordinates();
}
else if( len >= elen )
{
closest_point = pt1->coordinates();
}
else
{
closest_point.x ( pt0->x() + e0.x() * len );
closest_point.y ( pt0->y() + e0.y() * len );
closest_point.z ( pt0->z() + e0.z() * len );
}
return CUBIT_SUCCESS;
}
dgBigVector dgCollisionConvexHull::FaceNormal (const dgEdge *face, const dgBigVector* const pool) const
{
const dgEdge *edge = face;
dgBigVector p0 (pool[edge->m_incidentVertex]);
edge = edge->m_next;
dgBigVector p1 (pool[edge->m_incidentVertex]);
dgBigVector e1 (p1 - p0);
dgBigVector normal (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
for (edge = edge->m_next; edge != face; edge = edge->m_next) {
dgBigVector p2 (pool[edge->m_incidentVertex]);
dgBigVector e2 (p2 - p0);
dgBigVector n1 (e1 * e2);
#ifdef _DEBUG
dgFloat64 mag = normal % n1;
_ASSERTE ( mag >= -dgFloat32 (0.1f));
#endif
normal += n1;
e1 = e2;
}
dgFloat64 den = sqrt (normal % normal) + dgFloat64 (1.0e-24f);
normal = normal.Scale (dgFloat64 (1.0f)/ den);
#ifdef _DEBUG
edge = face;
dgBigVector e0 (pool[edge->m_incidentVertex] - pool[edge->m_prev->m_incidentVertex]);
do {
dgBigVector e1 (pool[edge->m_next->m_incidentVertex] - pool[edge->m_incidentVertex]);
dgBigVector n1 (e0 * e1);
dgFloat64 x = normal % n1;
_ASSERTE (x > -dgFloat64 (0.01f));
e0 = e1;
edge = edge->m_next;
} while (edge != face);
#endif
return normal;
}
static void var_dec(integer interpret)
{
char* name;
integer val;
{
scan_test(NULL, RDP_T_int, &var_dec_stop);
scan_();
{ /* Start of rdp_var_dec_3 */
while (1)
{
scan_test(NULL, SCAN_P_ID, &var_dec_stop);
{
scan_test(NULL, SCAN_P_ID, &var_dec_stop);
name = SCAN_CAST->id;
scan_();
_insert(name);
if (scan_test(NULL, RDP_T_61 /* = */, NULL))
{ /* Start of rdp_var_dec_1 */
while (1)
{
{
scan_test(NULL, RDP_T_61 /* = */, &var_dec_stop);
scan_();
val = e0();
_update(name, val);
}
break; /* hi limit is 1! */
}
} /* end of rdp_var_dec_1 */
}
if (SCAN_CAST->token != RDP_T_44 /* , */) break;
scan_();
}
} /* end of rdp_var_dec_3 */
scan_test_set(NULL, &var_dec_stop, &var_dec_stop);
}
}
/***********************************************************************//**
* @brief Test Cholesky decomposition
***************************************************************************/
void TestGMatrixSparse::matrix_cholesky(void)
{
// Setup matrix for Cholesky decomposition
GMatrixSparse chol_test(5,5);
chol_test(0,0) = 1.0;
chol_test(0,1) = 0.2;
chol_test(0,2) = 0.2;
chol_test(0,3) = 0.2;
chol_test(0,4) = 0.2;
chol_test(1,0) = 0.2;
chol_test(2,0) = 0.2;
chol_test(3,0) = 0.2;
chol_test(4,0) = 0.2;
chol_test(1,1) = 1.0;
chol_test(2,2) = 1.0;
chol_test(3,3) = 1.0;
chol_test(4,4) = 1.0;
// Try to solve now (should not work)
test_try("Try Cholesky solver without factorisation");
try {
GVector vector(5);
vector = chol_test.cholesky_solver(vector);
test_try_failure("Expected GException::matrix_not_factorised exception.");
}
catch (GException::matrix_not_factorised &e) {
test_try_success();
}
catch (std::exception &e) {
test_try_failure(e);
}
// Perform Cholesky decomposition
GMatrixSparse cd = chol_test.cholesky_decompose();
// Test Cholesky solver (first test)
GVector e0(5);
GVector a0(5);
e0[0] = 1.0;
a0[0] = 1.0;
a0[1] = 0.2;
a0[2] = 0.2;
a0[3] = 0.2;
a0[4] = 0.2;
GVector s0 = cd.cholesky_solver(a0);
double res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method - 1");
// Test Cholesky solver (second test)
e0[0] = 0.0;
e0[1] = 1.0;
a0[0] = 0.2;
a0[1] = 1.0;
a0[2] = 0.0;
a0[3] = 0.0;
a0[4] = 0.0;
s0 = cd.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method - 2");
// Test Cholesky solver (third test)
e0[1] = 0.0;
e0[2] = 1.0;
a0[1] = 0.0;
a0[2] = 1.0;
s0 = cd.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method - 3");
// Test Cholesky solver (forth test)
e0[2] = 0.0;
e0[3] = 1.0;
a0[2] = 0.0;
a0[3] = 1.0;
s0 = cd.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method - 4");
// Test Cholesky solver (fifth test)
e0[3] = 0.0;
e0[4] = 1.0;
a0[3] = 0.0;
a0[4] = 1.0;
s0 = cd.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method - 5");
// Setup matrix for Cholesky decomposition with zero row/col
GMatrixSparse chol_test_zero(6,6);
chol_test_zero(0,0) = 1.0;
chol_test_zero(0,1) = 0.2;
chol_test_zero(0,2) = 0.2;
chol_test_zero(0,4) = 0.2;
chol_test_zero(0,5) = 0.2;
chol_test_zero(1,0) = 0.2;
chol_test_zero(2,0) = 0.2;
chol_test_zero(4,0) = 0.2;
chol_test_zero(5,0) = 0.2;
chol_test_zero(1,1) = 1.0;
chol_test_zero(2,2) = 1.0;
chol_test_zero(4,4) = 1.0;
chol_test_zero(5,5) = 1.0;
// Test compressed Cholesky decomposition
GMatrixSparse cd_zero = chol_test_zero.cholesky_decompose();
// Test compressed Cholesky solver (first test)
e0 = GVector(6);
a0 = GVector(6);
e0[0] = 1.0;
a0[0] = 1.0;
a0[1] = 0.2;
a0[2] = 0.2;
a0[4] = 0.2;
a0[5] = 0.2;
s0 = cd_zero.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method - 1");
// Test compressed Cholesky solver (second test)
e0[0] = 0.0;
e0[1] = 1.0;
a0[0] = 0.2;
a0[1] = 1.0;
a0[2] = 0.0;
a0[4] = 0.0;
a0[5] = 0.0;
s0 = cd_zero.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method - 2");
// Test compressed Cholesky solver (third test)
e0[1] = 0.0;
e0[2] = 1.0;
a0[1] = 0.0;
a0[2] = 1.0;
s0 = cd_zero.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method - 3");
// Test compressed Cholesky solver (forth test)
e0[2] = 0.0;
e0[4] = 1.0;
a0[2] = 0.0;
a0[4] = 1.0;
s0 = cd_zero.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method - 4");
// Test compressed Cholesky solver (fifth test)
e0[4] = 0.0;
e0[5] = 1.0;
a0[4] = 0.0;
a0[5] = 1.0;
s0 = cd_zero.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method - 5");
// Setup matrix for Cholesky decomposition with zero row/col (unsymmetric case)
GMatrixSparse chol_test_zero2(6,5);
chol_test_zero2(0,0) = 1.0;
chol_test_zero2(0,1) = 0.2;
chol_test_zero2(0,2) = 0.2;
chol_test_zero2(0,3) = 0.2;
chol_test_zero2(0,4) = 0.2;
chol_test_zero2(1,0) = 0.2;
chol_test_zero2(2,0) = 0.2;
chol_test_zero2(4,0) = 0.2;
chol_test_zero2(5,0) = 0.2;
chol_test_zero2(1,1) = 1.0;
chol_test_zero2(2,2) = 1.0;
chol_test_zero2(4,3) = 1.0;
chol_test_zero2(5,4) = 1.0;
// Test compressed Cholesky decomposition (unsymmetric case)
GMatrixSparse cd_zero2 = chol_test_zero2.cholesky_decompose();
// Test compressed Cholesky solver (unsymmetric case)
e0 = GVector(5);
a0 = GVector(6);
e0[0] = 1.0;
a0[0] = 1.0;
a0[1] = 0.2;
a0[2] = 0.2;
a0[4] = 0.2;
a0[5] = 0.2;
s0 = cd_zero2.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15,
"Test unsymmetric compressed cholesky_solver() method - 1");
// Test compressed Cholesky solver (unsymmetric case)
e0[0] = 0.0;
e0[1] = 1.0;
a0[0] = 0.2;
a0[1] = 1.0;
a0[2] = 0.0;
a0[4] = 0.0;
a0[5] = 0.0;
s0 = cd_zero2.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15,
"Test unsymmetric compressed cholesky_solver() method - 2");
// Test compressed Cholesky solver (unsymmetric case)
e0[1] = 0.0;
e0[2] = 1.0;
a0[1] = 0.0;
a0[2] = 1.0;
s0 = cd_zero2.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15,
"Test unsymmetric compressed cholesky_solver() method - 3");
// Test compressed Cholesky solver (unsymmetric case)
e0[2] = 0.0;
e0[3] = 1.0;
a0[2] = 0.0;
a0[4] = 1.0;
s0 = cd_zero2.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15,
"Test unsymmetric compressed cholesky_solver() method - 4");
// Test compressed Cholesky solver (unsymmetric case)
e0[3] = 0.0;
e0[4] = 1.0;
a0[4] = 0.0;
a0[5] = 1.0;
s0 = cd_zero2.cholesky_solver(a0);
res = max(abs(s0-e0));
test_value(res, 0.0, 1.0e-15,
"Test unsymmetric compressed cholesky_solver() method - 5");
// Test Cholesky inverter (inplace)
GMatrixSparse unit(5,5);
unit(0,0) = 1.0;
unit(1,1) = 1.0;
unit(2,2) = 1.0;
unit(3,3) = 1.0;
unit(4,4) = 1.0;
GMatrixSparse chol_test_inv = chol_test.cholesky_invert();
GMatrixSparse ci_product = chol_test * chol_test_inv;
GMatrixSparse ci_residuals = ci_product - unit;
res = (ci_residuals.abs()).max();
test_value(res, 0.0, 1.0e-15, "Test Cholesky inverter");
// Test Cholesky inverter
/*
chol_test_inv = chol_test.cholesky_invert();
ci_product = chol_test * chol_test_inv;
ci_residuals = ci_product - unit;
res = (ci_residuals.abs()).max();
test_value(res, 0.0, 1.0e-15, "Test Cholesky inverter");
*/
// Test Cholesky inverter for compressed matrix
unit = GMatrixSparse(6,6);
unit(0,0) = 1.0;
unit(1,1) = 1.0;
unit(2,2) = 1.0;
unit(4,4) = 1.0;
unit(5,5) = 1.0;
GMatrixSparse chol_test_zero_inv = chol_test_zero.cholesky_invert();
GMatrixSparse ciz_product = chol_test_zero * chol_test_zero_inv;
GMatrixSparse ciz_residuals = ciz_product - unit;
res = (ciz_residuals.abs()).max();
test_value(res, 0.0, 1.0e-15, "Test compressed matrix Cholesky inverter");
// Return
return;
}
void f0() { e0(); }
/***********************************************************************//**
* @brief Test Cholesky decomposition
***************************************************************************/
void TestGSymMatrix::matrix_cholesky(void)
{
// Test Cholesky decomposition
GSymMatrix cd = cholesky_decompose(m_test);
GMatrix cd_lower = cd.extract_lower_triangle();
GMatrix cd_upper = transpose(cd_lower);
GMatrix cd_product = cd_lower * cd_upper;
GMatrix cd_residuals = GMatrix(m_test) - cd_product;
double res = (abs(cd_residuals)).max();
test_value(res, 0.0, 1.0e-15, "Test cholesky_decompose() method");
// Test compressed Cholesky decomposition
GSymMatrix test_zero = set_matrix_zero();
GSymMatrix cd_zero = cholesky_decompose(test_zero);
GMatrix cd_zero_lower = cd_zero.extract_lower_triangle();
GMatrix cd_zero_upper = transpose(cd_zero_lower);
GMatrix cd_zero_product = cd_zero_lower * cd_zero_upper;
GMatrix cd_zero_residuals = GMatrix(test_zero) - cd_zero_product;
res = (abs(cd_zero_residuals)).max();
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_decompose() method");
// Test Cholesky inplace decomposition
GSymMatrix test = m_test;
test.cholesky_decompose();
GMatrix cd_lower2 = test.extract_lower_triangle();
test_assert((cd_lower2 == cd_lower), "Test inplace cholesky_decompose() method");
// Test Cholesky solver (first test)
GVector e0(g_rows);
GVector a0(g_rows);
e0[0] = 1.0;
e0[1] = 0.0;
e0[2] = 0.0;
a0[0] = g_matrix[0];
a0[1] = g_matrix[3];
a0[2] = g_matrix[6];
GVector s0 = cd.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method");
// Test Cholesky solver (second test)
e0[0] = 0.0;
e0[1] = 1.0;
e0[2] = 0.0;
a0[0] = g_matrix[1];
a0[1] = g_matrix[4];
a0[2] = g_matrix[7];
s0 = cd.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method");
// Test Cholesky solver (third test)
e0[0] = 0.0;
e0[1] = 0.0;
e0[2] = 1.0;
a0[0] = g_matrix[2];
a0[1] = g_matrix[5];
a0[2] = g_matrix[8];
s0 = cd.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test cholesky_solver() method");
// Test compressed Cholesky solver (first test)
e0 = GVector(g_rows+1);
a0 = GVector(g_rows+1);
e0[0] = 1.0;
e0[1] = 0.0;
e0[2] = 0.0;
e0[3] = 0.0;
a0[0] = g_matrix[0];
a0[1] = g_matrix[3];
a0[2] = 0.0;
a0[3] = g_matrix[6];
s0 = cd_zero.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method");
// Test compressed Cholesky solver (second test)
e0[0] = 0.0;
e0[1] = 1.0;
e0[2] = 0.0;
e0[3] = 0.0;
a0[0] = g_matrix[1];
a0[1] = g_matrix[4];
a0[2] = 0.0;
a0[3] = g_matrix[7];
s0 = cd_zero.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method");
// Test compressed Cholesky solver (third test)
e0[0] = 0.0;
e0[1] = 0.0;
e0[2] = 0.0;
e0[3] = 1.0;
a0[0] = g_matrix[2];
a0[1] = g_matrix[5];
a0[2] = 0.0;
a0[3] = g_matrix[8];
s0 = cd_zero.cholesky_solver(a0) - e0;
res = max(abs(s0));
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_solver() method");
// Test Cholesky inverter
GSymMatrix unit(g_rows,g_cols);
unit(0,0) = unit(1,1) = unit(2,2) = 1.0;
GSymMatrix test_inv = m_test;
test_inv.cholesky_invert();
GMatrix ci_product = m_test * test_inv;
GMatrix ci_residuals = ci_product - unit;
res = (abs(ci_residuals)).max();
test_value(res, 0.0, 1.0e-15, "Test cholesky_invert method");
// Test Cholesky inverter for compressed matrix
unit = GSymMatrix(4,4);
unit(0,0) = unit(1,1) = unit(3,3) = 1.0;
GSymMatrix test_zero_inv = test_zero;
test_zero_inv.cholesky_invert();
GMatrix ciz_product = test_zero * test_zero_inv;
GMatrix ciz_residuals = ciz_product - unit;
res = (abs(ciz_residuals)).max();
test_value(res, 0.0, 1.0e-15, "Test compressed cholesky_invert method");
// Return
return;
}
dgInt32 dgCollisionConvexPolygon::CalculatePlaneIntersection (const dgVector& normalIn, const dgVector& origin, dgVector* const contactsOut, dgFloat32 normalSign) const
{
dgVector normal(normalIn);
dgInt32 count = 0;
dgFloat32 maxDist = dgFloat32 (1.0f);
dgFloat32 projectFactor = m_normal % normal;
if (projectFactor < dgFloat32 (0.0f)) {
projectFactor *= dgFloat32 (-1.0f);
normal = normal.Scale3 (dgFloat32 (-1.0f));
}
if (projectFactor > dgFloat32 (0.9999f)) {
for (dgInt32 i = 0; i < m_count; i ++) {
contactsOut[count] = m_localPoly[i];
count ++;
}
#ifdef _DEBUG
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
#endif
} else if (projectFactor > dgFloat32 (0.1736f)) {
maxDist = dgFloat32 (0.0f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if (side0 > dgFloat32 (0.0f)) {
maxDist = dgMax (maxDist, side0);
contactsOut[count] = p0 - plane.Scale3 (side0);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
if (side1 <= dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
} else if (side1 > dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
} else {
maxDist = dgFloat32 (1.0e10f);
dgPlane plane (normal, - (normal % origin));
dgVector p0 (m_localPoly[m_count - 1]);
dgFloat32 side0 = plane.Evalue (p0);
for (dgInt32 i = 0; i < m_count; i ++) {
dgVector p1 (m_localPoly[i]);
dgFloat32 side1 = plane.Evalue (p1);
if ((side0 * side1) < dgFloat32 (0.0f)) {
dgVector dp (p1 - p0);
dgFloat32 t = plane % dp;
dgAssert (dgAbsf (t) >= dgFloat32 (0.0f));
if (dgAbsf (t) < dgFloat32 (1.0e-8f)) {
t = dgSign(t) * dgFloat32 (1.0e-8f);
}
contactsOut[count] = p0 - dp.Scale3 (side0 / t);
count ++;
if (count > 1) {
dgVector edgeSegment (contactsOut[count - 1] - contactsOut[count - 2]);
dgFloat32 error = edgeSegment % edgeSegment;
if (error < dgFloat32 (1.0e-8f)) {
count --;
}
}
}
side0 = side1;
p0 = p1;
}
}
if (count > 1) {
if (maxDist < dgFloat32 (1.0e-3f)) {
dgVector maxPoint (contactsOut[0]);
dgVector minPoint (contactsOut[0]);
dgVector lineDir (m_normal * normal);
dgFloat32 proj = contactsOut[0] % lineDir;
dgFloat32 maxProjection = proj;
dgFloat32 minProjection = proj;
for (dgInt32 i = 1; i < count; i ++) {
proj = contactsOut[i] % lineDir;
if (proj > maxProjection) {
maxProjection = proj;
maxPoint = contactsOut[i];
}
if (proj < minProjection) {
minProjection = proj;
minPoint = contactsOut[i];
}
}
contactsOut[0] = maxPoint;
contactsOut[1] = minPoint;
count = 2;
}
dgVector error (contactsOut[count - 1] - contactsOut[0]);
if ((error % error) < dgFloat32 (1.0e-8f)) {
count --;
}
}
#ifdef _DEBUG
if (count > 1) {
dgInt32 j = count - 1;
for (dgInt32 i = 0; i < count; i ++) {
dgVector error (contactsOut[i] - contactsOut[j]);
dgAssert ((error % error) > dgFloat32 (1.0e-20f));
j = i;
}
if (count >= 3) {
dgVector n (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgVector e0 (contactsOut[1] - contactsOut[0]);
for (dgInt32 i = 2; i < count; i ++) {
dgVector e1 (contactsOut[i] - contactsOut[0]);
n += e0 * e1;
e0 = e1;
}
n = n.Scale3 (dgRsqrt(n % n));
dgFloat32 val = n % normal;
dgAssert (val > dgFloat32 (0.9f));
}
}
#endif
return count;
}
bool dgCollisionConvexHull::RemoveCoplanarEdge (dgPolyhedra& polyhedra, const dgBigVector* const hullVertexArray) const
{
bool removeEdge = false;
// remove coplanar edges
dgInt32 mark = polyhedra.IncLRU();
dgPolyhedra::Iterator iter (polyhedra);
for (iter.Begin(); iter; ) {
dgEdge* edge0 = &(*iter);
iter ++;
if (edge0->m_incidentFace != -1) {
if (edge0->m_mark < mark) {
edge0->m_mark = mark;
edge0->m_twin->m_mark = mark;
dgBigVector normal0 (FaceNormal (edge0, &hullVertexArray[0]));
dgBigVector normal1 (FaceNormal (edge0->m_twin, &hullVertexArray[0]));
dgFloat64 test = normal0 % normal1;
if (test > dgFloat64 (0.99995f)) {
if ((edge0->m_twin->m_next->m_twin->m_next != edge0) && (edge0->m_next->m_twin->m_next != edge0->m_twin)) {
#define DG_MAX_EDGE_ANGLE dgFloat32 (1.0e-3f)
if (edge0->m_twin == &(*iter)) {
if (iter) {
iter ++;
}
}
dgBigVector e1 (hullVertexArray[edge0->m_twin->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_incidentVertex] - hullVertexArray[edge0->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
dgFloat64 projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgBigVector e1 (hullVertexArray[edge0->m_next->m_next->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_incidentVertex]);
dgBigVector e0 (hullVertexArray[edge0->m_twin->m_incidentVertex] - hullVertexArray[edge0->m_twin->m_prev->m_incidentVertex]);
dgAssert ((e0 % e0) >= dgFloat64 (0.0f));
dgAssert ((e1 % e1) >= dgFloat64 (0.0f));
//e0 = e0.Scale3 (dgRsqrt (e0 % e0));
//e1 = e1.Scale3 (dgRsqrt (e1 % e1));
e0 = e0.Scale3 (dgFloat64 (1.0f) / sqrt (e0 % e0));
e1 = e1.Scale3 (dgFloat64 (1.0f) / sqrt (e1 % e1));
dgBigVector n1 (e0 * e1);
projection = n1 % normal0;
if (projection >= DG_MAX_EDGE_ANGLE) {
dgAssert (&(*iter) != edge0);
dgAssert (&(*iter) != edge0->m_twin);
polyhedra.DeleteEdge(edge0);
removeEdge = true;
}
}
} else {
dgEdge* next = edge0->m_next;
dgEdge* prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
for (edge0 = next; edge0->m_prev->m_twin == edge0; edge0 = next) {
next = edge0->m_next;
polyhedra.DeleteEdge(edge0);
}
for (edge0 = prev; edge0->m_next->m_twin == edge0; edge0 = prev) {
prev = edge0->m_prev;
polyhedra.DeleteEdge(edge0);
}
iter.Begin();
removeEdge = true;
}
}
}
}
}
return removeEdge;
}
void FEdgeXDetector::ProcessRidgeFace(WXFace *iFace)
{
// RIDGE LAYER
// Compute the RidgeFunction, that is the derivative of the ppal curvature along e1 at each vertex of the face
WVertex *v;
Vec3r v1v2;
real t;
vector<WXFaceLayer*> SmoothLayers;
WXFaceLayer *faceLayer;
Face_Curvature_Info *layer_info;
real K1_a(0), K1_b(0);
Vec3r Inter_a, Inter_b;
// find the ridge layer of the face
iFace->retrieveSmoothLayers(Nature::RIDGE, SmoothLayers);
if ( SmoothLayers.size()!=1 )
return;
faceLayer = SmoothLayers[0];
// retrieve the curvature info of this layer
layer_info = (Face_Curvature_Info *)faceLayer->userdata;
int numVertices = iFace->numberOfVertices();
for (int i = 0; i < numVertices; i++) {
v = iFace->GetVertex(i);
// vec_curvature_info[i] contains the curvature info of this vertex
Vec3r e2 = layer_info->vec_curvature_info[i]->K2*layer_info->vec_curvature_info[i]->e2;
Vec3r e1 = layer_info->vec_curvature_info[i]->K1*layer_info->vec_curvature_info[i]->e1;
e2.normalize();
WVertex::face_iterator fit = v->faces_begin();
WVertex::face_iterator fitend = v->faces_end();
for (; fit != fitend; ++fit) {
WXFace *wxf = dynamic_cast<WXFace*>(*fit);
WOEdge *oppositeEdge;
if (!(wxf->getOppositeEdge(v, oppositeEdge)))
continue;
v1v2 = oppositeEdge->GetbVertex()->GetVertex() - oppositeEdge->GetaVertex()->GetVertex();
GeomUtils::intersection_test res;
res = GeomUtils::intersectRayPlane(oppositeEdge->GetaVertex()->GetVertex(), v1v2, e2, -(v->GetVertex()*e2),
t, 1.0e-06);
if ((res == GeomUtils::DO_INTERSECT) && (t >= 0.0) && (t <= 1.0)) {
vector<WXFaceLayer*> second_ridge_layer;
wxf->retrieveSmoothLayers(Nature::RIDGE, second_ridge_layer);
if (second_ridge_layer.size() != 1)
continue;
Face_Curvature_Info *second_layer_info = (Face_Curvature_Info*)second_ridge_layer[0]->userdata;
unsigned index1 = wxf->GetIndex(oppositeEdge->GetaVertex());
unsigned index2 = wxf->GetIndex(oppositeEdge->GetbVertex());
real K1_1 = second_layer_info->vec_curvature_info[index1]->K1;
real K1_2 = second_layer_info->vec_curvature_info[index2]->K1;
real K1 = (1.0 - t) * K1_1 + t * K1_2;
Vec3r inter((1.0 - t) * oppositeEdge->GetaVertex()->GetVertex() +
t * oppositeEdge->GetbVertex()->GetVertex());
Vec3r vtmp(inter - v->GetVertex());
// is it K1_a or K1_b ?
if (vtmp * e1 > 0) {
K1_b = K1;
Inter_b = inter;
}
else {
K1_a = K1;
Inter_a = inter;
}
}
}
// Once we have K1 along the ppal direction compute the derivative : K1b - K1a put it in DotP
//real d = fabs(K1_b) - fabs(K1_a);
real d = 0;
real threshold = _meanK1 + (_maxK1 - _meanK1) / 7.0;
//real threshold = _meanK1;
//if ((fabs(K1_b) > threshold) || ((fabs(K1_a) > threshold)))
d = (K1_b) - (K1_a) / (Inter_b - Inter_a).norm();
faceLayer->PushDotP(d);
//faceLayer->PushDotP(layer_info->vec_curvature_info[i]->K1);
}
// Make the values relevant by checking whether all principal directions have the "same" direction:
Vec3r e0((layer_info->vec_curvature_info[0]->K1 * layer_info->vec_curvature_info[0]->e1));
e0.normalize();
Vec3r e1((layer_info->vec_curvature_info[1]->K1 * layer_info->vec_curvature_info[1]->e1));
e1.normalize();
Vec3r e2((layer_info->vec_curvature_info[2]->K1 * layer_info->vec_curvature_info[2]->e1));
e2.normalize();
if (e0 * e1 < 0)
// invert dotP[1]
faceLayer->ReplaceDotP(1, -faceLayer->dotP(1));
if (e0 * e2 < 0)
// invert dotP[2]
faceLayer->ReplaceDotP(2, -faceLayer->dotP(2));
#if 0 // remove the weakest values;
real minDiff = (_maxK1 - _minK1) / 10.0;
real minDiff = _meanK1;
if ((faceLayer->dotP(0) < minDiff) && (faceLayer->dotP(1) < minDiff) && (faceLayer->dotP(2) < minDiff)) {
faceLayer->ReplaceDotP(0, 0);
faceLayer->ReplaceDotP(1, 0);
faceLayer->ReplaceDotP(2, 0);
}
#endif
}
static void sha256_transform(u32 *state, const u8 *input)
{
u32 a, b, c, d, e, f, g, h, t1, t2;
u32 W[64];
int i;
/* load the input */
for (i = 0; i < 16; i++)
LOAD_OP(i, W, input);
/* now blend */
for (i = 16; i < 64; i++)
BLEND_OP(i, W);
/* load the state into our registers */
a=state[0]; b=state[1]; c=state[2]; d=state[3];
e=state[4]; f=state[5]; g=state[6]; h=state[7];
/* now iterate */
t1 = h + e1(e) + Ch(e,f,g) + 0x428a2f98 + W[ 0];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0x71374491 + W[ 1];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0xb5c0fbcf + W[ 2];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0xe9b5dba5 + W[ 3];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x3956c25b + W[ 4];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0x59f111f1 + W[ 5];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x923f82a4 + W[ 6];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0xab1c5ed5 + W[ 7];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0xd807aa98 + W[ 8];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0x12835b01 + W[ 9];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0x243185be + W[10];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0x550c7dc3 + W[11];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x72be5d74 + W[12];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0x80deb1fe + W[13];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x9bdc06a7 + W[14];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0xc19bf174 + W[15];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0xe49b69c1 + W[16];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0xefbe4786 + W[17];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0x0fc19dc6 + W[18];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0x240ca1cc + W[19];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x2de92c6f + W[20];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0x4a7484aa + W[21];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x5cb0a9dc + W[22];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0x76f988da + W[23];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0x983e5152 + W[24];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0xa831c66d + W[25];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0xb00327c8 + W[26];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0xbf597fc7 + W[27];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0xc6e00bf3 + W[28];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0xd5a79147 + W[29];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x06ca6351 + W[30];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0x14292967 + W[31];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0x27b70a85 + W[32];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0x2e1b2138 + W[33];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0x4d2c6dfc + W[34];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0x53380d13 + W[35];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x650a7354 + W[36];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0x766a0abb + W[37];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x81c2c92e + W[38];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0x92722c85 + W[39];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0xa2bfe8a1 + W[40];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0xa81a664b + W[41];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0xc24b8b70 + W[42];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0xc76c51a3 + W[43];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0xd192e819 + W[44];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0xd6990624 + W[45];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0xf40e3585 + W[46];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0x106aa070 + W[47];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0x19a4c116 + W[48];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0x1e376c08 + W[49];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0x2748774c + W[50];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0x34b0bcb5 + W[51];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x391c0cb3 + W[52];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0x4ed8aa4a + W[53];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0x5b9cca4f + W[54];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0x682e6ff3 + W[55];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
t1 = h + e1(e) + Ch(e,f,g) + 0x748f82ee + W[56];
t2 = e0(a) + Maj(a,b,c); d+=t1; h=t1+t2;
t1 = g + e1(d) + Ch(d,e,f) + 0x78a5636f + W[57];
t2 = e0(h) + Maj(h,a,b); c+=t1; g=t1+t2;
t1 = f + e1(c) + Ch(c,d,e) + 0x84c87814 + W[58];
t2 = e0(g) + Maj(g,h,a); b+=t1; f=t1+t2;
t1 = e + e1(b) + Ch(b,c,d) + 0x8cc70208 + W[59];
t2 = e0(f) + Maj(f,g,h); a+=t1; e=t1+t2;
t1 = d + e1(a) + Ch(a,b,c) + 0x90befffa + W[60];
t2 = e0(e) + Maj(e,f,g); h+=t1; d=t1+t2;
t1 = c + e1(h) + Ch(h,a,b) + 0xa4506ceb + W[61];
t2 = e0(d) + Maj(d,e,f); g+=t1; c=t1+t2;
t1 = b + e1(g) + Ch(g,h,a) + 0xbef9a3f7 + W[62];
t2 = e0(c) + Maj(c,d,e); f+=t1; b=t1+t2;
t1 = a + e1(f) + Ch(f,g,h) + 0xc67178f2 + W[63];
t2 = e0(b) + Maj(b,c,d); e+=t1; a=t1+t2;
state[0] += a; state[1] += b; state[2] += c; state[3] += d;
state[4] += e; state[5] += f; state[6] += g; state[7] += h;
/* clear any sensitive info... */
a = b = c = d = e = f = g = h = t1 = t2 = 0;
memset(W, 0, 64 * sizeof(u32));
}
void ComputeStiffnessMatrixNullspace::ComputeNullspace(int n, const double * vertexPos, double * basis, int includeRotationalNullspace, int generateOrthogonalBasis)
{
int basisSize = (includeRotationalNullspace ? 6 : 3);
memset(basis, 0, sizeof(double) * 3 * n * basisSize);
// translational part
for(int i=0; i<n; i++)
for(int j=0; j<3; j++)
basis[ELT(3*n, 3*i+j, j)] = 1.0;
// rotational part
if (includeRotationalNullspace)
{
for(int i=0; i<n; i++)
{
Vec3d p(vertexPos[3*i+0], vertexPos[3*i+1], vertexPos[3*i+2]);
Vec3d e0(1,0,0);
Vec3d e1(0,1,0);
Vec3d e2(0,0,1);
Vec3d r0 = cross(e0, p);
Vec3d r1 = cross(e1, p);
Vec3d r2 = cross(e2, p);
for(int j=0; j<3; j++)
{
basis[ELT(3*n, 3*i+j, 3)] = r0[j];
basis[ELT(3*n, 3*i+j, 4)] = r1[j];
basis[ELT(3*n, 3*i+j, 5)] = r2[j];
}
}
}
if (generateOrthogonalBasis)
{
// normalize translational vectors
for(int v=0; v<3; v++)
{
double norm2 = 0.0;
for(int i=0; i<3*n; i++)
norm2 += basis[ELT(3*n, i, v)] * basis[ELT(3*n, i, v)];
double invNorm = 1.0 / sqrt(norm2);
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, v)] *= invNorm;
}
// ortho-normalize rotational vectors
if (includeRotationalNullspace)
{
for(int v=0; v<3; v++)
{
for(int j=0; j<3+v; j++)
{
double dotp = 0.0;
for(int i=0; i<3*n; i++)
dotp += basis[ELT(3*n, i, j)] * basis[ELT(3*n, i, 3+v)];
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, 3+v)] -= dotp * basis[ELT(3*n, i, j)];
double norm2 = 0.0;
for(int i=0; i<3*n; i++)
norm2 += basis[ELT(3*n, i, 3+v)] * basis[ELT(3*n, i, 3+v)];
double invNorm = 1.0 / sqrt(norm2);
for(int i=0; i<3*n; i++)
basis[ELT(3*n, i, 3+v)] *= invNorm;
}
}
}
}
}
static void statement(rdp_tree_node_data* rdp_tree)
{
char* name;
{
if (scan_test(NULL, SCAN_P_ID, NULL))
{
if (rdp_tree_update) rdp_add_child(NULL, rdp_tree);
scan_test(NULL, SCAN_P_ID, &statement_stop);
name = SCAN_CAST->id;
scan_();
check_declared;
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, RDP_T_61 /* = */, &statement_stop);
scan_();
e0(rdp_add_child("e0", rdp_tree));
}
else
if (scan_test(NULL, RDP_T_if, NULL))
{
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, RDP_T_if, &statement_stop);
scan_();
e0(rdp_add_child("e0", rdp_tree));
scan_test(NULL, RDP_T_then, &statement_stop);
scan_();
statement(rdp_add_child("statement", rdp_tree));
if (scan_test(NULL, RDP_T_else, NULL))
{ /* Start of rdp_statement_2 */
/* WARNING - an LL(1) violation was detected at this point in the grammar */
while (1)
{
{
scan_test(NULL, RDP_T_else, &statement_stop);
scan_();
statement(rdp_add_child("statement", rdp_tree));
}
break; /* hi limit is 1! */
}
} /* end of rdp_statement_2 */
else
{
/* default action processing for rdp_statement_2*/
if (rdp_tree_update) {rdp_tree_node_data *temp = rdp_add_child(NULL, rdp_tree); temp->id = NULL; temp->token = SCAN_P_ID;}
}
}
else
if (scan_test(NULL, RDP_T_while, NULL))
{
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, RDP_T_while, &statement_stop);
scan_();
e0(rdp_add_child("e0", rdp_tree));
scan_test(NULL, RDP_T_do, &statement_stop);
scan_();
statement(rdp_add_child("statement", rdp_tree));
}
else
if (scan_test(NULL, RDP_T_print, NULL))
{
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, RDP_T_print, &statement_stop);
scan_();
scan_test(NULL, RDP_T_40 /* ( */, &statement_stop);
scan_();
{ /* Start of rdp_statement_7 */
while (1)
{
scan_test_set(NULL, &rdp_statement_7_first, &statement_stop);
{
if (scan_test_set(NULL, &rdp_statement_5_first, NULL))
{
e0(rdp_add_child("e0", rdp_tree));
}
else
if (scan_test(NULL, RDP_T_34 /* " */, NULL))
{
String(rdp_tree);
}
else
scan_test_set(NULL, &rdp_statement_7_first, &statement_stop) ;
}
if (SCAN_CAST->token != RDP_T_44 /* , */) break;
scan_();
}
} /* end of rdp_statement_7 */
scan_test(NULL, RDP_T_41 /* ) */, &statement_stop);
scan_();
}
else
if (scan_test(NULL, RDP_T_begin, NULL))
{
if (rdp_tree_update) memcpy(rdp_tree, text_scan_data, sizeof(scan_data));
scan_test(NULL, RDP_T_begin, &statement_stop);
scan_();
{ /* Start of rdp_statement_10 */
while (1)
{
scan_test_set(NULL, &rdp_statement_10_first, &statement_stop);
{
statement(rdp_add_child("statement", rdp_tree));
}
if (SCAN_CAST->token != RDP_T_59 /* ; */) break;
scan_();
}
} /* end of rdp_statement_10 */
scan_test(NULL, RDP_T_end, &statement_stop);
scan_();
}
else
scan_test_set(NULL, &statement_first, &statement_stop) ;
scan_test_set(NULL, &statement_stop, &statement_stop);
}
}