// discover all points using the bresenham_step
// RETURNS: number of points actually found
static inline int find_points(KZ_Point p1, KZ_Point p2, KZ_Point *points) {
KZ_Point greatP = p2;
KZ_Point littleP = p1;
order_endpoints(&littleP, &greatP);
int dx = greatP.x - littleP.x;
int dy = greatP.y > littleP.y ? greatP.y - littleP.y : littleP.y - greatP.y;
// y goes up if littleP.y is smaller than greatP.y, else it goes down
int ystep = littleP.y < greatP.y ? 1 : -1;
KZ_Point p = littleP;
double pcount = point_count(p1, p2);
double radstep = (greatP.r - littleP.r) / pcount;
double rstep = (greatP.kr - littleP.kr) / pcount;
double gstep = (greatP.kg - littleP.kg) / pcount;
double bstep = (greatP.kb - littleP.kb) / pcount;
if (pcount == 0)
radstep = rstep = gstep = bstep = 0;
int i = 0;
if (dx > dy) {
int acc = dx/2;
while (p.x <= greatP.x) {
points[i] = p;
bresenham_step(&acc, &p.x, &p.y, dx, dy, 1, ystep);
p.kr += rstep;
p.kg += gstep;
p.kb += bstep;
p.r += radstep;
p.ared = p.kr * ambient_red;
p.agreen = p.kg * ambient_green;
p.ablue = p.kb * ambient_blue;
i++;
}
} else {
int acc = dy/2;
char up = littleP.y < greatP.y;
while (i < pcount) {//(up ? p.y <= greatP.y : p.y >= greatP.y) {
points[i] = p;
bresenham_step(&acc, &p.y, &p.x, dy, dx, ystep, 1);
p.kr += rstep;
p.kg += gstep;
p.kb += bstep;
p.r += radstep;
p.ared = p.kr * ambient_red;
p.agreen = p.kg * ambient_green;
p.ablue = p.kb * ambient_blue;
i++;
}
}
return i;
}
static void elcount( int *npnts, int *npols )
{
int i, apts, apls, bpts, bpls, ppts, ppls;
getseq();
for ( i = 0, bpls = 0; i < seqlen; i++ )
bpls += bond_count( seq[ i ] );
if ( bond_type == BTYPE_LINE ) {
for ( i = 0, bpts = 0; i < seqlen; i++ )
bpts += point_count( seq[ i ] ) - 4;
bpts += 2;
}
else {
bpts = bpls * bond_nsides * ( 1 + bond_nsegments );
bpls *= bond_nsides * bond_nsegments + 2;
}
for ( i = 0, apls = 0; i < seqlen; i++ )
apls += atom_count( seq[ i ] );
if ( atom_type == ATYPE_POINT )
apts = ( bond_type == BTYPE_LINE ) ? 0 : apls;
else if ( atom_type == ATYPE_SPHERE ) {
apts = apls * ( atom_nsides * ( atom_nsegments - 1 ) + 2 );
apls *= atom_nsides * atom_nsegments;
}
else {
apts = 20 * apls;
apls *= 12;
}
if ( do_plates ) {
ppts = ( atom_type == ATYPE_POINT || bond_type == BTYPE_LINE ) ?
0 : 15 * seqlen;
ppls = 2 * seqlen;
}
else ppts = ppls = 0;
if ( npnts ) *npnts = apts + bpts + ppts;
if ( npols ) *npols = apls + bpls + ppls;
}
UInt32 RigidBodyShapeCollection::ComputeMeanShape(RigidBodyShape& mean, double distribution_tolerance, double convergence_accuracy) const
{
// minimal set of visiblity codes
RigidBodyVisibilityCanon canon;
ComputeVisiblityCanon(canon);
// re-order codes to ensure maximal ones go first
RigidBodyConnectedVisibilityCanon connected_canon;
UInt32 canon_result = canon.ComputeConnectedVisibilityCanon(connected_canon, distribution_tolerance);
// must be a complete covering of all points to work
if (canon_result != RigidBodyResult::success)
return canon_result;
// initialise shape
connected_canon.ComputeApproxMeanShape(mean);
// number of points
size_t num_markers = connected_canon.Shape(0).NumMarkers();
// zero vector to init markers
Vector3 zero(0.0);
// transform every shape to this one and update mean
// double previous_residual_rms = DBL_MAX;
for (size_t opt_count = 0; opt_count < 100; opt_count++)
{
// transform every shape and sum
std::vector<Vector3> point_sum(num_markers, zero);
std::vector<size_t> point_count(num_markers, 0);
// double dx2_sum = 0.0;
// size_t dx2_count = 0;
for (std::vector<RigidBodyShape>::const_iterator iter_shape( shape.begin() ); iter_shape != shape.end(); iter_shape++)
{
RigidTransform3 T;
iter_shape->ComputeFitTo(T, mean);
for (size_t marker_index = 0; marker_index < num_markers; marker_index++)
{
const RigidBodyMarker& x = iter_shape->Marker(marker_index);
if (x.visible)
{
// transform point to be nearest to mean as we can
Vector3 Tx;
RigidTransform3::MulVec(Tx, T, x.position);
point_sum[marker_index] += Tx;
point_count[marker_index]++;
// form sum residuals
// Vector3 dx;
// Vector3::Sub(dx, Tx, x.position);
// dx2_sum += dx.Modulus2();
// dx2_count++;
}
}
}
// update mean and compute max difference with previous
double max_difference = 0.0;
for (size_t marker_index = 0; marker_index < num_markers; marker_index++)
{
size_t n = point_count[marker_index];
if (n)
{
// compute updated marker position
Vector3& new_position = point_sum[marker_index];
new_position /= n;
// compute maximum coordinate difference with previous
RigidBodyMarker& m = mean.Marker(marker_index);
for (size_t dimension = 0; dimension < 3; dimension++)
{
double d = fabs(new_position[dimension] - m.position[dimension]);
if (d > max_difference)
max_difference = d;
}
// set new values
m.position = new_position;
}
}
// verify residuals as stopping criteria
// double residual_rms = sqrt( dx2_sum / dx2_count );
if (max_difference < convergence_accuracy)
{
// fprintf(stderr, "Iterations used: %u\n", opt_count);
return RigidBodyResult::success;
}
// previous_residual_rms = residual_rms;
}
return RigidBodyResult::did_not_converge;
}
static void meshedit( void )
{
double pt[ 3 ];
LWPntID id[ 57 ], vid[ 2 ];
int i, j, k, n, v[ 9 ], snum;
csMeshBegin( 0, 0, OPSEL_GLOBAL );
if ( bond_type == BTYPE_LINE ) {
mgMonitorBegin( "Line Bonds", NULL, seqlen );
for ( j = 0; j < seqlen; j++ ) {
n = point_count( seq[ j ] );
for ( i = 0; i < n; i++ ) {
vert_coords( seq[ j ], i, pt );
transform_point( j * 3.4, j * 36.0, pt );
id[ i ] = meAddPoint( pt );
}
n = bond_count( seq[ j ] );
for ( i = 0; i < n; i++ ) {
bond_info( seq[ j ], i, &v[ 0 ], &v[ 1 ], &snum );
vid[ 0 ] = id[ v[ 0 ]];
vid[ 1 ] = id[ v[ 1 ]];
meAddPoly( LWPOLTYPE_FACE, NULL, surface_name( snum ), 2, vid );
}
if ( userabort = mgMonitorStep( 1 ))
break;
}
mgMonitorDone();
}
if ( atom_type == ATYPE_POINT && !userabort ) {
mgMonitorBegin( "Point Atoms", NULL, seqlen );
for ( j = 0; j < seqlen; j++ ) {
n = atom_count( seq[ j ] );
for ( i = 0; i < n; i++ ) {
atom_info( seq[ j ], i, &v[ 0 ], &snum );
vert_coords( seq[ j ], v[ 0 ], pt );
transform_point( j * 3.4, j * 36.0, pt );
id[ 0 ] = meAddPoint( pt );
meAddPoly( LWPOLTYPE_FACE, NULL, surface_name( snum ), 1, &id[ 0 ] );
}
if ( userabort = mgMonitorStep( 1 ))
break;
}
mgMonitorDone();
}
else if ( atom_type == ATYPE_DODEC && !userabort ) {
mgMonitorBegin( "Dodecahedron Atoms", NULL, seqlen );
for ( j = 0; j < seqlen; j++ ) {
n = atom_count( seq[ j ] );
for ( i = 0; i < n; i++ ) {
atom_info( seq[ j ], i, &v[ 0 ], &snum );
vert_coords( seq[ j ], v[ 0 ], pt );
transform_point( j * 3.4, j * 36.0, pt );
dodec( pt, atom_radius[ snum ], surface_name( snum ));
}
if ( userabort = mgMonitorStep( 1 ))
break;
}
mgMonitorDone();
}
if ( do_plates && !userabort ) {
mgMonitorBegin( "Base Plates", NULL, seqlen );
for ( j = 0; j < seqlen; j++ ) {
for ( i = 0; i < 2; i++ ) {
plate_info( seq[ j ], i, &n, v, &snum );
for ( k = 0; k < n; k++ ) {
vert_coords( seq[ j ], v[ k ], pt );
transform_point( j * 3.4, j * 36.0, pt );
id[ k ] = meAddPoint( pt );
}
meAddPoly( LWPOLTYPE_FACE, NULL, surface_name( snum ), n, id );
}
if ( userabort = mgMonitorStep( 1 ))
break;
}
mgMonitorDone();
}
csMeshDone( EDERR_NONE, 0 );
}
// takes an array of six coordinates alternating x y z
void draw_triangle(KZ_Point a, KZ_Point b, KZ_Point c) {
int ab_count = point_count(a, b);
KZ_Point *ab_points = malloc((ab_count + 1) * sizeof(KZ_Point));
if (ab_points == NULL)
printf("ab broken\n");
ab_count = find_points(a, b, ab_points);
int bc_count = point_count(b, c);
KZ_Point *bc_points = malloc((bc_count + 1) * sizeof(KZ_Point));
if (bc_points == NULL)
printf("bc broken\n");
bc_count = find_points(b, c, bc_points);
int ca_count = point_count(c, a);
KZ_Point *ca_points = malloc((ca_count + 1) * sizeof(KZ_Point));
if (ca_points == NULL)
printf("ca broken\n");
ca_count = find_points(c, a, ca_points);
#if DRAW_FILL
// order arrays by y descending
KZ_Point *abs;
int abinc;
if (ab_points[ab_count - 1].y > ab_points[0].y) {
abs = ab_points + ab_count - 1;
abinc = -1;
} else {
abs = ab_points;
abinc = 1;
}
KZ_Point *bcs;
int bcinc;
if (bc_points[bc_count - 1].y > bc_points[0].y) {
bcs = bc_points + bc_count - 1;
bcinc = -1;
} else {
bcs = bc_points;
bcinc = 1;
}
KZ_Point *cas;
int cainc;
if (ca_points[ca_count - 1].y > ca_points[0].y) {
cas = ca_points + ca_count - 1;
cainc = -1;
} else {
cas = ca_points;
cainc = 1;
}
// find the mid y value
int max_y = a.y > b.y ? a.y:b.y;
max_y = c.y > max_y ? c.y:max_y;
int mid_y;
if (max_y == a.y) {
mid_y = b.y > c.y ? b.y : c.y;
} else if (max_y == b.y) {
mid_y = a.y > c.y ? a.y : c.y;
} else if (max_y == c.y) {
mid_y = b.y > a.y ? b.y : a.y;
} else {
printf("mid_y can't be found\n");
return;
}
//categorize lines by upper, lower, long
KZ_Point *upper_segment = NULL;
int upper_inc = 0;
int upper_count = 0;
KZ_Point *lower_segment = NULL;
int lower_inc = 0;
int lower_count = 0;
KZ_Point *long_segment = NULL;
int long_inc = 0;
int long_count = 0;
if (mid_y == a.y) {
long_inc = bcinc;
long_segment = bcs;
long_count = bc_count;
if (max_y == b.y) {
upper_inc = abinc;
upper_segment = abs;
upper_count = ab_count;
lower_inc = cainc;
lower_segment = cas;
lower_count = ca_count;
} else if (max_y == c.y) {
upper_inc = cainc;
upper_segment = cas;
upper_count = ca_count;
lower_inc = abinc;
lower_segment = abs;
lower_count = ab_count;
}
} else if (mid_y == b.y) {
long_inc = cainc;
long_segment = cas;
long_count = ca_count;
if (max_y == a.y) {
upper_inc = abinc;
upper_segment = abs;
upper_count = ab_count;
lower_inc = bcinc;
lower_segment = bcs;
lower_count = bc_count;
} else if (max_y == c.y) {
upper_inc = bcinc;
upper_segment = bcs;
upper_count = bc_count;
lower_inc = abinc;
lower_segment = abs;
lower_count = ab_count;
}
} else if (mid_y == c.y) {
long_inc = abinc;
long_segment = abs;
long_count = ab_count;
if (max_y == a.y) {
upper_inc = cainc;
upper_segment = cas;
upper_count = ca_count;
lower_inc = bcinc;
lower_segment = bcs;
lower_count = bc_count;
} else if (max_y == b.y) {
upper_inc = bcinc;
upper_segment = bcs;
upper_count = bc_count;
lower_inc = cainc;
lower_segment = cas;
lower_count = ca_count;
}
}
int longi = 0, shorti = 0;
while (shorti * upper_inc < upper_count
&& upper_segment[shorti + upper_inc].y == upper_segment[shorti].y) {
shorti += upper_inc;
}
while (longi * long_inc < long_count
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| long_segment[longi].y != upper_segment[shorti].y)) {
longi += long_inc;
}
while (shorti * upper_inc < upper_count && longi * long_inc < long_count) {
draw_horizontal(upper_segment[shorti], long_segment[longi]);
do
shorti += upper_inc;
while (shorti * upper_inc < upper_count - 1
&& upper_segment[shorti + upper_inc].y == upper_segment[shorti].y);
do
longi += long_inc;
while (longi * long_inc < long_count - 1
&& long_segment[longi + long_inc].y >= mid_y
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| (shorti * upper_inc < upper_count
&& long_segment[longi].y != upper_segment[shorti].y)
|| (shorti * upper_inc >= upper_count
&& long_segment[longi].y > mid_y)));
}
shorti = 0;
int stopper = long_segment[longi].y;
while (shorti * lower_inc < lower_count - 1
&& (lower_segment[shorti].y > stopper
|| lower_segment[shorti + lower_inc].y == lower_segment[shorti].y)) {
shorti += lower_inc;
}
while (shorti * lower_inc < lower_count && longi * long_inc < long_count) {
draw_horizontal(lower_segment[shorti], long_segment[longi]);
do
shorti += lower_inc;
while (shorti * lower_inc < lower_count
&& lower_segment[shorti + lower_inc].y == lower_segment[shorti].y);
do
longi += long_inc;
while (longi * long_inc < long_count
&& (long_segment[longi + long_inc].y == long_segment[longi].y
|| (shorti * lower_inc < lower_count
&& long_segment[longi].y != lower_segment[shorti].y)
|| (shorti * lower_inc >= lower_count
&& long_segment[longi].y > mid_y)));
}
#endif
#if DRAW_EDGES
int p;
for (p = 0; p < ab_count; p++) {
ab_points[p].r = -1;
ab_points[p].kr = EDGE_RED;
ab_points[p].kg = EDGE_GREEN;
ab_points[p].kb = EDGE_BLUE;
consider_KZ_Point(ab_points[p]);
}
for (p = 0; p < bc_count; p++) {
bc_points[p].r = -1;
bc_points[p].kr = EDGE_RED;
bc_points[p].kg = EDGE_GREEN;
bc_points[p].kb = EDGE_BLUE;
consider_KZ_Point(bc_points[p]);
}
for (p = 0; p < ca_count; p++) {
ca_points[p].r = -1;
ca_points[p].kr = EDGE_RED;
ca_points[p].kg = EDGE_GREEN;
ca_points[p].kb = EDGE_BLUE;
consider_KZ_Point(ca_points[p]);
}
#endif
#if DRAW_VERTICES
a.r = b.r = c.r = -1;
a.kr = b.kr = c.kr = VERTICES_RED;
a.kg = b.kg = c.kg = VERTICES_BLUE;
a.kb = b.kb = c.kb = VERTICES_GREEN;
if (pix_in_screen(a.x, a.y))
consider_KZ_Point(a);
if (pix_in_screen(b.x, b.y))
consider_KZ_Point(b);
if (pix_in_screen(c.x, c.y))
consider_KZ_Point(c);
#endif
free(ab_points);
free(bc_points);
free(ca_points);
}
