#!/usr/bin/perl
use Sphere;
use Math::Vector::Real;
use Math::MatrixReal;
use Math::Trig;
use Getopt::Long;
use Statistics::Descriptive;
my ($x, $y, $z, $t);
my $L = 1;
GetOptions( "x=f" => \$x,
"y=f" => \$y,
"z=f" => \$z,
"L=i" => \$L );
# ===== Main program =====
while ( <> ) {
chomp;
my @d = split ",", $_;
my ($x,$y,$z) = ($d[3], $d[4], $d[5] );
#print "Pt $x $y $z\n";
my ($fit, $error ) = &fit_point_to_grid( $x, $y, $z, $L);
print "Error = $error\n";
print "------------------------------\n";
}
sub fit_point_to_grid {
my ($x, $y, $z, $L) = @_;
# Check to ensure unit vector
my $Q = V( $x, $y, $z);
my $len = abs( $Q );
if (abs( $len - 1 ) > 1e-4) {
print "Not a unit vector. Length=$len Scaling\n";
$x = $x/$len;
$y = $y/$len;
$z = $z/$len;
}
@px = ( $x, $y, $z);
my $points = $Sphere::iv;
# Get the icosahedral vertices:
my @X = &Sphere::triangulation_vertices( $x, $y, $z, $points );
#print "------ past search for initial icosahedral vertices\n";
my @px = ($x, $y, $z );
my @Y = &Sphere::fit_to_triangular_subdivision
(
\@px, $X[0], $X[1], $X[2], $L
);
#print "---- past fit to triangular subdivision\n";
my $fitx = V( $Y[0], $Y[1], $Y[2] );
my $givenx = V( $x, $y, $z );
$fitx = $fitx->versor;
my $error = abs( $givenx - $fitx );
return ( $fit, $error );
}
#!/usr/bin/perl
package Sphere;
use strict;
use Math::Vector::Real;
use Math::MatrixReal;
use Math::Trig;
use Getopt::Long;
use List::Util qw(max min);
use Data::Dumper;
use vars qw( $iv $phi $icoindices);
BEGIN {
use Exporter();
@Sphere::ISA = qw( Exporter );
@Sphere::EXPORT = qw( $iv $phi $icoindices);
# The icosahedron has 12 vertices and 20 triangular faces.
# We produce a grid on the sphere by barycentric division of
# the icosahedron inscribed in the sphere, which has 12 vertices
# and 20 triangular faces
$phi = ( 1 + sqrt(5) ) / 2;
$iv = [
[ 0, 1, $phi ],
[ 0, -1, $phi ],
[ 0, 1, -$phi ],
[ 0, -1, -$phi ],
[ 1, $phi, 0 ],
[ 1, -$phi, 0 ],
[ -1, $phi, 0 ],
[ -1,-$phi, 0 ],
[ $phi, 0, 1 ],
[ $phi, 0, -1 ],
[ -$phi, 0, 1 ],
[ -$phi, 0, -1] ];
for (my $i = 0; $i < 12; $i++) {
$iv->[$i]->[0] /= sqrt( 1 + $phi * $phi);
$iv->[$i]->[1] /= sqrt( 1 + $phi * $phi);
$iv->[$i]->[2] /= sqrt( 1 + $phi * $phi);
}
sub icosahedron_indices {
my $indices;
my $eps = 1e-3;
my $p = $iv;
for (my $i = 0; $i < 12; $i++) {
for ( my $j = 0; $j < 12; $j++ ) {
for ( my $k = 0; $k < 12; $k++ ) {
my $u = sqrt(1+$phi*$phi)*V( @{$p->[$i]});
my $v = sqrt(1+$phi*$phi)*V( @{$p->[$j]});
my $w = sqrt(1+$phi*$phi)*V( @{$p->[$k]});
my $d1 = abs( $u - $v );
my $d2 = abs( $v - $w );
my $d3 = abs( $w - $u );
if ( abs( $d1 - 2) < $eps &&
abs( $d2 - 2) < $eps &&
abs( $d3 - 2) < $eps &&
$i < $j && $j < $k ) {
$indices->{$i}->{$j}->{$k}++;
}
}
}
}
my $ico_indices;
foreach my $a ( sort keys %$indices ) {
foreach my $b ( sort keys %{$indices->{$a}} ) {
foreach my $c ( sort keys %{$indices->{$a}->{$b}} ) {
push @$ico_indices, [$a, $b, $c];
}
}
}
return $ico_indices;
}
$icoindices = &icosahedron_indices();
}
sub fit_to_triangular_subdivision {
my ( $x, $u, $v, $w, $L ) = @_;
my ($x1, $x2, $x3 ) = @$x;
my ($u1, $u2, $u3 ) = @$u;
my ($v1, $v2, $v3 ) = @$v;
my ($w1, $w2, $w3 ) = @$w;
my @px = &project_to_plane($x, $u, $v, $w );
# if ( ! &pntriangle3D( \@px, $u, $v, $w ) ) {
# print "In fit to triangular subdivision\n";
# print "The point is not in the triangle formed by the vertices\n";
# return ($u1, $u2, $u3, $v1, $v2, $v3, $w1, $w2, $w3);
# } else {
# }
# Generate the subdivision to level L
my @newpx = ( $px[0], $px[1], $px[2] );
my @nu = ( $u1, $u2, $u3 );
my @nv = ( $v1, $v2, $v3 );
my @nw = ( $w1, $w2, $w3 );
for ( my $i = 0; $i < $L; $i++ ) {
my @X = &refine_triangular_subdivision( \@newpx, \@nu, \@nv, \@nw );
@nu = ( $X[0], $X[1], $X[2] );
@nv = ( $X[3], $X[4], $X[5] );
@nw = ( $X[6], $X[7], $X[8] );
}
# Now we can set x to the closest grid point, which will be nu
# Let us record the distance as well
my $error = abs( V($x1,$x2,$x3) - V($nu[0], $nu[1], $nu[2] ));
return ( $nu[0], $nu[1], $nu[2], $error );
}
# This is code for producing barycentric subdivision one level deeper
# and will give coordinates of the triangular corners where the point
# x falls
sub refine_triangular_subdivision {
my ( $x, $u, $v, $w, $L ) = @_;
my ($x1, $x2, $x3 ) = @$x;
my ($u1, $u2, $u3 ) = @$u;
my ($v1, $v2, $v3 ) = @$v;
my ($w1, $w2, $w3 ) = @$w;
# if ( ! &pntriangle3D( $x, $u, $v, $w ) ) {
# print "In refine triangular subdivision\n";
# print "The point is not in the triangle formed by the vertices\n";
# return ($u1, $u2, $u3, $v1, $v2, $v3, $w1, $w2, $w3);
# }
my $p = [ [ $u1, $u2, $u3],
[ $v1, $v2, $v3],
[ $w1, $w2, $w3],
[ ($u1 + $v1)/2, ($u2+$v2)/2, ($u3+$v3)/2 ],
[ ($w1 + $v1)/2, ($w2+$v2)/2, ($w3+$v3)/2 ],
[ ($w1 + $u1)/2, ($w2+$u2)/2, ($w3+$u3)/2 ],
];
# my @ix = &vertex_indices_triangulation( $x1, $x2, $x3, $points );
# return (
# $ix[0]->[0], $ix[0]->[1], $ix[0]->[2],
# $ix[1]->[0], $ix[1]->[1], $ix[1]->[2],
# $ix[2]->[0], $ix[2]->[1], $ix[2]->[2],
# );
if ( &pntriangle3D( $x, $p->[0], $p->[3], $p->[5] )) {
return ( $p->[0]->[0], $p->[0]->[1], $p->[0]->[2],
$p->[3]->[0], $p->[3]->[1], $p->[3]->[2],
$p->[5]->[0], $p->[5]->[1], $p->[5]->[2] ) ;
}
if ( &pntriangle3D( $x, $p->[1], $p->[3], $p->[4] )) {
return ( $p->[1]->[0], $p->[1]->[1], $p->[1]->[2],
$p->[3]->[0], $p->[3]->[1], $p->[3]->[2],
$p->[4]->[0], $p->[4]->[1], $p->[4]->[2] ) ;
}
if ( &pntriangle3D( $x, $p->[2], $p->[4], $p->[5] )) {
return ( $p->[2]->[0], $p->[2]->[1], $p->[2]->[2],
$p->[4]->[0], $p->[4]->[1], $p->[4]->[2],
$p->[5]->[0], $p->[5]->[1], $p->[5]->[2] ) ;
}
if ( &pntriangle3D( $x, $p->[3], $p->[4], $p->[5] )) {
return ( $p->[3]->[0], $p->[3]->[1], $p->[3]->[2],
$p->[4]->[0], $p->[4]->[1], $p->[4]->[2],
$p->[5]->[0], $p->[5]->[1], $p->[5]->[2] ) ;
}
my @ix = &triangulation_vertices_old( $x1, $x2, $x3, $p );
return (
$ix[0]->[0], $ix[0]->[1], $ix[0]->[2],
$ix[1]->[0], $ix[1]->[1], $ix[1]->[2],
$ix[2]->[0], $ix[2]->[1], $ix[2]->[2],
);
}
# Given an x on a 2-sphere, we want to be able to project it
# to the plane determined by three points. If the three points are u, v,w
# then this problem can be solved by translating by v
# so that the points are x-v, u-v, 0, w-v, projecting x-v to the
# plane determined by u-v and w-v and then translating the result P(x-v) back
# by adding v.
sub project_to_plane {
my ( $x, $u, $v, $w ) = @_;
my ($x1, $x2, $x3 ) = @$x;
my ($u1, $u2, $u3 ) = @$u;
my ($v1, $v2, $v3 ) = @$v;
my ($w1, $w2, $w3 ) = @$w;
# Translate to origin by subtracting v, the rotate so u-v, w-v are in xy
# plane, project to xy plane, reverse rotation add v
my $px = Math::MatrixReal->new_from_cols(
[ [ $x1 - $v1, $x2 - $v2, $x3 - $v3 ] ]);
my $v = Math::MatrixReal->new_from_cols(
[ [ $v1, $v2, $v3 ] ]);
my $up = V( $u1 - $v1, $u2 - $v2, $u3 - $v3 );
my $wp = V( $w1 - $v1, $w2 - $v2, $w3 - $v3 );
my $cuw = $up x $wp;
if (abs( $cuw) > 0) {
$cuw = $cuw->versor;
} else {
return ( $x->[0], $x->[1], $x->[2] );
}
my $zaxis = V( 0, 0, 1);
my $angle = acos( $cuw * $zaxis );
my $axis = $zaxis x $cuw;
if ( abs( $axis ) < 1e-6 ) {
$axis = $zaxis;
} else {
$axis = $axis->versor;
}
# We shall create a rotation matrix that transforms cuw to zaxis
# so that the images under the rotation of up and wp can be used
# in the planar triangle containment code
my $R = &rotation_matrix( $axis->[0], $axis->[1], $axis->[2], $angle);
my $Ri = $R->inverse;
my $Rp = $R->multiply( $px );
my $upm = Math::MatrixReal->new_from_cols(
[ [ $up->[0], $up->[1], $up->[2] ] ]);
my $wpm = Math::MatrixReal->new_from_cols(
[ [ $wp->[0], $wp->[1], $wp->[2] ] ]);
my $image_up = $R->multiply( $upm );
my $image_wp = $R->multiply( $wpm );
my $Rp_proj = Math::MatrixReal->new_from_cols(
[ [ $Rp->element(1,1), $Rp->element(2,1), 0 ] ] );
my $ppx = $Ri->multiply( $Rp_proj ) + $v;
return ( $ppx->element(1,1), $ppx->element(2,1), $ppx->element(3,1) );
}
# It is useful have the vector to translate the plane determined
# by three vectors to the origin by a vector orthogonal to the plane
sub orthogonal_translate {
my ( $u1, $u2, $u3, $v1, $v2, $v3, $w1, $w2, $w3 ) = @_;
my $v = V( $v1, $v2, $v3 );
my $tu = V( $u1-$v1, $u2 - $v2, $u3 - $v3 );
my $tw = V( $w1-$v1, $w2 - $v2, $w3 - $v3 );
# Subtract off the component parallel to the plane
my $normal = $tu x $tw;
$normal = $normal->versor;
my $tv = ( $v * $normal ) * $normal;
return ( $tv->[0], $tv->[1], $tv->[2] );
}
# The following function will take a unit vector on the 2-sphere
# and return the indices of the icosahedral vertices which bound the point
# on the sphere. It should be generalized to find the vertices bounding
# the point in a general triangulation
sub triangulation_vertices {
my ($x, $y, $z , $points) = @_;
&check_points( $points );
my @pv = ( $x, $y, $z);
my @I = @$Sphere::icoindices;
foreach my $t ( @I ) {
my @ti = @$t;
#print join(" ", @ti) . "\n";
my @u = @{$points->[$ti[0]]};
my @v = @{$points->[$ti[1]]};
my @w = @{$points->[$ti[2]]};
my @px = &project_to_plane( \@pv,
\@u,
\@v,
\@w );
if ( &pntriangle3D( \@px,
$points->[$t->[0]],
$points->[$t->[1]],
$points->[$t->[2]] )) {
return (
$points->[$t->[0]],
$points->[$t->[1]],
$points->[$t->[2]] );
}
}
print "triangulation_vertices: point not in triangle\n";
return ( $points->[0], $points->[1], $points->[2] );
}
sub triangulation_vertices_old {
my ($x, $y, $z , $points) = @_;
&check_points( $points );
my $p = V( $x, $y, $z);
sub distance {
my $qq = shift;
my $q = V( $qq->[0], $qq->[1], $qq->[2] );
# Use angular distance assuming $p is a unit vector
# return acos( $p * $q / (abs($q)*abs($p)));
return abs( $p - $q );
}
sub bydistance {
&distance($a) <=> &distance($b);
}
my @X = sort bydistance @$points;
my $N = @$points + 0;
my @pv = ($x, $y, $z);
my @px = &project_to_plane( \@pv, $X[0],$X[1],$X[2] );
if ( &pntriangle3D( \@pv, $X[0], $X[1], $X[2] ) ) {
return ( $X[0], $X[1], $X[2] );
} else {
=pod
print "triangulation_vertices: point not in triangle\n";
print "Projection:\n";
print join( " ", @px ) . "\n";
print "Vertices:\n";
print join( " ", @{$X[0]}) . "\n";
print join( " ", @{$X[1]}) . "\n";
print join( " ", @{$X[2]}) . "\n";
=cut
}
$p = V( $px[0], $px[1], $px[2] );
@X = sort bydistance @$points;
return ( $X[0], $X[1], $X[2] );
}
sub rotation_matrix {
my ( $x, $y, $z, $t ) = @_;
my $c = cos($t);
my $s = sin($t);
my $C = 1-$c;
my $Q = Math::MatrixReal->new_from_rows(
[
[ $x * $x * $C + $c, $x * $y * $C - $z*$s, $x*$z*$C + $y*$s],
[ $y * $x * $C + $z * $s, $y * $y * $C + $c, $y * $z * $C - $x * $s],
[ $z * $x * $C - $y * $s, $z * $y * $C + $x * $s, $z * $z * $C + $c]
]);
return $Q;
}
sub pntriangle3D {
my ( $x, $u, $v, $w ) = @_;
my ($x1, $x2, $x3 ) = @$x;
my ($u1, $u2, $u3 ) = @$u;
my ($v1, $v2, $v3 ) = @$v;
my ($w1, $w2, $w3 ) = @$w;
my @proj = &project_to_plane( $x, $u, $v, $w );
# my $px = Math::MatrixReal->new_from_cols(
# [ [ $proj[0]-$v1, $proj[1]-$v2, $proj[2]-$v3 ] ]);
my $px = Math::MatrixReal->new_from_cols(
[ [ $x1-$v1, $x2-$v2, $x3-$v3 ] ]);
my $up = V( $u1 - $v1, $u2 - $v2, $u3 - $v3 );
my $wp = V( $w1 - $v1, $w2 - $v2, $w3 - $v3 );
my $cuw = $up x $wp;
if ( abs( $cuw ) > 0 ) {
$cuw = $cuw->versor;
} else {
return 0;
}
my $zaxis = V( 0, 0, 1);
my $angle = acos( $cuw * $zaxis );
my $axis = $zaxis x $cuw;
if ( abs($axis) < 1e-5 ) {
$axis = $zaxis;
} else {
$axis = $axis->versor;
}
# We shall create a rotation matrix that transforms cuw to zaxis
# so that the images under the rotation of up and wp can be used
# in the planar triangle containment code
my $R = &rotation_matrix( $axis->[0], $axis->[1], $axis->[2], $angle);
my $Rp = $R->multiply( $px );
my $upm = Math::MatrixReal->new_from_cols(
[ [ $up->[0], $up->[1], $up->[2] ] ]);
my $wpm = Math::MatrixReal->new_from_cols(
[ [ $wp->[0], $wp->[1], $wp->[2] ] ]);
my $image_up = $R->multiply( $upm );
my $image_wp = $R->multiply( $wpm );
my @vertx = ( 0, $image_up->element(1,1), $image_wp->element(1,1) );
my @verty = ( 0, $image_up->element(2,1), $image_wp->element(2,1) );
=pod
print "In pntriangle3D:\n";
print "----------------\n";
print $Rp;
print "\n";
print $image_up;
print "\n";
print $image_wp;
print "\n";
=cut
return &pnpoly( 3, \@vertx, \@verty, $Rp->element( 1, 1),
$Rp->element(2, 1));
}
# This is simply a wrapper for point_in_polygon
#
sub pnpoly {
my ($nvert, $x, $y, $tx, $ty ) = @_;
# HACK: just create a triangle
my @polygon = (
[ [ $x->[0], $y->[0] ], [ $x->[1], $y->[1] ] ],
[ [ $x->[1], $y->[1] ], [ $x->[2], $y->[2] ] ],
[ [ $x->[2], $y->[2] ], [ $x->[0], $y->[0] ] ],
);
my @point = ( $tx, $ty );
return &point_in_polygon( \@point, \@polygon );
}
#
# point in polygon -- tested
#
sub point_in_polygon
{
my ($point, $polygon ) = @_;
my $count = 0;
foreach my $side ( @$polygon ) {
$count++ if &ray_intersect_segment( $point, $side );
}
return ( $count % 2 == 0 ) ? 0:1;
}
my $eps = 1e-6;
my $inf = 1e600;
sub ray_intersect_segment
{
my ( $point, $segment ) = @_;
my ( $A, $B ) = @$segment;
my @P = @$point;
($A, $B) = ( $B, $A) if ($A->[1] > $B->[1]);
$P[1] += $eps if ( $P[1] == $A->[1] ) || ( $P[1] == $B->[1]);
return 0 if ( $P[1] < $A->[1]) || ( $P[1] > $B->[1] ) || ($P[0] > max( $A->[0], $B->[1]));
return 1 if ( $P[0] < min ($A->[0],$B->[0]));
my $m_red = ($A->[0] != $B->[0]) ? ( $B->[1] - $A->[1] ) / ( $B->[0] - $A->[0] ) : $inf;
my $m_blue = ( $A->[0] != $P[0] ) ? ( $P[1] - $A->[1])/( $P[0] - $A->[0]) : $inf;
return ($m_blue >= $m_red)? 1:0;
}
sub check_points {
my $p = shift;
my $N = @$p+0;
for (my $i = 0; $i < $N; $i++) {
if ( ! $p->[$i]->[0] ) {
$p->[$i]->[0] = 0;
}
if ( ! $p->[$i]->[1] ) {
$p->[$i]->[1] = 0;
}
if ( ! $p->[$i]->[2] ) {
$p->[$i]->[2] = 0;
}
}
}
1;

Read Full Post »