public void Set(QefData rhs)
{
Set(rhs.ata_00, rhs.ata_01, rhs.ata_02, rhs.ata_11, rhs.ata_12,
rhs.ata_22, rhs.atb, rhs.btb,
rhs.massPoint,
rhs.numPoints);
}
public void set(QefData rhs)
{
set(rhs.ata_00, rhs.ata_01, rhs.ata_02, rhs.ata_11, rhs.ata_12,
rhs.ata_22, rhs.atb_x, rhs.atb_y, rhs.atb_z, rhs.btb,
rhs.massPoint_x, rhs.massPoint_y, rhs.massPoint_z,
rhs.numPoints);
}
public NodeInfo(int index, int corners, Vector3 position, Vector3 averageNormal, QefData qef)
{
this.index = -1;
this.corners = 0;
this.position = new Vector3(0, 0, 0);
this.avgNormal = new Vector3(0, 0, 0);
this.qef = new QefData();
}
public void Set(NodeInfo info)
{
this.index = info.index;
this.corners = info.corners;
this.position = info.position;
this.avgNormal = info.avgNormal;
this.qef = info.qef;
}
public QefSolver()
{
data = new QefData();
ata = new SMat3();
atb = new Vec3();
massPoint = new Vec3();
x = new Vec3();
hasSolution = false;
}
public QefSolver()
{
data = new QefData();
ata = new SMat3();
atb = Vector3.zero;
massPoint = Vector3.zero;
x = Vector3.zero;
hasSolution = false;
}
public void Add(QefData rhs)
{
ata_00 += rhs.ata_00;
ata_01 += rhs.ata_01;
ata_02 += rhs.ata_02;
ata_11 += rhs.ata_11;
ata_12 += rhs.ata_12;
ata_22 += rhs.ata_22;
atb += rhs.atb;
btb += rhs.btb;
massPoint += rhs.massPoint;
numPoints += rhs.numPoints;
}
public void Add(QefData rhs)
{
ata.m00 += rhs.ata.m00;
ata.m01 += rhs.ata.m01;
ata.m02 += rhs.ata.m02;
ata.m11 += rhs.ata.m11;
ata.m12 += rhs.ata.m12;
ata.m22 += rhs.ata.m22;
atb += rhs.atb;
btb += rhs.btb;
massSum += rhs.massSum;
numPoints += rhs.numPoints;
}
public static float Solve(out Vector3 outx, QefData data, float svd_tol, int svd_sweeps, float pinv_tol)
{
if (data.numPoints == 0)
{
throw new UnityException("Invalid argument");
}
Vector3 massPoint = data.MassPoint;
Vector3 atb = data.atb;
atb -= data.ata * massPoint;
float result = Svd.SolveSymmetric(data.ata, atb, out outx, svd_tol, svd_sweeps, pinv_tol);
outx += massPoint;
return result;
}
public void add(QefData rhs)
{
ata_00 += rhs.ata_00;
ata_01 += rhs.ata_01;
ata_02 += rhs.ata_02;
ata_11 += rhs.ata_11;
ata_12 += rhs.ata_12;
ata_22 += rhs.ata_22;
atb_x += rhs.atb_x;
atb_y += rhs.atb_y;
atb_z += rhs.atb_z;
btb += rhs.btb;
massPoint_x += rhs.massPoint_x;
massPoint_y += rhs.massPoint_y;
massPoint_z += rhs.massPoint_z;
numPoints += rhs.numPoints;
}
public void add(QefData rhs)
{
this.ata_00 += rhs.ata_00;
this.ata_01 += rhs.ata_01;
this.ata_02 += rhs.ata_02;
this.ata_11 += rhs.ata_11;
this.ata_12 += rhs.ata_12;
this.ata_22 += rhs.ata_22;
this.atb_x += rhs.atb_x;
this.atb_y += rhs.atb_y;
this.atb_y += rhs.atb_z;
this.btb += rhs.btb;
this.massPoint_x += rhs.massPoint_x;
this.massPoint_y += rhs.massPoint_y;
this.massPoint_z += rhs.massPoint_z;
this.numPoints += rhs.numPoints;
}
public void add(QefData rhs)
{
this.ata_00 += rhs.ata_00;
this.ata_01 += rhs.ata_01;
this.ata_02 += rhs.ata_02;
this.ata_11 += rhs.ata_11;
this.ata_12 += rhs.ata_12;
this.ata_22 += rhs.ata_22;
this.atb_x += rhs.atb_x;
this.atb_y += rhs.atb_y;
this.atb_y += rhs.atb_z;
this.btb += rhs.btb;
this.massPoint_x += rhs.massPoint_x;
this.massPoint_y += rhs.massPoint_y;
this.massPoint_z += rhs.massPoint_z;
this.numPoints += rhs.numPoints;
}
static OctreeNode ConstructLeaf(Shape shape, OctreeNode leaf)
{
if (leaf == null)
{
return null;
}
int corners = 0;
for (int i = 0; i < 8; ++i)
{
Vector3 cornerPos = leaf.min + CHILD_MIN_OFFSETS[i] * leaf.size;
float density = shape.ComputeDistance(cornerPos);
int material = density < 0 ? MATERIAL_SOLID : MATERIAL_AIR;
corners |= material << i;
}
if (corners == 0 || corners == 255)
{
// voxel is full inside or outside the volume
return null;
}
// otherwise the voxel contains the surface, so find the edge intersections
const int MAX_CROSSINGS = 6;
int nbEdges = 0;
Vector3 normalSum = Vector3.zero;
QefData qefData = new QefData();
for (int i = 0; i < 12 && nbEdges < MAX_CROSSINGS; ++i)
{
int c1 = edgevmap[i, 0];
int c2 = edgevmap[i, 1];
int m1 = (corners >> c1) & 1;
int m2 = (corners >> c2) & 1;
if (m1 == m2)
{
// no crossing on this edge
continue;
}
Vector3 p1 = leaf.min + CHILD_MIN_OFFSETS[c1] * leaf.size;
Vector3 p2 = leaf.min + CHILD_MIN_OFFSETS[c2] * leaf.size;
Vector3 p = ApproximateIntersection(shape, p1, p2);
Vector3 n = shape.ComputeNormal(p);
qefData.Add(p, n);
normalSum += n;
++nbEdges;
}
Vector3 qefPosition;
QefSolver.Solve(out qefPosition, qefData, QEF_ERROR, QEF_SWEEPS, QEF_ERROR);
OctreeNodeInfo nodeInfo = new OctreeNodeInfo();
nodeInfo.position = qefPosition;
nodeInfo.qefData = qefData;
Vector3 min = leaf.min;
Vector3 max = leaf.min + new Vector3(leaf.size, leaf.size, leaf.size);
//nodeInfo.position = Vector3.Min(max, Vector3.Max(min, qefData.MassPoint));
if (nodeInfo.position.x < min.x || nodeInfo.position.x > max.x ||
nodeInfo.position.y < min.y || nodeInfo.position.y > max.y ||
nodeInfo.position.z < min.z || nodeInfo.position.z > max.z)
{
int x = 0;
++x;
//nodeInfo.position = qefData.MassPoint;
}
{
float tolerance = 1.0f;
if (qefPosition.x - min.x < -tolerance
|| qefPosition.y - min.y < -tolerance
|| qefPosition.z - min.z < -tolerance
|| qefPosition.x - max.x > tolerance
|| qefPosition.y - max.y > tolerance
|| qefPosition.z - max.z > tolerance)
{
int x = 0;
++x;
}
}
nodeInfo.averageNormal = Vector3.Normalize(normalSum);
nodeInfo.corners = corners;
leaf.type = OctreeNode.Type.Leaf;
leaf.info = nodeInfo;
return leaf;
}
public QefData(QefData rhs)
{
set(rhs);
}
public void set(QefData rhs)
{
this.set(rhs.ata_00, rhs.ata_01, rhs.ata_02, rhs.ata_11, rhs.ata_12,
rhs.ata_22, rhs.atb_x, rhs.atb_y, rhs.atb_z, rhs.btb,
rhs.massPoint_x, rhs.massPoint_y, rhs.massPoint_z,
rhs.numPoints);
}
public QefData(QefData rhs)
{
this.set(rhs);
}
public void add(QefData rhs)
{
this.hasSolution = false;
this.data.add(rhs);
}
public QefSolver()
{
data = new QefData();
ata = new SMat3();
atb = new Vec3();
massPoint = new Vec3();
x = new Vec3();
hasSolution = false;
}
public void add(QefData rhs)
{
this.hasSolution = false;
this.data.add(rhs);
}
public QefData(QefData rhs)
{
this.set(rhs);
}
public void Add(QefData rhs)
{
hasSolution = false;
QefData.Add(rhs);
}
public void add(QefData rhs)
{
hasSolution = false;
data.add(rhs);
}
