Matrix.cpp 5.68 KB
Newer Older
1
2
3
/* -*- Mode: C++; tab-width: 8; indent-tabs-mode: nil; c-basic-offset: 2 -*- */
/* vim: set ts=8 sts=2 et sw=2 tw=80: */
/* This Source Code Form is subject to the terms of the Mozilla Public
4
5
 * License, v. 2.0. If a copy of the MPL was not distributed with this
 * file, You can obtain one at http://mozilla.org/MPL/2.0/. */
6

7
#include "Matrix.h"
8
#include "Quaternion.h"
9
#include "Tools.h"
10
#include <algorithm>
11
#include <ostream>
12
#include <math.h>
13
#include <float.h>  // for FLT_EPSILON
14

15
#include "mozilla/FloatingPoint.h"  // for UnspecifiedNaN
16

17
18
namespace mozilla {
namespace gfx {
19
20
21
22

/* Force small values to zero.  We do this to avoid having sin(360deg)
 * evaluate to a tiny but nonzero value.
 */
23
double FlushToZero(double aVal) {
24
25
26
27
28
29
30
31
32
33
34
35
  // XXX Is double precision really necessary here
  if (-FLT_EPSILON < aVal && aVal < FLT_EPSILON) {
    return 0.0f;
  } else {
    return aVal;
  }
}

/* Computes tan(aTheta).  For values of aTheta such that tan(aTheta) is
 * undefined or very large, SafeTangent returns a manageably large value
 * of the correct sign.
 */
36
double SafeTangent(double aTheta) {
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
  // XXX Is double precision really necessary here
  const double kEpsilon = 0.0001;

  /* tan(theta) = sin(theta)/cos(theta); problems arise when
   * cos(theta) is too close to zero.  Limit cos(theta) to the
   * range [-1, -epsilon] U [epsilon, 1].
   */

  double sinTheta = sin(aTheta);
  double cosTheta = cos(aTheta);

  if (cosTheta >= 0 && cosTheta < kEpsilon) {
    cosTheta = kEpsilon;
  } else if (cosTheta < 0 && cosTheta >= -kEpsilon) {
    cosTheta = -kEpsilon;
  }
  return FlushToZero(sinTheta / cosTheta);
}

56
57
template <>
Matrix Matrix::Rotation(Float aAngle) {
58
  Matrix newMatrix;
59

60
61
  Float s = sinf(aAngle);
  Float c = cosf(aAngle);
62

63
64
65
66
  newMatrix._11 = c;
  newMatrix._12 = s;
  newMatrix._21 = -s;
  newMatrix._22 = c;
67

68
69
  return newMatrix;
}
70

71
72
template <>
MatrixDouble MatrixDouble::Rotation(Double aAngle) {
73
74
75
76
77
78
79
80
81
  MatrixDouble newMatrix;

  Double s = sin(aAngle);
  Double c = cos(aAngle);

  newMatrix._11 = c;
  newMatrix._12 = s;
  newMatrix._21 = -s;
  newMatrix._22 = c;
82

83
  return newMatrix;
84
85
}

86
87
template <>
Matrix4x4 MatrixDouble::operator*(const Matrix4x4& aMatrix) const {
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
  Matrix4x4 resultMatrix;

  resultMatrix._11 = this->_11 * aMatrix._11 + this->_12 * aMatrix._21;
  resultMatrix._12 = this->_11 * aMatrix._12 + this->_12 * aMatrix._22;
  resultMatrix._13 = this->_11 * aMatrix._13 + this->_12 * aMatrix._23;
  resultMatrix._14 = this->_11 * aMatrix._14 + this->_12 * aMatrix._24;

  resultMatrix._21 = this->_21 * aMatrix._11 + this->_22 * aMatrix._21;
  resultMatrix._22 = this->_21 * aMatrix._12 + this->_22 * aMatrix._22;
  resultMatrix._23 = this->_21 * aMatrix._13 + this->_22 * aMatrix._23;
  resultMatrix._24 = this->_21 * aMatrix._14 + this->_22 * aMatrix._24;

  resultMatrix._31 = aMatrix._31;
  resultMatrix._32 = aMatrix._32;
  resultMatrix._33 = aMatrix._33;
  resultMatrix._34 = aMatrix._34;

105
106
107
108
109
110
111
112
  resultMatrix._41 =
      this->_31 * aMatrix._11 + this->_32 * aMatrix._21 + aMatrix._41;
  resultMatrix._42 =
      this->_31 * aMatrix._12 + this->_32 * aMatrix._22 + aMatrix._42;
  resultMatrix._43 =
      this->_31 * aMatrix._13 + this->_32 * aMatrix._23 + aMatrix._43;
  resultMatrix._44 =
      this->_31 * aMatrix._14 + this->_32 * aMatrix._24 + aMatrix._44;
113
114

  return resultMatrix;
115
116
}

117
118
119
120
121
122
123
124
// Intersect the polygon given by aPoints with the half space induced by
// aPlaneNormal and return the resulting polygon. The returned points are
// stored in aDestBuffer, and its meaningful subspan is returned.
template <typename F>
Span<Point4DTyped<UnknownUnits, F>> IntersectPolygon(
    Span<Point4DTyped<UnknownUnits, F>> aPoints,
    const Point4DTyped<UnknownUnits, F>& aPlaneNormal,
    Span<Point4DTyped<UnknownUnits, F>> aDestBuffer) {
125
  if (aPoints.Length() < 1 || aDestBuffer.Length() < 1) {
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
    return {};
  }

  size_t nextIndex = 0;  // aDestBuffer[nextIndex] is the next emitted point.

  // Iterate over the polygon edges. In each iteration the current edge
  // is the edge from *prevPoint to point. If the two end points lie on
  // different sides of the plane, we have an intersection. Otherwise,
  // the edge is either completely "inside" the half-space created by
  // the clipping plane, and we add curPoint, or it is completely
  // "outside", and we discard curPoint. This loop can create duplicated
  // points in the polygon.
  const auto* prevPoint = &aPoints[aPoints.Length() - 1];
  F prevDot = aPlaneNormal.DotProduct(*prevPoint);
  for (const auto& curPoint : aPoints) {
    F curDot = aPlaneNormal.DotProduct(curPoint);

    if ((curDot >= 0.0) != (prevDot >= 0.0)) {
      // An intersection with the clipping plane has been detected.
      // Interpolate to find the intersecting curPoint and emit it.
      F t = -prevDot / (curDot - prevDot);
      aDestBuffer[nextIndex++] = curPoint * t + *prevPoint * (1.0 - t);
      if (nextIndex >= aDestBuffer.Length()) {
        break;
      }
    }

    if (curDot >= 0.0) {
      // Emit any source points that are on the positive side of the
      // clipping plane.
      aDestBuffer[nextIndex++] = curPoint;
      if (nextIndex >= aDestBuffer.Length()) {
        break;
      }
    }

    prevPoint = &curPoint;
    prevDot = curDot;
  }

  return aDestBuffer.To(nextIndex);
}

template Span<Point4DTyped<UnknownUnits, Float>> IntersectPolygon(
    Span<Point4DTyped<UnknownUnits, Float>> aPoints,
    const Point4DTyped<UnknownUnits, Float>& aPlaneNormal,
    Span<Point4DTyped<UnknownUnits, Float>> aDestBuffer);
template Span<Point4DTyped<UnknownUnits, Double>> IntersectPolygon(
    Span<Point4DTyped<UnknownUnits, Double>> aPoints,
    const Point4DTyped<UnknownUnits, Double>& aPlaneNormal,
    Span<Point4DTyped<UnknownUnits, Double>> aDestBuffer);

178
179
}  // namespace gfx
}  // namespace mozilla