#
root/OpenSceneGraph/trunk/src/osgPlugins/3ds/lib3ds/lib3ds_quat.c
@
10853

Revision 10853, 6.2 kB (checked in by robert, 7 years ago) |
---|

Rev | Line | |
---|---|---|

[10853] | 1 | /* |

2 | Copyright (C) 1996-2008 by Jan Eric Kyprianidis <www.kyprianidis.com> | |

3 | All rights reserved. | |

4 | | |

5 | This program is free software: you can redistribute it and/or modify | |

6 | it under the terms of the GNU Lesser General Public License as published | |

7 | by the Free Software Foundation, either version 2.1 of the License, or | |

8 | (at your option) any later version. | |

9 | ||

10 | Thisprogram is distributed in the hope that it will be useful, | |

11 | but WITHOUT ANY WARRANTY; without even the implied warranty of | |

12 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |

13 | GNU Lesser General Public License for more details. | |

14 | | |

15 | You should have received a copy of the GNU Lesser General Public License | |

16 | along with this program; If not, see <http://www.gnu.org/licenses/>. | |

17 | */ | |

18 | #include "lib3ds_impl.h" | |

19 | ||

20 | ||

21 | /*! | |

22 | * Set a quaternion to Identity | |

23 | */ | |

24 | void | |

25 | lib3ds_quat_identity(float c[4]) { | |

26 | c[0] = c[1] = c[2] = 0.0f; | |

27 | c[3] = 1.0f; | |

28 | } | |

29 | ||

30 | ||

31 | /*! | |

32 | * Copy a quaternion. | |

33 | */ | |

34 | void | |

35 | lib3ds_quat_copy(float dest[4], float src[4]) { | |

36 | int i; | |

37 | for (i = 0; i < 4; ++i) { | |

38 | dest[i] = src[i]; | |

39 | } | |

40 | } | |

41 | ||

42 | ||

43 | /*! | |

44 | * Compute a quaternion from axis and angle. | |

45 | * | |

46 | * \param c Computed quaternion | |

47 | * \param axis Rotation axis | |

48 | * \param angle Angle of rotation, radians. | |

49 | */ | |

50 | void | |

51 | lib3ds_quat_axis_angle(float c[4], float axis[3], float angle) { | |

52 | double omega, s; | |

53 | double l; | |

54 | ||

55 | l = sqrt(axis[0] * axis[0] + axis[1] * axis[1] + axis[2] * axis[2]); | |

56 | if (l < LIB3DS_EPSILON) { | |

57 | c[0] = c[1] = c[2] = 0.0f; | |

58 | c[3] = 1.0f; | |

59 | } else { | |

60 | omega = -0.5 * angle; | |

61 | s = sin(omega) / l; | |

62 | c[0] = (float)s * axis[0]; | |

63 | c[1] = (float)s * axis[1]; | |

64 | c[2] = (float)s * axis[2]; | |

65 | c[3] = (float)cos(omega); | |

66 | } | |

67 | } | |

68 | ||

69 | ||

70 | /*! | |

71 | * Negate a quaternion | |

72 | */ | |

73 | void | |

74 | lib3ds_quat_neg(float c[4]) { | |

75 | int i; | |

76 | for (i = 0; i < 4; ++i) { | |

77 | c[i] = -c[i]; | |

78 | } | |

79 | } | |

80 | ||

81 | ||

82 | /*! | |

83 | * Compute the conjugate of a quaternion | |

84 | */ | |

85 | void | |

86 | lib3ds_quat_cnj(float c[4]) { | |

87 | int i; | |

88 | for (i = 0; i < 3; ++i) { | |

89 | c[i] = -c[i]; | |

90 | } | |

91 | } | |

92 | ||

93 | ||

94 | /*! | |

95 | * Multiply two quaternions. | |

96 | * | |

97 | * \param c Result | |

98 | * \param a,b Inputs | |

99 | */ | |

100 | void | |

101 | lib3ds_quat_mul(float c[4], float a[4], float b[4]) { | |

102 | float qa[4], qb[4]; | |

103 | lib3ds_quat_copy(qa, a); | |

104 | lib3ds_quat_copy(qb, b); | |

105 | c[0] = qa[3] * qb[0] + qa[0] * qb[3] + qa[1] * qb[2] - qa[2] * qb[1]; | |

106 | c[1] = qa[3] * qb[1] + qa[1] * qb[3] + qa[2] * qb[0] - qa[0] * qb[2]; | |

107 | c[2] = qa[3] * qb[2] + qa[2] * qb[3] + qa[0] * qb[1] - qa[1] * qb[0]; | |

108 | c[3] = qa[3] * qb[3] - qa[0] * qb[0] - qa[1] * qb[1] - qa[2] * qb[2]; | |

109 | } | |

110 | ||

111 | ||

112 | /*! | |

113 | * Multiply a quaternion by a scalar. | |

114 | */ | |

115 | void | |

116 | lib3ds_quat_scalar(float c[4], float k) { | |

117 | int i; | |

118 | for (i = 0; i < 4; ++i) { | |

119 | c[i] *= k; | |

120 | } | |

121 | } | |

122 | ||

123 | ||

124 | /*! | |

125 | * Normalize a quaternion. | |

126 | */ | |

127 | void | |

128 | lib3ds_quat_normalize(float c[4]) { | |

129 | double l, m; | |

130 | ||

131 | l = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2] + c[3] * c[3]); | |

132 | if (fabs(l) < LIB3DS_EPSILON) { | |

133 | c[0] = c[1] = c[2] = 0.0f; | |

134 | c[3] = 1.0f; | |

135 | } else { | |

136 | int i; | |

137 | m = 1.0f / l; | |

138 | for (i = 0; i < 4; ++i) { | |

139 | c[i] = (float)(c[i] * m); | |

140 | } | |

141 | } | |

142 | } | |

143 | ||

144 | ||

145 | /*! | |

146 | * Compute the inverse of a quaternion. | |

147 | */ | |

148 | void | |

149 | lib3ds_quat_inv(float c[4]) { | |

150 | double l, m; | |

151 | ||

152 | l = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2] + c[3] * c[3]); | |

153 | if (fabs(l) < LIB3DS_EPSILON) { | |

154 | c[0] = c[1] = c[2] = 0.0f; | |

155 | c[3] = 1.0f; | |

156 | } else { | |

157 | m = 1.0f / l; | |

158 | c[0] = (float)(-c[0] * m); | |

159 | c[1] = (float)(-c[1] * m); | |

160 | c[2] = (float)(-c[2] * m); | |

161 | c[3] = (float)(c[3] * m); | |

162 | } | |

163 | } | |

164 | ||

165 | ||

166 | /*! | |

167 | * Compute the dot-product of a quaternion. | |

168 | */ | |

169 | float | |

170 | lib3ds_quat_dot(float a[4], float b[4]) { | |

171 | return(a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + a[3]*b[3]); | |

172 | } | |

173 | ||

174 | ||

175 | float | |

176 | lib3ds_quat_norm(float c[4]) { | |

177 | return(c[0]*c[0] + c[1]*c[1] + c[2]*c[2] + c[3]*c[3]); | |

178 | } | |

179 | ||

180 | ||

181 | void | |

182 | lib3ds_quat_ln(float c[4]) { | |

183 | double om, s, t; | |

184 | ||

185 | s = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]); | |

186 | om = atan2(s, (double)c[3]); | |

187 | if (fabs(s) < LIB3DS_EPSILON) { | |

188 | t = 0.0f; | |

189 | } else { | |

190 | t = om / s; | |

191 | } | |

192 | { | |

193 | int i; | |

194 | for (i = 0; i < 3; ++i) { | |

195 | c[i] = (float)(c[i] * t); | |

196 | } | |

197 | c[3] = 0.0f; | |

198 | } | |

199 | } | |

200 | ||

201 | ||

202 | void | |

203 | lib3ds_quat_ln_dif(float c[4], float a[4], float b[4]) { | |

204 | float invp[4]; | |

205 | ||

206 | lib3ds_quat_copy(invp, a); | |

207 | lib3ds_quat_inv(invp); | |

208 | lib3ds_quat_mul(c, invp, b); | |

209 | lib3ds_quat_ln(c); | |

210 | } | |

211 | ||

212 | ||

213 | void | |

214 | lib3ds_quat_exp(float c[4]) { | |

215 | double om, sinom; | |

216 | ||

217 | om = sqrt(c[0] * c[0] + c[1] * c[1] + c[2] * c[2]); | |

218 | if (fabs(om) < LIB3DS_EPSILON) { | |

219 | sinom = 1.0f; | |

220 | } else { | |

221 | sinom = sin(om) / om; | |

222 | } | |

223 | { | |

224 | int i; | |

225 | for (i = 0; i < 3; ++i) { | |

226 | c[i] = (float)(c[i] * sinom); | |

227 | } | |

228 | c[3] = (float)cos(om); | |

229 | } | |

230 | } | |

231 | ||

232 | ||

233 | void | |

234 | lib3ds_quat_slerp(float c[4], float a[4], float b[4], float t) { | |

235 | double l; | |

236 | double om, sinom; | |

237 | double sp, sq; | |

238 | float flip = 1.0f; | |

239 | int i; | |

240 | ||

241 | l = a[0] * b[0] + a[1] * b[1] + a[2] * b[2] + a[3] * b[3]; | |

242 | if (l < 0) { | |

243 | flip = -1.0f; | |

244 | l = -l; | |

245 | } | |

246 | ||

247 | om = acos(l); | |

248 | sinom = sin(om); | |

249 | if (fabs(sinom) > LIB3DS_EPSILON) { | |

250 | sp = sin((1.0f - t) * om) / sinom; | |

251 | sq = sin(t * om) / sinom; | |

252 | } else { | |

253 | sp = 1.0f - t; | |

254 | sq = t; | |

255 | } | |

256 | sq *= flip; | |

257 | for (i = 0; i < 4; ++i) { | |

258 | c[i] = (float)(sp * a[i] + sq * b[i]); | |

259 | } | |

260 | } | |

261 | ||

262 | ||

263 | void | |

264 | lib3ds_quat_squad(float c[4], float a[4], float p[4], float q[4], float b[4], float t) { | |

265 | float ab[4]; | |

266 | float pq[4]; | |

267 | ||

268 | lib3ds_quat_slerp(ab, a, b, t); | |

269 | lib3ds_quat_slerp(pq, p, q, t); | |

270 | lib3ds_quat_slerp(c, ab, pq, 2*t*(1 - t)); | |

271 | } | |

272 | ||

273 | ||

274 | void | |

275 | lib3ds_quat_tangent(float c[4], float p[4], float q[4], float n[4]) { | |

276 | float dn[4], dp[4], x[4]; | |

277 | int i; | |

278 | ||

279 | lib3ds_quat_ln_dif(dn, q, n); | |

280 | lib3ds_quat_ln_dif(dp, q, p); | |

281 | ||

282 | for (i = 0; i < 4; i++) { | |

283 | x[i] = -1.0f / 4.0f * (dn[i] + dp[i]); | |

284 | } | |

285 | lib3ds_quat_exp(x); | |

286 | lib3ds_quat_mul(c, q, x); | |

287 | } | |

288 | ||

289 | ||

290 | void | |

291 | lib3ds_quat_dump(float q[4]) { | |

292 | printf("%f %f %f %f\n", q[0], q[1], q[2], q[3]); | |

293 | } |

**Note:**See TracBrowser for help on using the browser.