blob: 7c651c502b0613bef7d2c3033d8ca19a13bc8604 (
plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
|
/*
**************************************************************************
description
--------------------
copyright : (C) 2002 by Andreas Zehender
email : [email protected]
**************************************************************************
**************************************************************************
* *
* This program is free software; you can redistribute it and/or modify *
* it under the terms of the GNU General Public License as published by *
* the Free Software Foundation; either version 2 of the License, or *
* (at your option) any later version. *
* *
**************************************************************************/
#include "pmsorsegment.h"
#include "pmmath.h"
#include "pmmatrix.h"
#include "pmdebug.h"
#include <math.h>
PMVector PMSorSegment::point( double t ) const
{
t = m_t + t * m_s;
double r2 = t * ( t * ( t * m_a + m_b ) + m_c ) + m_d;
if( r2 < 0.0 )
r2 = 0.0;
return PMVector( sqrt( r2 ), t );
}
void PMSorSegment::calculateSor( const PMVector& p0, const PMVector& p1,
const PMVector& p2, const PMVector& p3 )
{
double b[4], h;
PMMatrix m;
m_t = p1[1];
m_s = p2[1] - p1[1];
if( approxZero( p2[1] - p0[1] ) || approxZero( p3[1] - p1[1] ) )
{
kdError( PMArea ) << "Incorrect points in PMSorSegment::calculateSor\n";
m_a = m_b = m_c = m_d = 0.0;
return;
}
// interpolate the points
// see povray documentation
b[0] = p1[0] * p1[0];
b[1] = p2[0] * p2[0];
b[2] = 2.0 * p1[0] * ( p2[0] - p0[0] ) / ( p2[1] - p0[1] );
b[3] = 2.0 * p2[0] * ( p3[0] - p1[0] ) / ( p3[1] - p1[1] );
h = p1[1];
m[0][0] = h * h * h;
m[0][1] = h * h;
m[0][2] = h;
m[0][3] = 1.0;
m[2][0] = 3.0 * h * h;
m[2][1] = 2.0 * h;
m[2][2] = 1.0;
m[2][3] = 0.0;
h = p2[1];
m[1][0] = h * h * h;
m[1][1] = h * h;
m[1][2] = h;
m[1][3] = 1.0;
m[3][0] = 3.0 * h * h;
m[3][1] = 2.0 * h;
m[3][2] = 1.0;
m[3][3] = 0.0;
// Calculate the coefficients
// x = M^-1 * b;
m = m.inverse( );
m_a = b[0] * m[0][0] + b[1] * m[0][1] + b[2] * m[0][2] + b[3] * m[0][3];
m_b = b[0] * m[1][0] + b[1] * m[1][1] + b[2] * m[1][2] + b[3] * m[1][3];
m_c = b[0] * m[2][0] + b[1] * m[2][1] + b[2] * m[2][2] + b[3] * m[2][3];
m_d = b[0] * m[3][0] + b[1] * m[3][1] + b[2] * m[3][2] + b[3] * m[3][3];
if( approxZero( m_a ) ) m_a = 0.0;
if( approxZero( m_b ) ) m_b = 0.0;
if( approxZero( m_c ) ) m_c = 0.0;
if( approxZero( m_d ) ) m_d = 0.0;
}
|
